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;
}