#include "heap.h"
#include <stdio.h>
#define SWAP(type, x, y) do {type tmp = x; x = y; y = tmp;} while(0)

unsigned heap_left(unsigned node) { return 2*node + 1; }
unsigned heap_right(unsigned node) { return 2*(node + 1); }
unsigned heap_parent(unsigned node) { return (node - 1) / 2; }

void heap_init(heap_t* heap, size_t max, int (*compare)(const void* a, const void* b))
{
    heap->records = malloc(max * sizeof(void*));
    heap->max     = max;
    heap->current = 0;
    heap->compare = compare;
}

void heap_free(heap_t* heap)
{
    free(heap->records);
}

void* heap_min(heap_t* heap) 
{
    if (!heap->current) {
        return NULL;
    }

    return heap->records[0];
}

void* heap_pop(heap_t* heap) {
    void* result = heap_min(heap);

    if (result) {
        heap_remove(heap, 0);
    }

    return result;
}

void heap_insert(heap_t* heap, void* record)
{
    unsigned pos = heap->current++;
    unsigned parent;

    heap->records[pos] = record;

    while (pos && heap->compare(record, heap->records[parent = heap_parent(pos)]) < 0) {
        SWAP(void*, heap->records[pos], heap->records[parent]);
        pos = parent;
    }
}

void heap_remove(heap_t* heap, unsigned pos)
{
    heap->records[pos] = heap->records[--heap->current];
    while (1) {
        unsigned left  = heap_left(pos);
        unsigned right = heap_right(pos);

        unsigned compared = left;
        
        if (left >= heap->current) {
            break;
        }

        if (right < heap->current) {
            compared = heap->compare(heap->records[left], heap->records[right]) < 0 ? left : right;
        }

        if (heap->compare(heap->records[compared], heap->records[pos]) < 0) {
            SWAP(void*, heap->records[compared], heap->records[pos]);
            pos = compared;
        } else {
            break;
        }
    }
}

void heap_print(heap_t* heap, void (*print)(void* record))
{
    printf("[ ");
    for (unsigned i = 0; i < heap->current; i++) {
        void* current = heap->records[i];
        print(current);
    }
    printf("]\n");
}