1 files changed, 38 insertions, 42 deletions

diff --git a/priority_queue.c b/priority_queue.c
index 79377c3..c8bd8b0 100644
--- a/priority_queue.c
+++ b/priority_queue.c
@@ -12,54 +12,46 @@ PositionPQ *ppq_new(Position pos, size_t priority) {
     return ppq;
 }
 
-/* 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 */
+void ppq_insert(PositionPQ **ppq, Position pos, size_t priority) {
+    PositionPQ *start = *ppq;
 
-    if (priority > ppq->priority) { /* Insert after */
-        /* printf(" reached end, inserting after\n"); */
-        PositionPQ *n = ppq_new(pos, priority);
-        n->prev = ppq;
-        ppq->next = n;
+    if ((*ppq) == NULL) {
+        (*ppq) = ppq_new(pos, priority);
         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;
+    }
+    
+    PositionPQ *n = ppq_new(pos, priority);
+    if (start->priority > priority) {
+        n->next = start;
+        start = n;
         return;
     }
+
+    PositionPQ *temp = *ppq;
+
+    while(temp->next != NULL && temp->next->priority <= priority)
+        temp = temp->next;
+
+    n->next = temp->next;
+    temp->next = n;
+
+    return;
 }
 
-Position ppq_pop(PositionPQ *ppq) {
-    Position pos = ppq->pos;
-    ppq->next->prev = NULL;
-    PositionPQ *next = ppq->next;
-    free(ppq);
-    ppq = next;
+Position ppq_pop(PositionPQ **ppq) {
+    if (*ppq == NULL) {
+        error("Tried to pop a NULL PositionPQ\n");
+    }
+    Position pos = (*ppq)->pos;
+    if ((*ppq)->next != NULL) { /* If there's a next node */
+        (*ppq)->next->prev = NULL;
+        PositionPQ *next = (*ppq)->next;
+        free((*ppq));
+        (*ppq) = next;
+    } else { /* No next node */
+        free((*ppq));
+        (*ppq) = NULL;
+    }
     return pos;
 }
 
@@ -69,6 +61,10 @@ void ppq_reprioritize(PositionPQ *ppq, Position pos, size_t priority) {
 }
 
 void ppq_print(PositionPQ *ppq) {
+    if (ppq == NULL) {
+        printf("ppq is NULL\n");
+        return;
+    }
     int i = 0;
     while (ppq->next != NULL) {
         printf("%i - %li: %li, %li\n", i++, ppq->priority, ppq->pos.x, ppq->pos.y);