#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "util.h"

typedef int T;

// https://stackoverflow.com/a/22900767

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

static void
swap(T heap[], int i, int j)
{
	T temp  = heap[i];
	heap[i] = heap[j];
	heap[j] = temp;
}

static void
fixup(T heap[], int i)
{
	int p = PARENT(i);

	while ( i > 0 && heap[p] < heap[i] ) {
		swap(heap, i, p);

		i = p;
		p = PARENT(i);
	}
}

static void
fixdown(T heap[], int i, int n)
{
	for ( ;; ) {
		const int l = LEFT(i);
		const int r = RIGHT(i);
		int m = i;

		if ( l < n && heap[m] < heap[l] )
			m = l;

		if ( r < n && heap[m] < heap[r] )
			m = r;

		if ( m == i )
			break;

		swap(heap, m, i);

		i = m;
	}
}

static void
heapify(T heap[], int n)
{
	for ( int i = n / 2 - 1; i >= 0; --i )
		fixdown(heap, i, n);
}

static void
my_heapsort(T a[], int n)
{
	heapify(a, n);

	for ( int i = n - 1; i >= 0; --i ) {
		swap(a, 0, i);
		fixdown(a, 0, i);
	}
}

// -------------------------------------------

struct pq {				// Priority Queue
	T heap[251];
	int sz;
};

void
pq_init(struct pq *pq)
{
	pq->sz = 0;
}

bool
pq_push(struct pq *pq, T data)
{
	if ( pq->sz == NELEM(pq->heap) )
		return false;

	pq->heap[pq->sz] = data;
	fixup(pq->heap, pq->sz);
	++pq->sz;
	return true;
}

bool
pq_pop(struct pq *pq, T* data)
{
	if ( pq->sz == 0 )
		return false;

	*data = pq->heap[0];
	--pq->sz;
	pq->heap[0] = pq->heap[pq->sz];
	fixdown(pq->heap, 0, pq->sz);
	return true;
}

// -------------------------------------------


void print_heap(T heap[], int n)
{
	if ( n ) {
		printf("%d", heap[0]);
		for ( int i = 1; i != n; ++i )
			printf(", %d", heap[i]);
		putchar('\n');
	}
}

int main(void)
{
#if 0 	// Heap-Testprogramm
	T heap[20] = { 18, 18, 16, 9, 7, 1, 9, 3, 7, 5 };

	print_heap(heap, 10);

	//heap[10] = 13; fixup(heap, 10);
	swap(heap, 0, 9); fixdown(heap, 0, 9);

	print_heap(heap, 9);
#endif

	struct pq pq[1];

	pq_init(pq);

	for ( int i = 0; i != 10; ++i )
		pq_push(pq, rand());

	T data;
	while ( pq_pop(pq, &data) )
		printf("%d\n", data);
}