#include <stdio.h>
#include <stdlib.h>
#include <assert.h>

typedef int bhnode;

typedef struct binary_heap_s {
    bhnode *body;
    int     bodysize;
    int     numnodes;
} binary_heap;

binary_heap *binary_heap_new(int maxsize) {
    binary_heap *bh = malloc(sizeof (binary_heap));
    bh->body = malloc(sizeof(bhnode) * (maxsize+1));
    bh->bodysize = maxsize;
    bh->numnodes = 0;
    return bh;
}

void binary_heap_destroy(binary_heap *bh) {
    free(bh->body);
    free(bh);
}

void binary_heap_swap(binary_heap *bh, int a, int b) {
    bhnode tmp = bh->body[a];
    bh->body[a] = bh->body[b];
    bh->body[b] = tmp;
}

bhnode binary_heap_fetch(binary_heap *bh) {
    assert(bh->numnodes > 0);
    return bh->body[1];
}

int binary_heap_insert(binary_heap *bh, bhnode node) {
    if (bh->numnodes > bh->bodysize) {
        return -1; // or expand
    }
    bh->body[++bh->numnodes] = node;
    for (size_t i = bh->numnodes;
         i > 1 && bh->body[i] > bh->body[i/2];
         i /= 2) {
        binary_heap_swap(bh, i, i/2);
    }
    return 0;
}

void binary_heap_erase(binary_heap *bh) {
    assert(bh->numnodes > 0);
    bh->body[1] = bh->body[--bh->numnodes];
    size_t index = 1;
    for (;;) {
         size_t left = index + index, right = left + 1;
        // Who is greater, [index], [left], [right]?
        size_t largest = index;
        if (left <= bh->numnodes && bh->body[left] > bh->body[index])
            largest = left;
        if (right <= bh->numnodes && bh->body[right] > bh->body[largest])
            largest = right;
        if (largest == index) break;
        binary_heap_swap(bh, index, largest);
        index = largest;
    }
}

int main() {
    binary_heap *bh = binary_heap_new(10);
    binary_heap_insert(bh, 3);
    binary_heap_insert(bh, 1);
    binary_heap_insert(bh, 4);
    binary_heap_insert(bh, 1);
    binary_heap_insert(bh, 5);
    assert(binary_heap_fetch(bh) == 5);
    binary_heap_erase(bh);
    assert(binary_heap_fetch(bh) == 4);
    binary_heap_erase(bh);
    assert(binary_heap_fetch(bh) == 3);
    binary_heap_erase(bh);
    assert(binary_heap_fetch(bh) == 1);
    binary_heap_erase(bh);
    assert(binary_heap_fetch(bh) == 1);
    binary_heap_erase(bh);

}