OSMServer/Pathfinding/RPriorityQueue.cs

namespace Pathfinding;

public class RPriorityQueue<TKey, TPriority> where TKey : notnull
{
    public Dictionary<TKey, TPriority> queue;
    public int Count => queue.Count;

    public RPriorityQueue()
    {
        queue = new();
    }

    public void Enqueue(TKey key, TPriority priority)
    {
        if (!queue.TryAdd(key, priority))
            queue[key] = priority;
    }

    public TKey Dequeue()
    {
        TKey retKey = queue.MinBy(item => item.Value).Key;
        queue.Remove(retKey);
        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);
        return before - Count;
    }

    public int RemoveExcept(IEnumerable<TKey> exceptKeys)
    {
        int before = Count;
        queue = queue.IntersectBy(exceptKeys, item => item.Key).ToDictionary(item => item.Key, item => item.Value);
        return before - Count;
    }

    public int Clear()
    {
        int before = Count;
        queue.Clear();
        return before;
    }
}