/* Priority queue as heap code V1.21 23/3/05
   See pqueue.h for docs
   Copyright 2008 Jeffrey Lee
   This file is part of WOUM.
   WOUM is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   WOUM is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with WOUM.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _PQUEUE_C
#define _PQUEUE_C

#include <stdlib.h>

#include "pqueue.h"

pqueue *pqueue_new(int size,int (*cmp)(void *,void *))
{
	pqueue *p;
	p = malloc(sizeof(pqueue)+4*size);
	p->size = 0;
	p->maxsize = size;
	p->cmp = cmp;
	return p;
}

void pqueue_free(pqueue *p)
{
	free(p);
}

void *pqueue_head(pqueue *p)
{
	if (p->size == 0)
		return 0;
	return p->queue[0];
}

static int getnewhole(pqueue *p,int hole,void *item)
{
	int left,right;
	left = hole*2+1;
	right = hole*2+2;
	if (left >= p->size)
		return hole;
	if (left == p->size-1)
	{
		/* Left child only */
		if ((p->cmp)(item,p->queue[left]) < 0)
			return left;
		return hole;
	}
	/* Else two children */
	if ((p->cmp)(p->queue[left],p->queue[right]) < 0)
		if ((p->cmp)(p->queue[right],item) <= 0)
			return hole;
		else
			return right;
	else
		if ((p->cmp)(p->queue[left],item) <= 0)
			return hole;
		else
			return left;
}

void pqueue_pop(pqueue *p)
{
	int hole,newhole;
	void *item;
	if (p->size == 0)
		return;
	/* Remove head */
	p->queue[0] = 0;
	/* Now move last item to the gap at the top of the tree, then swap it down until a resting place is found */
	/* Shuffle last item down from top position */
	item = p->queue[--(p->size)];
	hole = 0;
	newhole = getnewhole(p,hole,item);
	while (newhole != hole)
	{
		p->queue[hole] = p->queue[newhole]; /* Swap current contents up */
		hole = newhole;
		newhole = getnewhole(p,hole,item);
	}
	p->queue[hole] = item;
}

int pqueue_add(pqueue *p,void *i)
{
	int hole;
	if (p->size == p->maxsize)
		return 1;
	/* Places the new item at the end of the tree, then swaps it up until a resting place is found */
	hole = p->size++; /* Location of free space */
	while ((hole > 0) && ((p->cmp)(i,p->queue[(hole-1)/2]) > 0))
	{
		p->queue[hole] = p->queue[(hole-1)/2]; /* Shuffle item down tree */
		hole = (hole-1)/2;
	}
	p->queue[hole] = i;
	return 0;
}

int pqueue_remove(pqueue *p,void *i)
{
	int hole;
	/* Do linear search of tree for item 'i'
	   Then swap it up to the top of the tree, as if cmp(i,*) > 0
	   Then pop the top item */
	hole=0;
	while ((hole<p->size) && (p->queue[hole] != i))
		hole++;
	if (hole == p->size)
		return 1;
	while (hole > 0)
	{
		p->queue[hole] = p->queue[(hole-1)/2];
		hole = (hole-1)/2;
	}
	pqueue_pop(p);
	return 0;
}

int pqueue_numitems(pqueue *p)
{
	return p->size;
}

#endif
