using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Linq;

public class Pathfinding : MonoBehaviour
{
    public static Pathfinding Instance { get; private set; }
    private const int MOVE_STRAIGHT_COST = 10;
    private const int MOVE_DIAGONAL_COST = 14;

    [SerializeField] private Transform gridDebugObjectPrefab;
    [SerializeField] private LayerMask obstaclesLayerMask;
    private int width;
    private int height;
    private float cellSize;
    GridSystem<PathNode> gridSystem;

    private void Awake() {
        if (Instance != null)
        {
            Debug.LogError("There's more than one Pathfinding! " + transform + " - " + Instance);
            Destroy(gameObject);
            return;
        }
        Instance = this;
    }

    public void Setup(int width, int height, float cellSize) {
        this.width = width;
        this.height = height;
        this.cellSize = cellSize;

        gridSystem= new GridSystem<PathNode>(width, height, cellSize,
            (GridSystem<PathNode> g, GridPosition gridPosition) => new PathNode(gridPosition));
        //gridSystem.CreateDebugObjects(gridDebugObjectPrefab);

        for (int x = 0; x < gridSystem.GetWidth(); x++)
        {
            for (int z = 0; z < gridSystem.GetHeight(); z++)
            {
                GridPosition gridPosition = new GridPosition(x,z);
                Vector3 worldPosition = LevelGrid.Instance.GetWorldPosition(gridPosition);
                float raycaseOffsetDistance = 5f;
                if (Physics.Raycast(
                    worldPosition + Vector3.down * raycaseOffsetDistance
                    , Vector3.up
                    , raycaseOffsetDistance *2
                    , obstaclesLayerMask)) {
                        gridSystem.GetGridObject(x,z).SetIsWalkable(false);
                }
            }
        }

   }
    public void clearGrid() {
        for (int x = 0; x < gridSystem.GetWidth(); x++)
        {
            for (int z = 0; z < gridSystem.GetHeight(); z++)
            {
                PathNode pathNode = gridSystem.GetGridObject(x,z);

                pathNode.SetGCost(int.MaxValue);
                pathNode.SetHCost(0);
                pathNode.CalculateFCost();
                pathNode.ResetCameFromPathNode();
            }
        }       
    }
    public bool TryFindPath(GridPosition startGridPosition, GridPosition endGridPosition, out (IEnumerable<GridPosition> itinerary, int distance) result) {
        SortedSet<(int fCost, PathNode node)> openList = new SortedSet<(int fCost, PathNode node)>();
        HashSet<PathNode> closedSet = new HashSet<PathNode>();
        HashSet<PathNode> seenSet = new HashSet<PathNode>();

        PathNode startNode = gridSystem.GetGridObject(startGridPosition);
        PathNode endNode = gridSystem.GetGridObject(endGridPosition);

        clearGrid();

        openList.Add((0,startNode));

        startNode.SetGCost(0);
        startNode.SetHCost(CalculateDistance(startGridPosition, endGridPosition));
        startNode.CalculateFCost();

        while (openList.Count > 0) {
            (int fCost, PathNode currentNode)  = openList.Min;

            if (currentNode == endNode) {
                // Reached final node
                result.itinerary = CalculatePath(endNode);
                result.distance = endNode.GetGCost();
                return true;
            }

            openList.Remove((fCost, currentNode));
            closedSet.Add(currentNode);

            foreach (PathNode neighbourNode in GetNeighbourList(currentNode).Where(node => !closedSet.Contains(node))){
                if (!neighbourNode.IsWalkable()) {
                    closedSet.Add(neighbourNode);
                    continue;
                }
                int tentativeGCost = 
                    currentNode.GetGCost() + CalculateDistance(currentNode.GetGridPosition(), neighbourNode.GetGridPosition());

                if (tentativeGCost < neighbourNode.GetGCost())
                {
                    neighbourNode.SetCameFromPathNode(currentNode);
                    neighbourNode.SetGCost(tentativeGCost);
                    neighbourNode.SetHCost(CalculateDistance(neighbourNode.GetGridPosition(), endGridPosition));
                    neighbourNode.CalculateFCost();
                    openList.Add((neighbourNode.GetFCost(),neighbourNode));
                }
            }
        }

        // No path found
        result.itinerary = null;
        result.distance = int.MaxValue;
        return false;
    }

    public int CalculateDistance(GridPosition gridPositionA, GridPosition gridPositionB)
    {
        GridPosition gridPositionDistance = gridPositionA - gridPositionB;
        int xDistance = Mathf.Abs(gridPositionDistance.x);
        int zDistance = Mathf.Abs(gridPositionDistance.z);
        int remaining = Mathf.Abs(xDistance - zDistance);
        return MOVE_DIAGONAL_COST * Mathf.Min(xDistance, zDistance) + MOVE_STRAIGHT_COST * remaining;
    }

    private PathNode GetLowestFCostPathNode(List<PathNode> pathNodeList)
    {
        PathNode lowestFCostPathNode = pathNodeList[0];
        for (int i = 0; i < pathNodeList.Count; i++)
        {
            if (pathNodeList[i].GetFCost() < lowestFCostPathNode.GetFCost())
            {
                lowestFCostPathNode = pathNodeList[i];
            }
        }
        return lowestFCostPathNode;
    }

    private IEnumerable<PathNode> GetNeighbourList(PathNode currentNode)
    {
        GridPosition gridPosition = currentNode.GetGridPosition();

        for (int x = Mathf.Max(gridPosition.x - 1, 0) ; x <= Mathf.Min(gridPosition.x + 1,gridSystem.GetWidth() - 1); x++) {
            for (int z = Mathf.Max(gridPosition.z - 1,0); z <= Mathf.Min(gridPosition.z + 1,gridSystem.GetHeight() - 1); z++) {
                //Debug.Log($"GetNeighbourList {x},{z}");
                yield return gridSystem.GetGridObject(x, z);
            }
        }
    }

    private IEnumerable<GridPosition> CalculatePath(PathNode endNode)
    {
        return endNode.GetNextCameFromPathNode().Reverse().Select(node => node.GetGridPosition());
    }

    public void SetIsWalkableGridPosition(GridPosition gridPosition, bool isWalkable) {
        gridSystem.GetGridObject(gridPosition).SetIsWalkable(isWalkable);
    }

}