From 7eebc7b9979c5bb5c8d4cff22e9c9f40e60a2b4b Mon Sep 17 00:00:00 2001
From: GHOSCHT <31184695+GHOSCHT@users.noreply.github.com>
Date: Fri, 31 Mar 2023 12:29:07 +0200
Subject: [PATCH] Implement light fading

---
 Firmware/src/LightController/Controller.h | 91 +++++++++++++++++------
 1 file changed, 67 insertions(+), 24 deletions(-)

diff --git a/Firmware/src/LightController/Controller.h b/Firmware/src/LightController/Controller.h
index e5aa7d6..a9c1a3d 100644
--- a/Firmware/src/LightController/Controller.h
+++ b/Firmware/src/LightController/Controller.h
@@ -1,7 +1,9 @@
 #pragma once
 #include <Arduino.h>
+#include <FreeRTOS/Mutex.h>
 #include <FreeRTOS/Queue.h>
 #include <FreeRTOS/Task.h>
+#include <FreeRTOS/Util.h>
 #include <etl/optional.h>
 #include <etl/span.h>
 
@@ -11,13 +13,18 @@ using LightAction = etl::pair<LightActionType, etl::optional<uint8_t>>;
 
 template <int... PIN> class Controller {
 public:
-  Controller(size_t actionQueueSize, UBaseType_t priority, BaseType_t coreID)
+  Controller(size_t actionQueueSize, UBaseType_t controllerPriority,
+             BaseType_t controllerCoreID, UBaseType_t faderPriority,
+             BaseType_t faderCoreID)
       : lightPins{PIN...}, lightActions{actionQueueSize},
-        lightControllerTask{exec, "control lights", stackDepth,
-                            this, priority,         coreID} {
+        lightControllerTask{exec, "control lights",   stackDepth,
+                            this, controllerPriority, controllerCoreID},
+        lightFaderTask{execFade, "fade lights", stackDepth,
+                       this,     faderPriority, faderCoreID} {
     for (int pin : lightPins) {
       pinMode(pin, OUTPUT);
     }
+    lightStatesCurrent.second.unlock();
   }
 
   freertos::Queue<LightAction> &getActionQueue() { return lightActions; }
@@ -26,10 +33,7 @@ public:
 
 private:
   static void exec(void *controllerPtr) {
-    Controller<PIN...> *controller{
-        static_cast<Controller<PIN...> *>(controllerPtr)};
-    etl::array<uint8_t, sizeof...(PIN)> lightStates;
-    const uint8_t step = 5;
+    Controller *controller{static_cast<Controller *>(controllerPtr)};
 
     while (true) {
       auto action{controller->getActionQueue().pop()};
@@ -39,45 +43,84 @@ private:
 
         switch (type) {
         case LightActionType::INCREASE:
-          if (index.has_value() && index.value() < lightStates.size() &&
-              lightStates[index.value()] <= 255 - step) {
-            lightStates[index.value()] += step;
+          if (index.has_value() &&
+              index.value() < controller->lightStatesFinal.size() &&
+              controller->lightStatesFinal[index.value()] <=
+                  255 - controller->step) {
+            controller->lightStatesFinal[index.value()] += controller->step;
           }
           break;
         case LightActionType::DECREASE:
-          if (index.has_value() && index.value() < lightStates.size() &&
-              lightStates[index.value()] >= 0) {
-            lightStates[index.value()] -= step;
+          if (index.has_value() &&
+              index.value() < controller->lightStatesFinal.size() &&
+              controller->lightStatesFinal[index.value()] >= 0) {
+            controller->lightStatesFinal[index.value()] -= controller->step;
           }
           break;
         case LightActionType::TOGGLE:
-          if (index.has_value() && index.value() < lightStates.size()) {
-            lightStates[index.value()] =
-                lightStates[index.value()] == 0 ? 255 : 0;
+          if (index.has_value() &&
+              index.value() < controller->lightStatesFinal.size()) {
+            controller->lightStatesCurrent.second.lock();
+            controller->lightStatesFinal[index.value()] =
+                controller->lightStatesFinal[index.value()] == 0 ? 255 : 0;
+            controller->lightStatesCurrent.first[index.value()] =
+                controller->lightStatesFinal[index.value()];
+            controller->lightStatesCurrent.second.unlock();
           }
           break;
         case LightActionType::ON:
-          lightStates.fill(255);
+          controller->lightStatesCurrent.second.lock();
+          controller->lightStatesFinal.fill(255);
+          controller->lightStatesCurrent.first.fill(255);
+          controller->lightStatesCurrent.second.unlock();
           break;
         case LightActionType::OFF:
-          lightStates.fill(0);
+          controller->lightStatesCurrent.second.lock();
+          controller->lightStatesFinal.fill(0);
+          controller->lightStatesCurrent.first.fill(0);
+          controller->lightStatesCurrent.second.unlock();
           break;
         default:
           break;
         }
-
-        for (size_t lightIndex = 0; lightIndex < lightStates.size();
-             ++lightIndex) {
-          analogWrite(controller->getPins()[lightIndex],
-                      lightStates[lightIndex]);
-        }
       }
     }
   }
 
+  static void execFade(void *controllerPtr) {
+    Controller *controller{static_cast<Controller *>(controllerPtr)};
+    while (true) {
+      freertos::sleep(controller->fadeRate);
+      controller->lightStatesCurrent.second.lock();
+      for (size_t i = 0; i < controller->lightStatesFinal.size(); i++) {
+        if (controller->lightStatesCurrent.first[i] <
+            controller->lightStatesFinal[i]) {
+          controller->lightStatesCurrent.first[i] += 1;
+        } else if (controller->lightStatesCurrent.first[i] >
+                   controller->lightStatesFinal[i]) {
+          controller->lightStatesCurrent.first[i] -= 1;
+        }
+      }
+
+      for (size_t lightIndex = 0;
+           lightIndex < controller->lightStatesCurrent.first.size();
+           ++lightIndex) {
+        analogWrite(controller->getPins()[lightIndex],
+                    controller->lightStatesCurrent.first[lightIndex]);
+      }
+      controller->lightStatesCurrent.second.unlock();
+    }
+  }
+
   static const uint32_t stackDepth = 2048;
+  const uint8_t step = 5;
+  const size_t fadeRate = 50;
   etl::array<int, sizeof...(PIN)> lightPins;
   freertos::Queue<LightAction> lightActions;
   freertos::Task lightControllerTask;
+  freertos::Task lightFaderTask;
+  etl::array<uint8_t, sizeof...(PIN)> lightStatesFinal;
+  etl::pair<etl::array<uint8_t, sizeof...(PIN)>, freertos::Mutex>
+      lightStatesCurrent;
 };
 } // namespace LightController