Do not insert a duplicate tag into the tag-based priority queue

core/utils/pqueue_tag.c

@@ -14,6 +14,10 @@
 #include "util.h"     // For lf_print
 #include "platform.h" // For PRINTF_TAG
 
+#define LF_LEFT(i)   ((i) << 1)
+#define LF_RIGHT(i)  (((i) << 1) + 1)
+#define LF_PARENT(i) ((i) >> 1)
+
 //////////////////
 // Local functions, not intended for use outside this file.
 
@@ -35,7 +39,7 @@ static pqueue_pri_t pqueue_tag_get_priority(void *element) {
  * @param priority2 A pointer to a pqueue_tag_element_t, cast to pqueue_pri_t.
 */
 static int pqueue_tag_compare(pqueue_pri_t priority1, pqueue_pri_t priority2) {
-    return (lf_tag_compare(((pqueue_tag_element_t*) priority1)->tag, ((pqueue_tag_element_t*) priority2)->tag) >= 0);
+    return (lf_tag_compare(((pqueue_tag_element_t*) priority1)->tag, ((pqueue_tag_element_t*) priority2)->tag) > 0);
 }
 
 /**
@@ -78,6 +82,40 @@ static void pqueue_tag_print_element(void *element) {
     lf_print("Element with tag " PRINTF_TAG ".", tag.time, tag.microstep);
 }
 
+void* pqueue_tag_find_equal_same_priority(pqueue_t *q, void *e, int pos) {
+    if (pos < 0) {
+        lf_print_error_and_exit("find_equal_same_priority() called with a negative pos index.");
+    }
+
+    // Stop the recursion when we've reached the end of the
+    // queue. This has to be done before accessing the queue
+    // to avoid segmentation fault.
+    if (!q || (size_t)pos >= q->size) {
+        return NULL;
+    }
+
+    void* rval;
+    void* curr = q->d[pos];
+
+    // Stop the recursion once we've surpassed the priority of the element
+    // we're looking for.
+    if (!curr || q->cmppri(q->getpri(curr), q->getpri(e))) {
+        return NULL;
+    }
+
+    if (pqueue_tag_matches(q->getpri(curr), q->getpri(e)) && q->eqelem(curr, e)) {
+        return curr;
+    } else {
+        rval = pqueue_tag_find_equal_same_priority(q, e, LF_LEFT(pos));
+        if (rval) {
+            return rval;
+        } else {
+            return pqueue_tag_find_equal_same_priority(q, e, LF_RIGHT(pos));
+        }
+    }
+    return NULL;
+}
+
 //////////////////
 // Functions defined in pqueue_tag.h.
 
@@ -113,6 +151,11 @@ int pqueue_tag_insert_tag(pqueue_tag_t* q, tag_t t) {
     pqueue_tag_element_t* d = (pqueue_tag_element_t*) malloc(sizeof(pqueue_tag_element_t));
     d->is_dynamic = 1;
     d->tag = t;
+    if (pqueue_tag_find_equal_same_priority(q, d, 1) != NULL) {
+        LF_PRINT_LOG("The tag d " PRINTF_TAG " is already in the queue. Aborting.\n",
+        t.time, t.microstep);
+        return 1;
+    }
     return pqueue_tag_insert(q, d);
 }
 

test/general/utils/pqueue_test.c

@@ -22,6 +22,9 @@ static void insert_on_queue(pqueue_tag_t* q) {
     assert(!pqueue_tag_insert_tag(q, t2));
     assert(!pqueue_tag_insert_tag(q, t3));
     assert(!pqueue_tag_insert_tag(q, t4));
+
+    assert(pqueue_tag_insert_tag(q, t4));
+    assert(pqueue_tag_insert_tag(q, t1));
     printf("======== Contents of the queue:\n");
     pqueue_print((pqueue_t*)q, NULL);
     assert(pqueue_tag_size(q) == 4);