--- /dev/null
+/***
+ This file is part of catta.
+
+ catta is free software; you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as
+ published by the Free Software Foundation; either version 2.1 of the
+ License, or (at your option) any later version.
+
+ catta 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 Lesser General
+ Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with catta; if not, write to the Free Software
+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
+ USA.
+***/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <assert.h>
+#include <stdlib.h>
+
+#include <catta/malloc.h>
+
+#include "prioq.h"
+
+CattaPrioQueue* catta_prio_queue_new(CattaPQCompareFunc compare) {
+ CattaPrioQueue *q;
+ assert(compare);
+
+ if (!(q = catta_new(CattaPrioQueue, 1)))
+ return NULL; /* OOM */
+
+ q->root = q->last = NULL;
+ q->n_nodes = 0;
+ q->compare = compare;
+
+ return q;
+}
+
+void catta_prio_queue_free(CattaPrioQueue *q) {
+ assert(q);
+
+ while (q->last)
+ catta_prio_queue_remove(q, q->last);
+
+ assert(!q->n_nodes);
+ catta_free(q);
+}
+
+static CattaPrioQueueNode* get_node_at_xy(CattaPrioQueue *q, unsigned x, unsigned y) {
+ unsigned r;
+ CattaPrioQueueNode *n;
+ assert(q);
+
+ n = q->root;
+ assert(n);
+
+ for (r = 0; r < y; r++) {
+ assert(n);
+
+ if ((x >> (y-r-1)) & 1)
+ n = n->right;
+ else
+ n = n->left;
+ }
+
+ assert(n->x == x);
+ assert(n->y == y);
+
+ return n;
+}
+
+static void exchange_nodes(CattaPrioQueue *q, CattaPrioQueueNode *a, CattaPrioQueueNode *b) {
+ CattaPrioQueueNode *l, *r, *p, *ap, *an, *bp, *bn;
+ unsigned t;
+ assert(q);
+ assert(a);
+ assert(b);
+ assert(a != b);
+
+ /* Swap positions */
+ t = a->x; a->x = b->x; b->x = t;
+ t = a->y; a->y = b->y; b->y = t;
+
+ if (a->parent == b) {
+ /* B is parent of A */
+
+ p = b->parent;
+ b->parent = a;
+
+ if ((a->parent = p)) {
+ if (a->parent->left == b)
+ a->parent->left = a;
+ else
+ a->parent->right = a;
+ } else
+ q->root = a;
+
+ if (b->left == a) {
+ if ((b->left = a->left))
+ b->left->parent = b;
+ a->left = b;
+
+ r = a->right;
+ if ((a->right = b->right))
+ a->right->parent = a;
+ if ((b->right = r))
+ b->right->parent = b;
+
+ } else {
+ if ((b->right = a->right))
+ b->right->parent = b;
+ a->right = b;
+
+ l = a->left;
+ if ((a->left = b->left))
+ a->left->parent = a;
+ if ((b->left = l))
+ b->left->parent = b;
+ }
+ } else if (b->parent == a) {
+ /* A ist parent of B */
+
+ p = a->parent;
+ a->parent = b;
+
+ if ((b->parent = p)) {
+ if (b->parent->left == a)
+ b->parent->left = b;
+ else
+ b->parent->right = b;
+ } else
+ q->root = b;
+
+ if (a->left == b) {
+ if ((a->left = b->left))
+ a->left->parent = a;
+ b->left = a;
+
+ r = a->right;
+ if ((a->right = b->right))
+ a->right->parent = a;
+ if ((b->right = r))
+ b->right->parent = b;
+ } else {
+ if ((a->right = b->right))
+ a->right->parent = a;
+ b->right = a;
+
+ l = a->left;
+ if ((a->left = b->left))
+ a->left->parent = a;
+ if ((b->left = l))
+ b->left->parent = b;
+ }
+ } else {
+ CattaPrioQueueNode *apl = NULL, *bpl = NULL;
+
+ /* Swap parents */
+ ap = a->parent;
+ bp = b->parent;
+
+ if (ap)
+ apl = ap->left;
+ if (bp)
+ bpl = bp->left;
+
+ if ((a->parent = bp)) {
+ if (bpl == b)
+ bp->left = a;
+ else
+ bp->right = a;
+ } else
+ q->root = a;
+
+ if ((b->parent = ap)) {
+ if (apl == a)
+ ap->left = b;
+ else
+ ap->right = b;
+ } else
+ q->root = b;
+
+ /* Swap children */
+ l = a->left;
+ r = a->right;
+
+ if ((a->left = b->left))
+ a->left->parent = a;
+
+ if ((b->left = l))
+ b->left->parent = b;
+
+ if ((a->right = b->right))
+ a->right->parent = a;
+
+ if ((b->right = r))
+ b->right->parent = b;
+ }
+
+ /* Swap siblings */
+ ap = a->prev; an = a->next;
+ bp = b->prev; bn = b->next;
+
+ if (a->next == b) {
+ /* A is predecessor of B */
+ a->prev = b;
+ b->next = a;
+
+ if ((a->next = bn))
+ a->next->prev = a;
+ else
+ q->last = a;
+
+ if ((b->prev = ap))
+ b->prev->next = b;
+
+ } else if (b->next == a) {
+ /* B is predecessor of A */
+ a->next = b;
+ b->prev = a;
+
+ if ((a->prev = bp))
+ a->prev->next = a;
+
+ if ((b->next = an))
+ b->next->prev = b;
+ else
+ q->last = b;
+
+ } else {
+ /* A is no neighbour of B */
+
+ if ((a->prev = bp))
+ a->prev->next = a;
+
+ if ((a->next = bn))
+ a->next->prev = a;
+ else
+ q->last = a;
+
+ if ((b->prev = ap))
+ b->prev->next = b;
+
+ if ((b->next = an))
+ b->next->prev = b;
+ else
+ q->last = b;
+ }
+}
+
+/* Move a node to the correct position */
+void catta_prio_queue_shuffle(CattaPrioQueue *q, CattaPrioQueueNode *n) {
+ assert(q);
+ assert(n);
+ assert(n->queue == q);
+
+ /* Move up until the position is OK */
+ while (n->parent && q->compare(n->parent->data, n->data) > 0)
+ exchange_nodes(q, n, n->parent);
+
+ /* Move down until the position is OK */
+ for (;;) {
+ CattaPrioQueueNode *min;
+
+ if (!(min = n->left)) {
+ /* No children */
+ assert(!n->right);
+ break;
+ }
+
+ if (n->right && q->compare(n->right->data, min->data) < 0)
+ min = n->right;
+
+ /* min now contains the smaller one of our two children */
+
+ if (q->compare(n->data, min->data) <= 0)
+ /* Order OK */
+ break;
+
+ exchange_nodes(q, n, min);
+ }
+}
+
+CattaPrioQueueNode* catta_prio_queue_put(CattaPrioQueue *q, void* data) {
+ CattaPrioQueueNode *n;
+ assert(q);
+
+ if (!(n = catta_new(CattaPrioQueueNode, 1)))
+ return NULL; /* OOM */
+
+ n->queue = q;
+ n->data = data;
+
+ if (q->last) {
+ assert(q->root);
+ assert(q->n_nodes);
+
+ n->y = q->last->y;
+ n->x = q->last->x+1;
+
+ if (n->x >= ((unsigned) 1 << n->y)) {
+ n->x = 0;
+ n->y++;
+ }
+
+ q->last->next = n;
+ n->prev = q->last;
+
+ assert(n->y > 0);
+ n->parent = get_node_at_xy(q, n->x/2, n->y-1);
+
+ if (n->x & 1)
+ n->parent->right = n;
+ else
+ n->parent->left = n;
+ } else {
+ assert(!q->root);
+ assert(!q->n_nodes);
+
+ n->y = n->x = 0;
+ q->root = n;
+ n->prev = n->parent = NULL;
+ }
+
+ n->next = n->left = n->right = NULL;
+ q->last = n;
+ q->n_nodes++;
+
+ catta_prio_queue_shuffle(q, n);
+
+ return n;
+}
+
+void catta_prio_queue_remove(CattaPrioQueue *q, CattaPrioQueueNode *n) {
+ assert(q);
+ assert(n);
+ assert(q == n->queue);
+
+ if (n != q->last) {
+ CattaPrioQueueNode *replacement = q->last;
+ exchange_nodes(q, replacement, n);
+ catta_prio_queue_remove(q, n);
+ catta_prio_queue_shuffle(q, replacement);
+ return;
+ }
+
+ assert(n == q->last);
+ assert(!n->next);
+ assert(!n->left);
+ assert(!n->right);
+
+ q->last = n->prev;
+
+ if (n->prev) {
+ n->prev->next = NULL;
+ assert(n->parent);
+ } else
+ assert(!n->parent);
+
+ if (n->parent) {
+ assert(n->prev);
+ if (n->parent->left == n) {
+ assert(n->parent->right == NULL);
+ n->parent->left = NULL;
+ } else {
+ assert(n->parent->right == n);
+ assert(n->parent->left != NULL);
+ n->parent->right = NULL;
+ }
+ } else {
+ assert(q->root == n);
+ assert(!n->prev);
+ assert(q->n_nodes == 1);
+ q->root = NULL;
+ }
+
+ catta_free(n);
+
+ assert(q->n_nodes > 0);
+ q->n_nodes--;
+}
+