From b6ca87ea7277b060cae2d9402a35549ffcd1b235 Mon Sep 17 00:00:00 2001
From: C9Glax <13404778+C9Glax@users.noreply.github.com>
Date: Sun, 5 Feb 2023 20:03:47 +0100
Subject: [PATCH] Added first Pathfinding

---
 Pathfinding/PathNode.cs   | 16 ++-------
 Pathfinding/Pathfinder.cs | 69 +++++++++++++++++++++++++++++++++++----
 Server/Server.csproj      |  1 +
 3 files changed, 67 insertions(+), 19 deletions(-)

diff --git a/Pathfinding/PathNode.cs b/Pathfinding/PathNode.cs
index 22a4791..8899125 100644
--- a/Pathfinding/PathNode.cs
+++ b/Pathfinding/PathNode.cs
@@ -4,9 +4,9 @@ namespace Pathfinding;
 
 public class PathNode : OSMDatastructure.Node
 {
-    PathNode? previousNode = null;
-    private Dictionary<Connection, double> distanceDictionary = new();
-    double currentWeight = double.MaxValue;
+    public PathNode? previousPathNode = null;
+    public double currentPathWeight = double.MaxValue;
+    public double DirectDistanceToGoal = double.MaxValue;
     
     
     public PathNode(float lat, float lon) : base(lat, lon)
@@ -14,14 +14,4 @@ public class PathNode : OSMDatastructure.Node
             
     }
 
-    public double? DistanceTo(Connection connection)
-    {
-        if (!this.GetConnections().Contains(connection))
-            return null; //TODO exception is not actually connected
-        if (!this.distanceDictionary.ContainsKey(connection))
-            this.distanceDictionary.Add(connection, Utils.DistanceBetween(this, connection.endNodeCoordinates));
-        return this.distanceDictionary[connection];
-    }
-    
-    
 }
\ No newline at end of file
diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 77d8ced..d074571 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -6,15 +6,72 @@ namespace Pathfinding;
 public class Pathfinder
 {
 
-    private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
+    public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
     {
         RegionManager regionManager = new RegionManager(workingDir);
-        Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
-        PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
-        Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
-        PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
-        
+        Region startRegion, goalRegion;
+        PathNode startNode, goalNode;
+        try
+        {
+            startRegion = regionManager.GetRegion(start);
+            startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
+        }
+        catch (FileNotFoundException e)
+        {
+            throw new Exception(string.Format("No region at coordinates {0}", start), e);
+        }
+        try
+        {
+            goalRegion = regionManager.GetRegion(goal);
+            goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        }
+        catch (FileNotFoundException e)
+        {
+            throw new Exception(string.Format("No region at coordinates {0}", start), e);
+        }
         
+        List<PathNode> toVisit = new() { startNode };
+        startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
+        bool stop = false;
+
+        while (toVisit.Count > 0 && !stop)
+        {
+            PathNode closestNodeToGoal = toVisit.First();
+            foreach (PathNode node in toVisit)
+            {
+                if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
+                {
+                    closestNodeToGoal = node;
+                }
+            }
+
+            foreach (Connection connection in closestNodeToGoal.GetConnections())
+            {
+                PathNode? neighbor = (PathNode?)regionManager.GetNode(connection.endNodeCoordinates);
+                if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
+                {
+                    neighbor.previousPathNode = closestNodeToGoal;
+                    neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
+                                                 Utils.DistanceBetween(closestNodeToGoal, neighbor);
+                    
+                    if (neighbor.Equals(goalNode))
+                        stop = true;
+                    else
+                        toVisit.Add(neighbor);
+                }
+            }
+        }
+
+        List<PathNode> path = new();
+        PathNode currentNode = goalNode;
+        while (!currentNode.Equals(startNode))
+        {
+            path.Add(currentNode);
+            currentNode = currentNode.previousPathNode!;
+        }
+        path.Add(startNode);
+
+        return path;
     }
     
     private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
diff --git a/Server/Server.csproj b/Server/Server.csproj
index 35cc033..5843f3a 100644
--- a/Server/Server.csproj
+++ b/Server/Server.csproj
@@ -9,6 +9,7 @@
 
     <ItemGroup>
       <ProjectReference Include="..\OSMSplitter\OSMSplitter.csproj" />
+      <ProjectReference Include="..\Pathfinding\Pathfinding.csproj" />
     </ItemGroup>
 
 </Project>