From e0ad6d09d6971ab52ac9378b3adb92c179a07762 Mon Sep 17 00:00:00 2001
From: mitchellri <mitchell.rivett@hotmail.com>
Date: Sat, 21 Sep 2019 17:07:15 -0700
Subject: [PATCH] AStar add

---
 src/Dijkstra.NET/ShortestPath/AStar.cs        | 60 +++++++++++++++++++
 .../ShortestPath/DijkstraExtensions.cs        | 23 +++++++
 src/Dijkstra.NET/ShortestPath/NodeComparer.cs | 15 ++++-
 3 files changed, 96 insertions(+), 2 deletions(-)
 create mode 100644 src/Dijkstra.NET/ShortestPath/AStar.cs

diff --git a/src/Dijkstra.NET/ShortestPath/AStar.cs b/src/Dijkstra.NET/ShortestPath/AStar.cs
new file mode 100644
index 0000000..a2bbb50
--- /dev/null
+++ b/src/Dijkstra.NET/ShortestPath/AStar.cs
@@ -0,0 +1,60 @@
+using System;
+using System.Collections.Generic;
+using Dijkstra.NET.Graph;
+
+namespace Dijkstra.NET.ShortestPath
+{
+    internal static class AStar
+    {
+        public static ShortestPathResult GetShortestPath(IDijkstraGraph graph, uint from, uint to, Func<uint,uint,int> heuristic, int depth)
+        {
+            var path = new Dictionary<uint, uint>();
+            var distance = new Dictionary<uint, int> {[from] = 0};
+            var d = new Dictionary<uint, int> {[from] = 0};
+            var q = new SortedSet<uint>(new[] {from}, new NodeComparer(distance, heuristic));
+            var current = new HashSet<uint>();
+
+            int Distance(uint key)
+            {
+                return distance.ContainsKey(key) ? distance[key] : Int32.MaxValue;
+            }
+
+            do
+            {
+                uint u = q.Deque();
+
+                if (u == to)
+                {
+                    return new ShortestPathResult(from, to, distance[u], path);
+                }
+
+                current.Remove(u);
+
+                if (depth == d[u])
+                {
+                    continue;
+                }
+
+                graph[u]((node, cost) =>
+                {
+                    if (Distance(node) > Distance(u) + cost)
+                    {
+                        if (current.Contains(node))
+                        {
+                            q.Remove(node);
+                        }
+
+                        distance[node] = Distance(u) + cost;
+                        q.Add(node);
+                        current.Add(node);
+                        path[node] = u;
+                        d[node] = d[u] + 1;
+                    }
+                });
+
+            } while (q.Count > 0 && depth > 0);
+
+            return new ShortestPathResult(from, to);
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs b/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs
index 3fc200a..ba1730b 100644
--- a/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs
+++ b/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs
@@ -17,6 +17,16 @@ public static class DijkstraExtensions
         public static ShortestPathResult Dijkstra(this IDijkstraGraph graph, uint from, uint to)
             => Dijkstra(graph, from, to, Int32.MaxValue);
 
+        /// <summary>
+        /// Get path from @from to @to
+        /// </summary>
+        /// <param name="graph">Source graph</param>
+        /// <param name="from">Start node</param>
+        /// <param name="to">End node</param>
+        /// <returns>Value with path</returns>
+        public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func<uint,uint,int> heuristic)
+            => AStar(graph, from, to, heuristic, Int32.MaxValue);
+
         /// <summary>
         /// Get path from @from to @to
         /// </summary>
@@ -29,5 +39,18 @@ public static ShortestPathResult Dijkstra(this IDijkstraGraph graph, uint from,
         {
             return ShortestPath.Dijkstra.GetShortestPath(graph, from, to, depth);
         }
+        
+        /// <summary>
+        /// Get path from @from to @to
+        /// </summary>
+        /// <param name="graph">Source graph</param>
+        /// <param name="from">Start node</param>
+        /// <param name="to">End node</param>
+        /// <param name="depth">Depth of path</param>
+        /// <returns>Value with path</returns>
+        public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func<uint, uint, int> heuristic, int depth)
+        {
+            return ShortestPath.AStar.GetShortestPath(graph, from, to, heuristic, depth);
+        }
     }
 }
\ No newline at end of file
diff --git a/src/Dijkstra.NET/ShortestPath/NodeComparer.cs b/src/Dijkstra.NET/ShortestPath/NodeComparer.cs
index 57de565..519f515 100644
--- a/src/Dijkstra.NET/ShortestPath/NodeComparer.cs
+++ b/src/Dijkstra.NET/ShortestPath/NodeComparer.cs
@@ -6,16 +6,27 @@ namespace Dijkstra.NET.ShortestPath
     internal class NodeComparer : IComparer<uint>
     {
         private readonly IDictionary<uint, int> _distance;
+        private readonly Func<uint, uint, int> _heuristic;
 
         public NodeComparer(IDictionary<uint, int> distance)
         {
             _distance = distance;
+            _heuristic = null;
+        }
+        public NodeComparer(IDictionary<uint, int> distance, Func<uint, uint, int> heuristic)
+        {
+            _distance = distance;
+            _heuristic = heuristic;
         }
 
         public int Compare(uint x, uint y)
         {
-            int xDistance = _distance.ContainsKey(x) ? _distance[x] : Int32.MaxValue;
-            int yDistance = _distance.ContainsKey(y) ? _distance[y] : Int32.MaxValue;
+            int xDistance = _distance.ContainsKey(x) ?
+                _heuristic is null ? _distance[x] : _distance[x] + _heuristic(x, y)
+                : Int32.MaxValue;
+            int yDistance = _distance.ContainsKey(y) ?
+                _heuristic is null ? _distance[y] : _distance[y] + _heuristic(y, x)
+                : Int32.MaxValue;
 
             int comparer = xDistance.CompareTo(yDistance);