Implement the priority queue (without ppq_reorganize)

3 files changed, 68 insertions, 17 deletions

diff --git a/main.c b/main.c
index 4becc96..3d9e29c 100644
--- a/main.c
+++ b/main.c
@@ -7,13 +7,14 @@
 #include "structs.h"
 #include "config.h"
 #include "error.h"
+#include "priority_queue.h"
 
 /* So, TODO for now:
-   - Add a function/macro for errors, akin to sigint_handler()
    - Implement Dijkstra and greedy-best-first search algorithms
    - Implement the A* algorithm
    - Implement adding maps from files (with rle, preferably)
-   - Implement controls (to change maps, move start/goal, etc.) */
+   - Implement controls (to change maps, move start/goal, etc.)
+   - Clean up unused `#include`s */
 
 void sigint_handler(int sig) {
     (void)sig; /* We know it's a SIGINT */
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);
+}
diff --git a/priority_queue.h b/priority_queue.h
index 0dbd161..e24cd26 100644
--- a/priority_queue.h
+++ b/priority_queue.h
@@ -10,22 +10,24 @@ struct PositionPQNode_s {
     size_t priority; /* Lower is "better" */
     struct PositionPQNode_s *prev;
     struct PositionPQNode_s *next;
-} PositionPQNode_s;
+};
 
 typedef struct PositionPQNode_s PositionPQ;
 
 /* Create a new PositionPQ with pos and priority */
-PositionPQ ppq_new(Position pos, size_t priority);
+PositionPQ *ppq_new(Position pos, size_t priority);
 
 /* Insert a pos with priority into a given PositionPQ */
-void ppq_insert(PositionPQ ppq, Position pos, size_t priority);
+void ppq_insert(PositionPQ *ppq, Position pos, size_t priority);
 
 /* Remove and return the position with the lowest priority */
-Position ppq_pop(PositionPQ ppq);
+Position ppq_pop(PositionPQ *ppq);
 
 /* Change the priority of a given pos, moving it to a different place in the
  * linked list ("POTENTIALLY NOT NEEDED" since we don't use different weights */
-void ppq_reprioritize(PositionPQ ppq, Position pos, size_t priority);
+void ppq_reprioritize(PositionPQ *ppq, Position pos, size_t priority);
+
+void ppq_print(PositionPQ *ppq);
 
 
 #endif /* PRIORITY_QUEUE_H_ */