//  pqueue.h >>> //
#ifndef _PQUEUE_H_
        #define _PQUEUE_H_
        #include <stdlib.h>
        typedef struct pqueue_element {
                int priority;
                void *data;
        } pqueue_element;

        typedef struct pqueue {
                pqueue_element **table;
                size_t size;
                size_t alloc_size;
        } pqueue;
        #define PQUEUE_BLOCK_SIZE 8
        extern pqueue *pqueue_alloc();
        extern void *pqueue_pop(pqueue *);
        extern int pqueue_insert(pqueue *, int,void *);
#endif

// <<< pqueue.h //

/**
 * A sample tree with 5 elements.
 *            10
 *          /   \
 *         8     7
 *        / \
 *       5   1
 * table[0] = 10
 * table[1] = 8
 * table[2] = 7
 * table[3] = 5
 * table[4] = 1
 *
 * Insert a new element with priority 9 to the tree.
 * table[5] = 9
 * Move it up untill parent_priority > element_priority
 *           10
 *          /  \
 *         8    7
 *        / \   /   
 *       5   1 9
 * 
 *           10
 *          /  \
 *         8    9
 *        / \   /
 *       5  1  7
 *
 * Pop off element no. 1. And move the last element (last = smallest priority) to that position.
 *           1
 *          /  \
 *         8    9
 *        / \
 *       5   7
 *
 * Compare the priorities of current element, left element and right element.
 * If current element doesn't have highest priority swap the positions highest and current.
 *           9
 *          /  \
 *         8    1
 *        / \
 *       5   7
 * It's quite obvious that the tree isn't ordered, but that's not important.
 * The element stored in table[0] has the highest priority and that satisfies out needs.
*/

// pqueue.c >>> //
#include "pqueue.h"
#define get_left_node(id) (2*id)
#define get_right_node(id) (2*id+1)
#define get_upper_node(id) (int)(id>>1)

/**
 * pqueue *pqueue_alloc( void )
 * Allocates a new priority queue object.
 * If operation is successful a pointer to allocated object is retuned.
 * In case of a failure NULL is returned.
*/
pqueue *
pqueue_alloc( void )
{
        pqueue *new;
        if((new = malloc(sizeof(struct pqueue))) == NULL)
                return NULL;
        new->size = 0;
        new->alloc_size = 0;
        new->table = NULL;
        return new;
}

/**
 * void pqueue_free(pqueue *)
 * Frees a priority queue object allocated by pqueue_alloc().
 * If there are still elements in queue, they will be removed.
*/
void
pqueue_free(pqueue *PQ)
{
        int i;
        if(PQ == NULL)
                return;
        for(i = 0 ; i < PQ->size; i++) {
                if(PQ->table[i] != NULL)
                        free(PQ->table[i]);
        }
        free(PQ);
        return;
}
/**
 * static void pqueue_heapify(pqueue *, int)
 * Internal function for reordering the tree.
 * It compares the periorities of current, left and right node and determinates the one with
 * highest priority. If that element is not the current element (it's left or right one) the positions of the
 * current element and that one will be swapped. Function will make recursive calls untill current element has
 * the highest priority.
*/
static void
pqueue_heapify(pqueue *PQ,int i)
{
        pqueue_element *tmp;
        int l,r;
        int largest;

        l = get_left_node(i);
        r = get_right_node(i);

        if(l <= PQ->size && PQ->table[l-1]->priority > PQ->table[i-1]->priority)
                largest = l;
        else
                largest = i;
        if(r <= PQ->size && PQ->table[r-1]->priority > PQ->table[largest-1]->priority)
                largest = r;
        if(largest != i) {
                tmp = PQ->table[largest-1];
                PQ->table[largest-1] = PQ->table[i-1];
                PQ->table[i-1] = tmp;
                pqueue_heapify(PQ,largest);
        }
        return;
}
/**
 * void *pqueue_pop(pqueue *)
 * Returns a pointer to the data associated with topmost priority.
 * That element is removed from the queue and heap is reorganised.
*/
void *
pqueue_pop(pqueue *PQ)
{
        pqueue_element *max;   
        void *ret;
        if(PQ == NULL)
                return NULL;
        if(PQ->size == 0)
                return NULL; //nothing left
        max = PQ->table[0];
        PQ->table[0] = PQ->table[PQ->size-1];
        PQ->size--;
        pqueue_heapify(PQ,1);
        //check allocation size
        if((PQ->alloc_size - PQ->size) > PQUEUE_BLOCK_SIZE) {
                PQ->table = realloc(PQ->table,4*(PQ->alloc_size - PQUEUE_BLOCK_SIZE));
                PQ->alloc_size -= PQUEUE_BLOCK_SIZE;
        } else
                PQ->table[PQ->size] = NULL;
        ret = max->data;
        free(max);
        return ret;
}

/**
 * int pqueue_insert(pqueue *, int, void *)
 * Insert a new object to queue.
 * If operation is successful 0 is returned, if not return value is -1.
 * Element is inserted to the last position of the queue and then moved up the tree untill parent element
 * has bigger priority then the new one or the new element is at the top of the tree.
*/
int
pqueue_insert(pqueue *PQ, int priority,void *data)
{
        pqueue_element *tmp;
        int i;
        if(PQ == NULL)
                return -1;
        if((PQ->alloc_size - PQ->size) < 1) {
                if((PQ->table = realloc(PQ->table,4*(PQ->alloc_size + PQUEUE_BLOCK_SIZE))) == NULL) {
                        pqueue_free(PQ);
                        return -1;
                }
                PQ->alloc_size += PQUEUE_BLOCK_SIZE;
        }
        if((tmp = malloc(sizeof(pqueue_element))) == NULL)
                return -1;
        tmp->data = data;
        tmp->priority = priority;
        //start from the bottom
        PQ->table[PQ->size] = tmp;
        i = ++PQ->size;
        while(i > 1 && PQ->table[get_upper_node(i)-1]->priority < PQ->table[i-1]->priority) {
                //swap
                tmp = PQ->table[i-1];
                PQ->table[i-1] = PQ->table[get_upper_node(i)-1];
                PQ->table[get_upper_node(i)-1] = tmp;
                //move one step up (we're still working with the same element)
                i = get_upper_node(i);
        }
        return 0;
}
// <<< pqueue.c //

/** SAMPLE */
int main()
{
    pqueue *pq;
    char *data;
    pq = pqueue_alloc();
    pqueue_insert(pq,10,"foofaa 10");
    pqueue_insert(pq,9,"foofaa 9");
    pqueue_insert(pq,100,"foofaa 100");
    pqueue_insert(pq,14,"foofaa 14");
    pqueue_insert(pq,20,"foofaa 20");
    pqueue_insert(pq,50,"foofaa 50");
    pqueue_insert(pq,200,"foofaa 200");
    pqueue_insert(pq,15,"foofaa 15");
    pqueue_insert(pq,3,"foofaa 3");
    pqueue_insert(pq,-55,"foofaa -55");
    while((data = pqueue_pop(pq)) != NULL) {
        printf("%s\n",data);
    }
    pqueue_free(pq);
    return 0;
}