From e52c597d0e2ed5b9515eab33d93b108541752a28 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 21 Jan 2016 10:38:25 +0000
Subject: [PATCH] Minor optimisations to queueAdd and queueRemove

---
 src/main/scheduler.c | 14 ++++----------
 1 file changed, 4 insertions(+), 10 deletions(-)

diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index d7b3f9d0f..3350b4764 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -82,17 +82,14 @@ STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
 
 STATIC_UNIT_TESTED void queueAdd(cfTask_t *task)
 {
-    if (taskQueueSize >= TASK_COUNT -1) {
+    if ((taskQueueSize >= TASK_COUNT -1) || queueContains(task)) {
         return;
     }
-    if (queueContains(task)) {
-        return;
-    }
-    ++taskQueueSize;
-    for (int ii = 0; ii < taskQueueSize; ++ii) {
+    for (int ii = 0; ii <= taskQueueSize; ++ii) {
         if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
-            memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii - 1));
+            memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
             taskQueueArray[ii] = task;
+            ++taskQueueSize;
             return;
         }
     }
@@ -104,9 +101,6 @@ STATIC_UNIT_TESTED void queueRemove(cfTask_t *task)
         if (taskQueueArray[ii] == task) {
             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
             --taskQueueSize;
-            if (taskQueueSize == 0) {
-                taskQueueArray[0] = NULL; // ensure item zero is null when queue is empty
-            }
             return;
         }
     }