using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
namespace HeuristicLab.Algorithms.GraphRouting.PriorityQueues {
  // implementation based on C++ version from Peter Sanders
  // http://www.mpi-inf.mpg.de/~sanders/programs/spq/
  public sealed class BinaryHeapV2<K, V> : IHeap<K, V> where K : IComparable {
    private class KNElement {
      public K Key { get; set; }
      public V Value { get; set; }
    }

    private int capacity;
    private K supremum;
    private int size; // index of last used element
    private KNElement[] data;

    public BinaryHeapV2(K sup, K infimum, int cap) {
      size = 0;
      capacity = cap;
      data = new KNElement[cap + 2];
      for (int i = 0; i < cap + 2; i++) {
        data[i] = new KNElement();
      }
      data[0].Key = infimum;
      data[capacity + 1].Key = sup;
      supremum = sup;
      Reset();

    }

    public int Size {
      get { return size; }
    }

    public KeyValuePair<K, V> PeekMin() {
      if (size == 0) {
        throw new InvalidOperationException("Heap is empty");
      }
      return new KeyValuePair<K, V>(data[1].Key, data[1].Value);
    }

    public K PeekMinKey() {
      return data[1].Key;
    }

    public V PeekMinValue() {
      return data[1].Value;
    }

    public void RemoveMin() {
      int i;
      int l;
      int r;
      int smallest;

      if (size <= 0) {
        throw new InvalidOperationException("Cannot delete on an empty queue");
      }

      data[1].Key = data[size].Key;
      data[1].Value = data[size].Value;

      data[size].Key = supremum;
      size--;
      i = 1;
      for (; ; ) {
        l = (i << 1);
        r = (i << 1) + 1;
        if ((l <= size) && (data[l].Key.CompareTo(data[i].Key) == -1)) {
          smallest = l;
        } else {
          smallest = i;
        }
        if ((r <= size) && (data[r].Key.CompareTo(data[smallest].Key) == -1)) {
          smallest = r;
        }
        if (smallest == i) break;
        KNElement temp = data[i];
        data[i] = data[smallest];
        data[smallest] = temp;
        i = smallest;
      }
    }

    public void Insert(K key, V value) {
      size++;
      int hole = size;
      while ((hole > 1) && (data[hole >> 1].Key.CompareTo(key) == 1)) {
        data[hole].Key = data[hole >> 1].Key;
        data[hole].Value = data[hole >> 1].Value;
        hole = hole >> 1;
      }
      data[hole].Key = key;
      data[hole].Value = value;
    }

    public void DecreaseKey(K key, V value) {
      throw new NotImplementedException();
    }

    // reset size to 0 and fill data array with sentinels
    private void Reset() {
      size = 0;
      K sup = supremum;
      int cap = capacity;
      for (int i = 1; i <= cap; i++) {
        data[i].Key = sup;
      }
    }
  }
}