#include "MagicWand.h"
#include "FMagicWandSelectionTask.h"
#include "Components/LineBatchComponent.h"
#include "Generators/MarchingCubes.h"
#include "Kismet/GameplayStatics.h"
#include "MetaCastBachelor/PointStorage/PointCloud.h"

//	INITIALIZATION

class FMagicWandSelectionTask;

UMagicWand::UMagicWand() : CurrentSelection(), World(nullptr)
{
	// Create the procedural mesh component
	ProceduralMesh = CreateDefaultSubobject<UProceduralMeshComponent>(TEXT("GeneratedMesh"));
	MarchingCubeMesh = CreateDefaultSubobject<UProceduralMeshComponent>(TEXT("GeneratedMesh2"));
	MyLineBatchComponent = CreateDefaultSubobject<ULineBatchComponent>(TEXT("LineDrawer"));
}

void UMagicWand::BeginPlay()
{
	Super::BeginPlay();

	InitReferences();
	InitProceduralMesh();
}

void UMagicWand::InitProceduralMesh() const
{
	//ProceduralMesh->AttachToComponent(MyPointCloud->PointCloudVisualizer, FAttachmentTransformRules::SnapToTargetIncludingScale);
	ProceduralMesh->SetMaterial(0, SelectionVolumeMat);
	ProceduralMesh->SetMaterial(1, SelectionVolumeMat);
	ProceduralMesh->SetMaterial(2, SelectionVolumeMat);
	ProceduralMesh->bCastDynamicShadow = false;
	ProceduralMesh->bRenderCustomDepth = true;
	ProceduralMesh->SetCastShadow(false);
	ProceduralMesh->SetCollisionEnabled(ECollisionEnabled::NoCollision);
	ProceduralMesh->SetMobility(EComponentMobility::Movable);
	ProceduralMesh->SetVisibility(true);

	MarchingCubeMesh->SetMaterial(0, SelectionVolumeMat);
	MarchingCubeMesh->SetMaterial(1, SelectionVolumeMat);
	MarchingCubeMesh->SetMaterial(2, SelectionVolumeMat);
	MarchingCubeMesh->bCastDynamicShadow = false;
	MarchingCubeMesh->bRenderCustomDepth = true;
	MarchingCubeMesh->SetCastShadow(false);
	MarchingCubeMesh->SetCollisionEnabled(ECollisionEnabled::NoCollision);
	MarchingCubeMesh->SetMobility(EComponentMobility::Movable);
	MarchingCubeMesh->SetVisibility(true);
	MarchingCubeMesh->AttachToComponent(MyPointCloud->GetRootComponent(), FAttachmentTransformRules::SnapToTargetIncludingScale);
}

void UMagicWand::InitReferences()
{
	if (!MyPointCloud || !MyPointCloud->MyDensityField) {
		UE_LOG(LogTemp, Error, TEXT("Invalid Point Cloud or DensityField provided!"));
	}else
	{
		MyDensityField = MyPointCloud->MyDensityField;
	}
	
	World = GetWorld();
	
	if (!World) {
		UE_LOG(LogTemp, Error, TEXT("Invalid world provided."));
	}
}

void UMagicWand::EraseParticles(const FVector& InputPosition)
{
	
}

//	VISUALIZATION

void UMagicWand::GenerateVoxelMeshWithCubes(const TArray<int32> Voxels) const
{
	TArray<FVector> Vertices;
	TArray<int32> Triangles;
	TArray<FColor> VertexColors;

	// Generate cube vertices and triangles for each voxel
	for (const int32 VoxelIndex : Voxels)
	{
		if(!MyDensityField->IsValidIndex(VoxelIndex))
		{
			continue;
		}

		TArray<FVector> VoxelCorners = MyDensityField->IndexToVoxelCornersWorld(VoxelIndex);

		// Add vertices for the voxel
		const int32 BaseIndex = Vertices.Num();
		Vertices.Append(VoxelCorners);

		// Define the triangles (12 triangles for 6 faces of the cube)
		// Each face of the cube has 2 triangles (6 faces * 2 triangles per face = 12 triangles)
		Triangles.Append({
			BaseIndex + 0, BaseIndex + 1, BaseIndex + 2, BaseIndex + 0, BaseIndex + 2, BaseIndex + 3, // Bottom face
			BaseIndex + 4, BaseIndex + 6, BaseIndex + 5, BaseIndex + 4, BaseIndex + 7, BaseIndex + 6, // Top face
			BaseIndex + 0, BaseIndex + 4, BaseIndex + 1, BaseIndex + 1, BaseIndex + 4, BaseIndex + 5, // Front face
			BaseIndex + 1, BaseIndex + 5, BaseIndex + 2, BaseIndex + 2, BaseIndex + 5, BaseIndex + 6, // Right face
			BaseIndex + 2, BaseIndex + 6, BaseIndex + 3, BaseIndex + 3, BaseIndex + 6, BaseIndex + 7, // Back face
			BaseIndex + 3, BaseIndex + 7, BaseIndex + 0, BaseIndex + 0, BaseIndex + 7, BaseIndex + 4  // Left face
		});
		
		// Add red color for each corner vertex
		for (int32 i = 0; i < 8; ++i)
		{
			VertexColors.Add(FColor::Green);
		}
	}

	// Create the mesh section
	AsyncTask(ENamedThreads::Type::GameThread, [this, Vertices, Triangles, VertexColors]()
	{
		//CreateMeshSections(Vertices, Triangles, VertexColors, 1000);
		ProceduralMesh->CreateMeshSection(0, Vertices, Triangles, TArray<FVector>(), TArray<FVector2D>(), VertexColors, TArray<FProcMeshTangent>(), false);
		ProceduralMesh->SetVisibility(true);
	});
}

void UMagicWand::GenerateVoxelMeshSmooth(const TSet<int32>& Voxels)
{
	if(IsMarchingCubesRunning || Voxels.IsEmpty())
		return;

	IsMarchingCubesRunning = true;
	
	FScopeLock Lock(&ProceduralMeshGuard);
	AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [this, Voxels]()
	{
		FVector Min(FLT_MAX, FLT_MAX, FLT_MAX);
		FVector Max(-FLT_MAX, -FLT_MAX, -FLT_MAX);
		
		for(const int FloodedIndex : Voxels)
		{
			Min = FVector::Min(Min, MyDensityField->IndexToVoxelPosition(FloodedIndex));
			Max = FVector::Max(Max, MyDensityField->IndexToVoxelPosition(FloodedIndex));
		}

		UE::Geometry::FMarchingCubes MarchingCubes;
		MarchingCubes.Bounds = UE::Geometry::TAxisAlignedBox3(Min, Max);
		
		//MarchingCubes.CubeSize = MarchingCubeSize == 0 ? MyDensityField->GetStep().X : MarchingCubeSize;
		const float Volume = MarchingCubes.Bounds.Volume();
		// Logarithmic scaling of the cube size based on volume
		if(Volume > 50000) {
			MarchingCubes.CubeSize = (3.0f * log10((Volume + 500) / 1000) - 4);
		}else if(Volume > 500000)
		{
			MarchingCubes.CubeSize = 4.3;
		}else {
			MarchingCubes.CubeSize = MyDensityField->GetStep().X;
		}

	MarchingCubes.CubeSize = FMath::Clamp(MarchingCubes.CubeSize, MyDensityField->GetStep().X, 10.0f);
		
		MarchingCubes.CancelF = [this]() {
			return AbortMarchingCubes;
		};
		MarchingCubes.IsoValue = 0;

		AbortMarchingCubes = false;
		// Define the implicit function
		MarchingCubes.Implicit = [this, Voxels](const FVector3d& Pos) {
			const FVector PosConverted(Pos.X, Pos.Y, Pos.Z);
			const int Index = MyDensityField->WorldPositionToIndex(PosConverted);
			return Voxels.Contains(Index) ? 1 : -1;
		};

		MarchingCubes.bParallelCompute = true;
		const UE::Geometry::FMeshShapeGenerator& Generator = MarchingCubes.Generate();
		TArray<FVector3d> Vertices3d = Generator.Vertices;
		TArray<UE::Geometry::FIndex3i> Triangles = Generator.Triangles;
		TArray<FVector3f> Normals3F = Generator.Normals;

		// Convert FVector3d to FVector
		TArray<FVector> Vertices;
		Vertices.Reserve(Generator.Vertices.Num());
		for (const FVector3d& Vertex : Vertices3d)
		{
			Vertices.Add(FVector(Vertex.X, Vertex.Y, Vertex.Z));
		}

		// Convert FIndex3i to int32
		TArray<int32> Indices;
		Indices.Reserve(Generator.Triangles.Num() * 3);
		for (const UE::Geometry::FIndex3i& Triangle : Triangles)
		{
			Indices.Add(Triangle.A);
			Indices.Add(Triangle.B);
			Indices.Add(Triangle.C);
		}

		// Convert FVector3f to FVector
		TArray<FVector> Normals;
		Normals.Reserve(Generator.Normals.Num());
		for (const FVector3f& Normal : Normals3F)
		{
			Normals.Add(FVector(Normal.X, Normal.Y, Normal.Z));
		}

		TArray<FColor> VertexColors;
		VertexColors.Reserve(Vertices.Num());
		for (int32 i = 0; i < Vertices.Num(); ++i)
		{
			VertexColors.Add(FColor::Green);
		}


		AsyncTask(ENamedThreads::Type::GameThread, [this, Vertices, Indices, Normals, VertexColors]()
		{
			FScopeLock MeshLock(&ProceduralMeshGuard);
			MarchingCubeMesh->CreateMeshSection(0, Vertices, Indices, Normals, TArray<FVector2D>(), VertexColors, TArray<FProcMeshTangent>(), false);
			MarchingCubeMesh->SetVisibility(true);
			IsMarchingCubesRunning = false;
		});
	});
}

void UMagicWand::DrawPersistentLine(const FVector& Start, const FVector& End, const FColor& Color, const float Duration, const float Thickness) const
{
	if (MyLineBatchComponent)
	{
		MyLineBatchComponent->DrawLine(Start, End, Color, 0, Thickness, Duration);
	}
}

void UMagicWand::GenerateCylinderBetweenPoints(const FVector& Start, const FVector& End, const float Radius, const int32 NumSides, const FColor Color, const int MeshSectionIndex) const
{
    FVector Axis = End - Start;
    Axis.Normalize();

    TArray<FVector> Vertices;
    TArray<int32> Triangles;
    TArray<FVector> Normals;
    TArray<FVector2D> UVs;
    TArray<FProcMeshTangent> Tangents;

    // Bottom cap
    int32 VertIndex = 0;
    Vertices.Add(Start); // Center bottom
    Normals.Add(-Axis);
    UVs.Add(FVector2D(0.5f, 0.5f));
    for (int i = 0; i <= NumSides; i++)
    {
	    const float Angle = FMath::DegreesToRadians((360.f / NumSides) * i);
        FVector Point = Start + Radius * FVector(FMath::Cos(Angle), FMath::Sin(Angle), 0);
        Vertices.Add(Point);
        Normals.Add(-Axis);
        UVs.Add(FVector2D(FMath::Cos(Angle) * 0.5f + 0.5f, FMath::Sin(Angle) * 0.5f + 0.5f));

        if (i > 0)
        {
            Triangles.Add(VertIndex);
            Triangles.Add(VertIndex + i);
            Triangles.Add(VertIndex + i + 1);
        }
    }

    // Side faces
    const int32 BottomCenterIndex = VertIndex;
    const int32 TopCenterIndex = Vertices.Num();
    Vertices.Add(End); // Center top
    Normals.Add(Axis);
    UVs.Add(FVector2D(0.5f, 0.5f));
    VertIndex++;
    for (int i = 0; i <= NumSides; i++)
    {
	    const float Angle = FMath::DegreesToRadians((360.f / NumSides) * i);
        FVector Point = End + Radius * FVector(FMath::Cos(Angle), FMath::Sin(Angle), 0);
        Vertices.Add(Point);
        Normals.Add(Axis);
        UVs.Add(FVector2D(FMath::Cos(Angle) * 0.5f + 0.5f, FMath::Sin(Angle) * 0.5f + 0.5f));

        if (i > 0)
        {
            // Connect bottom and top
            Triangles.Add(BottomCenterIndex + i);
            Triangles.Add(TopCenterIndex + i);
            Triangles.Add(BottomCenterIndex + i + 1);

            Triangles.Add(TopCenterIndex + i);
            Triangles.Add(TopCenterIndex + i + 1);
            Triangles.Add(BottomCenterIndex + i + 1);
        }
    }

	TArray<FLinearColor> VertexColors;
	VertexColors.Init(Color, Vertices.Num());
	
    // Create mesh
    ProceduralMesh->CreateMeshSection_LinearColor(MeshSectionIndex, Vertices, Triangles, Normals, TArray<FVector2D>(), VertexColors, TArray<FProcMeshTangent>(), false);
}

//	INPUT HANDLING

void UMagicWand::SelectionStartAction()
{
	if(NumberThreads.GetValue() > 0) return;

	ProceduralMesh->ClearAllMeshSections();
	FScopeLock Lock(&SelectionCacheLock);
	SelectionCache.Empty();
	SortedProximityRanges.Empty();
	SeedPointIndex = INDEX_NONE;
	
	const FVector SelectionStartPositionWorld = SelectionObject->GetComponentLocation();
	const FVector SelectionStartPositionLocal = MyPointCloud->GetPointCloudVisualizer()->GetComponentTransform().InverseTransformPosition(SelectionStartPositionWorld);
	
	FindSeed(SelectionStartPositionLocal);
	if(SeedPointIndex == INDEX_NONE) return;

	CountSelectionTime = 1 / MagicWandUpdatesPerSecond;
	IsMagicWandInitialized = true;
}

void UMagicWand::SelectionEndAction()
{
	IsMagicWandInitialized = false;
	ProceduralMesh->ClearAllMeshSections();
	MarchingCubeMesh->SetVisibility(false);
	FScopeLock Lock(&SelectionCacheLock);
		
	if(SelectionEndSound)
	{
		UGameplayStatics::PlaySound2D(World, SelectionEndSound);
	}
	if(!CurrentSelection.IsValid()) return;

	TArray<FAtomicBool> SelectionArray = CurrentSelection.Get()->GetSelectionFlags();
	for(int i = 0; i < SelectionArray.Num(); i++)
	{
		if(SelectionArray[i].Value.load())
		{
			MyPointCloud->SetSelectionFlag(i, true);
		}
	}

	MyPointCloud->SaveStateAndUpdate();
}

void UMagicWand::HandleMetaEraseReleased(const FInputActionInstance& Instance)
{
	if(NumberThreads.GetValue() > 0)
	{
		return;
	}

	Super::HandleMetaEraseReleased(Instance);
	
	CurrentSelection = MakeShared<FSelectionManager>(MyPointCloud->GetNumberOfPoints(), MyDensityField->GetVoxelNumber());

	for(int i = 0; i < MyPointCloud->GetNumberOfPoints(); i++)
	{
		MyPointCloud->SetSelectionFlag(i, false);
	}

	MyPointCloud->SaveStateAndUpdate();
}

void UMagicWand::HandleUndoAction(const FInputActionInstance& Instance)
{
	if(NumberThreads.GetValue() > 0) return;

	Super::HandleUndoAction(Instance);
}

void UMagicWand::HandleRedoAction(const FInputActionInstance& Instance)
{
	if(NumberThreads.GetValue() > 0) return;
	
	Super::HandleRedoAction(Instance);
}

//	MAGIC WAND SELECTION

void UMagicWand::InitMagicWandSelection()
{	
	ProceduralMesh->ClearAllMeshSections();
	AbortMarchingCubes = true;
	//AbortMagicWand.Store(false);
}

void UMagicWand::PerformMagicWandSelection(const float ProximityThreshold)
{
	if (SeedPointIndex == INDEX_NONE) return;

	if(SelectionStartSound)	UGameplayStatics::PlaySound2D(World, SelectionStartSound);

	CurrentSelection = GetSelectionCacheResultCopy(ProximityThreshold);
	CurrentSelection.Get()->SelectIndex(SeedPointIndex);
		
	TQueue<int32>* ProcessQueue = new TQueue<int32>{};
		
	for (int32 Index : CurrentSelection.Get()->GetPointsAsIndexList())
	{
		ProcessQueue->Enqueue(Index);
	}
		
	FMagicWandSelectionTask* Task = new FMagicWandSelectionTask(this, ProcessQueue, ProximityThreshold, CurrentSelection.Get()->SelectedCount());
	NumberThreads.Increment();
	Task->Start();
}

void UMagicWand::FindSeed(const FVector& InputPosition)
{
	const int32 InputVoxelIndex = MyDensityField->WorldPositionToIndex(InputPosition);
	TArray<int32> Neighbors = MyDensityField->IndexToVoxelNeighbors(InputVoxelIndex);
	Neighbors.Add(InputVoxelIndex);
	float MinDistance = FLT_MAX;
	SeedPointIndex = INDEX_NONE;
	
	for(int NeighborDistance = 0; NeighborDistance < 5; ++NeighborDistance)
	{
		const int NumNeighbors = Neighbors.Num();

		for (int i = 0; i < NumNeighbors; ++i)
		{
			const int CurrentNeighborIndex = Neighbors[i];
			TArray<int32> NeighborsOfNeighbors = MyDensityField->IndexToVoxelNeighbors(CurrentNeighborIndex);

			for (int CurrentNeighborOfNeighborIndex : NeighborsOfNeighbors)
			{
				if (!Neighbors.Contains(CurrentNeighborOfNeighborIndex))
				{
					Neighbors.Add(CurrentNeighborOfNeighborIndex);
				}
			}
		}
	}
	    
	for (const int CurrentNeighborIndex : Neighbors)
	{
		TArray<int32> PointIndices = MyDensityField->VoxelPointLookupTable->GetPointsInVoxel(CurrentNeighborIndex);

		for(const int32 CurrentIndex : PointIndices)
		{
			FVector CurrentPosition = MyPointCloud->GetPositionVectors()[CurrentIndex];
			const float Distance = FVector::Dist(InputPosition, CurrentPosition);
			if (Distance < MinDistance)
			{
				MinDistance = Distance;
				SeedPointIndex = CurrentIndex;
			}
		}
	}
	
	if(SeedPointIndex != INDEX_NONE)
	{
		SeedPointPositionLocal = MyPointCloud->GetPositionVectors()[SeedPointIndex];
	}
}

void UMagicWand::ExpandFromAllPointsInQueue(TQueue<int32>* ProcessQueue, const float ProximityThreshold, int ThreadLoad)
{
	//UE_LOG(LogTemp, Warning, TEXT("Opened New Thread! Number of Threads now: %d"), NumberThreads.GetValue());

	const FVoxelPointLookupTable MyVoxelPointLookupTable = *MyDensityField->VoxelPointLookupTable;
			
	while (!ProcessQueue->IsEmpty())
	{    	
		if(ThreadLoad > 1000  && NumberThreads.GetValue() < MaxThreadCount) CreateNewExpansionThread(ProcessQueue, ThreadLoad, ProximityThreshold);

		int32 CurrentQueuePointIndex;
		ProcessQueue->Dequeue(CurrentQueuePointIndex);
		
		if(MyPointCloud && MyPointCloud->GetPositionVectors().IsValidIndex(CurrentQueuePointIndex))
		{
			const FVector CurrentQueuePointPosition = MyPointCloud->GetPositionVectors()[CurrentQueuePointIndex];

			ExpandFromPoint(ProcessQueue, ProximityThreshold, ThreadLoad, CurrentQueuePointPosition, &MyVoxelPointLookupTable);
		}
		
		ThreadLoad--;
	}
	
	FinishSelectionThread(ProximityThreshold);
}

void UMagicWand::ExpandFromPoint(TQueue<int32>* ProcessQueue, const float ProximityThreshold, int& ThreadLoad, const FVector& ExpansionPoint, const FVoxelPointLookupTable* MyVoxelPointLookupTable) const
{
	const FVector Step = MyDensityField->GetStep();
	const int32 StartX = FMath::Max(0, FMath::FloorToInt((ExpansionPoint.X - ProximityThreshold - MyPointCloud->GetMinBounds().X) / Step.X));
	const int32 EndX = FMath::Min(MyDensityField->GetXNum() - 1, FMath::FloorToInt((ExpansionPoint.X + ProximityThreshold - MyPointCloud->GetMinBounds().X) / Step.X));
	const int32 StartY = FMath::Max(0, FMath::FloorToInt((ExpansionPoint.Y - ProximityThreshold - MyPointCloud->GetMinBounds().Y) / Step.Y));
	const int32 EndY = FMath::Min(MyDensityField->GetYNum() - 1, FMath::FloorToInt((ExpansionPoint.Y + ProximityThreshold - MyPointCloud->GetMinBounds().Y) / Step.Y));
	const int32 StartZ = FMath::Max(0, FMath::FloorToInt((ExpansionPoint.Z - ProximityThreshold - MyPointCloud->GetMinBounds().Z) / Step.Z));
	const int32 EndZ = FMath::Min(MyDensityField->GetZNum() - 1, FMath::FloorToInt((ExpansionPoint.Z + ProximityThreshold - MyPointCloud->GetMinBounds().Z) / Step.Z));

	const float SquaredProximityThreshold = ProximityThreshold * ProximityThreshold;
		
	for (int32 Z = StartZ; Z <= EndZ; ++Z)
	{			
		for (int32 Y = StartY; Y <= EndY; ++Y)
		{
			for (int32 X = StartX; X <= EndX; ++X)
			{
				const int32 CurrentVoxelComparisonIndex = MyDensityField->GridPositionToIndex(X, Y, Z);


				if(CurrentSelection.Get()->IsFullVoxel(CurrentVoxelComparisonIndex))
				{
					continue;
				}
				
				TArray<int32> PointIndices = MyVoxelPointLookupTable->GetPointsInVoxel(CurrentVoxelComparisonIndex);

				int Selected = 0;
				for(const int32 CurrentPointComparisonIndex : PointIndices)
				{
					if(CurrentSelection.Get()->IsSelected(CurrentPointComparisonIndex))
					{
						Selected++;
						continue;
					}
    					
					FVector CurrentComparisonPosition = MyPointCloud->GetPositionVectors()[CurrentPointComparisonIndex];
					const double SquaredDistance = FVector::DistSquared(ExpansionPoint, CurrentComparisonPosition);

					if (SquaredDistance <= SquaredProximityThreshold)
					{
						ProcessQueue->Enqueue(CurrentPointComparisonIndex);
						ThreadLoad++;
						Selected++;
							
						CurrentSelection.Get()->SelectIndex(CurrentPointComparisonIndex);
					}
				}
				
				if(Selected == PointIndices.Num())
				{
					CurrentSelection.Get()->AddFullVoxel(CurrentVoxelComparisonIndex);
				}
			}
		}
	}
}

void UMagicWand::FinishSelectionThread(const float ProximityThreshold)
{
	NumberThreads.Decrement();
		
	//UE_LOG(LogTemp, Warning, TEXT("Thread Closed! Number of Threads now: %d"), NumberThreads.GetValue());
	if(NumberThreads.GetValue() == 0)
	{
		CacheSelectionResults(ProximityThreshold, CurrentSelection);
		
		FScopeLock Lock(&SelectionCacheLock);

		if(CurrentSelection.IsValid())
		{
			TSet<int32> Voxels;
			TArray<int32> PointIndices = CurrentSelection.Get()->GetPointsAsIndexList();
			for (int32 i = 0; i < PointIndices.Num(); i++)
			{
				const int Index = PointIndices[i];
				if(MyPointCloud->GetPositionVectors().IsValidIndex(Index))
				{
					FVector Point = MyPointCloud->GetPositionVectors()[Index];
					int32 VoxelFromIndex = MyDensityField->WorldPositionToIndex(Point);
					Voxels.Add(VoxelFromIndex);
				}
			}
		
			GenerateVoxelMeshSmooth(Voxels);
		}

		//CurrentSelection.Reset();
	}else if(NumberThreads.GetValue() < 0)
	{
		UE_LOG(LogTemp, Error, TEXT("More Threads closed than opened!"));
	}
}

void UMagicWand::CreateNewExpansionThread(TQueue<int32>* ProcessQueue, int& NumberToProcess, const float ProximityThreshold)
{
	NumberThreads.Increment();
	TQueue<int32>* TempQueue = new TQueue<int32>{};
	const int NumberRemove = NumberToProcess / 2;
			
	for(int i = 0; i < NumberRemove; ++i)
	{
		int32 CurrentQueuePointIndex;
		ProcessQueue->Dequeue(CurrentQueuePointIndex);
		NumberToProcess--;
		TempQueue->Enqueue(CurrentQueuePointIndex);
	}

	FMagicWandSelectionTask* Task = new FMagicWandSelectionTask(this, TempQueue, ProximityThreshold, NumberRemove);
	Task->Start();
}

void UMagicWand::CacheSelectionResults(const float ProximityThreshold, const TSharedPtr<FSelectionManager>& SelectionResults)
{
	FScopeLock Lock(&SelectionCacheLock);
	// Check if the proximity range is already present
	if (!SortedProximityRanges.Contains(ProximityThreshold))
	{
		// Add new range to the array and sort it
		SortedProximityRanges.Add(ProximityThreshold);
		SortedProximityRanges.Sort();

		// Update the cache with new indices for a given proximity range

		const TSharedRef<FSelectionManager> CacheCopy = MakeShared<FSelectionManager>(*SelectionResults.Get());  // This will return a shared pointer
		SelectionCache.Add(ProximityThreshold, CacheCopy);
	}
}

TSharedPtr<FSelectionManager> UMagicWand::GetSelectionCacheResultCopy(const float ProximityThreshold) {
	FScopeLock Lock(&SelectionCacheLock);
	
	for (int i = SortedProximityRanges.Num() - 1; i >= 0; --i) {
		
		if (SortedProximityRanges[i] <= ProximityThreshold) {
			FSelectionManager* CopiedManager(SelectionCache[SortedProximityRanges[i]].Get());
			return MakeShared<FSelectionManager>(*CopiedManager);  // This will return a shared pointer
		}
	}

	// If no valid selection manager is found, create a new one and add it to the map
	TSharedPtr<FSelectionManager> NewManager = MakeShared<FSelectionManager>(MyPointCloud->GetNumberOfPoints(), MyDensityField->GetVoxelNumber());
	SelectionCache.Add(ProximityThreshold, NewManager);
	SortedProximityRanges.Add(ProximityThreshold);
	SortedProximityRanges.Sort();

	return NewManager; // Return the newly created manager
}

//	TICK

void UMagicWand::TickComponent(const float DeltaTime, const ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

	CountSelectionTime += DeltaTime;

	if(!Select)
	{
		MarchingCubeMesh->SetVisibility(false);
	}
}

void UMagicWand::SelectParticles(const FVector& InputPosition)
{
	if(!IsMagicWandInitialized) return;
	
	const FVector CurrentSelectionPositionWorld = SelectionObject->GetComponentLocation();
	const FVector CurrentSelectionPositionLocal = MyPointCloud->GetPointCloudVisualizer()->GetComponentTransform().InverseTransformPosition(CurrentSelectionPositionWorld);
	const float ProximityThreshold =  FMath::Clamp(FVector::Dist(SeedPointPositionLocal, CurrentSelectionPositionLocal) / ThresholdDistanceScaling, MinThreshold, MaxThreshold);

	const FVector SelectionDirectionLocal = CurrentSelectionPositionLocal - SeedPointPositionLocal;
	const FVector ProximityEndPointLocal = SeedPointPositionLocal + (SelectionDirectionLocal.GetSafeNormal() * ProximityThreshold);

	const FVector SelectionStartPositionWorld = MyPointCloud->GetPointCloudVisualizer()->GetComponentTransform().TransformPosition(SeedPointPositionLocal);
	const FVector ProximityEndPointWorld = MyPointCloud->GetPointCloudVisualizer()->GetComponentTransform().TransformPosition(ProximityEndPointLocal);
	
	GenerateCylinderBetweenPoints(SelectionStartPositionWorld, ProximityEndPointWorld, 0.05, 8, FColor::Red, 1);
	GenerateCylinderBetweenPoints((CurrentSelectionPositionWorld - ProximityEndPointWorld).GetSafeNormal() * 0.05 + ProximityEndPointWorld, CurrentSelectionPositionWorld, 0.05, 8, FColor::Green, 0);

	if(CountSelectionTime >= 1.0f / MagicWandUpdatesPerSecond && NumberThreads.GetValue() == 0)
	{
		CountSelectionTime = 0;
		PerformMagicWandSelection(ProximityThreshold);
	}
}