Initial commit

srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/PriorityQueue..cs

@@ -0,0 +1,25 @@
+namespace RobotNet10.GlobalPathPlanner;
+
+public class PriorityQueue<T>(Comparison<T> comparison)
+{
+    public List<T> Items => items;
+    private readonly List<T> items = [];
+    private readonly IComparer<T> comparer = Comparer<T>.Create(comparison);
+
+    public void Enqueue(T item)
+    {
+        int index = items.BinarySearch(item, comparer);
+        if (index < 0) index = ~index;
+        items.Insert(index, item);
+    }
+
+    public T Dequeue()
+    {
+        if (items.Count == 0) throw new InvalidOperationException("Queue is empty");
+        var item = items[0];
+        items.RemoveAt(0);
+        return item;
+    }
+
+    public int Count => items.Count;
+}