diff --git a/Pathfinding/Pathfinder_Distance.cs b/Pathfinding/Pathfinder_Distance.cs
index 2a5d9b9..c024751 100644
--- a/Pathfinding/Pathfinder_Distance.cs
+++ b/Pathfinding/Pathfinder_Distance.cs
@@ -27,18 +27,18 @@ public static partial class Pathfinder
                 OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                 if (neighbor is not null)
                 {
+                    if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
+                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
                     double newPotentialLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor);
                     if (newPotentialLength < neighbor.currentPathLength)
                     {
                         neighbor.previousPathNode = currentNode;
                         neighbor.currentPathLength = newPotentialLength;
-                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
 
-                        if (neighbor.Equals(goalNode))
-                        {
-                            stop = true;
-                        }
-                        else if(!stop)
+                        if(neighbor.Equals(goalNode))
+                            return GetRouteFromCalc(goalNode, regionManager);
+                            //stop = true;
+                        if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop)
                         {
                             toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
                         }
diff --git a/Pathfinding/Pathfinder_Time.cs b/Pathfinding/Pathfinder_Time.cs
index 7391f3e..4ef5fff 100644
--- a/Pathfinding/Pathfinder_Time.cs
+++ b/Pathfinding/Pathfinder_Time.cs
@@ -29,22 +29,24 @@ public static partial class Pathfinder
                 OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                 if (neighbor is not null)
                 {
+                    if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
+                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
+                    
                     double newPotentialWeight = currentNode.currentPathWeight +
                                                 EdgeWeight(currentNode, edge, vehicle, regionManager);
+                    
                     if (newPotentialWeight < neighbor.currentPathWeight)
                     {
                         neighbor.previousPathNode = currentNode;
                         neighbor.currentPathLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor);
                         neighbor.currentPathWeight = newPotentialWeight;
-                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
-
-                        if (neighbor.Equals(goalNode))
+                        
+                        if(neighbor.Equals(goalNode))
+                            return GetRouteFromCalc(goalNode, regionManager);
+                            //stop = true;
+                        if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop)
                         {
-                            stop = true;
-                        }
-                        else if(!stop)
-                        {
-                            toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
+                            toVisit.Enqueue(neighbor, neighbor.currentPathWeight + neighbor.directDistanceToGoal);
                         }
                     }
                 }