using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
namespace HeuristicLab.Algorithms.GraphRouting.PriorityQueues {
  public sealed class BinaryHeap<K, V> : IHeap<K, V>
    where K : IComparable
    where V : IComparable {
    private class HeapElement {
      public K Key { get; set; }
      public V Value { get; set; }
    }

    private int capacity;
    private int size;
    private HeapElement[] data;

    public BinaryHeap(int cap) {
      size = 0;
      capacity = cap;
      data = new HeapElement[capacity];
    }

    public int Size {
      get { return size; }
    }

    public void Insert(K key, V value) {
      HeapElement elem = new HeapElement();
      elem.Key = key;
      elem.Value = value;
      data[size] = elem;
      HeapifyDown(size);
      size++;
    }

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

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

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

    public void RemoveMin() {
      RemoveAt(0);
    }

    public void DecreaseKey(K key, V value) {
      for (int i = 0; i < size; i++) {
        if (data[i].Value.CompareTo(value) == 0) {
          RemoveAt(i);
          Insert(key, value);
          break;
        }
      }
    }

    private void RemoveAt(int idx) {
      if (size == 1) {
        size--;
      } else {
        if (idx < size - 1) {
          Swap(idx, size - 1);
          HeapifyUp(idx);
        }
        size--;
      }
    }

    private void HeapifyDown(int start) {
      int i = start;
      int j = (i - 1) / 2;
      while (i > 0 && (data[i].Key.CompareTo(data[j].Key) == -1)) {
        Swap(i, j);
        i = j;
        j = (i - 1) / 2;
      }
    }

    private void HeapifyUp(int start) {
      int idx = start;
      int left = 2 * idx + 1;
      int right = 2 * idx + 2;
      while ((left < size - 1 && data[idx].Key.CompareTo(data[left].Key) != -1) ||
             (right < size - 1 && data[idx].Key.CompareTo(data[right].Key) != -1)) {
        if ((right >= size - 1) || (data[left].Key.CompareTo(data[right].Key) == -1)) {
          Swap(left, idx);
          idx = left;
        } else {
          Swap(right, idx);
          idx = right;
        }
        left = 2 * idx + 1;
        right = 2 * idx + 2;
      }
    }

    private void Swap(int i, int j) {
      var elem = data[i];
      data[i] = data[j];
      data[j] = elem;
    }
  }
}