diff --git a/OSMDatastructure/OSMEdge.cs b/OSMDatastructure/OSMEdge.cs
index 6a6377b..35ab170 100644
--- a/OSMDatastructure/OSMEdge.cs
+++ b/OSMDatastructure/OSMEdge.cs
@@ -44,7 +44,7 @@ public class OsmEdge
         highway, oneway, footway, sidewalk, cycleway, busway, forward, maxspeed, name, surface, lanes, access, tracktype, id, EMPTY
     }
 
-    public static Dictionary<wayType, byte> speedcar = new() {
+    private static readonly Dictionary<wayType, byte> defaultSpeedCar = new() {
         { wayType.NONE, 0 },
         { wayType.motorway, 110 },
         { wayType.trunk, 100 },
@@ -76,7 +76,7 @@ public class OsmEdge
         { wayType.construction, 0 }
     };
 
-    public static Dictionary<wayType, byte> speedped = new() {
+    private static readonly Dictionary<wayType, byte> defaultSpeedPedestrian = new() {
         { wayType.NONE, 0 },
         { wayType.motorway, 0 },
         { wayType.trunk, 0 },
@@ -115,7 +115,7 @@ public class OsmEdge
     {
         KeyValuePair<tagType, object> tag = ConvertToTag(key, value);
         if(tag.Key != tagType.EMPTY)
-            this.tags.Add(tag.Key, tag.Value);
+            tags.Add(tag.Key, tag.Value);
     }
 
     public static KeyValuePair<tagType, object> ConvertToTag(string key, string value)
@@ -165,32 +165,34 @@ public class OsmEdge
 
     public ulong GetId()
     {
-        return this.tags.ContainsKey(tagType.id) ? (ulong)this.tags[tagType.id] : 0;
+        return tags.ContainsKey(tagType.id) ? (ulong)tags[tagType.id] : 0;
     }
 
     public wayType GetHighwayType()
-    { 
-        return this.tags.ContainsKey(tagType.highway) ? (wayType)this.tags[tagType.highway] : wayType.NONE;
+    {
+        if (!tags.ContainsKey(tagType.highway))
+            throw new Exception("Not a road?");
+        return (wayType)tags[tagType.highway];
     }
 
     public bool IsOneWay()
     {
-        return this.tags.ContainsKey(tagType.oneway) && (bool)this.tags[tagType.oneway];
+        return tags.ContainsKey(tagType.oneway) && (bool)tags[tagType.oneway];
     }
 
-    public byte? GetMaxSpeed(speedType type)
+    public byte GetMaxSpeed(speedType type)
     {
-        if(type == speedType.road)
+        switch (type)
         {
-            return this.tags.ContainsKey(tagType.maxspeed) ? (byte)this.tags[tagType.maxspeed] : null;
+            case speedType.road:
+                return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : (byte)0;
+            case speedType.car:
+                return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : defaultSpeedCar[GetHighwayType()];
+            case speedType.pedestrian:
+                return defaultSpeedPedestrian[GetHighwayType()];
+            default:
+                return 0;
         }
-        if(type == speedType.car)
-        {
-            return this.tags.ContainsKey(tagType.maxspeed)
-                ? (byte)this.tags[tagType.maxspeed]
-                : speedcar[this.GetHighwayType()];
-        }
-        return speedped[this.GetHighwayType()];
     }
 
     public bool IsForward()
@@ -201,11 +203,11 @@ public class OsmEdge
     public double GetWeight(OsmNode parentNode, speedType vehicle)
     {
         double distance = Utils.DistanceBetween(parentNode, neighborCoordinates);
-        byte? speedByte = GetMaxSpeed(vehicle);
-        if (speedByte != null && speedByte > 0)
+        byte speedByte = GetMaxSpeed(vehicle);
+        if (speedByte > 0)
         {
             double speed = Convert.ToDouble(speedByte);
-            return distance / (speed * 1000 / 360);
+            return distance / speed;
         }
         else
         {
diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 8703bfb..521c8f6 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -6,7 +6,7 @@ namespace Pathfinding;
 public class Pathfinder
 {
 
-    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
+    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmEdge.speedType vehicle)
     {
         RegionManager regionManager = new RegionManager(workingDir);
         Region startRegion, goalRegion;
@@ -32,6 +32,7 @@ public class Pathfinder
 
         while (toVisit.Count > 0 && !stop)
         {
+            Console.WriteLine("toVisit-length: {0}", toVisit.Count);
             closestNodeToGoal = toVisit.First();
             foreach (OsmNode node in toVisit)
             {
@@ -48,16 +49,20 @@ public class Pathfinder
             foreach (OsmEdge edge in closestNodeToGoal.edges)
             {
                 OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates);
-                if (neighbor != null && neighbor.currentPathWeight > closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
+                if (neighbor != null)
                 {
-                    neighbor.previousPathNode = closestNodeToGoal;
-                    neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
-                                                 Utils.DistanceBetween(closestNodeToGoal, neighbor);
+                    double newPotentialWeight =
+                        closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle);
+                    if (neighbor.currentPathWeight > newPotentialWeight)
+                    {
+                        neighbor.previousPathNode = closestNodeToGoal;
+                        neighbor.currentPathWeight = newPotentialWeight;
                     
-                    if (neighbor.Equals(goalNode))
-                        stop = true;
-                    else
-                        toVisit.Add(neighbor);
+                        if (neighbor.Equals(goalNode))
+                            stop = true;
+                        else
+                            toVisit.Add(neighbor);
+                    }
                 }
             }
 
@@ -72,7 +77,8 @@ public class Pathfinder
             currentNode = currentNode.previousPathNode;
         }
         path.Add(startNode);
-
+        path.Reverse();
+        
         return path;
     }