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/
  // Fast Binary Heap
  public sealed class BinaryHeapV3<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 BinaryHeapV3(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() {
      // first move up elements on a min-path
      int hole = 1;
      int succ = 2;
      int sz = size;
      KNElement[] dat = data;

      while (succ < sz) {
        K key1 = dat[succ].Key;
        K key2 = dat[succ + 1].Key;
        if (key1.CompareTo(key2) > 0) {
          succ++;
          dat[hole].Key = key2;
          dat[hole].Value = dat[succ].Value;
        } else {
          dat[hole].Key = key1;
          dat[hole].Value = dat[succ].Value;
        }
        hole = succ;
        succ <<= 1;
      }

      // bubble up rightmost element
      K bubble = dat[sz].Key;
      int pred = hole >> 1;
      while (dat[pred].Key.CompareTo(bubble) > 0) {
        //dat[hole] = dat[pred];
        dat[hole].Key = dat[pred].Key;
        dat[hole].Value = dat[pred].Value;

        hole = pred;
        pred >>= 1;
      }

      // finally move data to hole
      dat[hole].Key = bubble;
      dat[hole].Value = dat[sz].Value;

      dat[size].Key = supremum; // mark as deleted
      size = sz - 1;
    }

    public void Insert(K key, V value) {
      KNElement[] dat = data;
      size++;

      int hole = size;
      int pred = hole >> 1;
      K predKey = dat[pred].Key;

      while (predKey.CompareTo(key) > 0) {  // must terminate due to sentinel at 0
        dat[hole].Key = predKey;
        dat[hole].Value = dat[pred].Value;
        hole = pred;
        pred >>= 1;
        predKey = dat[pred].Key;
      }

      // finally move data to hole
      dat[hole].Key = key;
      dat[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;
      }
    }
  }
}