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

    PositionPQ *n = ppq_new(pos, priority);

    if (start->priority > priority) {
        n->next = start;
        start = n;
        *ppq = start;
        ppq_remove(&(n->next), pos);
        return PPQ_INSERT_SUCCESS;
    }

    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) {
        if (temp->pos.x == pos.x && temp->pos.y == pos.y && temp->priority <= priority) {
            free(n);
            return PPQ_INSERT_ALREADY;
        }
        temp = temp->next;
    }
    if (temp->pos.x == pos.x && temp->pos.y == pos.y && temp->priority <= priority) {
        free(n);
        return PPQ_INSERT_ALREADY;
    }

    n->next = temp->next;
    temp->next = n;

    ppq_remove(&(n->next), pos);

    return PPQ_INSERT_SUCCESS;
}

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 */
        PositionPQ *next = (*ppq)->next;
        free((*ppq));
        (*ppq) = next;
    } else { /* No next node */
        free((*ppq));
        (*ppq) = NULL;
    }
    return pos;
}

void ppq_remove(PositionPQ **ppq, Position pos) {
    PositionPQ *temp = *ppq;

    if (temp == NULL) {
        return;
    }

    PositionPQ *prev = NULL;

    while (temp->next != NULL && (pos.x != temp->pos.x || pos.y != temp->pos.y)) {
        prev = temp;
        temp = temp->next;
    }

    if (pos.x == temp->pos.x && pos.y == temp->pos.y) {
        if (prev != NULL) {
            prev->next = temp->next;
            free(temp);
        } else {
            prev = *ppq;
            *ppq = (*ppq)->next;
            free(prev);
        }
    }
}

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);
}

void ppq_free(PositionPQ *ppq) {
    if (ppq == NULL) return;
    while(ppq->next != NULL) {
        PositionPQ *t = ppq;
        ppq = ppq->next;
        free(t);
    }
    if (ppq != NULL) free(ppq);
}