From dca4d56866bccbcef4cb3c5e802144d20bc3031a Mon Sep 17 00:00:00 2001
From: glax <johanna@bernloehr.eu>
Date: Sun, 23 Apr 2023 14:50:16 +0200
Subject: [PATCH] Keep searching for the same amount of time already search to
 get shorter path.

---
 Pathfinding/Pathfinder.cs | 43 +++++++++++++++++++++++++++++++--------
 1 file changed, 34 insertions(+), 9 deletions(-)

diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 2eafa07..dd8236a 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -54,17 +54,31 @@ public class Pathfinder
         gScore = new() { { startNode, 0 } };
         _cameFromDict = new();
 
-        while (openSetfScore.Count > 0)
+        bool found = false;
+        bool stop = false;
+        TimeSpan firstFound = TimeSpan.MaxValue;
+        double maxGscore = double.MaxValue;
+
+        while (openSetfScore.Count > 0 && !stop)
         {
-            OsmNode currentNode = openSetfScore.Dequeue();
+            OsmNode currentNode = openSetfScore.Dequeue()!;
             if (currentNode.Equals(goalNode))
             {
-                TimeSpan calcTime = DateTime.Now - startCalc;
-                pathResult = GetPath(goalNode, calcTime);
-                Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
-                return this;
+                if (!found)
+                {
+                    firstFound = DateTime.Now - startCalc;
+                    found = true;
+                }
+                maxGscore = gScore[goalNode];
+                /*
+                int removed = openSetfScore.Remove(gScore.Where(item => item.Value > maxGscore)
+                    .ToDictionary(item => item.Key, item => item.Value).Keys);
+                Console.WriteLine($"Removed {removed}");*/
             }
 
+            if (found && DateTime.Now - startCalc > firstFound.Multiply(2))
+                stop = true;
+
             foreach (OsmEdge edge in currentNode.edges)
             {
                 OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
@@ -72,7 +86,7 @@ public class Pathfinder
                 {
                     double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge);
                     gScore.TryAdd(neighbor, double.MaxValue);
-                    if (tentativeGScore < gScore[neighbor])
+                    if ((!found || (found && tentativeGScore < maxGscore)) && tentativeGScore < gScore[neighbor])
                     {
                         if(!_cameFromDict.TryAdd(neighbor, currentNode))
                             _cameFromDict[neighbor] = currentNode;
@@ -83,8 +97,19 @@ public class Pathfinder
                 }
             }
         }
-
-        pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0);
+        
+        TimeSpan calcTime = DateTime.Now - startCalc;
+        if (!found)
+        {
+            pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0);
+            Console.Write("No path found.");
+            return this;
+        }
+        else
+        {
+            pathResult = GetPath(goalNode, calcTime);
+        }
+        Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
         return this;
     }