AnotherAStar

using Microsoft.VisualStudio.TestTools.UnitTesting;
using Priority_Queue;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CodeReview.AnotherAStar
{
    public class SharedFunctions
    {
        public static float GetDistanceFromLatLonInMeters(float lat1, float long1, float lat2, float long2)
        {
            // I am not a geographer
            float dlat = lat1 - lat2;
            float dlong = lat2 - long2;
            return (float)Math.Sqrt(dlat * dlat + dlong * dlong);
        }
    }

    // I have no idea what a NodeRow is... so these are just Nodes
    // are your nodes structs? I am confused why you use the Id everywhere... (if they are classes, then you can probably drop all the nodeId stuff: it just adds unhelpful indirection)
    public class NodeRow
    {
        public NodeRow(string type, string nodeId, float lattitude, float longitude)
        {
            NodeId = nodeId;

            Lattitude = lattitude;
            Longitude = longitude;
        }

        public string Type { get; }
        public string NodeId { get; }

        public float Lattitude { get; }
        public float Longitude { get; }

        private readonly List<NodeEdgeRow> _edges = new List<NodeEdgeRow>();
        public IReadOnlyList<NodeEdgeRow> Edges => _edges;

        /// <summary>
        /// Connects this node to another with a two-way edge
        /// </summary>
        public void ConnectWith(NodeRow other, float weight)
        {
            // enforce contracts on the topology
            if (other == null)
                throw new ArgumentNullException(nameof(other));

            if (other == this)
                throw new ArgumentException("Cannot connect a node to itself", nameof(other));
            
            var edge = new NodeEdgeRow(this, other, weight);

            _edges.Add(edge);
            other._edges.Add(edge);
        }

        // much nicer
        public bool IsCorridor => Type == "C" || Type == "c";

        public float DistanceInMetersTo(NodeRow other)
        {
            // it is really easy to mess calls like this up: hide them behind a logical 'business' method
            return SharedFunctions.GetDistanceFromLatLonInMeters(Lattitude, Longitude, other.Lattitude, other.Longitude);
        }
    }

    // now idea what node-edge row is either, so here is just an Edge
    /// <summary>
    /// A two-way edge, to be shared between connected Nodes (connected nodes are unorded)
    /// </summary>
    public class NodeEdgeRow
    {
        public NodeEdgeRow(NodeRow node1, NodeRow node2, float weight)
        {
            // really important assumptions: document and enforce them
            if (node1 == node2)
                throw new ArgumentException("Cannot connect a node to itself");
            if (weight < 0)
                throw new ArgumentException("Edge weight cannot be less than 0");

            Node1 = node1;
            Node2 = node2;

            Weight = weight;
        }

        public NodeRow Node1 { get; }
        public NodeRow Node2 { get; }

        public float Weight { get; }

        public bool TryGetOther(NodeRow known, out NodeRow other)
        {
            if (known.NodeId == Node1.NodeId)
            {
                other = Node2;
                return true;
            }
            else if (known.NodeId == Node2.NodeId)
            {
                other = Node1;
                return true;
            }
            else
            {
                other = null;
                return false;
            }
        }
    }

    public class Route
    {
        public Route(float totalCost, IReadOnlyList<NodeRow> nodes)
        {
            TotalCost = totalCost;
            Nodes = nodes;
        }

        public float TotalCost { get; }
        public IReadOnlyList<NodeRow> Nodes { get; }
    }

    public class AStar
    {
        /// <summary>
        /// Attempts to route a path from start to end through the given nodes
        /// (Assumes that all nodes in the graph are found in allNodes)
        /// </summary>
        // If you can stop using NodeIds everywhere, you can ditch allNodes, and just pass the nodes to this method
        public static Route BuildAStarOptimisation2(Dictionary<string, NodeRow> allNodes, string start, string end)
        {
            if (!allNodes.ContainsKey(start))
                throw new ArgumentException("Start node is not present in allNodes");

            if (!allNodes.ContainsKey(end))
                throw new ArgumentException("End node is not present in allNodes");

            if (start == end)
            { // job done
                return new Route(0f, new NodeRow[0]);
            }

            // lookup start and end, and find endCorridor
            NodeRow startNode = allNodes[start];
            NodeRow endNode = allNodes[end];

            var endCorridor = FindNextCorridorNode(endNode);
            
            // frontier priority queue
            FastPriorityQueue<AStarNode> frontier = new FastPriorityQueue<AStarNode>(allNodes.Count);
            
            // mapping between NodeId and AStarNode
            Dictionary<string, AStarNode> queueTable = new Dictionary<string, AStarNode>();
            
            float heuristicFunction(NodeRow node) => node.IsCorridor ? node.DistanceInMetersTo(endCorridor) : 0f; // this heuristic no sense

            // enque the first node...
            var starter = new AStarNode(startNode, null, 0f);
            queueTable.Add(startNode.NodeId, starter);
            frontier.Enqueue(starter, heuristicFunction(startNode));

            // ... and off we go
            while (frontier.Count > 0)
            {
                AStarNode current = frontier.Dequeue();
                float currentCost = current.TrueCost;

                if (current.Node.NodeId == endCorridor.NodeId)
                {
                    // route found: finish
                    return new Route(currentCost, BackTrack(current));
                }

                current.MarkVisited();

                foreach (var edge in current.Node.Edges)
                {
                    if (!edge.TryGetOther(current.Node, out NodeRow edgeEnd))
                        throw new Exception("Invalid topology: <insert descriptive exception message here so that someone who has to read it has a hope of debugging it without looking at your code");
                    
                    // find the node correspodning to edgeEnd if it already exists
                    bool edgeEndAlreadyPresent = queueTable.TryGetValue(edgeEnd.NodeId, out var target);
                    bool alreadyVisitedEdgeEnd = edgeEndAlreadyPresent && target.Visited;

                    if (alreadyVisitedEdgeEnd)
                        continue; // under assumptions, we can't perform better than this
                    
                    float trueCost = currentCost + edge.Weight;
                    float heuristicCost = heuristicFunction(edgeEnd);
                    float combinedCost = trueCost + heuristicCost;

                    if (edgeEndAlreadyPresent)
                    {
                        // only use our new route if it is better
                        if (combinedCost < target.TrueCost)
                        {
                            target.Update(current, trueCost);
                            frontier.UpdatePriority(target, combinedCost);
                        }
                    }
                    else
                    {
                        target = new AStarNode(edgeEnd, current, trueCost);
                        queueTable.Add(edgeEnd.NodeId, target);
                        frontier.Enqueue(target, combinedCost);
                    }
                }
            }
            
            return null;
        }

        private static IReadOnlyList<NodeRow> BackTrack(AStarNode lastNode)
        {
            List<NodeRow> route = new List<NodeRow>();

            while (lastNode != null)
            {
                route.Add(lastNode.Node);
                lastNode = lastNode.LeadingNode;
            }

            route.Reverse();
            return route;
        }

        // this can be static: make it so (it communicates that it can't be changing any state)
        private static NodeRow FindNextCorridorNode(NodeRow node)
        {
            var provisional = TryFindNextCorridorNode(node, new HashSet<string>());

            if (provisional != null)
            {
                return provisional;
            }
            else
            {
                throw new Exception("Invalid Graph"); // this should have a better message
            }
        }

        // I'm guessing at what this is meant to do
        private static NodeRow TryFindNextCorridorNode(NodeRow node, HashSet<string> seen)
        {
            if (!seen.Add(node.NodeId))
                return null;

            if (node.IsCorridor)
            {
                return node;
            }

            foreach (NodeEdgeRow edge in node.Edges)
            {
                if (edge.TryGetOther(node, out var other))
                {
                    return other;
                }
                else
                {
                    throw new Exception("Invalid toplogy, etc. etc.");
                }
            }

            foreach (NodeEdgeRow edge in node.Edges)
            {
                var provisional = FindNextCorridorNode(edge.Node2);
                if (provisional != null)
                    return provisional;
            }

            return null;
        }

        private class AStarNode : FastPriorityQueueNode
        {
            /// <summary>
            /// The Node wherefor this AStarNode is responsible
            /// </summary>
            public NodeRow Node { get; } // immutability is usually a good thing
            
            /// <summary>
            /// The node which leads to this node
            /// null if there is no leading node
            /// </summary>
            public AStarNode LeadingNode { get; private set; }

            /// <summary>
            /// The total cost of the route up to this Node
            /// </summary>
            public float TrueCost { get; private set; }

            /// <summary>
            /// Indicates whether this node has been visited yet
            /// </summary>
            public bool Visited { get; private set; }

            /// <summary>
            /// Initiaislises an un-visited AStarNode
            /// </summary>
            public AStarNode(NodeRow node, AStarNode leadingNode, float trueCost)
            {
                Node = node;
                LeadingNode = leadingNode;
                TrueCost = trueCost;

                Visited = false;
            }

            public void MarkVisited()
            {
                System.Diagnostics.Debug.Assert(!Visited);

                Visited = true;
            }

            public void Update(AStarNode newLeadingNode, float newTrueCost)
            {
                System.Diagnostics.Debug.Assert(newTrueCost < TrueCost);

                LeadingNode = newLeadingNode;
                TrueCost = newTrueCost;
            }
        }
    }

    [TestClass]
    public class AStarTest
    {
        [TestMethod]
        public void ExampleTest()
        {
            List<NodeRow> nodes = new List<NodeRow>();

            NodeRow newNode(string type, string nodeId, float lat, float @long)
            {
                var node = new NodeRow(type, nodeId, lat, @long);
                nodes.Add(node);
                return node;
            }

            var startNode = newNode("c", "s", 0f, 0f);
            var endNode = newNode("notc", "en", 1000f, 0f);
            var endCorridor = newNode("C", "e", 8000f, 0f);

            endNode.ConnectWith(endCorridor, 0); // NOTE: the implementation doesn't consider this weight

            var a = newNode("c", "a", 400, 300);
            var b = newNode("c", "b", 400, 0);
            var c = newNode("c", "c", 400, -300);

            var s = startNode;
            var e = endCorridor;

            // contrived map: best option is s->a->b->e
            int five = 500;
            s.ConnectWith(a, five);
            s.ConnectWith(b, 400 + 1000);
            s.ConnectWith(c, five + 1000);

            a.ConnectWith(b, 300);
            b.ConnectWith(c, 300);
            
            e.ConnectWith(a, five + 1000);
            e.ConnectWith(b, 400);
            e.ConnectWith(c, five + 1000);
            float optimalRoute = five + 400 + 300;
            
            Dictionary<string, NodeRow> allNodes = nodes.ToDictionary(node => node.NodeId, node => node);

            var route = AStar.BuildAStarOptimisation2(allNodes, startNode.NodeId, endNode.NodeId);

            Assert.IsNotNull(route, "Route should not be null");
            Assert.IsNotNull(route.Nodes, "Routes nodes should not be null");
            Assert.AreEqual(optimalRoute, route.TotalCost, 0.001, "Route length was outside tollerances");
            Assert.AreEqual(MakeRouteString(s, a, b, e), MakeRouteString(route.Nodes), false, "Route nodes incorrect");
        }

        private static string MakeRouteString(params NodeRow[] path) => MakeRouteString((IReadOnlyList<NodeRow>)path);

        private static string MakeRouteString(IReadOnlyList<NodeRow> path)
        {
            return string.Join("->", path.Select(n => n.NodeId));
        }
    }
}

Indeed, if you replace the heuristic function with return 0 then it is (should be) equivalent to a Dijkstra's.