src/basic/prioq.c

   1 /* SPDX-License-Identifier: LGPL-2.1-or-later */
   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 "alloc-util.h"
  14 #include "prioq.h"
  15
  16 struct prioq_item {
  17         void *data;
  18         unsigned *idx;
  19 };
  20
  21 struct Prioq {
  22         compare_func_t compare_func;
  23         unsigned n_items;
  24         struct prioq_item *items;
  25 };
  26
  27 Prioq* prioq_new(compare_func_t compare_func) {
  28         Prioq *q;
  29
  30         q = new(Prioq, 1);
  31         if (!q)
  32                 return q;
  33
  34         *q = (Prioq) {
  35                 .compare_func = compare_func,
  36         };
  37
  38         return q;
  39 }
  40
  41 Prioq* prioq_free(Prioq *q) {
  42         if (!q)
  43                 return NULL;
  44
  45         /* Invalidate the index fields of any remaining objects */
  46         FOREACH_ARRAY(item, q->items, q->n_items)
  47                 if (item->idx)
  48                         *(item->idx) = PRIOQ_IDX_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                 assert((*q)->compare_func == compare_func);
  59                 return 0;
  60         }
  61
  62         *q = prioq_new(compare_func);
  63         if (!*q)
  64                 return -ENOMEM;
  65
  66         return 0;
  67 }
  68
  69 static void swap(Prioq *q, unsigned j, unsigned k) {
  70         assert(q);
  71         assert(j < q->n_items);
  72         assert(k < q->n_items);
  73
  74         assert(!q->items[j].idx || *(q->items[j].idx) == j);
  75         assert(!q->items[k].idx || *(q->items[k].idx) == k);
  76
  77         SWAP_TWO(q->items[j].data, q->items[k].data);
  78         SWAP_TWO(q->items[j].idx, q->items[k].idx);
  79
  80         if (q->items[j].idx)
  81                 *q->items[j].idx = j;
  82
  83         if (q->items[k].idx)
  84                 *q->items[k].idx = k;
  85 }
  86
  87 static unsigned shuffle_up(Prioq *q, unsigned idx) {
  88         assert(q);
  89         assert(idx < q->n_items);
  90
  91         while (idx > 0) {
  92                 unsigned k;
  93
  94                 k = (idx-1)/2;
  95
  96                 if (q->compare_func(q->items[k].data, q->items[idx].data) <= 0)
  97                         break;
  98
  99                 swap(q, idx, k);
 100                 idx = k;
 101         }
 102
 103         return idx;
 104 }
 105
 106 static unsigned shuffle_down(Prioq *q, unsigned idx) {
 107         assert(q);
 108
 109         for (;;) {
 110                 unsigned j, k, s;
 111
 112                 k = (idx+1)*2; /* right child */
 113                 j = k-1;       /* left child */
 114
 115                 if (j >= q->n_items)
 116                         break;
 117
 118                 if (q->compare_func(q->items[j].data, q->items[idx].data) < 0)
 119
 120                         /* So our left child is smaller than we are, let's
 121                          * remember this fact */
 122                         s = j;
 123                 else
 124                         s = idx;
 125
 126                 if (k < q->n_items &&
 127                     q->compare_func(q->items[k].data, q->items[s].data) < 0)
 128
 129                         /* So our right child is smaller than we are, let's
 130                          * remember this fact */
 131                         s = k;
 132
 133                 /* s now points to the smallest of the three items */
 134
 135                 if (s == idx)
 136                         /* No swap necessary, we're done */
 137                         break;
 138
 139                 swap(q, idx, s);
 140                 idx = s;
 141         }
 142
 143         return idx;
 144 }
 145
 146 int prioq_put(Prioq *q, void *data, unsigned *idx) {
 147         unsigned k;
 148
 149         assert(q);
 150
 151         if (!GREEDY_REALLOC(q->items, MAX(q->n_items + 1, 16u)))
 152                 return -ENOMEM;
 153
 154         k = q->n_items++;
 155         q->items[k] = (struct prioq_item) {
 156                 .data = data,
 157                 .idx = idx,
 158         };
 159
 160         if (idx)
 161                 *idx = k;
 162
 163         shuffle_up(q, k);
 164
 165         return 0;
 166 }
 167
 168 int _prioq_ensure_put(Prioq **q, compare_func_t compare_func, void *data, unsigned *idx) {
 169         int r;
 170
 171         r = prioq_ensure_allocated(q, compare_func);
 172         if (r < 0)
 173                 return r;
 174
 175         return prioq_put(*q, data, idx);
 176 }
 177
 178 static void remove_item(Prioq *q, struct prioq_item *i) {
 179         struct prioq_item *l;
 180
 181         assert(q);
 182         assert(i);
 183
 184         /* Let's invalidate the index pointer stored in the user's object to indicate the item is now removed
 185          * from the priority queue */
 186         if (i->idx)
 187                 *(i->idx) = PRIOQ_IDX_NULL;
 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                 assert(i >= q->items);
 201                 k = i - q->items;
 202
 203                 i->data = l->data;
 204                 i->idx = l->idx;
 205                 if (i->idx)
 206                         *i->idx = k;
 207                 q->n_items--;
 208
 209                 k = shuffle_down(q, k);
 210                 shuffle_up(q, k);
 211         }
 212 }
 213
 214 static struct prioq_item* find_item(Prioq *q, void *data, unsigned *idx) {
 215         struct prioq_item *i;
 216
 217         assert(q);
 218
 219         if (q->n_items <= 0)
 220                 return NULL;
 221
 222         if (idx) {
 223                 if (*idx == PRIOQ_IDX_NULL ||
 224                     *idx >= q->n_items)
 225                         return NULL;
 226
 227                 i = q->items + *idx;
 228                 if (i->data != data)
 229                         return NULL;
 230
 231                 return i;
 232         } else {
 233                 for (i = q->items; i < q->items + q->n_items; i++)
 234                         if (i->data == data)
 235                                 return i;
 236                 return NULL;
 237         }
 238 }
 239
 240 int prioq_remove(Prioq *q, void *data, unsigned *idx) {
 241         struct prioq_item *i;
 242
 243         if (!q)
 244                 return 0;
 245
 246         i = find_item(q, data, idx);
 247         if (!i)
 248                 return 0;
 249
 250         remove_item(q, i);
 251         return 1;
 252 }
 253
 254 void prioq_reshuffle(Prioq *q, void *data, unsigned *idx) {
 255         struct prioq_item *i;
 256         unsigned k;
 257
 258         assert(q);
 259
 260         i = find_item(q, data, idx);
 261         if (!i)
 262                 return;
 263
 264         assert(i >= q->items);
 265         k = i - q->items;
 266         k = shuffle_down(q, k);
 267         shuffle_up(q, k);
 268 }
 269
 270 void* prioq_peek_by_index(Prioq *q, unsigned idx) {
 271         if (!q)
 272                 return NULL;
 273
 274         if (idx >= q->n_items)
 275                 return NULL;
 276
 277         return q->items[idx].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 }