// GraphAStarNavMesh.h

#pragma once

#include "CoreMinimal.h"
#include "AStarGrid.h"
#include "GraphAStar.h"
#include "NavMesh/RecastNavMesh.h"
#include "GraphAStarNavMesh.generated.h"

 /**
    * Generic graph A* implementation
    * TGraph holds graph representation.Needs to implement functions :
 */
struct FAStarGridGraph
{
public:
    void SetAStarGrid(AAStarGrid* InWorldGrid)
    {
        AStarGrid = InWorldGrid;
    }

    typedef FIntPoint FNodeRef;

    int32 GetNeighbourCount(FNodeRef NodeRef) const
    {
        return 8;
    }

    bool IsValidRef(FNodeRef NodeRef) const
    {
        return AStarGrid->IsValidGridCell(NodeRef);
    }

    FNodeRef GetNeighbour(const FNodeRef NodeRef, const int32 NeighbourIndex) const
    {
        static const FIntPoint NeighbourOffset[8] =
        {
            { 0, 1 }, // D
            { 1, 1 }, // N
            { 1, 0 }, // C
            { 1, -1 }, // C
            { 0, -1 }, // D
            { -1, -1 }, // C
            { -1, 0 }, // N
            { -1, 1 }, // C
        };

        return NodeRef + NeighbourOffset[NeighbourIndex];
    }

private:
    class AAStarGrid* AStarGrid;
};

/**
 * TQueryFilter (FindPath's parameter) filter class is what decides which graph edges can be used and at what cost.
 */
struct FAStarGridGraphQueryFilter
{
public:
    // 
    FAStarGridGraphQueryFilter() : AStarGrid(nullptr) {}

    void SetAStarGrid(AAStarGrid* InWorldGrid)
    {
        AStarGrid = InWorldGrid;
    }

    float GetHeuristicScale() const
    {
        return 1;
    }

    float GetHeuristicCost(const FIntPoint StartNodeRef, const FIntPoint EndNodeRef) const
    {
        return FMath::Abs(StartNodeRef.X - EndNodeRef.X) + FMath::Abs(StartNodeRef.Y - EndNodeRef.Y);
    }

    float GetTraversalCost(const FIntPoint StartNodeRef, const FIntPoint EndNodeRef) const
    {
        return 1;
    }

    bool IsTraversalAllowed(const FIntPoint NodeA, const FIntPoint NodeB) const
    {
        if (!AStarGrid->IsGridCellWalkable(NodeA) || !AStarGrid->IsGridCellWalkable(NodeB))
            return false;

        static const FIntPoint kXOffset(1, 0);
        static const FIntPoint kYOffset(0, 1);

        if (NodeB.X < NodeA.X)
        {
            if (NodeB.Y < NodeA.Y)
            {
                if (!AStarGrid->IsGridCellWalkable(NodeA - kXOffset) || !AStarGrid->IsGridCellWalkable(NodeA - kYOffset))
                    return false;
            }
            else if (NodeB.Y > NodeA.Y)
            {
                if (!AStarGrid->IsGridCellWalkable(NodeA - kXOffset) || !AStarGrid->IsGridCellWalkable(NodeA + kYOffset))
                    return false;
            }
        }
        else if (NodeB.X > NodeA.X)
        {
            if (NodeB.Y < NodeA.Y)
            {
                if (!AStarGrid->IsGridCellWalkable(NodeA + kXOffset) || !AStarGrid->IsGridCellWalkable(NodeA - kYOffset))
                    return false;
            }
            else if (NodeB.Y > NodeA.Y)
            {
                if (!AStarGrid->IsGridCellWalkable(NodeA + kXOffset) || !AStarGrid->IsGridCellWalkable(NodeA + kYOffset))
                    return false;
            }
        }

        return true;
    }

    bool WantsPartialSolution() const
    {
        return false;
    }

    bool ShouldIgnoreClosedNodes() const
    {
        return true;
    }

    bool ShouldIncludeStartNodeInPath() const
    {
        return false;
    }

    int32 GetMaxSearchNodes() const
    {
        return 500;
    }

    float GetCostLimit() const
    {
        return 3000.0f;
    }

private:
    class AAStarGrid* AStarGrid;
};

struct FAStarNavMeshPath : public FNavMeshPath
{
    FORCEINLINE
        virtual float GetCostFromIndex(int32 PathPointIndex) const override
    {
        return CurrentPathCost;
    }

    FORCEINLINE
        virtual float GetLengthFromPosition(FVector SegmentStart, uint32 NextPathPointIndex) const override
    {
        // We exclude the starting point so -1	
        return PathPoints.Num() - 1;
    }

    float CurrentPathCost{ 0 };
};

UCLASS(config = Engine, defaultconfig, hidecategories = (Input, Rendering, Tags, "Utilities|Transformation", Actor, Layers, Replication), notplaceable)
class AGraphAStarNavMesh : public ARecastNavMesh
{
	GENERATED_BODY()
	
private:
    class AAStarGrid* AStarGrid;

    AGraphAStarNavMesh(const FObjectInitializer& ObjectInitializer);

    bool ProjectPoint(const FVector& Point, FNavLocation& OutLocation, const FVector& Extent, FSharedConstNavQueryFilter Filter, const UObject* Querier) const;

	// Manually set the function pointer in your new navigation class constructor
	static FPathFindingResult FindPath(const FNavAgentProperties& AgentProperties, const FPathFindingQuery& Query);
};

// GraphAStarNavMesh.cpp
#include "AStar/GraphAStarNavMesh.h"
#include "AI/Navigation/NavigationTypes.h"
#include "EngineUtils.h"

AGraphAStarNavMesh::AGraphAStarNavMesh(const FObjectInitializer& ObjectInitializer)
{
	if (HasAnyFlags(RF_ClassDefaultObject) == false)
	{
		FindPathImplementation = FindPath;
		FindHierarchicalPathImplementation = FindPath;
	}
}

bool AGraphAStarNavMesh::ProjectPoint(const FVector& Point, FNavLocation& OutLocation, const FVector& Extent, FSharedConstNavQueryFilter Filter, const UObject* Querier) const
{
	FIntPoint GridPos;
	if (AStarGrid->GetGridCellForWorldPos(Point, GridPos))
	{
		OutLocation = FNavLocation(Point);
		return true;
	}

	return false;
}

FPathFindingResult AGraphAStarNavMesh::FindPath(const FNavAgentProperties& AgentProperties, const FPathFindingQuery& Query)
{
	const ANavigationData* Self = Query.NavData.Get();
	check(Cast<const AGraphAStarNavMesh>(Self));

	const AGraphAStarNavMesh* AStar{ Cast<const AGraphAStarNavMesh>(Self) };
	check(AStar != nullptr);

	if (AStar == nullptr)
		return ENavigationQueryResult::Error;

	FPathFindingResult PathFindingResult(ENavigationQueryResult::Error);
	PathFindingResult.Path = Query.PathInstanceToFill.IsValid() ? Query.PathInstanceToFill : Self->CreatePathInstance<FNavigationPath>(Query);

	FNavigationPath* NavPath = PathFindingResult.Path.Get();
	FNavMeshPath* NavMeshPath = NavPath ? NavPath->CastPath<FNavMeshPath>() : nullptr;

	if (NavMeshPath)
	{
		PathFindingResult.Path = Query.PathInstanceToFill;
		NavMeshPath->ResetForRepath();
	}
	else
	{
		PathFindingResult.Path = Self->CreatePathInstance<FNavMeshPath>(Query);
		NavPath = PathFindingResult.Path.Get();
		NavMeshPath = NavPath ? NavPath->CastPath<FNavMeshPath>() : nullptr;
	}

	const FNavigationQueryFilter* NavFilter = Query.QueryFilter.Get();
	if (NavMeshPath && NavFilter)
	{
		NavMeshPath->ApplyFlags(Query.NavDataFlags);

		const FVector AdjustedEndLocation = NavFilter->GetAdjustedEndLocation(Query.EndLocation);
		if ((Query.StartLocation - AdjustedEndLocation).IsNearlyZero() == true)
		{
			PathFindingResult.Path->GetPathPoints().Reset();
			PathFindingResult.Path->GetPathPoints().Add(FNavPathPoint(AdjustedEndLocation));
			PathFindingResult.Result = ENavigationQueryResult::Success;
		}
	}
	else
	{
		PathFindingResult.Path->GetPathPoints().Reset();

		if ((Query.StartLocation - Query.EndLocation).IsNearlyZero())
		{
			PathFindingResult.Path->GetPathPoints().Add(FNavPathPoint(Query.EndLocation));
		}
		else if (Query.QueryFilter.IsValid())
		{
			FIntPoint CurrentGridPos, TargetGridPos;
			AStar->AStarGrid->GetGridCellForWorldPos(Query.StartLocation, CurrentGridPos);
			AStar->AStarGrid->GetGridCellForWorldPos(Query.EndLocation, TargetGridPos);

			TArray<FIntPoint> Path;

			FAStarGridGraph AStarGridGraph;
			AStarGridGraph.SetAStarGrid(AStar->AStarGrid);
			FGraphAStar<FAStarGridGraph>Pathfinder(AStarGridGraph);

			FAStarGridGraphQueryFilter AStarGridGraphQueryFilter;
			AStarGridGraphQueryFilter.SetAStarGrid(AStar->AStarGrid);

			/*
			see reference to function template instantiation 'EGraphAStarResult FGraphAStar<FAStarGridGraph,FGraphAStarDefaultPolicy,FGraphAStarDefaultNode<TGraph>,false>::FindPath<FAStarGridGraphQueryFilter,TArray<FIntPoint,FDefaultAllocator>>(const FGraphAStarDefaultNode<TGraph> &,const FGraphAStarDefaultNode<TGraph> &,const TQueryFilter &,TResultPathInfo &)' being compiled with
			[
				TGraph=FAStarGridGraph,
				TQueryFilter=FAStarGridGraphQueryFilter,
				TResultPathInfo=TArray<FIntPoint,FDefaultAllocator>
			]
			*/
			EGraphAStarResult AStarResult{Pathfinder.FindPath(CurrentGridPos, TargetGridPos,AStarGridGraphQueryFilter, Path)};

			// PathIndices array computed by FGraphAStar will not contain the starting point, so we need to add it manually to the Path::PathPoints array, make sure to convert from the query location to the grid position
			Path.Insert(CurrentGridPos, 0);

			switch (AStarResult)
			{
				case SearchFail:
					PathFindingResult.Result = ENavigationQueryResult::Fail;
					break;
				case SearchSuccess:
					for (auto& Point : Path)
						NavPath->GetPathPoints().Add(FNavPathPoint(AStar->AStarGrid->GetWorldPosForGridCellCentre(Point)));

					PathFindingResult.Result = ENavigationQueryResult::Success;

					NavPath->MarkReady();
					break;
				case GoalUnreachable:
					PathFindingResult.Result = ENavigationQueryResult::Invalid;
					break;
				case InfiniteLoop:
					PathFindingResult.Result = ENavigationQueryResult::Error;
					break;
			}
		}
	}

	return PathFindingResult;
}

// AStarGrid.h
#pragma once

#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "AStarGrid.generated.h"

UCLASS()
class FOURSOCIALBURDENS_API AAStarGrid : public AActor
{
	GENERATED_BODY()
	
public:
	AAStarGrid();

	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Grid")
		USceneComponent* DefaultSceneRoot;

	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Grid")
		int GridSizeValue;

	FVector GetWorldPosForGridCell(const FIntPoint& Pos) const;

	FVector GetWorldPosForGridCellCentre(const FIntPoint& Pos) const;

	bool GetGridCellForWorldPos(const FVector& WorldPos, FIntPoint& GridPos) const;

	bool IsValidGridCell(const FIntPoint& Location) const;

	bool IsGridCellWalkable(const FIntPoint& Location) const;

private:
    // Grid cell size depends on character's bounds
	UFUNCTION(BlueprintPure, Category = "Grid")
		FVector2D SetUnitBounds(FVector2D ActorBoundsHalfExtent);

	FVector2D UnitBounds;

	UFUNCTION(BlueprintPure, Category = "Grid")
		FIntPoint GridSize() const;

	UFUNCTION(BlueprintPure, Category = "Grid")
		FIntPoint GridCellSize() const;

	UFUNCTION(BlueprintPure, Category = "Grid")
		FIntPoint CellAmount() const;

	FIntPoint CellDisplacement() const;

	uint32 ClosestNumberInArray(TArray<uint32> Arr, float Target) const;

	int32 GetCellIndex(const FVector& WorldLocation);
};

// AStarGrid.cpp
#include "AStar/AStarGrid.h"

AAStarGrid::AAStarGrid()
{
	DefaultSceneRoot = CreateDefaultSubobject<USceneComponent>("DefaultSceneRoot");
	
	SetRootComponent(DefaultSceneRoot);
	DefaultSceneRoot->SetMobility(EComponentMobility::Static);
}

bool AAStarGrid::GetGridCellForWorldPos(const FVector& WorldPos, FIntPoint& GridPos) const
{
	GridPos.Y = WorldPos.X / GridCellSize().X;
	GridPos.X = WorldPos.Y / GridCellSize().Y;

	return (GridPos.X > -(CellAmount().X) && GridPos.Y > -(CellAmount().Y) && GridPos.X < CellAmount().X&& GridPos.Y < CellAmount().Y);
}

bool AAStarGrid::IsValidGridCell(const FIntPoint& Location) const
{
	return (Location.X > -(CellAmount().X) && Location.Y > -(CellAmount().Y) && Location.X < CellAmount().X&& Location.Y < CellAmount().Y);
}

// Change later to check via raycast
bool AAStarGrid::IsGridCellWalkable(const FIntPoint& Location) const
{
	return true;
}

FVector2D AAStarGrid::SetUnitBounds(FVector2D ActorBoundsHalfExtent)
{
	return UnitBounds = ActorBoundsHalfExtent * 2;
}

FVector AAStarGrid::GetWorldPosForGridCell(const FIntPoint& Pos) const
{
	return FVector(Pos.Y * GridCellSize().X + GetActorLocation().X - CellDisplacement().X, Pos.X * GridCellSize().Y + GetActorLocation().Y - CellDisplacement().Y, GetActorLocation().Z);
}

FVector AAStarGrid::GetWorldPosForGridCellCentre(const FIntPoint& Pos) const
{
	return GetWorldPosForGridCell(Pos) + (FVector(GridCellSize().X, GridCellSize().Y, 0) * 0.5f);
}

int32 AAStarGrid::GetCellIndex(const FVector& WorldLocation)
{
	FIntPoint GridPos = FIntPoint(WorldLocation.X, WorldLocation.Y);
	return GetGridCellForWorldPos(WorldLocation, GridPos) ? (WorldLocation.X * CellAmount().Y) + WorldLocation.Y : INDEX_NONE;
}

FIntPoint AAStarGrid::GridSize() const
{
	return FIntPoint(GridSizeValue);
}

FIntPoint AAStarGrid::GridCellSize() const
{
	float UnitCellSize = (UnitBounds.X > UnitBounds.Y) ? UnitBounds.X : UnitBounds.Y;

	if (FMath::Fmod(UnitCellSize, GridSizeValue) == 0)
		return FIntPoint(UnitCellSize);

	TArray<uint32> Factors;

	for (int Factor = 2; Factor <= GridSizeValue; Factor++)
		if (FMath::Fmod(GridSizeValue, Factor) == 0)
			Factors.Add(Factor);

	return FIntPoint(ClosestNumberInArray(Factors, UnitCellSize));
}

FIntPoint AAStarGrid::CellAmount() const
{
	return FIntPoint(GridSize().X / GridCellSize().X, GridSize().Y / GridCellSize().Y);
}

FIntPoint AAStarGrid::CellDisplacement() const
{
	return FIntPoint(GridCellSize().X / 2, GridCellSize().Y / 2);
}

uint32 AAStarGrid::ClosestNumberInArray(TArray<uint32> Arr, float Target) const
{
	//  https://stackoverflow.com/questions/8584902/get-the-closest-number-out-of-an-array: 'Poor runtime with large datasets'
	uint32 Curr = Arr[0];

	for (uint32& Val : Arr)
		if (FMath::Abs(Target - Val) < FMath::Abs(Target - Curr))
			Curr = Val;

	return Curr;
}