1 files changed, 10 insertions, 4 deletions

diff --git a/priority_queue.c b/priority_queue.c
index eae56dd..7a1fa19 100644
--- a/priority_queue.c
+++ b/priority_queue.c
@@ -11,19 +11,21 @@ PositionPQ *ppq_new(Position pos, size_t priority) {
     return ppq;
 }
 
-/* FIXME: URGENT: check the order PLEAES */
 int ppq_insert(PositionPQ **ppq, Position pos, size_t priority) {
     //printf("Inserting %zu %zu\n", pos.x, pos.y);
     PositionPQ *start = *ppq;
 
+    /* If ppq is empty, create one */
     if ((*ppq) == NULL) {
         (*ppq) = ppq_new(pos, priority);
         return PPQ_INSERT_NEW;
     }
 
+    /* The new node */
     PositionPQ *n = ppq_new(pos, priority);
 
-    if (start->priority > priority) {
+    /* If new priority is better than that of the start, insert in the beginning */
+    if (start->priority >= priority) {
         n->next = start;
         start = n;
         *ppq = start;
@@ -31,21 +33,25 @@ int ppq_insert(PositionPQ **ppq, Position pos, size_t priority) {
         return PPQ_INSERT_SUCCESS;
     }
 
+    /* To avoid changing ppq as it's passed as pointed */
     PositionPQ *temp = *ppq;
 
-    /* The '=' is commented out because we want to insert before, for speed. To insert after, uncomment it*/
-    while(temp->next != NULL && temp->next->priority </*=*/ priority) {
+    /* Skip nodes with smaller priority */
+    while(temp->next != NULL && temp->next->priority < priority) {
+        /* Node is already in ppq */
         if (temp->pos.x == pos.x && temp->pos.y == pos.y && temp->priority <= priority) {
             free(n);
             return PPQ_INSERT_ALREADY;
         }
         temp = temp->next;
     }
+    /* Node is already in ppq */
     if (temp->pos.x == pos.x && temp->pos.y == pos.y && temp->priority <= priority) {
         free(n);
         return PPQ_INSERT_ALREADY;
     }
 
+    /* Insert */
     n->next = temp->next;
     temp->next = n;