#ifndef _PQ_H
#define _PQ_H 1

#include <stdlib.h>
#include <string.h>

#define DEFAULT_HEAP_SIZE 16

#define DEFINE_PQ(T, N, P) \
struct _##N##HeapNode { \
    P priority; \
    T data; \
}; \
typedef struct { \
    size_t heap_size, next; \
    struct _##N##HeapNode* heap; \
} N; \
static void _##N##_swap(N* pq, size_t i, size_t j) { \
    struct _##N##HeapNode tmp = pq->heap[i]; \
    pq->heap[i] = pq->heap[j]; \
    pq->heap[j] = tmp; \
} \
static void _##N##_heapify_up(N* pq) { \
    size_t i = pq->next - 1; \
    while (i > 0) { \
        size_t parent = i / 2; \
        if (pq->heap[parent].priority > pq->heap[i].priority) { \
            _##N##_swap(pq, parent, i); \
            i = parent; \
        } else return; \
    } \
} \
static void _##N##_heapify_down(N* pq) { \
    size_t i = 0; \
    while (i < pq->next) { \
        size_t ch1 = i * 2, ch2 = i * 2 + 1; \
        if (ch1 >= pq->next) return; \
        if (ch2 >= pq->next) { \
            if (pq->heap[ch1].priority < pq->heap[i].priority) { \
                _##N##_swap(pq, i, ch1); \
            } \
            return; \
        } \
        if ((pq->heap[i].priority < pq->heap[ch1].priority) && \
            (pq->heap[i].priority < pq->heap[ch2].priority)) { \
            return; \
        } \
        if (pq->heap[ch1].priority < pq->heap[ch2].priority) { \
            _##N##_swap(pq, i, ch1); \
            i = ch1; \
        } else { \
            _##N##_swap(pq, i, ch2); \
            i = ch2; \
        } \
    } \
}

#define NEW_PQ(T, N) \
    T N = (T) { .heap_size = DEFAULT_HEAP_SIZE, .next = 0, .heap = calloc(DEFAULT_HEAP_SIZE, sizeof(struct _##T##HeapNode)) }

// WARNING: S cannot be an expression that has side effects!
#define NEW_PQ_SZ(T, N, S) \
    T N = (T) { .heap_size = (S), .next = 0, .heap = calloc((S), sizeof(struct _##T##HeapNode)) }

#define PUSH_PQ(Q, N, I, P) { \
    if ((Q).next >= (Q).heap_size) { \
        (Q).heap = realloc((Q).heap, (Q).heap_size * 2 * sizeof(struct _##N##HeapNode)); \
        (Q).heap_size *= 2; \
    } \
    (Q).heap[(Q).next].data = (I); \
    (Q).heap[(Q).next].priority = (P); \
    (Q).next++; \
    _##N##_heapify_up(&(Q)); \
}

#define EMPTY_PQ(Q) ((Q).next == 0)

// check for emptiness first!
#define POP_PQ(Q, N, DST_I, DST_P) { \
    DST_I = (Q).heap[0].data; \
    DST_P = (Q).heap[0].priority; \
    _##N##_swap(&(Q), 0, (Q).next-1); \
    (Q).next--; \
    _##N##_heapify_down(&(Q)); \
}

#define FOREACH_PQ(Q, DST_DATA, DST_P) \
    for (size_t idx = 0; idx < (Q).next; idx++) { \
        DST_DATA = (Q).heap[idx].data; \
        DST_P = (Q).heap[idx].priority;

#define FOREACH_PQ_END }

// warning: be sure to clean up your data to avoid leaks!
#define FREE_PQ(Q) { \
    free((Q).heap); \
    (Q).heap_size = 0; \
    (Q).next = 0; \
}

#endif // !def(_PQ_H)