OSMServer/Pathfinding/RPriorityQueue.cs

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