using System;
using System.Collections;
using System.Collections.Generic;
namespace HeuristicLab.Algorithms.GraphRouting.Utilities {
  public class PriorityQueue<TPriority, TValue> : ICollection<KeyValuePair<TPriority, TValue>> {
    private List<KeyValuePair<TPriority, TValue>> heap;
    private Dictionary<TValue, KeyValuePair<TPriority, TValue>> values;
    private IComparer<TPriority> comparer;

    #region Constructors

    public PriorityQueue(IComparer<TPriority> comparer) {
      if (comparer == null)
        throw new ArgumentNullException();

      heap = new List<KeyValuePair<TPriority, TValue>>();
      values = new Dictionary<TValue, KeyValuePair<TPriority, TValue>>();
      this.comparer = comparer;
    }

    #endregion

    #region Priority queue methods

    public void Enqueue(TPriority priority, TValue value) {
      Insert(priority, value);
    }

    public KeyValuePair<TPriority, TValue> Dequeue() {
      if (Count != 0) {
        KeyValuePair<TPriority, TValue> item = heap[0];
        RemoveRoot();
        return item;
      } else {
        throw new InvalidOperationException("Priority queue is empty");
      }
    }

    public TValue DequeueValue() {
      return Dequeue().Value;
    }

    #endregion

    #region ICollection members

    public void Add(KeyValuePair<TPriority, TValue> item) {
      Enqueue(item.Key, item.Value);
    }

    public void Clear() {
      heap.Clear();
      values.Clear();
    }

    public bool Contains(KeyValuePair<TPriority, TValue> item) {
      return heap.Contains(item);
    }

    public bool Contains(TValue value) {
      return values.ContainsKey(value);
    }

    public void CopyTo(KeyValuePair<TPriority, TValue>[] array, int arrayIndex) {
      heap.CopyTo(array, arrayIndex);
    }

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

    public bool IsReadOnly {
      get { return false; }
    }

    public bool Remove(KeyValuePair<TPriority, TValue> item) {
      int idx = heap.IndexOf(item);
      if (idx < 0) {
        return false;
      }

      values.Remove(item.Value);

      heap[idx] = heap[heap.Count - 1];
      heap.RemoveAt(heap.Count - 1);

      HeapUp(idx);
      HeapDown(idx);

      return true;
    }

    public bool Remove(TValue value) {
      KeyValuePair<TPriority, TValue> item = values[value];
      return Remove(item);
    }

    #endregion

    #region IEnumerable Members

    public IEnumerator<KeyValuePair<TPriority, TValue>> GetEnumerator() {
      return heap.GetEnumerator();
    }

    IEnumerator IEnumerable.GetEnumerator() {
      return this.GetEnumerator();
    }

    #endregion

    #region Private methods

    private void SwapElements(int pos1, int pos2) {
      KeyValuePair<TPriority, TValue> element = heap[pos1];
      heap[pos1] = heap[pos2];
      heap[pos2] = element;
    }

    private void Insert(TPriority priority, TValue value) {
      KeyValuePair<TPriority, TValue> element = new KeyValuePair<TPriority, TValue>(priority, value);
      heap.Add(element);
      values.Add(value, element);

      HeapUp(heap.Count - 1);
    }

    private void RemoveRoot() {
      if (heap.Count <= 1) {
        heap.Clear();
        values.Clear();
      } else {
        KeyValuePair<TPriority, TValue> item = heap[0];
        values.Remove(item.Value);

        heap[0] = heap[heap.Count - 1];
        heap.RemoveAt(heap.Count - 1);

        HeapDown(0);
      }
    }

    private void HeapUp(int pos) {
      if (pos < heap.Count) {
        while (pos > 0) {
          int parent = (pos - 1) / 2;
          if (comparer.Compare(heap[parent].Key, heap[pos].Key) > 0) {
            SwapElements(parent, pos);
            pos = parent;
          } else {
            break;
          }
        }
      }
    }

    private void HeapDown(int pos) {
      while (true) {
        int smallest = pos;
        int left = 2 * pos + 1;
        int right = 2 * pos + 2;
        if (left < heap.Count && comparer.Compare(heap[smallest].Key, heap[left].Key) > 0) {
          smallest = left;
        }
        if (right < heap.Count && comparer.Compare(heap[smallest].Key, heap[right].Key) > 0) {
          smallest = right;
        }

        if (smallest != pos) {
          SwapElements(smallest, pos);
          pos = smallest;
        } else {
          break;
        }
      }
    }

    #endregion
  }
}