#include "heapq.h"

#define QUEUE_MAX 1024 * 4

heapq_t *
init_queue(void)
{
    heapq_t *q;
    q = (heapq_t *)PyMem_Malloc(sizeof(heapq_t));
    if(q == NULL){
        return NULL;
    }
    q->max = QUEUE_MAX;
    q->size = 0;
    q->heap = (TimerObject**)malloc(sizeof(TimerObject*) * q->max);
    if(q->heap == NULL){
        PyMem_Free(q);
        return NULL;
    }
    GDEBUG("alloc heapq_t : %p ", q);
    return q;
}

void
destroy_queue(heapq_t *q)
{
    TimerObject *obj;
    while(q->size > 0){
        obj = heappop(q);
        Py_XDECREF(obj);
    }
    free(q->heap);
    GDEBUG("dealloc heapq_t : %p ", q);
    PyMem_Free(q);
}

static void
_siftdown(heapq_t *q, uint32_t startpos, uint32_t pos)
{
    TimerObject *newitem = NULL, *parent = NULL;
    uint32_t parentpos;
    TimerObject **p = q->heap;
    newitem =  p[pos];

    while(likely(pos > startpos)){
        parentpos = (pos - 1) >> 1;
        parent = p[parentpos];
        if(newitem->seconds < parent->seconds){
            p[pos] = parent;
            pos = parentpos;
        }else{
            break;
        }
    }
    p[pos] = newitem;
}

static void
_siftup(heapq_t *q, uint32_t pos)
{
    uint32_t startpos, childpos, rightpos;
    TimerObject *newitem, *childpositem;
    uint32_t size = q->size;
    TimerObject **p = q->heap;

    startpos = pos;
    newitem = p[pos];
    childpos = (pos << 1) + 1;

    while(likely(childpos < size)){
        rightpos = childpos + 1;
        childpositem = p[childpos];
        if(rightpos < size && childpositem->seconds > p[rightpos]->seconds){
            childpos = rightpos;
            childpositem = p[childpos];
        }
        p[pos] = childpositem;
        pos = childpos;
        childpos = (pos << 1) + 1;
    }
    p[pos] = newitem;
    _siftdown(q, startpos, pos);
}

TimerObject *
heappop(heapq_t *q)
{
    uint32_t size;
    TimerObject *last = NULL, *retitem = NULL;
    TimerObject **p = q->heap;

    if(unlikely(q->size == 0)){
        return NULL;
    }

    q->size--;
    size = q->size;
    last = p[size];

    p[size] = NULL;

    if(likely(size > 0)){
        retitem = *p;
        *p = last;
        _siftup(q, 0);
    }else{
        retitem = last;
    }
    return retitem;
}

int
heappush(heapq_t *q, TimerObject *val)
{
    TimerObject **new_heap;
    uint32_t max;

    DEBUG("heappush size %d", q->size);
    if(q->size >= q->max){
        //realloc
        max = q->max * 2;
        new_heap = (TimerObject**)realloc(q->heap, sizeof(TimerObject*) * max);
        if(new_heap == NULL){
            PyErr_SetString(PyExc_Exception, "size over timer queue");
            return -1;
        }
        q->max = max;
        q->heap = new_heap;
        DEBUG("realloc max:%d", q->max);
    }
    Py_INCREF(val);
    q->heap[q->size] = val;
    q->size++;
    _siftdown(q, 0, q->size -1);
    return 1;
}