public AIPathfiding(int x, int y, Vector3 origin)
 {
     instance = this;
     grid     = new PathGrid <PathfindingNode>(x, y, 1f, origin);
     for (int i = 0; i < grid.GetGridWidth(); i++)
     {
         for (int j = 0; j < grid.GetGridHeight(); j++)
         {
             PathfindingNode newNode = new PathfindingNode(grid, i, j);
             grid.SetGridObject(i, j, newNode);
         }
     }
 }
    public List <PathfindingNode> FindTarget(Vector2 startPos, Vector2 endPos)
    {
        if (grid.GetGridObject(endPos) == null)
        {
            return(null);
        }
        PathfindingNode startNode = grid.GetGridObject(startPos);
        PathfindingNode endNode   = grid.GetGridObject(endPos);

        openNodes = new List <PathfindingNode> {
            startNode
        };
        closedNodes = new List <PathfindingNode>();

        for (int i = 0; i < grid.GetGridWidth(); i++)
        {
            for (int j = 0; j < grid.GetGridHeight(); j++)
            {
                PathfindingNode node = grid.GetGridObject(i, j);
                node.gCost = int.MaxValue;
                node.CalculateFCost();
                node.parentNode = null;
            }
        }

        startNode.gCost = 0;
        startNode.hCost = GetDistanceValue(startNode, endNode);
        startNode.CalculateFCost();

        while (openNodes.Count > 0)
        {
            PathfindingNode currentNode = GetLowestValueNode(openNodes);
            if (currentNode == endNode)
            {
                return(CalculateFinalPath(endNode));
            }

            openNodes.Remove(currentNode);
            closedNodes.Add(currentNode);

            foreach (PathfindingNode neighbor in FindNeighbor(currentNode))
            {
                if (closedNodes.Contains(neighbor))
                {
                    continue;
                }

                if (!neighbor.isWalkable)
                {
                    closedNodes.Add(neighbor);

                    /*
                     * for(int i = -2; i < 3; i++)
                     * {
                     *  for(int j = -2; j < 3; j++)
                     *  {
                     *      closedNodes.Add(grid.GetGridObject(neighbor.GetX() + i, neighbor.GetY() + j));
                     *  }
                     * }
                     */
                    continue;
                }

                int newGCost = currentNode.gCost + GetDistanceValue(currentNode, neighbor);

                if (newGCost < neighbor.gCost)
                {
                    neighbor.parentNode = currentNode;
                    neighbor.gCost      = newGCost;
                    neighbor.hCost      = GetDistanceValue(currentNode, endNode);
                    neighbor.CalculateFCost();
                }

                if (!openNodes.Contains(neighbor))
                {
                    openNodes.Add(neighbor);
                }
            }
        }
        return(null);
    }