1 files changed, 146 insertions, 0 deletions
diff --git a/core/priority_queue.hpp b/core/priority_queue.hpp
new file mode 100644
index 0000000..efcdc7e
--- /dev/null
+++ b/core/priority_queue.hpp
@@ -0,0 +1,146 @@
+#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