#include <stdlib.h>
#include <string.h>
#include <sys/types.h>

#include "heap.h"

#ifdef _WIN32
#include <basetsd.h>
typedef SSIZE_T ssize_t;
#endif

/*
 * As found in any basic Data Structures & Algorithms book.
 */

#define   LEFT(i)  (2*(i) + 1)
#define  RIGHT(i)  (2*(i) + 2)
#define PARENT(i)  (((i) - 1) / 2)

static void heap_build_array(void **A, size_t n, comparison_fn compare);
static void heapify_down(void **A, size_t n, size_t i, comparison_fn compare);
static void heapify_up(void **A, size_t i, comparison_fn compare);


Heap *heap_new(comparison_fn compare) {

    Heap *h = malloc(sizeof(Heap));
    heap_init(h, compare);

    return h;

}

void heap_delete(Heap *h) {

    heap_clear(h);
    free(h);

}

void heap_init(Heap *h, comparison_fn compare) {

    h->elements = NULL;
    h->capacity = 0;
    h->size = 0;
    h->compare = compare;

}

void heap_clear(Heap *h) {

    if (h->elements) {
        free(h->elements);
        heap_init(h, h->compare);
    }

}

void heap_build(Heap *h, const void **array, size_t n) {

    heap_grow(h, n);
    memcpy(h->elements, array, n * sizeof(void*));
    h->size = n;

    if (n > 1) {
        heap_build_array(h->elements, n, h->compare);
    }

}

void heap_grow(Heap *h, size_t n) {

    if (n == 0) {
        n = HEAP_DEFAULT_CAPACITY;
    }

    if (n > h->capacity) {
        h->elements = realloc(h->elements, n * sizeof(void*));
        h->capacity = n;
    }

}

void heap_insert(Heap *h, void *element) {

    size_t n = h->size;

    if (n == h->capacity) {
        heap_grow(h, h->capacity << 1);
    }

    void **A = h->elements;
    A[n] = element;
    h->size = n + 1;

    heapify_up(A, n, h->compare);

}

void *heap_pop(Heap *h) {

    size_t n = h->size;

    if (n == 0) {
        return NULL;
    }

    void **A = h->elements;
    void *max = A[0];

    size_t m = n-1;
    h->size = m;

    if (m > 0) {
        A[0] = A[m];
        heapify_down(A, m, 0, h->compare);
    }

    return max;

}

void heap_update(Heap *h, void *element) {

    size_t n = h->size;
    void **A = h->elements;
    void **end = A + n;

    for (void **p = A; p < end; p++) {

        if (*p != element) {
            continue;
        }

        size_t i = p - A;
        comparison_fn compare = h->compare;

        if (i == 0) {
            heapify_down(A, n, i, compare);
        }
        else {
            void *parent = A[PARENT(i)];
            int c = compare(element, parent);
            if (c > 0) {
                heapify_up(A, i, compare);
            }
            else if (c < 0) {
                heapify_down(A, n, i, compare);
            }
        }

        break;

    }

}

void heap_update_root(Heap *h) {

    heapify_down(h->elements, h->size, 0, h->compare);

}

void heap_sort(void **array, size_t n, comparison_fn compare) {

    if (n < 2) {
        return;
    }

    heap_build_array(array, n, compare);

    for (size_t i = n-1; i >= 1; i--) {
        // Swap 0 and i
        void *max = array[0];
        array[0] = array[i];
        array[i] = max;
        // Heapify, leaving out the max which is now at the end
        heapify_down(array, i, 0, compare);
    }

}

static void heap_build_array(void **A, size_t n, comparison_fn compare) {

    for (ssize_t i = (n/2)-1; i >= 0; i--) {
        heapify_down(A, n, i, compare);
    }

}

static void heapify_down(void **A, size_t n, size_t i, comparison_fn compare) {

    size_t l = LEFT(i);
    size_t r = l + 1;
    size_t max = i;

    if (l < n && compare(A[l], A[max]) > 0) {
        max = l;
    }

    if (r < n && compare(A[r], A[max]) > 0) {
        max = r;
    }

    if (max == i) {
        return;
    }

    // Swap
    void *value = A[i];
    A[i] = A[max];
    A[max] = value;

    // Recurse
    heapify_down(A, n, max, compare);

}

static void heapify_up(void **A, size_t i, comparison_fn compare) {

    while (i > 0) {

        size_t p = PARENT(i);

        if (compare(A[p], A[i]) >= 0) {
            break;
        }

        // Swap
        void *value = A[i];
        A[i] = A[p];
        A[p] = value;

        // Move up
        i = p;

    }

}
