From cd3b584879216610b99f132c1702c0a426bb051b Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 6 Feb 2016 08:07:21 +0000
Subject: [PATCH] Added test to check filling up and emptying queue.

---
 src/main/scheduler.c                | 20 ++++++++--------
 src/test/unit/scheduler_unittest.cc | 36 +++++++++++++++++++++++++++--
 2 files changed, 45 insertions(+), 11 deletions(-)

diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index 54ba180626..b61d34c33e 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -82,30 +82,32 @@ STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
     return false;
 }
 
-STATIC_UNIT_TESTED void queueAdd(cfTask_t *task)
+STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task)
 {
     if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
-        return;
+        return false;
     }
     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));
             taskQueueArray[ii] = task;
             ++taskQueueSize;
-            return;
+            return true;
         }
     }
+    return false;
 }
 
-STATIC_UNIT_TESTED void queueRemove(cfTask_t *task)
+STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task)
 {
     for (int ii = 0; ii < taskQueueSize; ++ii) {
         if (taskQueueArray[ii] == task) {
             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
             --taskQueueSize;
-            return;
+            return true;
         }
     }
+    return false;
 }
 
 /*
@@ -216,8 +218,8 @@ void scheduler(void)
     const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
 
     // The task to be invoked
-    uint32_t selectedTaskDynPrio = 0;
     cfTask_t *selectedTask = NULL;
+    uint16_t selectedTaskDynamicPriority = 0;
 
     // Update task dynamic priorities
     uint16_t waitingTasks = 0;
@@ -247,13 +249,13 @@ void scheduler(void)
             }
         }
 
-        if (task->dynamicPriority > selectedTaskDynPrio) {
+        if (task->dynamicPriority > selectedTaskDynamicPriority) {
             const bool taskCanBeChosenForScheduling =
                 (outsideRealtimeGuardInterval) ||
                 (task->taskAgeCycles > 1) ||
                 (task->staticPriority == TASK_PRIORITY_REALTIME);
             if (taskCanBeChosenForScheduling) {
-                selectedTaskDynPrio = task->dynamicPriority;
+                selectedTaskDynamicPriority = task->dynamicPriority;
                 selectedTask = task;
             }
         }
@@ -264,8 +266,8 @@ void scheduler(void)
 
     currentTask = selectedTask;
 
-    // Found a task that should be run
     if (selectedTask != NULL) {
+        // Found a task that should be run
         selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt;
         selectedTask->lastExecutedAt = currentTime;
         selectedTask->dynamicPriority = 0;
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index 2356d29a9c..722d178b97 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -77,8 +77,8 @@ extern "C" {
     extern void queueClear(void);
     extern int queueSize();
     extern bool queueContains(cfTask_t *task);
-    extern void queueAdd(cfTask_t *task);
-    extern void queueRemove(cfTask_t *task);
+    extern bool queueAdd(cfTask_t *task);
+    extern bool queueRemove(cfTask_t *task);
     extern cfTask_t *queueFirst(void);
     extern cfTask_t *queueNext(void);
 }
@@ -171,6 +171,38 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 }
 
+TEST(SchedulerUnittest, TestQueueAddAndRemove)
+{
+    queueClear();
+    taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
+
+    // fill up the queue
+    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
+        const bool added = queueAdd(&cfTasks[taskId]);
+        EXPECT_EQ(true, added);
+        EXPECT_EQ(taskId + 1, queueSize());
+        EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
+    }
+    // double check end of queue
+    EXPECT_EQ(TASK_COUNT, queueSize());
+    EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue
+    EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved
+    EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error
+
+    // and empty it again
+    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
+        const bool removed = queueRemove(&cfTasks[taskId]);
+        EXPECT_EQ(true, removed);
+        EXPECT_EQ(TASK_COUNT - taskId - 1, queueSize());
+        EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]);
+        EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
+    }
+    // double check size and end of queue
+    EXPECT_EQ(0, queueSize()); // queue is indeed empty
+    EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu
+    EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue
+}
+
 TEST(SchedulerUnittest, TestQueueArray)
 {
     // test there are no "out by one" errors or buffer overruns when items are added and removed