source: branches/1894_RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueues/Heap4.cs

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 Heap4<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 int finalLayerSize; // size of first layer with free space
    private int finalLayerDist; // distance to end of layer
    //private KNElement[] rawData;
    private KNElement[] data;    // alligned version of rawData

    public Heap4(K sup, K infimum, int cap) {
      capacity = cap;
      supremum = sup;
      data = new KNElement[capacity + 4];

      for (int i = 0; i < capacity + 4; i++) {
        data[i] = new KNElement();
      }

      data[0].Key = infimum; // sentinel
      data[capacity + 1].Key = supremum;
      data[capacity + 2].Key = supremum;
      data[capacity + 3].Key = supremum;
      Reset();
    }

    public int Size {
      get { return size; }
    }

    public K Supremum {
      get { return data[capacity + 1].Key; }
    }

    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() {
      K minKey;
      K otherKey;
      int delta;

      // first move up elements on a min-path
      int hole = 1;
      int succ = 2;
      int layerSize = 4;
      int layerPos = 0;
      int sz = size;
      size = sz - 1;
      finalLayerDist++;
      if (finalLayerDist == finalLayerSize) {
        finalLayerSize >>= 2;
        finalLayerDist = 0;
      }

      while (succ < sz) {
        minKey = data[succ].Key;
        delta = 0;

        otherKey = data[succ + 1].Key;
        if (otherKey.CompareTo(minKey) < 0) {
          minKey = otherKey;
          delta = 1;
        }
        otherKey = data[succ + 2].Key;
        if (otherKey.CompareTo(minKey) < 0) {
          minKey = otherKey;
          delta = 2;
        }
        otherKey = data[succ + 3].Key;
        if (otherKey.CompareTo(minKey) < 0) {
          minKey = otherKey;
          delta = 3;
        }
        succ += delta;
        layerPos += delta;

        // move min successor up
        data[hole].Key = minKey;
        data[hole].Value = data[succ].Value;

        // step to next layer
        hole = succ;
        succ = succ - layerPos + layerSize; // beginning of next layer
        layerPos <<= 2;
        succ += layerPos; // now correct value
        layerSize <<= 2;
      }

      // bubble up rightmost element
      K bubble = data[sz].Key;
      layerSize >>= 2; // now size of hole's layer
      layerPos >>= 2; // now pos of hole within its layer

      int layerDist = layerSize - layerPos - 1; // hole's dist to end of layer
      int pred = hole + layerDist - layerSize; // end of pred's layer for now
      layerSize >>= 2; // now size of pred's layer
      layerDist >>= 2; // now pred's pos in layer
      pred = pred - layerDist; // finally preds index

      while (data[pred].Key.CompareTo(bubble) > 0) {  // must terminate since inf at root
        data[hole].Key = data[pred].Key;
        data[hole].Value = data[pred].Value;
        hole = pred;
        pred = hole + layerDist - layerSize; // end of hole's layer for now
        layerSize >>= 2;
        layerDist >>= 2;
        pred = pred - layerDist; // finally preds index
      }

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

      data[sz].Key = Supremum; // mark as deleted
    }

    public void Insert(K key, V value) {
      int layerSize = finalLayerSize;
      int layerDist = finalLayerDist;
      finalLayerDist--;

      if (finalLayerDist == -1) { // layer full
        // start next layer
        finalLayerSize <<= 2;
        finalLayerDist = finalLayerSize - 1;
      }

      size++;

      int hole = size;
      int pred = hole + layerDist - layerSize; // end of pred's layer for now
      layerSize >>= 2; // now size of pred's layer
      layerDist >>= 2;
      pred = pred - layerDist; // finally preds index
      K predKey = data[pred].Key;
      while (predKey.CompareTo(key) > 0) {
        data[hole].Key = predKey;
        data[hole].Value = data[pred].Value;
        hole = pred;
        pred = hole + layerDist - layerSize; // end of pred's layer for now
        layerSize >>= 2; // now isze of pred's layer
        layerDist >>= 2;
        pred = pred - layerDist; // finally preds index
        predKey = data[pred].Key;
      }

      // finally move data to hole
      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;
      finalLayerSize = 1;
      finalLayerDist = 0;
      K sup = Supremum;
      int cap = capacity;
      for (int i = 1; i <= cap; i++) {
        data[i].Key = sup;
      }
    }
  }
}