[C#] Priority Queue（優先度付き待ち行列）の、SortedDictionaryとQueueを使った平易な実装

競技プログラミング用のSortedDictionaryとQueueを使った平易な実装。キーの重複が可能で、キーが同一のものはFIFO（先入れ先出し）になります。確認はしていないけれど挿入と取り出しがO(log n)で出来るはずです。IEnumerableなどはコードが長くなるので面倒なので実装していません。

// キーの重複がOKな優先度付きキュー
public class PriorityQueue<TKey, TValue>
{
    SortedDictionary<TKey, Queue<TValue>> dict = new SortedDictionary<TKey, Queue<TValue>>();
    int count = 0;

    public int Count
    {
        get
        {
            return count;
        }
    }

    public void Add(TKey key, TValue value)
    {
        if (!dict.ContainsKey(key))
        {
            dict[key] = new Queue<TValue>();
        }

        dict[key].Enqueue(value);
        count++;
    }

    public KeyValuePair<TKey, TValue> Dequeue()
    {
        var queue = dict.First();
        if (queue.Value.Count <= 1)
        {
            dict.Remove(queue.Key);
        }
        count--;
        return new KeyValuePair<TKey, TValue>(queue.Key, queue.Value.Dequeue());
    }
}

使い方：

var p = new PriorityQueue<int, int>();

while (p.Count != 0)
{
    var kvpair = p.Dequeue();
    Console.WriteLine(kvpair.Key + ", " + kvpair.Value);
    ...
    p.Add(key, value);
}