3 files changed, 111 insertions, 0 deletions

diff --git a/src/runtime/c/gu/fun.h b/src/runtime/c/gu/fun.h
index 0004e9923..f4c1a5a38 100644
--- a/src/runtime/c/gu/fun.h
+++ b/src/runtime/c/gu/fun.h
@@ -62,4 +62,10 @@ struct GuEquality {
 	bool (*is_equal)(GuEquality* self, const void* a, const void* b);
 };
 
+typedef const struct GuOrder GuOrder;
+
+struct GuOrder {
+	int (*compare)(GuOrder* self, const void* a, const void* b);
+};
+
 #endif // GU_FUN_H_
diff --git a/src/runtime/c/gu/seq.c b/src/runtime/c/gu/seq.c
index 660cf5af6..ab1ba7f29 100644
--- a/src/runtime/c/gu/seq.c
+++ b/src/runtime/c/gu/seq.c
@@ -3,6 +3,7 @@
 #include <gu/fun.h>
 #include <gu/assert.h>
 #include <string.h>
+#include <stdlib.h>
 
 
 struct GuBuf {
@@ -162,6 +163,96 @@ gu_buf_freeze(GuBuf* buf, GuPool* pool)
 	return seq;
 }
 
+static void
+gu_heap_siftdown(GuBuf *buf, GuOrder *order, int startpos, int pos)
+{
+	void *newitem = alloca(buf->elem_size);
+	memcpy(newitem, &buf->data[buf->elem_size * pos], buf->elem_size);
+
+	while (pos > startpos) {
+		int parentpos = (pos - 1) >> 1;
+        void *parent = &buf->data[buf->elem_size * parentpos];
+        
+		if (order->compare(order, newitem, parent) < 0) {
+			memcpy(&buf->data[buf->elem_size * pos], parent, buf->elem_size);
+			pos = parentpos;
+			continue;
+		}
+		break;
+	}
+
+	memcpy(&buf->data[buf->elem_size * pos], newitem, buf->elem_size);
+}
+
+static void
+gu_heap_siftup(GuBuf *buf, GuOrder *order, int pos)
+{
+	int startpos = pos;
+	int endpos = gu_buf_length(buf);
+	void *newitem = alloca(buf->elem_size);
+	memcpy(newitem, &buf->data[buf->elem_size * pos], buf->elem_size);
+   
+	int childpos = 2*pos + 1;
+	while (childpos < endpos) {
+		int rightpos = childpos + 1;
+		if (rightpos < endpos &&
+		    order->compare(order, 
+			               &buf->data[buf->elem_size * childpos],
+			               &buf->data[buf->elem_size * rightpos]) >= 0) {
+			childpos = rightpos;
+		}
+
+		memcpy(&buf->data[buf->elem_size * pos], 
+		       &buf->data[buf->elem_size * childpos], buf->elem_size);
+		pos = childpos;
+		childpos = 2*pos + 1;
+   }
+   
+   memcpy(&buf->data[buf->elem_size * pos], newitem, buf->elem_size);
+   gu_heap_siftdown(buf, order, startpos, pos);
+}
+
+void
+gu_buf_heap_push(GuBuf *buf, GuOrder *order, void *value)
+{
+	memcpy(gu_buf_extend(buf), value, buf->elem_size);
+    gu_heap_siftdown(buf, order, 0, gu_buf_length(buf)-1);
+}
+
+void
+gu_buf_heap_pop(GuBuf *buf, GuOrder *order, void* data_out)
+{
+	const void* last = gu_buf_trim(buf); // raises an error if empty
+
+	if (gu_buf_length(buf) > 0) {
+		memcpy(data_out, buf->data, buf->elem_size);
+		memcpy(buf->data, last, buf->elem_size);
+		gu_heap_siftup(buf, order, 0);
+	} else {
+		memcpy(data_out, last, buf->elem_size);
+	}
+}
+
+void
+gu_buf_heap_replace(GuBuf *buf, GuOrder *order, void *value, void *data_out)
+{
+	gu_require(gu_buf_length(buf) > 0);
+
+	memcpy(data_out, buf->data, buf->elem_size);
+	memcpy(buf->data, value, buf->elem_size);
+	gu_heap_siftup(buf, order, 0);
+}
+
+void
+gu_buf_heapify(GuBuf *buf, GuOrder *order)
+{
+	size_t middle = gu_buf_length(buf) / 2;
+	
+	for (size_t i = 0; i < middle; i++) {
+		gu_heap_siftup(buf, order, i);
+	}
+}
+
 typedef struct GuBufOut GuBufOut;
 struct GuBufOut
 {
diff --git a/src/runtime/c/gu/seq.h b/src/runtime/c/gu/seq.h
index 257d71e5f..b21933a65 100644
--- a/src/runtime/c/gu/seq.h
+++ b/src/runtime/c/gu/seq.h
@@ -124,6 +124,20 @@ gu_buf_trim(GuBuf* buf);
 void
 gu_seq_resize_tail(GuSeq seq, ptrdiff_t change);
 
+
+// Using a buffer as a heap
+void
+gu_buf_heap_push(GuBuf *buf, GuOrder *order, void *value);
+
+void
+gu_buf_heap_pop(GuBuf *buf, GuOrder *order, void* data_out);
+
+void
+gu_buf_heap_replace(GuBuf *buf, GuOrder *order, void *value, void *data_out);
+
+void
+gu_buf_heapify(GuBuf *buf, GuOrder *order);
+
 #if 0
 void
 gu_buf_resize_head(GuBuf* buf, ptrdiff_t change);