1 files changed, 58 insertions, 10 deletions

diff --git a/priority_queue.c b/priority_queue.c
index 4db7597..1cf0ac0 100644
--- a/priority_queue.c
+++ b/priority_queue.c
@@ -3,24 +3,72 @@
 #include "priority_queue.h"
 #include "error.h"
 
-/*TODO: implement */
-
-PositionPQ ppq_new(Position pos, size_t priority) {
-    (void)pos, (void)priority;
-    todo();
+PositionPQ *ppq_new(Position pos, size_t priority) {
+    PositionPQ *ppq = malloc(sizeof(PositionPQ));
+    ppq->pos = pos;
+    ppq->priority = priority;
+    ppq->prev = NULL;
+    ppq->next = NULL;
+    return ppq;
 }
 
-void ppq_insert(PositionPQ ppq, Position pos, size_t priority) {
-    (void)ppq, (void)pos, (void)priority;
-    todo();
+/* It works, but I don't trust it.
+ * Left the `printf`s as they were when I was debugging this shit, I have a
+ * feeling I'll need them later. */
+void ppq_insert(PositionPQ *ppq, Position pos, size_t priority) {
+    /* printf("Inserting %li %li with prio %li\n", pos.x, pos.y, priority); */
+    while (ppq->next != NULL) {
+        if (ppq->priority > priority) {
+            printf(" inserting in middle\n");
+            PositionPQ *n = ppq_new(pos, priority);
+            n->prev = ppq->prev;
+            n->next = ppq;
+            if (ppq->prev != NULL) {
+                ppq->prev->next = n;
+            }
+            ppq->prev = n;
+            return;
+        } else {
+            ppq = ppq->next;
+        }
+    }
+
+    /* Reached end node */
+
+    if (priority > ppq->priority) { /* Insert after */
+        /* printf(" reached end, inserting after\n"); */
+        PositionPQ *n = ppq_new(pos, priority);
+        n->prev = ppq;
+        ppq->next = n;
+        return;
+    } else { /* Insert before */
+        /* printf(" reached end, inserting before\n"); */
+        PositionPQ *n = ppq_new(pos, priority);
+        n->prev = ppq->prev;
+        n->next = ppq;
+        if (ppq->prev != NULL) {
+            ppq->prev->next = n;
+        }
+        ppq->prev = n;
+        return;
+    }
 }
 
-Position ppq_pop(PositionPQ ppq) {
+Position ppq_pop(PositionPQ *ppq) {
     (void)ppq;
     todo();
 }
 
-void ppq_reprioritize(PositionPQ ppq, Position pos, size_t priority) {
+void ppq_reprioritize(PositionPQ *ppq, Position pos, size_t priority) {
     (void)ppq, (void)pos, (void)priority;
     todo();
 }
+
+void ppq_print(PositionPQ *ppq) {
+    int i = 0;
+    while (ppq->next != NULL) {
+        printf("%i - %li: %li, %li\n", i++, ppq->priority, ppq->pos.x, ppq->pos.y);
+        ppq = ppq->next;
+    }
+    printf("%i - %li: %li, %li\n", i++, ppq->priority, ppq->pos.x, ppq->pos.y);
+}