Implement basic light controller logic

modules/control/Firmware/src/LightController/Controller.h

@@ -0,0 +1,84 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <etl/optional.h>
+#include <etl/span.h>
+
+namespace LightController {
+enum class LightActionType { INCREASE, DECREASE, TOGGLE, ON, OFF };
+using LightAction = etl::pair<LightActionType, etl::optional<uint8_t>>;
+void init(const etl::span<int> pins);
+
+template <int... PIN> class Controller {
+public:
+  Controller(size_t actionQueueSize, UBaseType_t priority, BaseType_t coreID)
+      : lightPins{PIN...}, lightActions{actionQueueSize},
+        lightControllerTask{exec, "control lights", stackDepth,
+                            this, priority,         coreID} {
+    for (auto pin : lightPins) {
+      pinMode(pin, OUTPUT);
+    }
+  }
+
+  freertos::Queue<LightAction> &getActionQueue() { return lightActions; }
+  constexpr size_t lightCount() { return lightPins.size(); }
+  const etl::span<int> getPins() { return lightPins; }
+
+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;
+
+    while (true) {
+      auto action{controller->getActionQueue().pop()};
+      if (action.has_value()) {
+        LightActionType type{action.value().first};
+        etl::optional<uint8_t> index{action.value().second};
+
+        switch (type) {
+        case LightActionType::INCREASE:
+          if (index.has_value() && index.value() < lightStates.size() &&
+              lightStates[index.value()] <= 255 - step) {
+            lightStates[index.value()] += step;
+          }
+          break;
+        case LightActionType::DECREASE:
+          if (index.has_value() && index.value() < lightStates.size() &&
+              lightStates[index.value()] >= 0) {
+            lightStates[index.value()] -= step;
+          }
+          break;
+        case LightActionType::TOGGLE:
+          if (index.has_value() && index.value() < lightStates.size()) {
+            lightStates[index.value()] =
+                lightStates[index.value()] == 0 ? 255 : 0;
+          }
+          break;
+        case LightActionType::ON:
+          lightStates.fill(255);
+          break;
+        case LightActionType::OFF:
+          lightStates.fill(0);
+          break;
+        default:
+          break;
+        }
+
+        for (size_t lightIndex = 0; lightIndex < lightStates.size();
+             ++lightIndex) {
+          analogWrite(controller->getPins()[lightIndex],
+                      lightStates[lightIndex]);
+        }
+      }
+    }
+  }
+
+  static const uint32_t stackDepth = 2048;
+  etl::array<int, sizeof...(PIN)> lightPins;
+  freertos::Queue<LightAction> lightActions;
+  freertos::Task lightControllerTask;
+};
+} // namespace LightController