Speed up Trick::ScheduledJobQueue::push (#1694)

* Speed up Trick::ScheduledJobQueue::push

* Make comparator a static function

* Use upper_bound instead

* Use explicit types

* Update comment

* Fix formatting

trick_source/sim_services/ScheduledJobQueue/ScheduledJobQueue.cpp

@@ -1,8 +1,9 @@
-
+#include <algorithm>
 #include <iostream>
 #include <sstream>
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 
 #include "trick/ScheduledJobQueue.hh"
 #include "trick/ScheduledJobQueueInstrument.hh"
@@ -34,84 +35,63 @@ Trick::ScheduledJobQueue::~ScheduledJobQueue( ) {
     }
 }
 
+static bool compare_job_data(const Trick::JobData *a, const Trick::JobData *b) {
+    {
+        int ajc = a->job_class;
+        int bjc = b->job_class;
+        if (ajc < bjc)
+            return true;
+        if (ajc > bjc)
+            return false;
+    }
+    {
+        unsigned short ap = a->phase;
+        unsigned short bp = b->phase;
+        if (ap < bp)
+            return true;
+        if (ap > bp)
+            return false;
+    }
+    {
+        int asoi = a->sim_object_id;
+        int bsoi = b->sim_object_id;
+        if (asoi < bsoi)
+            return true;
+        if (asoi > bsoi)
+            return false;
+    }
+    return a->id < b->id;
+}
+
 /**
 @design
 -# Allocate additional memory for the incoming job
 -# Find the insertion point in the queue based on the job_class, the phase,
    the sim_object id, and the job_id
--# While searching for the correct insertion spot, copy all jobs that precede
-   the incoming job to the newly allocated queue space followed by the new job.
--# Copy jobs that are ordered after the incoming job to the new queue
+-# Move the jobs after the insertion point to the right by one.
+-# Insert the new job at the insertion point.
 -# Increment the size of the queue.
 */
 int Trick::ScheduledJobQueue::push( JobData * new_job ) {
 
-    unsigned int ii , jj ;
-
     /* Allocate additional memory for the additional job in the queue */
-    JobData ** new_list = (JobData **)calloc( list_size + 1 , sizeof(JobData *)) ;
-
-    new_job->set_handled(true) ;
-
-    /* Find the correct insertion spot in the queue by comparing
-       the job_class, the phase, the sim_object id, and the job_id in that order. */
-    /* While searching for the correct insertion spot, copy all jobs that precede
-       the incoming job to the newly allocated queue space. */
-    for ( ii = jj = 0 ; ii < list_size ; ii++ ) {
-        if ( list[ii]->job_class == new_job->job_class ) {
-            if ( list[ii]->phase == new_job->phase ) {
-                if ( list[ii]->sim_object_id ==  new_job->sim_object_id ) {
-                    if ( list[ii]->id <= new_job->id ) {
-                        new_list[jj++] = list[ii] ;
-                    } else {
-                        new_list[jj++] = new_job ;
-                        break ;
-                    }
-                } else if ( list[ii]->sim_object_id < new_job->sim_object_id ) {
-                    new_list[jj++] = list[ii] ;
-                } else {
-                    new_list[jj++] = new_job ;
-                    break ;
-                }
-            } else if ( list[ii]->phase < new_job->phase ) {
-                new_list[jj++] = list[ii] ;
-            } else {
-                new_list[jj++] = new_job ;
-                break ;
-            }
-        } else if ( list[ii]->job_class < new_job->job_class ) {
-            new_list[jj++] = list[ii] ;
-        } else {
-            new_list[jj++] = new_job ;
-            break ;
-        }
+    JobData ** new_list = (JobData **)realloc(list, (list_size + 1) * sizeof(JobData *)) ;
+    if (!new_list) {
+        abort();
     }
-
-    /* Copy remaining jobs that execute after the incoming job to the new queue space. */
-    if ( ii == list_size ) {
-        /* ii == list_size means the incoming job is the last job */
-        new_list[list_size] = new_job ;
-    } else {
-        /* Inserted new job before the current job. Increment curr_index to point to the correct job */
-        if ( ii < curr_index ) {
-            curr_index++ ;
-        }
-        for ( ; ii < list_size ; ii++ ) {
-            new_list[jj++] = list[ii] ;
-        }
+    list = new_list;
+    JobData ** list_end = list + list_size;
+    JobData ** insert_pt = std::upper_bound(list, list_end, new_job, compare_job_data);
+    if (insert_pt != list_end) {
+        memmove(insert_pt + 1, insert_pt, (list_end - insert_pt) * sizeof(JobData *));
     }
+    *insert_pt = new_job;
+
+    new_job->set_handled(true) ;
 
     /* Increment the size of the queue */
     list_size++ ;
 
-    /* Free the old queue space */
-    if ( list ) {
-        free(list) ;
-    }
-
-    /* Assign the queue pointer to the new space */
-    list = new_list ;
-
     return(0) ;
 
 }
@@ -534,4 +514,3 @@ int Trick::ScheduledJobQueue::instrument_remove(std::string job_name) {
 
     return 0 ;
 }
-