diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 45b3c71..4511e50 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -62,14 +62,9 @@ public class Pathfinder
                     gScore.TryAdd(neighbor, double.MaxValue);
                     if (tentativeGScore < gScore[neighbor])
                     {
-                        if (cameFromDict.ContainsKey(neighbor))
+                        if(!cameFromDict.TryAdd(neighbor, currentNode))
                             cameFromDict[neighbor] = currentNode;
-                        else
-                            cameFromDict.Add(neighbor, currentNode);
-                        if (gScore.ContainsKey(neighbor))
-                            gScore[neighbor] = tentativeGScore;
-                        else
-                            gScore.Add(neighbor, tentativeGScore);
+                        gScore[neighbor] = tentativeGScore;
                         double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle,
                             heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
                         //Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}");