/* 
 * @File: mq_prio.c
 *
 * @Contents: Priority queue for POSIX message queue implementation
 *
 * Copyright (C) 2002 Sergio Saez <ssaez@disca.upv.es>
 * Project OCERA (Open Components for Realtime Embedded Applications)
 *
 * Copyright (C) 2000 Ismael Ripoll (Valencia, Spain) Original version.
 *
 *
 * 'mq_prio.c' has the functions that implement a prioritized queue.
 *
 */

#define MQ_PRIO_C

#define MODULE

/* Include code to perform sanity checks... but it is not for free */
/* Comment this define to improve the performance */
#define DEBUG_SANITY_CHECKS

/*
 * Required include files
 */

#include <rtl_stdlib.h>

#include <mq_prio.h>

/*** Local functions ***/

/*
 * heapify - push a data item down the heap until in the proper place
 *
 * @heap: the heap structure to work with
 * @top: heap element index to start pushing
 *
 * This is an "internal" function used by heap_remove.
 * 
 */
static inline void heapify(mq_heap_t * heap, int top) {

    mq_heap_item_t tempvalue= MQ_HEAP_ITEM(heap, top); /* Data to pushdown */
    int child;                /* left child of top */

    for (child= top<<1; child <= MQ_HEAP_LENGTH(heap); top= child, child <<= 1 ) {
        if (child < MQ_HEAP_LENGTH(heap) && 
            higherpriority(MQ_HEAP_ITEM_KEY(MQ_HEAP_ITEM(heap, child+1)),
                           MQ_HEAP_ITEM_KEY(MQ_HEAP_ITEM(heap, child))))
            child++;
        if (higherpriority(MQ_HEAP_ITEM_KEY(tempvalue),
                           MQ_HEAP_ITEM_KEY(MQ_HEAP_ITEM(heap, child))))
            break;
        MQ_HEAP_ITEM(heap, top)= MQ_HEAP_ITEM(heap, child);
    }
    MQ_HEAP_ITEM(heap, top)= tempvalue;

} /* end heapify */

/*** Extern functions ***/

/* 
 * mq_heap_init - initializes the heap structure
 *
 * @heap: the heap structure to work with
 *
 * Initializes the heap structure. Memory is statically allocated.
 */
int mq_heap_init(mq_heap_t * heap, unsigned size) {

    if (size != MQ_HEAP_MAX_LENGTH(heap)) /* Memory is statically allocated. This only
                                             checks that is requested the same amount
                                             that was allocated. */
	return -EINVAL;

    MQ_HEAP_LENGTH(heap)= 0;

    return 0;

} /* end mq_heap_init */

/* 
 * mq_heap_destroy - destroys the heap structure
 *
 * @heap: the heap structure to work with
 *
 * Destroys the heap structure. Memory is statically allocated.
 */
int mq_heap_destroy(mq_heap_t * heap) {

    MQ_HEAP_LENGTH(heap)= 0;

    return 0;

} /* end mq_heap_destroy */

/*
 * mq_heap_top - returns the data associated to the top of the heap
 *
 * @heap: heap to get the top
 *
 * This function does not modify the heap structure.
 */
mq_heap_item_t mq_heap_top(mq_heap_t * heap) {

#ifdef DEBUG_SANITY_CHECKS
    if (MQ_HEAP_LENGTH(heap) == 0) {
#ifdef DEBUG_HEAP
        printf("Heap: call to 'mq_heap_top' function with an empty heap\n");
#endif
        return NULL_ITEM;
    } /* endif */
#endif 

    return MQ_HEAP_TOP(heap);

} /* end mq_heap_top */

/* 
 * mq_heap_pop - removes the top of the heap.
 *
 * @heap: the heap structure to work with
 *
 * Remove the smallest element from the heap and recreate the heap with
 * one element less.
 */
int mq_heap_pop(mq_heap_t * heap) {

#ifdef DEBUG_SANITY_CHECKS
    if (MQ_HEAP_LENGTH(heap) == 0) {
#ifdef DEBUG_HEAP
        printf("Heap: call to 'mq_heap_pop' function with an empty heap\n");
#endif
        return -1;
    } /* endif */
#endif 

    MQ_HEAP_ITEM(heap, FIRST)= MQ_HEAP_ITEM(heap, MQ_HEAP_LENGTH(heap));
    MQ_HEAP_LENGTH(heap)--;

    heapify(heap, FIRST);

    return 0;

} /* end mq_heap_pop */

/* 
 * mq_heap_push - insert a new item into the heap
 *
 * @heap: the heap structure to work with
 * @item: data to be inserted into the heap
 *
 * Inserts an item into the heap
 */
int mq_heap_push(mq_heap_t * heap, mq_heap_item_t item) {

    int position;

    if (MQ_HEAP_LENGTH(heap) >= MQ_HEAP_MAX_LENGTH(heap)) {
        /* Heap maximum capacity reached */
#ifdef DEBUG_HEAP
        printf("Heap: Warning the heap is full\n");
#endif
        return -1;
    } /* endif */

    position= ++MQ_HEAP_LENGTH(heap);

    while (position > FIRST && 
           higherpriority(MQ_HEAP_ITEM_KEY(item), 
                          MQ_HEAP_ITEM_KEY(MQ_HEAP_ITEM(heap, position>>1)))) {
        /* If the new child has a priority higher than the father's one then move down
         * the father */
        MQ_HEAP_ITEM(heap, position)= MQ_HEAP_ITEM(heap, position>>1);
                                /* It is assume assign is defined for item type */
        position= position >> 1;
    } /* endwhile */

    MQ_HEAP_ITEM(heap, position)= item;

    return 0;

} /* mq_heap_push */