PriorityQueue.cs

using System;
using System.Collections.Generic;
using System.Runtime.CompilerServices;

namespace Evenge
{
    public class PriorityQueue<T> : IPriorityQueue<T>
    {
        private readonly List<Entry> heap;
        private readonly Dictionary<T, int> indices = new Dictionary<T, int>();
        private readonly Comparer<T> comparer;
        private uint nextInsertionIndex = 0;

        public PriorityQueue(Comparer<T> comparer = null)
        {
            heap = new List<Entry>();
            if (comparer == null) { comparer = Comparer<T>.Default; }
            this.comparer = comparer;
        }

        /// <summary>
        /// Creates a copy of another priority queue.
        /// </summary>
        /// <param name="other"></param>
        public PriorityQueue(PriorityQueue<T> other)
        {
            heap = new List<Entry>(other.heap);
            indices = new Dictionary<T, int>(other.indices);
            comparer = other.comparer;
            nextInsertionIndex = other.nextInsertionIndex;
        }

        public T Dequeue()
        {
            if (Count == 0) { throw new InvalidOperationException("Priority queue is empty"); }
            return extract(first);
        }

        public void Enqueue(T item)
        {
            heap.Add(new Entry(item, nextInsertionIndex++));
            indices[item] = last;
            upHeap(last);
        }

        public void Update(T item)
        {
            // This works, but it's potentially wasteful.
            // TODO: Consider first comparing with the parent to decide which method to call.
            // Alternatively, manually swap the elements and then reheap each.
            int index = indices[item];
            if (upHeap(index) == index)
            {
                downHeap(index);
            }
        }

        public void Remove(T item)
        {
            extract(indices[item]);
        }

        private T extract(int index)
        {
            // Grab the item so we can return it later.
            T item = heap[index].Element;

            // Replace the extracted item with the last in the heap.
            heap[index] = heap[last];
            heap.RemoveAt(last);
            indices.Remove(item);

            // Restore heap property.
            downHeap(index);

            // Return the item we popped off earlier.
            return item;
        }

        public bool Contains(T item)
        {
            return indices.ContainsKey(item);
        }

        public int Count
        {
            get { return heap.Count; }
        }

        public T First
        {
            get
            {
                if (Count == 0) { throw new InvalidOperationException("Priority queue is empty"); }
                return heap[first].Element;
            }
        }

        public IEnumerator<T> GetEnumerator()
        {
            foreach (var entry in heap)
            {
                yield return entry.Element;
            }
        }

        System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
        {
            return GetEnumerator();
        }

        private int upHeap(int index)
        {
            while (index != first)
            {
                int p = parent(index);
                if (compare(heap[p], heap[index]) <= 0)
                {
                    break;
                }
                swap(p, index);
                index = p;
            }
            return index;
        }

        private int downHeap(int index)
        {
            while (true)
            {
                int left = leftChild(index);
                int right = rightChild(index);
                int smallest = index;
                if ((left < heap.Count) && compare(heap[left], heap[smallest]) < 0)
                {
                    smallest = left;
                }
                if ((right < heap.Count) && compare(heap[right], heap[smallest]) < 0)
                {
                    smallest = right;
                }
                if (smallest != index)
                {
                    swap(index, smallest);
                    index = smallest;
                }
                else
                {
                    break;
                }
            }
            return index;
        }

        private const int first = 0;

        private int last
        {
            get { return heap.Count - 1; }
        }

        private void swap(int a, int b)
        {
            var ha = heap[a];
            var hb = heap[b];
            heap[a] = hb;
            heap[b] = ha;
            indices[ha.Element] = b;
            indices[hb.Element] = a;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int parent(int index)
        {
            return (index - 1) / 2;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int leftChild(int index)
        {
            return 2 * index + 1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int rightChild(int index)
        {
            return 2 * index + 2;
        }

        private int compare(Entry a, Entry b)
        {
            int comparison = comparer.Compare(a.Element, b.Element);
            if (comparison != 0) { return comparison; }
            return a.InsertionIndex.CompareTo(b.InsertionIndex);
        }

        private struct Entry
        {
            public T Element { get; private set; }
            public uint InsertionIndex { get; private set; }

            public Entry(T element, uint insertionIndex) : this()
            {
                Element = element;
                InsertionIndex = insertionIndex;
            }
        }
    }

    public interface IPriorityQueue<T> : IEnumerable<T>
    {
        /// <summary>
        /// Removes and returns the element with the highest queue priority.
        /// </summary>
        T Dequeue();

        /// <summary>
        /// Inserts an element into the queue.
        /// </summary>
        void Enqueue(T item);

        /// <summary>
        /// Updates the given element's position in the queue.
        /// </summary>
        void Update(T item);

        /// <summary>
        /// Removes the given element from the queue.
        /// </summary>
        void Remove(T item);

        /// <summary>
        /// Returns whether the given item is in the queue.
        /// </summary>
        bool Contains(T item);

        /// <summary>
        /// Number of elements in the queue.
        /// </summary>
        int Count { get; }

        /// <summary>
        /// Element with the highest queue priority.
        /// </summary>
        T First { get; }
    }
}