Use generic PQ

lib/pq.h

@@ -1,15 +1,101 @@
 #ifndef _PQ_H
 #define _PQ_H 1
 
-#include <stdbool.h>
-#include "grid.h"
+#include <stdlib.h>
+#include <string.h>
 
-struct pq;
+#define DEFAULT_HEAP_SIZE 16
 
-struct pq* pq_new(size_t heap_size);
-void pq_add(struct pq* q, unsigned int priority, size_t x, size_t y, mazeedge_dir_t dir);
-int pq_pop(struct pq* q, size_t* x, size_t* y, mazeedge_dir_t *dir);
-bool pq_empty(struct pq const* q);
-void pq_free(struct pq* q);
+#define DEFINE_PQ(T, N, P) \
+struct _##N##HeapNode { \
+    P priority; \
+    T data; \
+}; \
+typedef struct { \
+    size_t heap_size, next; \
+    struct _##N##HeapNode* heap; \
+} N; \
+static void _##N##_swap(N* pq, size_t i, size_t j) { \
+    struct _##N##HeapNode tmp = pq->heap[i]; \
+    pq->heap[i] = pq->heap[j]; \
+    pq->heap[j] = tmp; \
+} \
+static void _##N##_heapify_up(N* pq) { \
+    size_t i = pq->next - 1; \
+    while (i > 0) { \
+        size_t parent = i / 2; \
+        if (pq->heap[parent].priority > pq->heap[i].priority) { \
+            _##N##_swap(pq, parent, i); \
+            i = parent; \
+        } else return; \
+    } \
+} \
+static void _##N##_heapify_down(N* pq) { \
+    size_t i = 0; \
+    while (i < pq->next) { \
+        size_t ch1 = i * 2, ch2 = i * 2 + 1; \
+        if (ch1 >= pq->next) return; \
+        if (ch2 >= pq->next) { \
+            if (pq->heap[ch1].priority < pq->heap[i].priority) { \
+                _##N##_swap(pq, i, ch1); \
+            } \
+            return; \
+        } \
+        if ((pq->heap[i].priority < pq->heap[ch1].priority) && \
+            (pq->heap[i].priority < pq->heap[ch2].priority)) { \
+            return; \
+        } \
+        if (pq->heap[ch1].priority < pq->heap[ch2].priority) { \
+            _##N##_swap(pq, i, ch1); \
+            i = ch1; \
+        } else { \
+            _##N##_swap(pq, i, ch2); \
+            i = ch2; \
+        } \
+    } \
+}
+
+#define NEW_PQ(T, N) \
+    T N = (T) { .heap_size = DEFAULT_HEAP_SIZE, .next = 0, .heap = calloc(DEFAULT_HEAP_SIZE, sizeof(struct _##T##HeapNode)) }
+
+// WARNING: S cannot be an expression that has side effects!
+#define NEW_PQ_SZ(T, N, S) \
+    T N = (T) { .heap_size = (S), .next = 0, .heap = calloc((S), sizeof(struct _##T##HeapNode)) }
+
+#define PUSH_PQ(Q, N, I, P) { \
+    if ((Q).next >= (Q).heap_size) { \
+        (Q).heap = realloc((Q).heap, (Q).heap_size * 2 * sizeof(struct _##N##HeapNode)); \
+        (Q).heap_size *= 2; \
+    } \
+    (Q).heap[(Q).next].data = (I); \
+    (Q).heap[(Q).next].priority = (P); \
+    (Q).next++; \
+    _##N##_heapify_up(&(Q)); \
+}
+
+#define EMPTY_PQ(Q) ((Q).next == 0)
+
+// check for emptiness first!
+#define POP_PQ(Q, N, DST_I, DST_P) { \
+    DST_I = (Q).heap[0].data; \
+    DST_P = (Q).heap[0].priority; \
+    _##N##_swap(&(Q), 0, (Q).next-1); \
+    (Q).next--; \
+    _##N##_heapify_down(&(Q)); \
+}
+
+#define FOREACH_PQ(Q, DST_DATA, DST_P) \
+    for (size_t idx = 0; idx < (Q).next; idx++) { \
+        DST_DATA = (Q).heap[idx].data; \
+        DST_P = (Q).heap[idx].priority;
+
+#define FOREACH_PQ_END }
+
+// warning: be sure to clean up your data to avoid leaks!
+#define FREE_PQ(Q) { \
+    free((Q).heap); \
+    (Q).heap_size = 0; \
+    (Q).next = 0; \
+}
 
 #endif // !def(_PQ_H)