diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 3a0fca7..147d193 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -1,4 +1,5 @@
-using System.Text.Json;
+using System.Diagnostics;
+using System.Text.Json;
 using OSMDatastructure;
 using OSMDatastructure.Graph;
 using static OSMDatastructure.Tag;
@@ -26,7 +27,7 @@ public class Pathfinder
     
     public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates,
         SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
-        double heuristicFewJunctionsPriority)
+        double heuristicFewJunctionsPriority, double angleWeightFactor)
     {
         DateTime startCalc = DateTime.Now;
         regionManager = new RegionManager(workingDir);
@@ -49,8 +50,9 @@ public class Pathfinder
             OsmNode currentNode = openSetfScore.Dequeue();
             if (currentNode.Equals(goalNode))
             {
-                Console.WriteLine("Path found.");
-                pathResult = GetPath(goalNode, DateTime.Now - startCalc);
+                TimeSpan calcTime = DateTime.Now - startCalc;
+                Console.WriteLine($"Path found. {calcTime}");
+                pathResult = GetPath(goalNode, calcTime);
                 return this;
             }
 
@@ -60,7 +62,7 @@ public class Pathfinder
                 if (neighbor is not null)
                 {
                     double tentativeGScore =
-                        gScore[currentNode] + Weight(currentNode, neighbor, edge);
+                        gScore[currentNode] + Weight(currentNode, neighbor, edge, angleWeightFactor);
                     gScore.TryAdd(neighbor, double.MaxValue);
                     if (tentativeGScore < gScore[neighbor])
                     {
@@ -108,19 +110,31 @@ public class Pathfinder
         return new PathResult(calcFinished, path);
     }
 
-    private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge)
+    private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge, double angleWeightFactor)
     {
-        double distance = Utils.DistanceBetween(fromNode, neighborNode);
-        double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, _speedType);
-        //double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
-        return distance / speed;
+        double distance = Utils.DistanceBetween(currentNode, neighborNode);
+        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
+        
+        double angle = 1;
+        if (_cameFromDict!.ContainsKey(currentNode))
+        {
+            OsmNode previousNode = _cameFromDict[currentNode];
+            Vector v1 = new(currentNode.coordinates.longitude - previousNode.coordinates.longitude,
+                currentNode.coordinates.latitude - previousNode.coordinates.latitude);
+            Vector v2 = new(currentNode.coordinates.longitude - neighborNode.coordinates.longitude,
+                currentNode.coordinates.latitude - neighborNode.coordinates.latitude);
+            double nodeAngle = v1.Angle(v2);
+            angle = (nodeAngle / 180) * angleWeightFactor;
+        }
+        double prio = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!);
+        return distance / (speed + angle + prio * 0.5);
     }
 
-    private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
+    private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
     {
-        double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
+        double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor;
         
-        TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager;
+        TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
         TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
 
         bool sameName = false;
@@ -138,9 +152,34 @@ public class Pathfinder
         double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;
 
         double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
-        
+
         return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount);
     }
+
+    private class Vector
+    {
+        public float x, y;
+
+        public Vector(float x, float y)
+        {
+            this.x = x;
+            this.y = y;
+        }
+
+        public double Angle(Vector v2)
+        {
+            return Angle(this, v2);
+        }
+
+        public static double Angle(Vector v1, Vector v2)
+        {
+            double dotProd = v1.x * v2.x + v1.y * v2.y;
+            double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y);
+            double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y);
+            double ang = 180 - Math.Acos(dotProd / (v1L * v2L));
+            return ang;
+        }
+    }
     
     private double GetPriorityVehicleRoad(OsmEdge edge, Region region)
     {
@@ -156,6 +195,7 @@ public class Pathfinder
                 case WayType.motorway:
                 case WayType.motorway_link:
                 case WayType.motorroad:
+                    return 17;
                 case WayType.trunk:
                 case WayType.trunk_link:
                 case WayType.primary: