1 files changed, 91 insertions, 0 deletions
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
 {