390 lines
8.2 KiB
C
390 lines
8.2 KiB
C
/*
|
|
PriQueue - simple heap based priority queue
|
|
|
|
Copyright (C) Lumiera.org
|
|
2011, Christian Thaeter <ct@pipapo.org>
|
|
|
|
This program 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 2 of
|
|
the License, or (at your option) any later version.
|
|
|
|
This program 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 this program; if not, write to the Free Software
|
|
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
|
|
* *****************************************************/
|
|
|
|
|
|
#include "lib/priqueue.h"
|
|
#include "include/logging.h"
|
|
|
|
#include <stdint.h>
|
|
#include <nobug.h>
|
|
|
|
|
|
|
|
|
|
|
|
PriQueue
|
|
priqueue_init (PriQueue self,
|
|
size_t element_size,
|
|
priqueue_cmp_fn cmpfn,
|
|
priqueue_copy_fn copyfn,
|
|
priqueue_resize_fn resizefn)
|
|
{
|
|
TRACE (priqueue, "%p", self);
|
|
|
|
REQUIRE (element_size);
|
|
REQUIRE (cmpfn);
|
|
|
|
if (self)
|
|
{
|
|
self->queue = NULL;
|
|
self->element_size = element_size;
|
|
self->used = self->high_water = self->low_water = 0;
|
|
self->cmpfn = cmpfn;
|
|
|
|
if (!copyfn)
|
|
copyfn = memcpy;
|
|
self->copyfn = copyfn;
|
|
|
|
if (!resizefn)
|
|
resizefn = priqueue_clib_resize;
|
|
self->resizefn = resizefn;
|
|
|
|
self = self->resizefn (self);
|
|
}
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
PriQueue
|
|
priqueue_destroy (PriQueue self)
|
|
{
|
|
TRACE (priqueue, "%p", self);
|
|
if (self)
|
|
{
|
|
WARN_IF (self->used, priqueue, "queue was not empty");
|
|
self->used = 0;
|
|
self = self->resizefn (self);
|
|
}
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
PriQueue
|
|
priqueue_reserve (PriQueue self, unsigned elements)
|
|
{
|
|
TRACE (priqueue, "%p %d", self, elements);
|
|
if (self)
|
|
{
|
|
if (self->used+elements >= self->high_water)
|
|
{
|
|
self->used += elements;
|
|
self = self->resizefn (self);
|
|
if (!self)
|
|
{
|
|
ERROR (priqueue, "resize failed");
|
|
return NULL;
|
|
}
|
|
self->used -= elements;
|
|
}
|
|
self->low_water = 0;
|
|
}
|
|
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
PriQueue
|
|
priqueue_clib_resize (PriQueue self)
|
|
{
|
|
if (self)
|
|
{
|
|
if (!self->queue)
|
|
{
|
|
INFO (priqueue, "%p: initial alloc", self);
|
|
self->queue = malloc (64*self->element_size);
|
|
ERROR_IF (!self->queue, priqueue, "%p: allocation failed", self);
|
|
if (!self->queue)
|
|
return NULL;
|
|
self->high_water = 64;
|
|
}
|
|
else
|
|
{
|
|
if (self->used)
|
|
{
|
|
if (self->used >= self->high_water)
|
|
{
|
|
unsigned newwater = self->high_water;
|
|
while (self->used >= newwater)
|
|
newwater *= 2;
|
|
|
|
INFO (priqueue, "%p: resize %d -> %d", self, self->high_water, newwater);
|
|
|
|
void* newqueue = realloc (self->queue, self->element_size * newwater);
|
|
ERROR_IF (!newqueue, priqueue, "%p: allocation failed", self);
|
|
if (!newqueue)
|
|
return NULL;
|
|
self->queue = newqueue;
|
|
self->high_water = newwater;
|
|
self->low_water = self->high_water/8-8;
|
|
TRACE (priqueue, "%p: low_water: %d", self, self->low_water);
|
|
}
|
|
else
|
|
{
|
|
INFO (priqueue, "%p: shrink %d -> %d", self, self->high_water, (self->low_water+8)*4);
|
|
void* newqueue = realloc (self->queue, self->element_size * (self->low_water+8)*4);
|
|
|
|
ERROR_IF (!newqueue, priqueue, "allocation failed");
|
|
if (!newqueue)
|
|
return NULL;
|
|
self->queue = newqueue;
|
|
self->high_water = (self->low_water+8)*4;
|
|
self->low_water = self->high_water/8-8;
|
|
TRACE (priqueue, "%p: low_water: %d", self, self->low_water);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
INFO (priqueue, "%p: freeing", self);
|
|
free (self->queue);
|
|
self->queue = NULL;
|
|
}
|
|
}
|
|
}
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
static inline void*
|
|
pq_index (PriQueue self, unsigned nth)
|
|
{
|
|
return (char*)self->queue+self->element_size*nth;
|
|
}
|
|
|
|
|
|
static inline void
|
|
pq_up (PriQueue self, void* tmp)
|
|
{
|
|
unsigned i = self->used;
|
|
unsigned p = i/2;
|
|
|
|
while (p && self->cmpfn (tmp, pq_index(self, p-1)) < 0)
|
|
{
|
|
self->copyfn (pq_index (self, i-1), pq_index (self, p-1), self->element_size);
|
|
i=p; p=i/2;
|
|
}
|
|
|
|
self->copyfn (pq_index (self, i-1), tmp, self->element_size);
|
|
}
|
|
|
|
|
|
|
|
PriQueue
|
|
priqueue_insert (PriQueue self, void* element)
|
|
{
|
|
TRACE (priqueue, "%p: insert %p", self, element);
|
|
|
|
if (self && self->used >= self->high_water)
|
|
self = self->resizefn (self);
|
|
|
|
if (self)
|
|
{
|
|
++self->used;
|
|
pq_up (self, element);
|
|
}
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
|
|
static inline void
|
|
pq_down (PriQueue self, void* tmp)
|
|
{
|
|
if (!self->used)
|
|
return;
|
|
|
|
unsigned i = 1;
|
|
|
|
while (i <= self->used/2)
|
|
{
|
|
unsigned n=i+i;
|
|
if (n<self->used && self->cmpfn (pq_index(self, n-1), pq_index(self, n)) >= 0)
|
|
++n;
|
|
|
|
if (self->cmpfn (tmp, pq_index(self, n-1)) < 0)
|
|
break;
|
|
|
|
self->copyfn (pq_index (self, i-1), pq_index (self, n-1), self->element_size);
|
|
i = n;
|
|
}
|
|
self->copyfn (pq_index (self, i-1), tmp, self->element_size);
|
|
}
|
|
|
|
|
|
PriQueue
|
|
priqueue_remove (PriQueue self)
|
|
{
|
|
TRACE (priqueue, "%p: remove", self);
|
|
|
|
if (self)
|
|
{
|
|
if (!self->used)
|
|
return NULL;
|
|
|
|
--self->used;
|
|
pq_down (self, pq_index (self, self->used));
|
|
|
|
if (self->used < self->low_water)
|
|
self = self->resizefn (self);
|
|
}
|
|
|
|
return self;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef PRIQUEUE_TEST /* testing */
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
void
|
|
nobug_priqueue_invariant (PriQueue self, int depth, const struct nobug_context invariant_context, void* extra)
|
|
{
|
|
intptr_t n = 1+(intptr_t)extra;
|
|
|
|
intptr_t m=n+n;
|
|
|
|
if (self && depth && m <= self->used)
|
|
{
|
|
INVARIANT_ASSERT (self->cmpfn (pq_index(self, n-1), pq_index(self, m-1)) <= 0, "%d %d", (int)n-1, (int)m-2);
|
|
nobug_priqueue_invariant (self, depth-1, invariant_context, (void*)m-1);
|
|
|
|
if (m<self->used)
|
|
{
|
|
INVARIANT_ASSERT (self->cmpfn (pq_index(self, n-1), pq_index(self, m)) <= 0, "%d %d", (int)n-1, (int)m-1);
|
|
nobug_priqueue_invariant (self, depth-1, invariant_context, (void*)m);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
static int
|
|
cmpintptr (void* a, void* b)
|
|
{
|
|
return *(int*)a - *(int*)b;
|
|
}
|
|
|
|
NOBUG_DEFINE_FLAG (priqueue_test);
|
|
|
|
int main()
|
|
{
|
|
NOBUG_INIT;
|
|
NOBUG_INIT_FLAG (priqueue_test);
|
|
|
|
priqueue pq;
|
|
|
|
PriQueue r;
|
|
|
|
int data;
|
|
|
|
r = priqueue_init (&pq,
|
|
sizeof (int),
|
|
cmpintptr,
|
|
NULL,
|
|
NULL);
|
|
ENSURE (r==&pq);
|
|
|
|
#if 1
|
|
data = 10;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
#endif
|
|
|
|
#if 0
|
|
data = 5;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
#endif
|
|
|
|
|
|
#if 0
|
|
data = 15;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
|
|
data = 20;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if 1
|
|
for (int i = 0; i < 1000000; ++i)
|
|
{
|
|
data = i;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
}
|
|
|
|
#endif
|
|
|
|
#if 1
|
|
for (int i = 0; i < 1000000; ++i)
|
|
{
|
|
data = rand()%1000000;
|
|
r = priqueue_insert (&pq, &data);
|
|
ENSURE (r==&pq);
|
|
TRACE (priqueue_test, "inserted %d", data);
|
|
}
|
|
#endif
|
|
|
|
NOBUG_INVARIANT(priqueue, &pq, 100, NULL);
|
|
|
|
|
|
#if 1
|
|
for (int i = 0; pq.used; ++i)
|
|
{
|
|
TRACE (priqueue_test, "TOP: %d", *(int*)priqueue_peek (&pq));
|
|
r = priqueue_remove (&pq);
|
|
ENSURE (r==&pq);
|
|
}
|
|
#endif
|
|
|
|
|
|
r = priqueue_destroy (&pq);
|
|
ENSURE (r==&pq);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
#endif
|