Add a function for finding an element in a priority queue with the pr…

…iority

core/federated/federate.c

@@ -1252,9 +1252,6 @@ static void handle_provisional_tag_advance_grant() {
   // (which it should be). Do not do this if the federate has not fully
   // started yet.
 
-  // instant_t dummy_event_time = PTAG.time;
-  // microstep_t dummy_event_microstep = PTAG.microstep;
-
   if (lf_tag_compare(env->current_tag, PTAG) == 0) {
     // The current tag can equal the PTAG if we are at the start time
     // or if this federate has been able to advance time to the current

core/modal_models/modes.c

@@ -411,16 +411,16 @@ void _lf_process_mode_changes(environment_t* env, reactor_mode_state_t* states[]
                                     .microstep = (local_remaining_delay == 0 ? current_logical_tag.microstep + 1 : 0)};
               _lf_schedule_at_tag(env, event->trigger, schedule_tag, event->token);
 
-              // Also schedule events stacked up in super dense time.
-              event_t* e = event;
-              while (e->next != NULL) {
-                schedule_tag.microstep++;
-                _lf_schedule_at_tag(env, e->next->trigger, schedule_tag, e->next->token);
-                event_t* tmp = e->next;
-                e = tmp->next;
-                // A fresh event was created by schedule, hence, recycle old one
-                lf_recycle_event(env, tmp);
-              }
+              // // Also schedule events stacked up in super dense time.
+              // event_t* e = event;
+              // while (e->next != NULL) {
+              //   schedule_tag.microstep++;
+              //   _lf_schedule_at_tag(env, e->next->trigger, schedule_tag, e->next->token);
+              //   event_t* tmp = e->next;
+              //   e = tmp->next;
+              //   // A fresh event was created by schedule, hence, recycle old one
+              //   lf_recycle_event(env, tmp);
+              // }
             }
             // A fresh event was created by schedule, hence, recycle old one
             lf_recycle_event(env, event);

core/utils/pqueue.c

@@ -13,7 +13,7 @@
 #include "util.h"
 #include "lf_types.h"
 
-int in_reverse_order(pqueue_pri_t thiz, pqueue_pri_t that) { return 1 ? (thiz > that) : 0 ? (thiz == that) : -1; }
+int in_reverse_order(pqueue_pri_t thiz, pqueue_pri_t that) { return (thiz > that) ? 1 : (thiz < that) ? -1 : 0; }
 
 int in_no_particular_order(pqueue_pri_t thiz, pqueue_pri_t that) { return 0; }
 

core/utils/pqueue_base.c

@@ -76,6 +76,39 @@ void* find_equal(pqueue_t* q, void* e, int pos, pqueue_pri_t max) {
   return NULL;
 }
 
+void* find_same_priority(pqueue_t* q, void* e, int pos) {
+  if (pos < 0) {
+    lf_print_error_and_exit("find_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)) == 1) {
+    return NULL;
+  }
+
+  if (q->cmppri(q->getpri(curr), q->getpri(e)) == 0) {
+    return curr;
+  } else {
+    rval = find_same_priority(q, e, LF_LEFT(pos));
+    if (rval)
+      return rval;
+    else
+      return find_same_priority(q, e, LF_RIGHT(pos));
+  }
+  return NULL;
+}
+
 void* 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.");
@@ -194,10 +227,12 @@ static void percolate_down(pqueue_t* q, size_t i) {
   q->setpos(moving_node, i);
 }
 
-void* pqueue_find_equal_same_priority(pqueue_t* q, void* e) { return find_equal_same_priority(q, e, 1); }
-
 void* pqueue_find_equal(pqueue_t* q, void* e, pqueue_pri_t max) { return find_equal(q, e, 1, max); }
 
+void* pqueue_find_same_priority(pqueue_t* q, void* e) { return find_same_priority(q, e, 1); }
+
+void* pqueue_find_equal_same_priority(pqueue_t* q, void* e) { return find_equal_same_priority(q, e, 1); }
+
 int pqueue_insert(pqueue_t* q, void* d) {
   void** tmp;
   size_t i;

core/utils/pqueue_tag.c

@@ -106,11 +106,10 @@ int pqueue_tag_insert_tag(pqueue_tag_t* q, tag_t t) {
 }
 
 pqueue_tag_element_t* pqueue_tag_find_with_tag(pqueue_tag_t* q, tag_t t) {
-  // Create elements on the stack. These elements are only needed during
-  // the duration of this function call, so putting them on the stack is OK.
+  // Create an element on the stack. This element is only needed during
+  // the duration of this function call, so putting it on the stack is OK.
   pqueue_tag_element_t element = {.tag = t, .pos = 0, .is_dynamic = false};
-  pqueue_tag_element_t forever = {.tag = FOREVER_TAG, .pos = 0, .is_dynamic = false};
-  return pqueue_find_equal((pqueue_t*)q, (void*)&element, (pqueue_pri_t)&forever);
+  return pqueue_find_same_priority((pqueue_t*)q, (void*)&element);
 }
 
 pqueue_tag_element_t* pqueue_tag_find_equal_same_tag(pqueue_tag_t* q, pqueue_tag_element_t* e) {

include/core/utils/pqueue_base.h

@@ -140,23 +140,31 @@ void* pqueue_pop(pqueue_t* q);
 void pqueue_empty_into(pqueue_t** dest, pqueue_t** src);
 
 /**
- * Find the highest-ranking item with the same priority that matches the
- * supplied entry.
+ * Find the highest-ranking item with priority up to and including the given
+ * maximum priority that matches the supplied entry.
  * @param q the queue
  * @param e the entry to compare against
+ * @param max_priority the maximum priority to consider
  * @return NULL if no matching event has been found, otherwise the entry
  */
-void* pqueue_find_equal_same_priority(pqueue_t* q, void* e);
+void* pqueue_find_equal(pqueue_t* q, void* e, pqueue_pri_t max_priority);
 
 /**
- * Find the highest-ranking item with priority up to and including the given
- * maximum priority that matches the supplied entry.
+ * Find the highest-ranking item with the same priority.
  * @param q the queue
  * @param e the entry to compare against
- * @param max_priority the maximum priority to consider
  * @return NULL if no matching event has been found, otherwise the entry
  */
-void* pqueue_find_equal(pqueue_t* q, void* e, pqueue_pri_t max_priority);
+void* pqueue_find_same_priority(pqueue_t* q, void* e);
+
+/**
+ * Find the highest-ranking item with the same priority that matches the
+ * supplied entry.
+ * @param q the queue
+ * @param e the entry to compare against
+ * @return NULL if no matching event has been found, otherwise the entry
+ */
+void* pqueue_find_equal_same_priority(pqueue_t* q, void* e);
 
 /**
  * Remove an item from the queue.