#include <stdlib.h>
#include "pq.h"

typedef struct {
    unsigned int priority;
    size_t x, y;
    mazeedge_dir_t dir;
} _point_t;

struct pq {
    size_t heap_size, next;
    _point_t* heap; 
};

struct pq* pq_new(size_t heap_size) {
    struct pq* result = malloc(sizeof(struct pq));
    result->heap_size = heap_size;
    result->next = 0;
    result->heap = calloc(heap_size, sizeof(_point_t));
    return result;
}

static void _swap(struct pq* q, size_t idx1, size_t idx2) {
    unsigned int priority = q->heap[idx1].priority;
    size_t x = q->heap[idx1].x;
    size_t y = q->heap[idx1].y;
    mazeedge_dir_t dir = q->heap[idx1].dir;

    q->heap[idx1].priority = q->heap[idx2].priority;
    q->heap[idx1].x = q->heap[idx2].x;
    q->heap[idx1].y = q->heap[idx2].y;
    q->heap[idx1].dir = q->heap[idx2].dir;

    q->heap[idx2].priority = priority;
    q->heap[idx2].x = x;
    q->heap[idx2].y = y;
    q->heap[idx2].dir = dir;
}

static void _heapify_up(struct pq* q) {
    size_t i = q->next - 1;
    while (i > 0) {
        size_t parent = i / 2;
        if (q->heap[parent].priority > q->heap[i].priority) {
            _swap(q, parent, i);
            i = parent;
        } else {
            // done
            return;
        }
    }
}

void pq_add(struct pq* q, unsigned int priority, size_t x, size_t y, mazeedge_dir_t dir) {
    q->heap[q->next].x = x;
    q->heap[q->next].y = y;
    q->heap[q->next].priority = priority;
    q->heap[q->next].dir = dir;
    q->next++;
    _heapify_up(q);
}

bool pq_empty(struct pq const* q) {
    return q->next == 0;
}

static void _heapify_down(struct pq* q) {
    size_t i = 0;
    while (i < q->next) {
        size_t ch1 = i * 2, ch2 = i * 2 + 1;
        if (ch1 >= q->next) return;
        if (ch2 >= q->next) {
            if (q->heap[ch1].priority < q->heap[i].priority) {
                _swap(q, i, ch1);
            }
            return;
        }

        // three-way compare
        if ((q->heap[i].priority < q->heap[ch1].priority) &&
            (q->heap[i].priority < q->heap[ch2].priority)) {
            return; // all done - partial order ensured
        }
        if (q->heap[ch1].priority < q->heap[ch2].priority) {
            _swap(q, i, ch1);
            i = ch1;
        } else {
            _swap(q, i, ch2);
            i = ch2;
        }
    }
}

int pq_pop(struct pq* q, size_t* x, size_t* y, mazeedge_dir_t* dir) {
    if (pq_empty(q)) return 0;
    *x = q->heap[0].x;
    *y = q->heap[0].y;
    *dir = q->heap[0].dir;
    _swap(q, 0, q->next-1);
    q->next--;
    _heapify_down(q);
    return 1;
}

void pq_free(struct pq* q) {
    free(q->heap); q->heap = NULL;
    q->heap_size = 0;
    q->next = 0;
    free(q);
}