src/basic/prioq.c

   1 /* SPDX-License-Identifier: LGPL-2.1+ */
   2
   3 /*
   4  * Priority Queue
   5  * The prioq object implements a priority queue. That is, it orders objects by
   6  * their priority and allows O(1) access to the object with the highest
   7  * priority. Insertion and removal are Θ(log n). Optionally, the caller can
   8  * provide a pointer to an index which will be kept up-to-date by the prioq.
   9  *
  10  * The underlying algorithm used in this implementation is a Heap.
  11  */
  12
  13 #include <errno.h>
  14 #include <stdlib.h>
  15
  16 #include "alloc-util.h"
  17 #include "hashmap.h"
  18 #include "prioq.h"
  19
  20 struct prioq_item {
  21         void *data;
  22         unsigned *idx;
  23 };
  24
  25 struct Prioq {
  26         compare_func_t compare_func;
  27         unsigned n_items, n_allocated;
  28
  29         struct prioq_item *items;
  30 };
  31
  32 Prioq *prioq_new(compare_func_t compare_func) {
  33         Prioq *q;
  34
  35         q = new(Prioq, 1);
  36         if (!q)
  37                 return q;
  38
  39         *q = (Prioq) {
  40                 .compare_func = compare_func,
  41         };
  42
  43         return q;
  44 }
  45
  46 Prioq* prioq_free(Prioq *q) {
  47         if (!q)
  48                 return NULL;
  49
  50         free(q->items);
  51         return mfree(q);
  52 }
  53
  54 int prioq_ensure_allocated(Prioq **q, compare_func_t compare_func) {
  55         assert(q);
  56
  57         if (*q)
  58                 return 0;
  59
  60         *q = prioq_new(compare_func);
  61         if (!*q)
  62                 return -ENOMEM;
  63
  64         return 0;
  65 }
  66
  67 static void swap(Prioq *q, unsigned j, unsigned k) {
  68         void *saved_data;
  69         unsigned *saved_idx;
  70
  71         assert(q);
  72         assert(j < q->n_items);
  73         assert(k < q->n_items);
  74
  75         assert(!q->items[j].idx || *(q->items[j].idx) == j);
  76         assert(!q->items[k].idx || *(q->items[k].idx) == k);
  77
  78         saved_data = q->items[j].data;
  79         saved_idx = q->items[j].idx;
  80         q->items[j].data = q->items[k].data;
  81         q->items[j].idx = q->items[k].idx;
  82         q->items[k].data = saved_data;
  83         q->items[k].idx = saved_idx;
  84
  85         if (q->items[j].idx)
  86                 *q->items[j].idx = j;
  87
  88         if (q->items[k].idx)
  89                 *q->items[k].idx = k;
  90 }
  91
  92 static unsigned shuffle_up(Prioq *q, unsigned idx) {
  93         assert(q);
  94         assert(idx < q->n_items);
  95
  96         while (idx > 0) {
  97                 unsigned k;
  98
  99                 k = (idx-1)/2;
 100
 101                 if (q->compare_func(q->items[k].data, q->items[idx].data) <= 0)
 102                         break;
 103
 104                 swap(q, idx, k);
 105                 idx = k;
 106         }
 107
 108         return idx;
 109 }
 110
 111 static unsigned shuffle_down(Prioq *q, unsigned idx) {
 112         assert(q);
 113
 114         for (;;) {
 115                 unsigned j, k, s;
 116
 117                 k = (idx+1)*2; /* right child */
 118                 j = k-1;       /* left child */
 119
 120                 if (j >= q->n_items)
 121                         break;
 122
 123                 if (q->compare_func(q->items[j].data, q->items[idx].data) < 0)
 124
 125                         /* So our left child is smaller than we are, let's
 126                          * remember this fact */
 127                         s = j;
 128                 else
 129                         s = idx;
 130
 131                 if (k < q->n_items &&
 132                     q->compare_func(q->items[k].data, q->items[s].data) < 0)
 133
 134                         /* So our right child is smaller than we are, let's
 135                          * remember this fact */
 136                         s = k;
 137
 138                 /* s now points to the smallest of the three items */
 139
 140                 if (s == idx)
 141                         /* No swap necessary, we're done */
 142                         break;
 143
 144                 swap(q, idx, s);
 145                 idx = s;
 146         }
 147
 148         return idx;
 149 }
 150
 151 int prioq_put(Prioq *q, void *data, unsigned *idx) {
 152         struct prioq_item *i;
 153         unsigned k;
 154
 155         assert(q);
 156
 157         if (q->n_items >= q->n_allocated) {
 158                 unsigned n;
 159                 struct prioq_item *j;
 160
 161                 n = MAX((q->n_items+1) * 2, 16u);
 162                 j = reallocarray(q->items, n, sizeof(struct prioq_item));
 163                 if (!j)
 164                         return -ENOMEM;
 165
 166                 q->items = j;
 167                 q->n_allocated = n;
 168         }
 169
 170         k = q->n_items++;
 171         i = q->items + k;
 172         i->data = data;
 173         i->idx = idx;
 174
 175         if (idx)
 176                 *idx = k;
 177
 178         shuffle_up(q, k);
 179
 180         return 0;
 181 }
 182
 183 static void remove_item(Prioq *q, struct prioq_item *i) {
 184         struct prioq_item *l;
 185
 186         assert(q);
 187         assert(i);
 188
 189         l = q->items + q->n_items - 1;
 190
 191         if (i == l)
 192                 /* Last entry, let's just remove it */
 193                 q->n_items--;
 194         else {
 195                 unsigned k;
 196
 197                 /* Not last entry, let's replace the last entry with
 198                  * this one, and reshuffle */
 199
 200                 k = i - q->items;
 201
 202                 i->data = l->data;
 203                 i->idx = l->idx;
 204                 if (i->idx)
 205                         *i->idx = k;
 206                 q->n_items--;
 207
 208                 k = shuffle_down(q, k);
 209                 shuffle_up(q, k);
 210         }
 211 }
 212
 213 _pure_ static struct prioq_item* find_item(Prioq *q, void *data, unsigned *idx) {
 214         struct prioq_item *i;
 215
 216         assert(q);
 217
 218         if (q->n_items <= 0)
 219                 return NULL;
 220
 221         if (idx) {
 222                 if (*idx == PRIOQ_IDX_NULL ||
 223                     *idx >= q->n_items)
 224                         return NULL;
 225
 226                 i = q->items + *idx;
 227                 if (i->data != data)
 228                         return NULL;
 229
 230                 return i;
 231         } else {
 232                 for (i = q->items; i < q->items + q->n_items; i++)
 233                         if (i->data == data)
 234                                 return i;
 235                 return NULL;
 236         }
 237 }
 238
 239 int prioq_remove(Prioq *q, void *data, unsigned *idx) {
 240         struct prioq_item *i;
 241
 242         if (!q)
 243                 return 0;
 244
 245         i = find_item(q, data, idx);
 246         if (!i)
 247                 return 0;
 248
 249         remove_item(q, i);
 250         return 1;
 251 }
 252
 253 int prioq_reshuffle(Prioq *q, void *data, unsigned *idx) {
 254         struct prioq_item *i;
 255         unsigned k;
 256
 257         assert(q);
 258
 259         i = find_item(q, data, idx);
 260         if (!i)
 261                 return 0;
 262
 263         k = i - q->items;
 264         k = shuffle_down(q, k);
 265         shuffle_up(q, k);
 266         return 1;
 267 }
 268
 269 void *prioq_peek(Prioq *q) {
 270
 271         if (!q)
 272                 return NULL;
 273
 274         if (q->n_items <= 0)
 275                 return NULL;
 276
 277         return q->items[0].data;
 278 }
 279
 280 void *prioq_pop(Prioq *q) {
 281         void *data;
 282
 283         if (!q)
 284                 return NULL;
 285
 286         if (q->n_items <= 0)
 287                 return NULL;
 288
 289         data = q->items[0].data;
 290         remove_item(q, q->items);
 291         return data;
 292 }
 293
 294 unsigned prioq_size(Prioq *q) {
 295
 296         if (!q)
 297                 return 0;
 298
 299         return q->n_items;
 300 }
 301
 302 bool prioq_isempty(Prioq *q) {
 303
 304         if (!q)
 305                 return true;
 306
 307         return q->n_items <= 0;
 308 }