using program;

using System.Collections.Generic;

​

public class AStar {

​

    private List<Node> openList;

    private List<Node> closeList;

    private List<Node> neighbors;

    private List<Node> finalPath;

​

    private Node start;

    private Node end;

​

    public AStar()

    {

        openList = new List<Node>();

        closeList = new List<Node>();

        neighbors = new List<Node>();

        finalPath = new List<Node>();

​

    }

​

    public void FindPath(MazeCell startCell, MazeCell goalCell)

    {

        start = new Node(0, 0, 0, null, startCell);

        end = new Node(0, 0, 0, null, goalCell);

​

        openList.Add(start);

​

        bool keepSearching = true;

        bool pathExists = true;

​

        while(keepSearching && pathExists)

        {

            Node currentNode = ExtractBestNodeFromOpenList();

​

            if (currentNode == null)

            {

                pathExists = false;

                break;

            }

​

            closeList.Add(currentNode);

​

            if (NodeIsGoal(currentNode))

            {

                keepSearching = false;

            } else

            {

                FindValidFourNeighbors(currentNode);

            }

​

            foreach(Node neighbor in neighbors)

            {

                if (FindInList(neighbor, closeList) != null)

                    continue;

​

                Node inOpenList = FindInList(neighbor, openList);

                if (inOpenList == null)

                {

                    openList.Add(neighbor);

                } else

                {

                    if (neighbor.G < inOpenList.G)

                    {

                        inOpenList.G = neighbor.G;

                        inOpenList.F = inOpenList.G + inOpenList.H;

                        inOpenList.parent = currentNode;

                    }

                }

​

            }

        }

​

        if (pathExists)

        {

            Node n = FindInList(end, closeList);

            while (n != null)

            {

                finalPath.Add(n);

                n = n.parent;

            }

        }

​

    }

​

    public bool ContainsCoordinates(IntVector2 coordinate)

    {

        return coordinate.x >= 0 && coordinate.x < GameConfig.mazeSize && coordinate.z >= 0 && coordinate.z < GameConfig.mazeSize;

    }

​

    private void FindValidFourNeighbors(Node node)

    {

        neighbors.Clear();

​

        // Check South of this Cell //

​

        int south = node.cell.mazeCellCoordinates.z - 1;

        IntVector2 SouthNeighbor = new IntVector2(node.cell.mazeCellCoordinates.x , south);

​

        int north = node.cell.mazeCellCoordinates.z + 1;

        IntVector2 NorthNeighbor = new IntVector2(node.cell.mazeCellCoordinates.x, north);

​

        int east = node.cell.mazeCellCoordinates.x + 1;

        IntVector2 EastNeighbor = new IntVector2(east, node.cell.mazeCellCoordinates.z);

​

        int west = node.cell.mazeCellCoordinates.x - 1;

        IntVector2 WestNeighbor = new IntVector2(west, node.cell.mazeCellCoordinates.z);

​

        IntVector2 SouthEastNeighbor = new IntVector2(south, east);

​

        IntVector2 SouthWestNeighbor = new IntVector2(south, west);

​

        IntVector2 NorthEastNeighbor = new IntVector2(north, east);

​

        IntVector2 NorthWestNeighbor = new IntVector2(north, west);

​

        if (ContainsCoordinates(SouthNeighbor))

        {

            if (Maze.Instance.cellStorage[SouthNeighbor.x, SouthNeighbor.z].IsWalkable)

            {

                MazeCell c = Maze.Instance.cellStorage[SouthNeighbor.x, SouthNeighbor.z];

​

                if (!(c.cellEdges[(int)MazeDirection.North] is MazeCellWall))

                {

                    Node vn = PrepareNewNode(node, 0, -1);

                    neighbors.Add(vn);

                }

            }

        }

​

​

        if (ContainsCoordinates(NorthNeighbor))

        {

            if (Maze.Instance.cellStorage[NorthNeighbor.x, NorthNeighbor.z].IsWalkable)

            {

                MazeCell c = Maze.Instance.cellStorage[NorthNeighbor.x, NorthNeighbor.z];

​

                if (!(c.cellEdges[(int)MazeDirection.South] is MazeCellWall))

                {

                    Node vn = PrepareNewNode(node, 0, 1);

                    neighbors.Add(vn);

                }

            }

        }

​

        if (ContainsCoordinates(WestNeighbor))

        {

            if (Maze.Instance.cellStorage[WestNeighbor.x, WestNeighbor.z].IsWalkable)

            {

                MazeCell c = Maze.Instance.cellStorage[WestNeighbor.x, WestNeighbor.z];

​

                if (!(c.cellEdges[(int)MazeDirection.East] is MazeCellWall))

                {

                    Node vn = PrepareNewNode(node, -1, 0);

                    neighbors.Add(vn);

                }

            }

        }

​

        if (ContainsCoordinates(EastNeighbor))

        {

            if (Maze.Instance.cellStorage[EastNeighbor.x, EastNeighbor.z].IsWalkable)

            {

                MazeCell c = Maze.Instance.cellStorage[EastNeighbor.x, EastNeighbor.z];

​

                if (!(c.cellEdges[(int)MazeDirection.West] is MazeCellWall))

                {

                    Node vn = PrepareNewNode(node, 1, 0);

                    neighbors.Add(vn);

                }

            }

        }

​

    }

​

    private float Heuristic(Node n)

    {

        return Mathf.Sqrt((n.cell.mazeCellCoordinates.x - end.cell.mazeCellCoordinates.x) * (n.cell.mazeCellCoordinates.x - end.cell.mazeCellCoordinates.x) + (n.cell.mazeCellCoordinates.z - end.cell.mazeCellCoordinates.z) * (n.cell.mazeCellCoordinates.z - end.cell.mazeCellCoordinates.z));

    }

​

    private float MovementCost(Node a, Node b)

    {

        return Maze.Instance.cellStorage[b.cell.mazeCellCoordinates.x, b.cell.mazeCellCoordinates.z].movementCost();

    }

​

    private Node PrepareNewNode(Node n, int x, int z)

    {

        IntVector2 iv = new IntVector2(n.cell.mazeCellCoordinates.x + x, n.cell.mazeCellCoordinates.z + z);

        Node node = new Node(0, 0, 0, n, Maze.Instance.cellStorage[iv.x, iv.z]);

        node.G = n.G + MovementCost(n, node);

        node.H = Heuristic(node);

        node.F = node.G + node.H;

        node.parent = n;

        return node;

    }

​

    public List<MazeCell> CellsFromPath()

    {

        List<MazeCell> path = new List<MazeCell>();

        foreach (Node n in finalPath)

        {

            path.Add(n.cell);

        }

​

        if (path.Count != 0)

        {

            path.Reverse();

            path.RemoveAt(0);

        }

        return path;

    }

​

    public List<int> PointsFromPath()

    {

        List<int> points = new List<int>();

        foreach (Node n in finalPath)

        {

            points.Add(n.cell.mazeCellCoordinates.x);

            points.Add(n.cell.mazeCellCoordinates.z);

        }

        return points;

    }

​

    bool NodeIsGoal(Node node)

    {

        return ((node.cell.mazeCellCoordinates.x == end.cell.mazeCellCoordinates.x) && (node.cell.mazeCellCoordinates.z == end.cell.mazeCellCoordinates.z));

    }

​

    Node FindInList(Node n, List<Node> xl)

    {

        foreach (Node nn in xl)

        {

            if ((nn.cell.mazeCellCoordinates.x == n.cell.mazeCellCoordinates.x) && (nn.cell.mazeCellCoordinates.z == n.cell.mazeCellCoordinates.z))

                return nn;

        }

        return null;

    }

​

    private Node ExtractBestNodeFromOpenList()

    {

        float minF = float.MaxValue;

        Node bestNode = null;

​

        foreach(Node n in openList)

        {

            if (n.F < minF)

            {

                minF = n.F;

                bestNode = n;

            }

        }

​

        if (bestNode != null)

        openList.Remove(bestNode);

        return bestNode;

    }

    // https://stackoverflow.com/questions/40331231/in-c-how-would-i-add-diagonals-to-my-a-star-path-finding-class

​

}