git.meshlink.io Git - catta/blob - prioq.c

   1 #include "prioq.h"
   2
   3 flxPrioQueue* flx_prio_queue_new(gint (*compare) (gconstpointer a, gconstpointer b)) {
   4     flxPrioQueue *q;
   5     g_assert(compare);
   6
   7     q = g_new(flxPrioQueue, 1);
   8     q->root = q->last = NULL;
   9     q->n_nodes = 0;
  10     q->compare = compare;
  11     return q;
  12 }
  13
  14 void flx_prio_queue_free(flxPrioQueue *q) {
  15     g_assert(q);
  16
  17     while (q->last)
  18         flx_prio_queue_remove(q, q->last);
  19
  20     g_assert(!q->n_nodes);
  21     g_free(q);
  22 }
  23
  24 static flxPrioQueueNode* get_node_at_xy(flxPrioQueue *q, guint x, guint y) {
  25     guint r;
  26     flxPrioQueueNode *n;
  27     g_assert(q);
  28
  29     n = q->root;
  30     g_assert(n);
  31
  32     for (r = 0; r < y; r++) {
  33         g_assert(n);
  34
  35         if ((x >> (y-r-1)) & 1)
  36             n = n->right;
  37         else
  38             n = n->left;
  39     }
  40
  41     g_assert(n->x == x);
  42     g_assert(n->y == y);
  43
  44     return n;
  45 }
  46
  47 static void exchange_nodes(flxPrioQueue *q, flxPrioQueueNode *a, flxPrioQueueNode *b) {
  48     flxPrioQueueNode *l, *r, *p, *ap, *an, *bp, *bn, *apl, *bpl;
  49     gint t;
  50     g_assert(q);
  51     g_assert(a);
  52     g_assert(b);
  53     g_assert(a != b);
  54
  55     /* Swap positions */
  56     t = a->x; a->x = b->x; b->x = t;
  57     t = a->y; a->y = b->y; b->y = t;
  58
  59     if (a->parent == b) {
  60         /* B is parent of A */
  61
  62         p = b->parent;
  63         b->parent = a;
  64
  65         if ((a->parent = p)) {
  66             if (a->parent->left == b)
  67                 a->parent->left = a;
  68             else
  69                 a->parent->right = a;
  70         } else
  71             q->root = a;
  72
  73         if (b->left == a) {
  74             if ((b->left = a->left))
  75                 b->left->parent = b;
  76             a->left = b;
  77
  78             r = a->right;
  79             if ((a->right = b->right))
  80                 a->right->parent = a;
  81             if ((b->right = r))
  82                 b->right->parent = b;
  83
  84         } else {
  85             if ((b->right = a->right))
  86                 b->right->parent = b;
  87             a->right = b;
  88
  89             l = a->left;
  90             if ((a->left = b->left))
  91                 a->left->parent = a;
  92             if ((b->left = l))
  93                 b->left->parent = b;
  94         }
  95     } else if (b->parent == a) {
  96         /* A ist parent of B */
  97
  98         p = a->parent;
  99         a->parent = b;
 100
 101         if ((b->parent = p)) {
 102             if (b->parent->left == a)
 103                 b->parent->left = b;
 104             else
 105                 b->parent->right = b;
 106         } else
 107             q->root = b;
 108
 109         if (a->left == b) {
 110             if ((a->left = b->left))
 111                 a->left->parent = a;
 112             b->left = a;
 113
 114             r = a->right;
 115             if ((a->right = b->right))
 116                 a->right->parent = a;
 117             if ((b->right = r))
 118                 b->right->parent = b;
 119         } else {
 120             if ((a->right = b->right))
 121                 a->right->parent = a;
 122             b->right = a;
 123
 124             l = a->left;
 125             if ((a->left = b->left))
 126                 a->left->parent = a;
 127             if ((b->left = l))
 128                 b->left->parent = b;
 129         }
 130     } else {
 131         /* Swap parents */
 132         ap = a->parent;
 133         bp = b->parent;
 134
 135         if (ap)
 136             apl = ap->left;
 137         if (bp)
 138             bpl = bp->left;
 139
 140         if ((a->parent = bp)) {
 141             if (bpl == b)
 142                 bp->left = a;
 143             else
 144                 bp->right = a;
 145         } else
 146             q->root = a;
 147
 148         if ((b->parent = ap)) {
 149             if (apl == a)
 150                 ap->left = b;
 151             else
 152                 ap->right = b;
 153         } else
 154             q->root = b;
 155
 156         /* Swap children */
 157         l = a->left;
 158         r = a->right;
 159
 160         if ((a->left = b->left))
 161             a->left->parent = a;
 162
 163         if ((b->left = l))
 164             b->left->parent = b;
 165
 166         if ((a->right = b->right))
 167             a->right->parent = a;
 168
 169         if ((b->right = r))
 170             b->right->parent = b;
 171     }
 172
 173     /* Swap siblings */
 174     ap = a->prev; an = a->next;
 175     bp = b->prev; bn = b->next;
 176
 177     if (a->next == b) {
 178         /* A is predecessor of B */
 179         a->prev = b;
 180         b->next = a;
 181
 182         if ((a->next = bn))
 183             a->next->prev = a;
 184         else
 185             q->last = a;
 186
 187         if ((b->prev = ap))
 188             b->prev->next = b;
 189
 190     } else if (b->next == a) {
 191         /* B is predecessor of A */
 192         a->next = b;
 193         b->prev = a;
 194
 195         if ((a->prev = bp))
 196             a->prev->next = a;
 197
 198         if ((b->next = an))
 199             b->next->prev = b;
 200         else
 201             q->last = b;
 202
 203     } else {
 204         /* A is no neighbour of B */
 205
 206         if ((a->prev = bp))
 207             a->prev->next = a;
 208
 209         if ((a->next = bn))
 210             a->next->prev = a;
 211         else
 212             q->last = a;
 213
 214         if ((b->prev = ap))
 215             b->prev->next = b;
 216
 217         if ((b->next = an))
 218             b->next->prev = b;
 219         else
 220             q->last = b;
 221     }
 222 }
 223
 224 /* Move a node to the correct position */
 225 void flx_prio_queue_shuffle(flxPrioQueue *q, flxPrioQueueNode *n) {
 226     g_assert(q);
 227     g_assert(n);
 228
 229     /* Move up until the position is OK */
 230     while (n->parent && q->compare(n->parent->data, n->data) > 0)
 231         exchange_nodes(q, n, n->parent);
 232
 233     /* Move down until the position is OK */
 234     for (;;) {
 235         flxPrioQueueNode *min;
 236
 237         if (!(min = n->left)) {
 238             /* No children */
 239             g_assert(!n->right);
 240             break;
 241         }
 242
 243         if (n->right && q->compare(n->right->data, min->data) < 0)
 244             min = n->right;
 245
 246         /* min now contains the smaller one of our two children */
 247
 248         if (q->compare(n->data, min->data) <= 0)
 249             /* Order OK */
 250             break;
 251
 252         exchange_nodes(q, n, min);
 253     }
 254 }
 255
 256 flxPrioQueueNode* flx_prio_queue_put(flxPrioQueue *q, gpointer data) {
 257     flxPrioQueueNode *n;
 258     g_assert(q);
 259
 260     n = g_new(flxPrioQueueNode, 1);
 261     n->queue = q;
 262     n->data = data;
 263
 264     if (q->last) {
 265         g_assert(q->root);
 266         g_assert(q->n_nodes);
 267
 268         n->y = q->last->y;
 269         n->x = q->last->x+1;
 270
 271         if (n->x >= ((guint) 1 << n->y)) {
 272             n->x = 0;
 273             n->y++;
 274         }
 275
 276         q->last->next = n;
 277         n->prev = q->last;
 278
 279         g_assert(n->y > 0);
 280         n->parent = get_node_at_xy(q, n->x/2, n->y-1);
 281
 282         if (n->x & 1)
 283             n->parent->right = n;
 284         else
 285             n->parent->left = n;
 286     } else {
 287         g_assert(!q->root);
 288         g_assert(!q->n_nodes);
 289
 290         n->y = n->x = 0;
 291         q->root = n;
 292         n->prev = n->parent = NULL;
 293     }
 294
 295     n->next = n->left = n->right = NULL;
 296     q->last = n;
 297     q->n_nodes++;
 298
 299     flx_prio_queue_shuffle(q, n);
 300
 301     return n;
 302 }
 303
 304 void flx_prio_queue_remove(flxPrioQueue *q, flxPrioQueueNode *n) {
 305     g_assert(q);
 306     g_assert(n);
 307
 308     if (n != q->last) {
 309         flxPrioQueueNode *replacement = q->last;
 310         exchange_nodes(q, replacement, n);
 311         flx_prio_queue_remove(q, n);
 312         flx_prio_queue_shuffle(q, replacement);
 313         return;
 314     }
 315
 316     g_assert(n == q->last);
 317     g_assert(!n->next);
 318     g_assert(!n->left);
 319     g_assert(!n->right);
 320
 321     q->last = n->prev;
 322
 323     if (n->prev) {
 324         n->prev->next = NULL;
 325         g_assert(n->parent);
 326     } else
 327         g_assert(!n->parent);
 328
 329     if (n->parent) {
 330         g_assert(n->prev);
 331         if (n->parent->left == n) {
 332             if (n->parent->right != NULL) {
 333                 g_message("fuck");
 334                 for (;;);
 335
 336             }
 337
 338             g_assert(n->parent->right == NULL);
 339             n->parent->left = NULL;
 340         } else {
 341             g_assert(n->parent->right == n);
 342             g_assert(n->parent->left != NULL);
 343             n->parent->right = NULL;
 344         }
 345     } else {
 346         g_assert(q->root == n);
 347         g_assert(!n->prev);
 348         g_assert(q->n_nodes == 1);
 349         q->root = NULL;
 350     }
 351
 352     g_free(n);
 353
 354     g_assert(q->n_nodes > 0);
 355     q->n_nodes--;
 356 }
 357