Universe.BotManager.AStar.AStar.FindPath C# (CSharp) Method

        public void FindPath(AStarNode aStartNode, AStarNode aGoalNode)
        {
            FStartNode = aStartNode;
            FGoalNode = aGoalNode;

            FOpenList.Add(FStartNode);
            int i = 0;
            while (FOpenList.Count > 0 && i < 2000)
            {
                // Get the node with the lowest TotalCost
                AStarNode NodeCurrent = (AStarNode) FOpenList.Pop();

                // If the node is the goal copy the path to the solution array
                if (NodeCurrent.IsGoal())
                {
                    while (NodeCurrent != null)
                    {
                        FSolution.Insert(0, NodeCurrent);
                        NodeCurrent = NodeCurrent.Parent;
                    }
                    break;
                }

                // Get successors to the current node
                NodeCurrent.GetSuccessors(FSuccessors);
                foreach (AStarNode NodeSuccessor in FSuccessors)
                {
                    // Test if the current successor node is on the open list, if it is and
                    // the TotalCost is higher, we will throw away the current successor.
                    AStarNode NodeOpen = null;
                    if (FOpenList.Contains(NodeSuccessor))
                        NodeOpen = (AStarNode) FOpenList[FOpenList.IndexOf(NodeSuccessor)];
                    if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
                        continue;

                    // Test if the current successor node is on the closed list, if it is and
                    // the TotalCost is higher, we will throw away the current successor.
                    AStarNode NodeClosed = null;
                    if (FClosedList.Contains(NodeSuccessor))
                        NodeClosed = (AStarNode) FClosedList[FClosedList.IndexOf(NodeSuccessor)];
                    if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
                        continue;

                    // Remove the old successor from the open list
                    FOpenList.Remove(NodeOpen);

                    // Remove the old successor from the closed list
                    FClosedList.Remove(NodeClosed);

                    // Add the current successor to the open list
                    FOpenList.Push(NodeSuccessor);
                }
                // Add the current node to the closed list
                FClosedList.Add(NodeCurrent);
                i++;
            }

            if (i == 2000)
            {
                m_pathPossible = false;
            }
        }

        static void Main(string[] args)
        {
            Console.WriteLine("Starting...");

            AStar astar = new AStar();

            AStarNode2D GoalNode = new AStarNode2D(null, null, 0, 9, 9);
            AStarNode2D StartNode = new AStarNode2D(null, GoalNode, 0, 0, 0) {GoalNode = GoalNode};
            astar.FindPath(StartNode, GoalNode);

            PrintSolution(astar.Solution);
            Console.ReadLine();
        }