diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 65e94fc..2eafa07 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -49,7 +49,7 @@ public class Pathfinder
             return this;
         }
 
-        PriorityQueue<OsmNode, double> openSetfScore = new();
+        RPriorityQueue<OsmNode, double> openSetfScore = new();
         openSetfScore.Enqueue(startNode, 0);
         gScore = new() { { startNode, 0 } };
         _cameFromDict = new();
@@ -78,7 +78,7 @@ public class Pathfinder
                             _cameFromDict[neighbor] = currentNode;
                         gScore[neighbor] = tentativeGScore;
                         double h = Heuristic(currentNode, neighbor, goalNode, edge);
-                        openSetfScore.Enqueue(neighbor, tentativeGScore + h); //Currently set includes a lot of duplicates
+                        openSetfScore.Enqueue(neighbor, tentativeGScore + h);
                     }
                 }
             }
diff --git a/Pathfinding/RPriorityQueue.cs b/Pathfinding/RPriorityQueue.cs
new file mode 100644
index 0000000..5463e37
--- /dev/null
+++ b/Pathfinding/RPriorityQueue.cs
@@ -0,0 +1,36 @@
+namespace Pathfinding;
+
+public class RPriorityQueue<TKey, TPriority> where TKey : notnull
+{
+    public Dictionary<TKey, TPriority> queue;
+    public int Count { get; private set; }
+
+    public RPriorityQueue()
+    {
+        queue = new();
+    }
+
+    public void Enqueue(TKey key, TPriority priority)
+    {
+        if (!queue.TryAdd(key, priority))
+            queue[key] = priority;
+        Count = queue.Count;
+    }
+
+    public TKey? Dequeue()
+    {
+        TKey? retKey = queue.MinBy(item => item.Value).Key;
+        queue.Remove(retKey);
+        Count = queue.Count;
+        return retKey;
+    }
+
+    public int Remove(IEnumerable<TKey> elements)
+    {
+        int before = Count;
+        queue = queue.Where(queueitem => !elements.Contains(queueitem.Key))
+            .ToDictionary(item => item.Key, item => item.Value);
+        Count = queue.Count;
+        return before - Count;
+    }
+}
\ No newline at end of file