Expand PathNode

Pathfinding/PathNode.cs

@@ -1,9 +1,27 @@
+using OSMDatastructure;
+
 namespace Pathfinding;
 
 public class PathNode : OSMDatastructure.Node
 {
+    PathNode? previousNode = null;
+    private Dictionary<Connection, double> distanceDictionary = new();
+    double currentWeight = double.MaxValue;
+    
+    
     public PathNode(float lat, float lon) : base(lat, lon)
     {
             
     }
+
+    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];
+    }
+    
+    
 }

Pathfinding/Pathfinder.cs

@@ -10,9 +10,10 @@ public class Pathfinder
     {
         RegionManager regionManager = new RegionManager(workingDir);
         Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
-        Node startNode = ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
+        PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
         Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
-        Node goalNode = ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        
         
     }