AStar Shortest Path using C#

Question

I have been trying to write a function to find the shortest path, implementing the AStar algorithm. I have gone through many solutions on net and on this forum. But my bad, I am having a tough time understanding where exactly I need to 'remove the node from the path' if the path did not hit the destination. Infact the adding and removing nodes as we go along a path and as we come back after reaching a dead-end in the recursion, seemed a bit challenging to understand. At the end of the recursion, if the path is not found, I am clearing the path and returning it, which I know is not the way to implement. However, I am sharing the code here. Could someone kindly help me understand what I am doing wrong?

Here is the Node class;

public class Node
{
    public string Name { get; private set; }    
    public Coordinate Location { get; private set; }
    public double g { get; set; }
    public double h { get; set; }
    public double cost { get { return this.g + this.h; } }
    public List<Node> Neighbours { get; set; }

    public Node(string name, Coordinate location)
    {
        Name = name;
        Location = location;
        Neighbours= new List<Node>();
    }

    public void AddNeighbours(List<Node> neighbours)
    {
        Neighbours.AddRange(neighbours);
    }

    public double distanceTo(Node node)
    {
        return Location.Distance(node.Location);
    }

}

...and here is the Graph class.

public class Graph
{
    List<Node> Nodes = new List<Node>();

    public Graph(List<Node> nodes) 
    { 
        Nodes = nodes; 
    }

    public List<Node> GetShortestPath(Node source, Node destination, HashSet<Node> visited = null, List<Node> path = null )
    {
        if ( visited == null ) { visited = new HashSet<Node>(); } // Initialize the visited nodes list
        if(path == null ) { path = new List<Node>() {}; } // initialize the shortest path list
        if (source == destination){ path.Add(destination); return path;}

        visited.Add(source); // Currently visiting this node. So, add to the visited nodes
        path.Add(source); // Add the current source to the path
        foreach (Node neighbour in source.Neighbours) // for each neighbour node
        {
            // Update the g and h distances 
            neighbour.g = source.g + source.distanceTo(neighbour);
            neighbour.h = neighbour.distanceTo(destination);
        }

        // Collect the non-visited neighbours
        List<Node> nonVisitedNeighbours = source.Neighbours.Where(n => !visited.Contains(n)).ToList();

        if (nonVisitedNeighbours.Count > 0) // if non-visited neighbours not empty
        {
            // sort the neighbours in ascending order and take the first one.
            // that will be the closest neighbour with the lowest cost 
            Node nextNeighbour = nonVisitedNeighbours.OrderBy(n => n.cost).ToList().First();
            return GetShortestPath(nextNeighbour, destination, visited, path);
        }
        path.Clear(); // I hope this is not the right way, but somewhere path.Remove(source) to be added. But not clear where...
        Console.WriteLine("No path found!");
        return path; // This should return an empty list
    }
}

Answer 1

Ok... I fixed it. Here is the final implementation...

        public List<Node> GetPath(
        Node startNode, 
        Node targetNode
    )
    {
        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            List<Node>  nodesWithLesserCost  = openSet
                .Skip(1)
                .ToList()
                .Where(node => node.Cost < currentNode.Cost || node.Cost == currentNode.Cost && node.H < currentNode.H)
                .OrderBy(n => n.Cost)
                .ToList();
            
            if (nodesWithLesserCost.Any())
            {
                currentNode = nodesWithLesserCost.First();
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);
                           
            if (currentNode == targetNode)
            {
                return RetracePath(startNode, targetNode);
            }

            foreach (Node neighbour in currentNode.Neighbours)
            {
                if (closedSet.Contains(neighbour))
                {
                    continue;
                }

                double newMovementCostToNeighbour = currentNode.G + currentNode.DistanceTo(neighbour);
                if(newMovementCostToNeighbour < neighbour.G || !openSet.Contains(neighbour))
                {
                    neighbour.G = newMovementCostToNeighbour;
                    neighbour.H = neighbour.DistanceTo(targetNode);
                    neighbour.Parent = currentNode;
                    if (!openSet.Contains(neighbour))
                    {
                        openSet.Add(neighbour);
                    }
                }
            }
        }
        Console.WriteLine("No Path Found...!");
        return new List<Node>();
    }