#ifndef RIoeyayKWeHMNQr5V3eTPolQ4fQ
#define RIoeyayKWeHMNQr5V3eTPolQ4fQ

#include <assert.h>
#include <boost/swap.hpp>
#include <inttypes.h>
#include <string.h>

template<typename Val, typename Payload>
struct PriorityQueue {
  // types
  typedef Payload payload_t;

  // creation
  PriorityQueue();
  PriorityQueue(const PriorityQueue &);

  // access
  inline Val& minVal();
  inline Payload& minPayload();
  inline void insert(const Val val, const Payload payload);
  inline void removeMin();

  inline bool isEmpty() const;
  size_t getSize() const; // returns size in bytes, _not_ # of elements

  // intern
  typedef uint32_t Id;
  inline void swap(const Id id1, const Id id2);
  void heapify(const Id id);

  // data
  Id length;
  struct Element {
    Val val;
    Payload payload;
  } heap[0];
  static const size_t elementSize = sizeof(Element);

};

template<typename Val, typename Payload>
PriorityQueue<Val, Payload>::PriorityQueue() {
  length = 0;
}

template<typename Val, typename Payload>
PriorityQueue<Val, Payload>::PriorityQueue(const PriorityQueue &src) {
  length = src.length;
  memcpy(heap, src.heap, length * sizeof(Element));
}


template<typename Val, typename Payload>
inline Val& PriorityQueue<Val, Payload>::minVal() {
  assert(!isEmpty());
  return heap[0].val;
}

template<typename Val, typename Payload>
inline Payload& PriorityQueue<Val, Payload>::minPayload() {
  assert(!isEmpty());
  return heap[0].payload;
}

template<typename Val, typename Payload>
inline void PriorityQueue<Val, Payload>::insert(const Val val, const Payload payload) {
  heap[length].val = val;
  heap[length].payload = payload;
  length++;
  heapify(length-1);
}

template<typename Val, typename Payload>
void PriorityQueue<Val, Payload>::removeMin() {
  length--;
  swap(0, length);
  heapify(0);
}

template<typename Val, typename Payload>
inline bool PriorityQueue<Val, Payload>::isEmpty() const {
  return (length==0);
}

template<typename Val, typename Payload>
size_t PriorityQueue<Val, Payload>::getSize() const {
  return ((char*) &(heap[length])) - (char*) this;
}

template<typename Val, typename Payload>
inline void PriorityQueue<Val, Payload>::swap(const Id id1, const Id id2) {
  boost::swap(heap[id1], heap[id2]);
}

template<typename Val, typename Payload>
void PriorityQueue<Val, Payload>::heapify(const Id id) {
  Id prev(id),
    next((id-1)/2);

  // upward
  while (prev != 0 // this is not the root of the tree
	 && heap[prev].val // and our element can still
	 < heap[next].val  // bubble further up ...
	 ) {
    swap(prev, next);
    prev = next;
    next = ((next-1)/2);
  }

  // downward
  while (true) {
    next = 2*prev + 1;

    // no child?
    if (next >= length)
      return;

    // one child?
    if (next == length - 1) {
      if (heap[next].val < heap[prev].val) {
	swap(prev, next);
	continue;
      }
      return;
    }

    // two childs!
    if ((heap[next].val < heap[next+1].val) && (heap[next].val < heap[prev].val)) {
      swap(next, prev);
      prev = next;
      continue;
    }

    if ((heap[next+1].val <= heap[next].val) && (heap[next+1].val < heap[prev].val)) {
      swap(next+1, prev);
      prev = next+1;
      continue;
    }

    // we are bigger than both childs
    return;
  }
}

#endif // RIoeyayKWeHMNQr5V3eTPolQ4fQ