using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;

namespace HeuristicLab.Algorithms.GraphRouting.PriorityQueues {
  public sealed class FibonacciHeap<K, V> : IHeap<K, V>
    where K : IComparable
    where V : IComparable {

    private HeapNode min;
    private int size;

    public int Size {
      get { return size; }
    }

    public FibonacciHeap() {
      size = 0;
      min = null;
    }

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

    public K PeekMinKey() {
      if (size == 0) {
        throw new InvalidOperationException("Heap is empty");
      }
      return min.Key;
    }

    public V PeekMinValue() {
      if (size == 0) {
        throw new InvalidOperationException("Heap is empty");
      }
      return min.Value;
    }

    public void Insert(K key, V value) {
      HeapNode n = new HeapNode(key, value);
      if (min == null) {
        min = n;
      } else {
        // concateneate the root list (becoming left sibling of root)
        n.Right = min;
        n.Left = min.Left;
        n.Right.Left = n;
        n.Left.Right = n;
        // update point of minimum if necessary
        if (n.Key.CompareTo(min.Key) < 0) {
          min = n;
        }
      }
      size++;
    }

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

    public void RemoveMin() {
      if (size == 0) {
        throw new InvalidOperationException("Heap is empty");
      }
      HeapNode z = min;
      if (z.Child != null) {
        // set all z's childs parent to null ...
        z.Child.Parent = null;
        HeapNode x = z.Child.Right;
        while (x != z.Child) {
          x.Parent = null;
          x = x.Right;
        }
        // ... and make all of them roots 
        HeapNode minLeft = min.Left;
        min.Left = z.Child.Left;
        min.Left.Right = min;
        z.Child.Left = minLeft;
        minLeft.Right = z.Child;
      }

      // remove z frome root list
      z.Left.Right = z.Right;
      z.Right.Left = z.Left;

      if (z == z.Right) {
        min = null;
      } else {
        // point min to a node other than z
        // (not necessarily going to be the new minimum)
        min = z.Right;
        Consolidate();
      }
      size--;

      //var zNode = new KeyValuePair<K, V>(z.Key, z.Value);
      z = null;
      //return zNode;
    }

    private void Link(HeapNode y, HeapNode z) {
      // remove y from the root list
      y.Left.Right = y.Right;
      y.Right.Left = y.Left;

      // make y a child of x, ...
      y.Parent = z;
      if (z.Child == null) {
        z.Child = y;
        y.Right = y;
        y.Left = y;
      } else {
        y.Left = z.Child;
        y.Right = z.Child.Right;
        z.Child.Right = y;
        y.Right.Left = y;
      }

      // ... incrementing degree[x]
      z.Degree++;
      y.Mark = false;
    }

    private void Consolidate() {
      int dn = (int)Math.Floor(Math.Log(size) / Math.Log(2)) + 1; // log2size[Heap]
      HeapNode[] A = new HeapNode[dn];  // consolidation array
      HeapNode start = min;
      HeapNode w = min;

      // for each node w in the root list
      do {
        HeapNode x = w;
        HeapNode nextW = w.Right;
        int d = x.Degree;

        while (A[d] != null) {
          HeapNode y = A[d];     // another node with the same degree as x
          if (x.Key.CompareTo(y.Key) > 0) {
            // exchange x <-> y
            HeapNode tmp = x;
            x = y;
            y = tmp;
          }
          if (y == start) {
            start = start.Right;
          }
          if (y == nextW) {
            nextW = nextW.Right;
          }
          Link(y, x);
          A[d] = null;
          d++;
        }

        A[d] = x;
        w = nextW;
      } while (w != start);

      min = null;

      // find the new minimum key
      for (int i = 0; i < dn; i++) {
        if (A[i] != null) {
          HeapNode n = A[i];
          if (min == null) {
            min = n;
          } else {
            if (n.Key.CompareTo(min.Key) < 0) {
              min = n;
            }
          }
        }
      }
    }

    private class HeapNode {
      public K Key;
      public V Value;
      public int Degree;
      public bool Mark;
      public HeapNode Left;
      public HeapNode Right;
      public HeapNode Parent;
      public HeapNode Child;

      public HeapNode(K key, V value) {
        this.Key = key;
        this.Value = value;
        this.Right = this;
        this.Left = this;
        this.Parent = null;
        this.Child = null;
        this.Degree = 0;
        this.Mark = false;
      }
    }
  }
}