TurnBasedStrategyCourse/Assets/Scripts/Pathfinding.cs

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

public class Pathfinding : MonoBehaviour {
    private const int MOVE_STRAIGHT_COST = 10;
    private const int MOVE_DIAGONAL_COST = 14;
    private const float RAYCAST_OFFSET_DISTANCE = 1f;

    [SerializeField] private Transform gridDebugObjectPrefab;
    [SerializeField] private LayerMask obstaclesLayerMask;
    [SerializeField] private LayerMask floorLayerMask;
    [SerializeField] private Transform pathfindingLinkContainer;
    public static Pathfinding Instance { get; private set; }
    private List<GridSystem<PathNode>> GridSystemList { get; set; }
    private List<PathfindingLink> PathfindingLinkList { get; set; }

    private int Width { get; set; }
    private int Height { get; set; }
    public float CellSize { get; private set; }
    private int FloorAmount { get; set; }

    private void Awake() {
        if (Instance is not null) {
            Debug.LogError($"There is more than one Pathfinding! {transform} - {Instance}");
            Destroy(gameObject);
            return;
        }

        Instance = this;
    }

    public void Setup(int width, int height, float cellSize, int floorAmount) {
        Width = width;
        Height = height;
        CellSize = cellSize;
        FloorAmount = floorAmount;

        GridSystemList = new();

        for (int floor = 0; floor < FloorAmount; floor++) {
            GridSystem<PathNode> gridSystem = new(width, height, cellSize, floor, LevelGrid.FLOOR_HEIGHT, (_, gridPosition) => new(gridPosition));
            // GridSystem.CreateDebugObjects(gridDebugObjectPrefab);
            GridSystemList.Add(gridSystem);
        }

        for (int xPosition = 0; xPosition < Width; xPosition++) {
            for (int zPosition = 0; zPosition < Height; zPosition++) {
                for (int floor = 0; floor < FloorAmount; floor++) {
                    GridPosition gridPosition = new(xPosition, zPosition, floor);
                    Vector3 worldPosition = LevelGrid.Instance.GetWorldPosition(gridPosition);

                    // GetNode(xPosition, zPosition, floor).IsWalkable = false;

                    if (Physics.Raycast(worldPosition + Vector3.up * RAYCAST_OFFSET_DISTANCE, Vector3.down, RAYCAST_OFFSET_DISTANCE * 2, floorLayerMask))
                        GetNode(xPosition, zPosition, floor).IsWalkable = true; //has Floor

                    if (Physics.Raycast(worldPosition + Vector3.down * RAYCAST_OFFSET_DISTANCE, Vector3.up, RAYCAST_OFFSET_DISTANCE * 2, obstaclesLayerMask))
                        GetNode(xPosition, zPosition, floor).IsWalkable = false; //has Obstacle
                }
            }
        }

        PathfindingLinkList = new();
        foreach (Transform pathfindinLinkTransform in pathfindingLinkContainer) {
            if (!pathfindinLinkTransform.TryGetComponent(out PathfindingLinkMonoBehaviour pathfindingLinkMonoBehaviour)) continue;
            PathfindingLink pathfindingLink = pathfindingLinkMonoBehaviour.GetPathfindingLink();
            // Debug.Log($"PathfindingLink: {pathfindingLink.GridPositionA} -> {pathfindingLink.GridPositionB}");
            PathfindingLinkList.Add(pathfindingLink);
        }
    }

    public bool IsWalkableGridPosition(GridPosition gridPosition) => GetGridSystem(gridPosition.Floor).GetGridObject(gridPosition).IsWalkable;

    public void SetWalkableGridPosition(GridPosition gridPosition, bool isWalkable) => GetGridSystem(gridPosition.Floor).GetGridObject(gridPosition).IsWalkable = isWalkable;

    public List<GridPosition> FindPath(GridPosition startGridPosition, GridPosition endGridPosition, out int pathLength) {
        List<PathNode> openList = new();
        List<PathNode> closedList = new();

        PathNode startNode = GetGridSystem(startGridPosition.Floor).GetGridObject(startGridPosition);
        PathNode endNode = GetGridSystem(endGridPosition.Floor).GetGridObject(endGridPosition);
        openList.Add(startNode);

        for (int xPosition = 0; xPosition < Width; xPosition++) {
            for (int zPosition = 0; zPosition < Height; zPosition++) {
                for (int floor = 0; floor < FloorAmount; floor++) {
                    PathNode pathNode = GetGridSystem(floor).GetGridObject(new(xPosition, zPosition, floor));
                    pathNode.GCost = int.MaxValue;
                    pathNode.HCost = 0;
                    pathNode.CameFromPathNode = null;
                }
            }
        }

        startNode.GCost = 0;
        startNode.HCost = CalculateDistance(startGridPosition, endGridPosition);

        while (openList.Count > 0) {
            PathNode currentNode = GetLowestFCostPathNode(openList);

            if (currentNode == endNode) {
                // Reached final node
                pathLength = endNode.FCost;
                return CalculatePath(endNode);
            }

            openList.Remove(currentNode);
            closedList.Add(currentNode); //currentNode is tested

            foreach (PathNode neighbourNode in GetNeighbourList(currentNode).Where(neighbourNode => !closedList.Contains(neighbourNode))) {
                if (!neighbourNode.IsWalkable) {
                    closedList.Add(neighbourNode); // Not walkable
                    continue;
                }

                int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode.GridPosition, neighbourNode.GridPosition);
                if (tentativeGCost >= neighbourNode.GCost) continue;
                neighbourNode.CameFromPathNode = currentNode;
                neighbourNode.GCost = tentativeGCost;
                neighbourNode.HCost = CalculateDistance(neighbourNode.GridPosition, endGridPosition);
                if (!openList.Contains(neighbourNode)) openList.Add(neighbourNode);
            }
        }

        pathLength = 0; //no path found
        return null;
    }

    private static int CalculateDistance(GridPosition gridPositionA, GridPosition gridPositionB) {
        GridPosition gridPositionDistance = gridPositionA - gridPositionB;
        int xDistance = Mathf.Abs(gridPositionDistance.XPosition);
        int zDistance = Mathf.Abs(gridPositionDistance.ZPosition);
        int remaining = Mathf.Abs(xDistance - zDistance);
        return MOVE_DIAGONAL_COST * Mathf.Min(xDistance, zDistance) + MOVE_STRAIGHT_COST * remaining;
    }

    private static PathNode GetLowestFCostPathNode(IReadOnlyList<PathNode> pathNodeList) {
        PathNode lowestFCostPathNode = pathNodeList[0];
        foreach (PathNode currentPathNode in pathNodeList.Where(t => t.FCost < lowestFCostPathNode.FCost)) lowestFCostPathNode = currentPathNode;
        return lowestFCostPathNode;
    }

    private IEnumerable<PathNode> GetNeighbourList(PathNode currentNode) {
        List<PathNode> neighbourList = new();
        GridPosition gridPosition = currentNode.GridPosition;

        if (gridPosition.XPosition - 1 >= 0) {
            neighbourList.Add(GetNode(gridPosition.XPosition - 1, gridPosition.ZPosition + 0, gridPosition.Floor)); //Left
            if (gridPosition.ZPosition - 1 >= 0) neighbourList.Add(GetNode(gridPosition.XPosition - 1, gridPosition.ZPosition - 1, gridPosition.Floor)); //LeftDown
            if (gridPosition.ZPosition < Height) neighbourList.Add(GetNode(gridPosition.XPosition - 1, gridPosition.ZPosition + 1, gridPosition.Floor)); //LeftUp
        }

        if (gridPosition.XPosition + 1 < Width) {
            neighbourList.Add(GetNode(gridPosition.XPosition + 1, gridPosition.ZPosition + 0, gridPosition.Floor)); //Right
            if (gridPosition.ZPosition - 1 >= 0) neighbourList.Add(GetNode(gridPosition.XPosition + 1, gridPosition.ZPosition - 1, gridPosition.Floor)); //RightDown
            if (gridPosition.ZPosition + 1 < Height) neighbourList.Add(GetNode(gridPosition.XPosition + 1, gridPosition.ZPosition + 1, gridPosition.Floor)); //RightUp
        }

        if (gridPosition.ZPosition - 1 >= 0) neighbourList.Add(GetNode(gridPosition.XPosition - 0, gridPosition.ZPosition - 1, gridPosition.Floor)); //Down
        if (gridPosition.ZPosition + 1 < Height) neighbourList.Add(GetNode(gridPosition.XPosition - 0, gridPosition.ZPosition + 1, gridPosition.Floor)); //Up

        List<PathNode> totalNeighbourList = new();
        totalNeighbourList.AddRange(neighbourList);

        IEnumerable<GridPosition> pathfindingLinkGridPositionList = GetPathfindingLinkConnectedGridPositionList(gridPosition);
        totalNeighbourList.AddRange(pathfindingLinkGridPositionList.Select(GetNode));

        return totalNeighbourList;
    }

    private IEnumerable<GridPosition> GetPathfindingLinkConnectedGridPositionList(GridPosition gridPosition) {
        List<GridPosition> gridPositionList = new();
        foreach (PathfindingLink pathfindingLink in PathfindingLinkList) {
            if (pathfindingLink.GridPositionA == gridPosition) gridPositionList.Add(pathfindingLink.GridPositionB);
            if (pathfindingLink.GridPositionB == gridPosition) gridPositionList.Add(pathfindingLink.GridPositionA);
        }

        return gridPositionList;
    }

    private GridSystem<PathNode> GetGridSystem(int floor) {
        if (floor >= 0 && floor < FloorAmount) return GridSystemList[floor];
        Debug.LogError($"Try to access floor {floor} and has only {GridSystemList.Count} GridSystems in list!");
        return null;
    }

    private PathNode GetNode(int x, int z, int floor) => GetGridSystem(floor).GetGridObject(new(x, z, floor));
    private PathNode GetNode(GridPosition gridPosition) => GetGridSystem(gridPosition.Floor).GetGridObject(gridPosition);

    private static List<GridPosition> CalculatePath(PathNode endNode) {
        List<PathNode> pathNodeList = new() { endNode };
        PathNode currentNode = endNode;
        while (currentNode.CameFromPathNode != null) {
            pathNodeList.Add(currentNode.CameFromPathNode);
            currentNode = currentNode.CameFromPathNode;
        }

        pathNodeList.Reverse();
        return pathNodeList.Select(pathNode => pathNode.GridPosition).ToList();
    }

    public bool HasPath(GridPosition startGridPosition, GridPosition endGridPosition) => FindPath(startGridPosition, endGridPosition, out _) != null;

    public int GetPathLength(GridPosition startGridPosition, GridPosition endGridPosition) {
        FindPath(startGridPosition, endGridPosition, out int pathLength);
        return pathLength;
    }
}