#include "Python.h"
#include <stdio.h>
 
#define PQueueObject_Check(v)    ((v)->ob_type == &PyQueue_Type)
#define PQPRIO(p) (p->priority)
 
staticforward PyTypeObject PyPQueue_Type;

/*
 *  Priority queue structure
 */
typedef struct {
  int priority;
  PyObject *data;
} node;

typedef struct {
	int size, avail, step;
	node **d;
} pqueue;
 
typedef struct {
        PyObject_HEAD
	pqueue *queue;
} PQueueObject;

/*
 *  pqinit: initialize the queue.
 *
 *  Parameters:
 *
 *    q           Pointer to a priority queue, or NULL if the user
 *                wishes to leave it to pqinit to allocate the queue.
 *
 *    n           Numer of queue items for which memory should be
 *                preallocated, that is, the initial size of the
 *                item array the queue uses. If you insert more than
 *                n items to the queue, another n items will
 *                be allocated automatically.
 *
 *  Return values:
 *
 *   non-NULL     Priority queue has been initialized.
 *
 *   NULL         Insufficient memory.
 */
pqueue *
pqinit(pqueue *q, int n)
{
	if (!q) {
	  return NULL;
	}
	if (!(q->d = (node **)malloc(sizeof(node *) * n))) {
		return NULL;
	}
	q->avail = q->step = n;
	q->size = 1;
	return q;
}

void pqdump(pqueue *q) {
  printf("size %d, avail %d, step %d\n", q->size, q->avail, q->step);
}

/*                  
 *  pqinsert: insert an item into the queue.
 *
 *  Parameters:
 *
 *    q           Pointer to a priority queue.
 *
 *    d           Datum to be inserted.
 *
 *  Return values:
 *
 *    1           The item has been inserted.
 *
 *    0           The item could not be appended. Either the queue i
 *                pointer provided was NULL, or the function was unable 
 *                to allocate the amount of memory needed for 
 *                the new item.
 */
int
pqinsert(pqueue *q, node * d)
{
	node * *tmp;
	int i, newsize;

	if (!q) return 0;
	
	/* allocate more memory if necessary */
	if (q->size >= q->avail) {
		newsize = q->size + q->step;
		if (!(tmp = realloc(q->d, sizeof(node *) * newsize))) {
			return 0;
		};
		q->d = tmp;
		q->avail = newsize;		
	}

	/* insert item */
	i = q->size++; // new size
	while (i > 1 && PQPRIO(q->d[i / 2]) < PQPRIO(d)) {
	        // while middle < priority
		q->d[i] = q->d[i / 2];
		// set new end = middle
		i /= 2;
	}
	q->d[i] = d;
	return 1;	
} 

/*
 *  pqremove: remove the highest-ranking item from the queue.
 *
 *  Parameters:
 *
 *    p           Pointer to a priority queue.
 *
 *    d           Pointer to the node * variable that will hold the 
 *                datum corresponding to the queue item removed.               
 *
 *  Return values:
 *
 *    non-NULL    An item has been removed. The variable that d points
 *                to now contains the datum associated with the item
 *                in question.
 *
 *    NULL        No item could be removed. Either the queue pointer
 *                provided was NULL, or the queue was empty. The chunk
 *                of memory that d points to has not been modified.
 */
node *
pqremove(pqueue *q, node *d)
{	
        node *tmp;
	int i = 1, j;

	if (!q || q->size == 1) return NULL;
	d = q->d[1]; // first node
	tmp = q->d[--q->size];
	while (i <= q->size / 2) {
		j = 2 * i;
		if (j < q->size && 
			PQPRIO(q->d[j]) < PQPRIO(q->d[j + 1])) {
			j++;
		}
		if (PQPRIO(q->d[j]) <= PQPRIO(tmp)) {
			break;
		}
		q->d[i] = q->d[j];
		i = j;
	}
	q->d[i] = tmp;
	return d;	
} 

/*
 *  pqpeek: access highest-ranking item without removing it.
 *
 *  Parameters:
 *
 *    q           Pointer to a priority queue.
 *
 *    d           Pointer to the PQDATUM variable that will hold the
 *                datum corresponding to the highest-ranking item.
 *                
 *  Return values:
 *
 *    non-NULL   Success. The variable that d points to now contains
 *               the datum associated with the highest-ranking item.
 *
 *    NULL       Failure. Either the queue pointer provided was NULL,
 *               or the queue was empty. The chunk of memory that d
 *               points to has not been modified.
 */
node *
pqpeek(pqueue *q, node *d)
{
	if (!q || q->size == 1) return NULL;
	d = q->d[1];
	return d;
}

static void
PQueue_free_node(node *np) {
  PyObject * pob;

  pob = np->data;
  Py_DECREF(pob);
  free(np);
}

static void
PQueue_dealloc(PQueueObject *self) {
  node *p;
  while ((p = pqremove(self->queue, p)) != NULL) {
    PQueue_free_node(p);
  }
  free(self->queue);
  self->queue = NULL;
  PyObject_Del(self);
}

static PyObject *
PQueue_peek(PQueueObject *self, PyObject *args) {
  node *np;
  PyObject *pob;

  if ((np = pqpeek(self->queue, np)) == NULL)
    return NULL;
  
  pob = np->data;
  Py_INCREF(pob);
  return pob;
}

static PyObject *
PQueue_pop(PQueueObject *self, PyObject *args) {
  node *np;
  PyObject *pob;
  PyObject *ret_tup;

  if ((np = pqremove(self->queue, np)) == NULL) {
    return NULL;
  }

  ret_tup = np->data;
  Py_INCREF(ret_tup);
  PQueue_free_node(np); // calls DECREF on pob
  return ret_tup;
}

static PyObject *
PQueue_push(PQueueObject *self, PyObject *args) {
  int priority;
  node *np;
  PyObject *tup;

  if (!PyArg_ParseTuple(args, "O!:PQueue", &PyTuple_Type, &tup))
    return NULL;

  if (PyTuple_GET_SIZE(tup) != 2) {
    PyErr_SetString(PyExc_ValueError, "argument must be a tuple of size two (priority, data)");
    return NULL;
  }

  np = (node *) malloc(sizeof(node));
  Py_INCREF(tup);
  np->data = tup;
  priority = (int)PyInt_AsLong(PyTuple_GET_ITEM(tup, 0));
  np->priority = - priority;

  if (!pqinsert(self->queue, np)) {
    return NULL;
  }

  Py_INCREF(self);
  return (PyObject *)self; // just reutrn ourselves as an lvalue
}

static PyMethodDef PQueue_methods[] = {
  {"peek", (PyCFunction)PQueue_peek, METH_NOARGS},
  {"push", (PyCFunction)PQueue_push, METH_VARARGS},
  {"pop", (PyCFunction)PQueue_pop, METH_NOARGS},
  {NULL,                NULL}           /* sentinel */
};

static PyObject *
PQueue_getattr(PQueueObject *self, char *name)
{
        return Py_FindMethod(PQueue_methods, (PyObject *)self, name);
}

static int
PQueue_length(PQueueObject *self) {
  return (self->queue->size - 1);
}

static PySequenceMethods PQueue_as_sequence = {
        (inquiry)PQueue_length, /*sq_length*/
};

statichere PyTypeObject PyPQueue_Type = {
        /* The ob_type field must be initialized in the module init function
         * to be portable to Windows without using C++. */
        PyObject_HEAD_INIT(&PyType_Type)
        0,                      /*ob_size*/
        "PQueue",               /*tp_name*/
        sizeof(PQueueObject),   /*tp_basicsize*/
        0,                      /*tp_itemsize*/
        /* methods */
        (destructor)PQueue_dealloc, /*tp_dealloc*/
        0,                      /*tp_print*/
        (getattrfunc)PQueue_getattr, /*tp_getattr*/
        0, //(setattrfunc)Permute_setattr, /*tp_setattr*/
        0,                      /*tp_compare*/
        0,                      /*tp_repr*/
        0,                      /*tp_as_number*/
        &PQueue_as_sequence,                      /*tp_as_sequence*/
        0,                      /*tp_as_mapping*/
        0,                      /*tp_hash*/
        0,                      /*tp_call*/
        0,                      /*tp_str*/
        0,                      /*tp_getattro*/
        0,                      /*tp_setattro*/
        0,                      /*tp_as_buffer*/
        Py_TPFLAGS_DEFAULT,     /*tp_flags*/
        0,                      /*tp_doc*/
        0,                      /*tp_traverse*/
        0,                      /*tp_clear*/
        0,                      /*tp_richcompare*/
        0,                      /*tp_weaklistoffset*/
        0,                      /*tp_iter*/
        0,                      /*tp_iternext*/
        PQueue_methods,         /*tp_methods*/
        0,                      /*tp_members*/
        0,                      /*tp_getset*/
        0,                      /*tp_base*/
        0,                      /*tp_dict*/
        0,                      /*tp_descr_get*/
        0,                      /*tp_descr_set*/
        0,                      /*tp_dictoffset*/
        0,                      /*tp_init*/
        0,                      /*tp_alloc*/
        0,                      /*tp_new*/
        0,                      /*tp_free*/
        0,                      /*tp_is_gc*/
};

// not static so ifcp_module.c can see it
PyObject *
pqueue_new(PyObject *self, PyObject *args)
{
        PQueueObject *rv;
        int initial_size = 0;
 
        if (!PyArg_ParseTuple(args, "|i:PQueue", &initial_size))
                return NULL;
 
	if (!initial_size)
	  initial_size = 100;

        // call object create function and return
        rv = PyObject_New(PQueueObject, &PyPQueue_Type);
        if (rv == NULL)
          return NULL;
 
	rv->queue = (pqueue *) malloc(sizeof(pqueue));
	pqinit(rv->queue, initial_size);
 
        return (PyObject *)rv;
}