Reorganize code into library for reuse

lib/pq.c

@@ -0,0 +1,110 @@
+#include <stdlib.h>
+#include "pq.h"
+
+typedef struct {
+    unsigned int priority;
+    size_t x, y;
+    mazeedge_dir_t dir;
+} _point_t;
+
+struct pq {
+    size_t heap_size, next;
+    _point_t* heap; 
+};
+
+struct pq* pq_new(size_t heap_size) {
+    struct pq* result = malloc(sizeof(struct pq));
+    result->heap_size = heap_size;
+    result->next = 0;
+    result->heap = calloc(heap_size, sizeof(_point_t));
+    return result;
+}
+
+static void _swap(struct pq* q, size_t idx1, size_t idx2) {
+    unsigned int priority = q->heap[idx1].priority;
+    size_t x = q->heap[idx1].x;
+    size_t y = q->heap[idx1].y;
+    mazeedge_dir_t dir = q->heap[idx1].dir;
+
+    q->heap[idx1].priority = q->heap[idx2].priority;
+    q->heap[idx1].x = q->heap[idx2].x;
+    q->heap[idx1].y = q->heap[idx2].y;
+    q->heap[idx1].dir = q->heap[idx2].dir;
+
+    q->heap[idx2].priority = priority;
+    q->heap[idx2].x = x;
+    q->heap[idx2].y = y;
+    q->heap[idx2].dir = dir;
+}
+
+static void _heapify_up(struct pq* q) {
+    size_t i = q->next - 1;
+    while (i > 0) {
+        size_t parent = i / 2;
+        if (q->heap[parent].priority > q->heap[i].priority) {
+            _swap(q, parent, i);
+            i = parent;
+        } else {
+            // done
+            return;
+        }
+    }
+}
+
+void pq_add(struct pq* q, unsigned int priority, size_t x, size_t y, mazeedge_dir_t dir) {
+    q->heap[q->next].x = x;
+    q->heap[q->next].y = y;
+    q->heap[q->next].priority = priority;
+    q->heap[q->next].dir = dir;
+    q->next++;
+    _heapify_up(q);
+}
+
+bool pq_empty(struct pq const* q) {
+    return q->next == 0;
+}
+
+static void _heapify_down(struct pq* q) {
+    size_t i = 0;
+    while (i < q->next) {
+        size_t ch1 = i * 2, ch2 = i * 2 + 1;
+        if (ch1 >= q->next) return;
+        if (ch2 >= q->next) {
+            if (q->heap[ch1].priority < q->heap[i].priority) {
+                _swap(q, i, ch1);
+            }
+            return;
+        }
+
+        // three-way compare
+        if ((q->heap[i].priority < q->heap[ch1].priority) &&
+            (q->heap[i].priority < q->heap[ch2].priority)) {
+            return; // all done - partial order ensured
+        }
+        if (q->heap[ch1].priority < q->heap[ch2].priority) {
+            _swap(q, i, ch1);
+            i = ch1;
+        } else {
+            _swap(q, i, ch2);
+            i = ch2;
+        }
+    }
+}
+
+int pq_pop(struct pq* q, size_t* x, size_t* y, mazeedge_dir_t* dir) {
+    if (pq_empty(q)) return 0;
+    *x = q->heap[0].x;
+    *y = q->heap[0].y;
+    *dir = q->heap[0].dir;
+    _swap(q, 0, q->next-1);
+    q->next--;
+    _heapify_down(q);
+    return 1;
+}
+
+void pq_free(struct pq* q) {
+    free(q->heap); q->heap = NULL;
+    q->heap_size = 0;
+    q->next = 0;
+    free(q);
+}