#include <stdio.h>
#include <curses.h>
#include "priority_queue.h"
#include "error.h"

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) {
    PositionPQ *start = *ppq;

    if ((*ppq) == NULL) {
        (*ppq) = ppq_new(pos, priority);
        return;
    }
    
    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) {
    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;
}

void ppq_reprioritize(PositionPQ *ppq, Position pos, size_t priority) {
    (void)ppq, (void)pos, (void)priority;
    todo();
}

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);
        ppq = ppq->next;
    }
    printf("%i - %li: %li, %li\n", i++, ppq->priority, ppq->pos.x, ppq->pos.y);
}