Add nix flake

Provides system-locket platformio

Firmware/flake.lock

@@ -0,0 +1,61 @@
+{
+  "nodes": {
+    "flake-utils": {
+      "inputs": {
+        "systems": "systems"
+      },
+      "locked": {
+        "lastModified": 1701680307,
+        "narHash": "sha256-kAuep2h5ajznlPMD9rnQyffWG8EM/C73lejGofXvdM8=",
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "rev": "4022d587cbbfd70fe950c1e2083a02621806a725",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1611574965,
+        "narHash": "sha256-IYtGS077nJFIqSq6Cb8WEJwbqqd4K7S4vUuwPZKfLeo=",
+        "owner": "nixos",
+        "repo": "nixpkgs-channels",
+        "rev": "4762fba469e2baa82f983b262e2c06ac2fdaae67",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nixos",
+        "ref": "nixos-unstable",
+        "repo": "nixpkgs-channels",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-utils": "flake-utils",
+        "nixpkgs": "nixpkgs"
+      }
+    },
+    "systems": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

Firmware/flake.nix

@@ -0,0 +1,28 @@
+{
+  inputs = {
+    nixpkgs = {
+      url = "github:nixos/nixpkgs-channels/nixos-unstable";
+    };
+    flake-utils = {
+      url = "github:numtide/flake-utils";
+    };
+  };
+  outputs = {
+    nixpkgs,
+    flake-utils,
+    ...
+  }:
+    flake-utils.lib.eachDefaultSystem (
+      system: let
+        pkgs = import nixpkgs {
+          inherit system;
+        };
+      in rec {
+        devShell = pkgs.mkShell {
+          buildInputs = with pkgs; [
+            platformio
+          ];
+        };
+      }
+    );
+}

Firmware/src/Communication/Serial/SerialReceiver.h

@@ -16,7 +16,12 @@ public:
                  UBaseType_t priority, BaseType_t coreID)
       : port{port}, messageQueue{messageQueue},
         serialReceiverTask{
-            exec, "receive serial data", stackDepth, this, priority, coreID} {}
+            exec, "receive serial data", stackDepth, this, priority, coreID} {
+    for (size_t i = 0; i < bufferSize; ++i) {
+      message::Message &msg = buffer.pop();
+      msg.second.give();
+    }
+  }
 
 private:
   static void exec(void *receiverPtr) {
@@ -28,22 +33,32 @@ private:
       receiver->magicNumberBuf.clear();
       receiver->magicNumberBuf.resize(receiver->magicNumber.length());
       size_t availableBytes = receiver->port.available();
-      if (availableBytes >= receiver->magicNumber.length() + 1) {
+      if (receiver->port.available() > 0 && receiver->port.peek() != 'H') {
+        receiver->port.read();
+        receiver->port.println("flush");
+      } else if (availableBytes >= receiver->magicNumber.length() + 1) {
         receiver->port.readBytes(receiver->magicNumberBuf.data(),
                                  receiver->magicNumber.length());
-        receiver->port.println(receiver->magicNumberBuf ==
-                               receiver->magicNumber);
+        receiver->port.println("Received Magic Number:");
+        receiver->port.println(receiver->magicNumberBuf == receiver->magicNumber
+                                   ? "correct"
+                                   : "incorrect");
 
-        uint8_t size;
-        receiver->port.readBytes(&size, sizeof(size));
-        if (receiver->port.available() >= size) {
-          message::Message &msg = receiver->buffer.pop();
-          msg.second.lock();
-          msg.first.clear();
-          msg.first.resize(size);
-          receiver->port.readBytes(msg.first.data(), size);
-          msg.second.unlock();
-          receiver->messageQueue.push(&msg);
+        if (receiver->magicNumberBuf == receiver->magicNumber) {
+          uint8_t size;
+          receiver->port.readBytes(&size, sizeof(size));
+          if (receiver->port.available() >= size) {
+            message::Message &msg = receiver->buffer.pop();
+            receiver->port.printf("Current semaphore count: %u\n",
+                                  msg.second.getCount());
+            msg.second.take();
+            msg.first.clear();
+            msg.first.resize(size);
+            receiver->port.readBytes(msg.first.data(), size);
+            // msg.second.unlock();
+            receiver->port.println("Push message to queue");
+            receiver->messageQueue.push(&msg);
+          }
         }
       }
     }

Firmware/src/FreeRTOS/BinarySemaphore.cpp

@@ -14,11 +14,11 @@ auto freertos::BinarySemaphore::operator=(
 }
 auto freertos::BinarySemaphore::take(TickType_t timeout) -> bool {
   BaseType_t success = xSemaphoreTake(handle, timeout);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }
 auto freertos::BinarySemaphore::give() -> bool {
   BaseType_t success = xSemaphoreGive(handle);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }
 
 auto freertos::BinarySemaphore::getCount() -> size_t {

Firmware/src/FreeRTOS/Mutex.cpp

@@ -12,9 +12,9 @@ auto freertos::Mutex::operator=(const Mutex &&other) noexcept -> Mutex & {
 }
 auto freertos::Mutex::lock(TickType_t timeout) -> bool{
   BaseType_t success = xSemaphoreTake(handle, timeout);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }
 auto freertos::Mutex::unlock() -> bool {
   BaseType_t success = xSemaphoreGive(handle);
-  return success == pdTRUE ? true : false;
+  return success == pdTRUE;
 }

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

Firmware/src/Messages/Composer.h

@@ -1,9 +1,21 @@
 #pragma once
 #include "Protocol.h"
+#include <etl/optional.h>
+#include <etl/span.h>
 #include <etl/string.h>
 
 namespace message {
 namespace composer {
+enum class ComposableType {
+  MESSAGE_LIGHTDATA,
+  MESSAGE_INFO,
+  MESSAGE_WARNING,
+  MESSAGE_ERROR,
+  MESSAGE_SUCCESS,
+  COMMAND_HELP,
+  COMMAND_VERSION,
+};
+using Composable = etl::pair<ComposableType, etl::optional<etl::span<uint8_t>>>;
 bool sendMessage(protocol::Message type, etl::string_view content);
 } // namespace composer
 } // namespace message

Firmware/src/Messages/Message.h

@@ -1,12 +1,13 @@
 #pragma once
 #include <Arduino.h>
-#include <FreeRTOS/Mutex.h>
+#include <FreeRTOS/BinarySemaphore.h>
 #include <Messages/Protocol.h>
 #include <etl/utility.h>
 #include <etl/vector.h>
 
 namespace message {
+/// @brief first: data, second: data ready to reuse
 using Message =
     etl::pair<etl::vector<uint8_t, message::protocol::MAX_PAYLOAD_LEN>,
-              freertos::Mutex>;
+              freertos::BinarySemaphore>;
 } // namespace message

Firmware/src/Messages/Parser.cpp

@@ -1,58 +1,23 @@
 #include "Parser.h"
+#include <FreeRTOS/Util.h>
+#include <PinMap/PinMap.h>
 #include <etl/vector.h>
 
 message::parser::StateVisitor::StateVisitor(
-    etl::byte_stream_reader &&sreader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback)
-    : stream{etl::move(sreader)}, invalidCallback{invalidCallback},
-      messageLightDataCallback{messageLightDataCallback},
-      messageInfoCallback{messageInfoCallback},
-      messageWarningCallback{messageWarningCallback},
-      messageErrorCallback{messageErrorCallback},
-      messageSuccessCallback{messageSuccessCallback},
-      settingsSetBaudCallback{settingsSetBaudCallback},
-      settingsWifiPasswordCallback{settingsWifiPasswordCallback},
-      settingsWifiSSIDCallback{settingsWifiSSIDCallback},
-      lightControlOnCallback{lightControlOnCallback},
-      lightControlOffCallback{lightControlOffCallback},
-      lightControlToggleCallback{lightControlToggleCallback},
-      lightControlIncreaseCallback{lightControlIncreaseCallback},
-      lightControlDecreaseCallback{lightControlDecreaseCallback},
-      commandRequestLightDataCallback{commandRequestLightDataCallback},
-      commandEnterConsoleFlashingCallback{commandEnterConsoleFlashingCallback},
-      commandExitConsoleFlashingCallback{commandExitConsoleFlashingCallback},
-      commandPairBluetoothCallback{commandPairBluetoothCallback},
-      commandHelpCallback{commandHelpCallback}, commandVersionCallback{
-                                                    commandVersionCallback} {}
+    message::Message *message,
+    freertos::Queue<LightController::LightAction> &lightActionQueue)
+    : stream{message->first.begin(), message->first.size(), etl::endian::big},
+      message{message}, lightActionQueue{lightActionQueue} {}
 
 auto message::parser::StateVisitor::operator()(state::ModeSelection) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("ModeSelection: Stream not available");
+    // invalidCallback("ModeSelection: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("ModeSelection: Mode has no value");
+    // invalidCallback("ModeSelection: Mode has no value");
     return state::Invalid{};
   }
 
@@ -66,20 +31,20 @@ auto message::parser::StateVisitor::operator()(state::ModeSelection) -> State {
   case static_cast<uint8_t>(protocol::Mode::Command):
     return state::Command{};
   default:
-    invalidCallback("ModeSelection: Invalid index");
+    // invalidCallback("ModeSelection: Invalid index");
     return state::Invalid{};
   }
 }
 
 auto message::parser::StateVisitor::operator()(state::Message) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Message: Stream not available");
+    // invalidCallback("Message: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Message: Mode has no value");
+    // invalidCallback("Message: Mode has no value");
     return state::Invalid{};
   }
 
@@ -95,7 +60,7 @@ auto message::parser::StateVisitor::operator()(state::Message) -> State {
   case static_cast<uint8_t>(protocol::Message::Success):
     return state::MessageSuccess{};
   default:
-    invalidCallback("Message: Invalid index");
+    // invalidCallback("Message: Invalid index");
     return state::Invalid{};
   }
 }
@@ -103,84 +68,84 @@ auto message::parser::StateVisitor::operator()(state::Message) -> State {
 auto message::parser::StateVisitor::operator()(state::MessageLightData)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageLightData: Stream not available");
+    // invalidCallback("MessageLightData: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageLightData: Message has no value");
+    // invalidCallback("MessageLightData: Message has no value");
     return state::Invalid{};
   }
-  messageLightDataCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageInfo) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageInfo: Stream not available");
+    // invalidCallback("MessageInfo: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageInfo: Message has no value");
+    // invalidCallback("MessageInfo: Message has no value");
     return state::Invalid{};
   }
-  messageInfoCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageWarning) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageWarning: Stream not available");
+    // invalidCallback("MessageWarning: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageWarning: Message has no value");
+    // invalidCallback("MessageWarning: Message has no value");
     return state::Invalid{};
   }
-  messageWarningCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageError) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageError: Stream not available");
+    // invalidCallback("MessageError: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageError: Message has no value");
+    // invalidCallback("MessageError: Message has no value");
     return state::Invalid{};
   }
-  messageErrorCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::MessageSuccess) -> State {
   if (!stream.available<char>()) {
-    invalidCallback("MessageSuccess: Stream not available");
+    // invalidCallback("MessageSuccess: Stream not available");
     return state::Invalid{};
   }
 
   auto msg = stream.read<char>(stream.available<char>());
   if (!msg.has_value()) {
-    invalidCallback("MessageSuccess: Message has no value");
+    // invalidCallback("MessageSuccess: Message has no value");
     return state::Invalid{};
   }
-  messageSuccessCallback(msg.value());
-  return state::Complete{};
+
+  return state::Invalid{};
 }
 
 auto message::parser::StateVisitor::operator()(state::Settings) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Settings: Stream not available");
+    // invalidCallback("Settings: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Settings: Mode has no value");
+    // invalidCallback("Settings: Mode has no value");
     return state::Invalid{};
   }
 
@@ -192,69 +157,66 @@ auto message::parser::StateVisitor::operator()(state::Settings) -> State {
   case static_cast<uint8_t>(protocol::Settings::SetWifiSSID):
     return state::SettingsWifiSSID{};
   default:
-    invalidCallback("Settings: Invalid index");
+    // invalidCallback("Settings: Invalid index");
     return state::Invalid{};
   }
 }
 auto message::parser::StateVisitor::operator()(state::SettingsSetBaud)
     -> State {
   if (!stream.available<uint32_t>()) {
-    invalidCallback("Settings: Stream not available");
+    // invalidCallback("Settings: Stream not available");
     return state::Invalid{};
   }
 
   auto baud = stream.read<uint32_t>();
 
   if (!baud.has_value()) {
-    invalidCallback("SettingsSetBaud: Baud has no value");
+    // invalidCallback("SettingsSetBaud: Baud has no value");
     return state::Invalid{};
   }
 
-  settingsSetBaudCallback(baud.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::SettingsWifiPassword)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("SettingsWifiPassword: Stream not available");
+    // invalidCallback("SettingsWifiPassword: Stream not available");
     return state::Invalid{};
   }
 
   auto passwd = stream.read<char>(stream.available<char>());
   if (!passwd.has_value()) {
-    invalidCallback("SettingsWifiPassword: Passwd has no value");
+    // invalidCallback("SettingsWifiPassword: Passwd has no value");
     return state::Invalid{};
   }
 
-  settingsWifiPasswordCallback(passwd.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::SettingsWifiSSID)
     -> State {
   if (!stream.available<char>()) {
-    invalidCallback("SettingsWifiSSID: Stream not available");
+    // invalidCallback("SettingsWifiSSID: Stream not available");
     return state::Invalid{};
   }
 
   auto ssid = stream.read<char>(stream.available<char>());
   if (!ssid.has_value()) {
-    invalidCallback("SettingsWifiSSID: SSID has no value");
+    // invalidCallback("SettingsWifiSSID: SSID has no value");
     return state::Invalid{};
   }
 
-  settingsWifiSSIDCallback(ssid.value());
-  return state::Complete{};
+  return state::Invalid{};
 }
 
 auto message::parser::StateVisitor::operator()(state::LightControl) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControl: Stream not available");
+    // invalidCallback("LightControl: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("LightControl: Mode has no value");
+    // invalidCallback("LightControl: Mode has no value");
     return state::Invalid{};
   }
 
@@ -270,83 +232,92 @@ auto message::parser::StateVisitor::operator()(state::LightControl) -> State {
   case static_cast<uint8_t>(protocol::LightControl::Decrease):
     return state::LightControlDecrease{};
   default:
-    invalidCallback("LightControl: Invalid index");
+    // invalidCallback("LightControl: Invalid index");
     return state::Invalid{};
   }
 }
 
 auto message::parser::StateVisitor::operator()(state::LightControlOn) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("LightControlOn: Too many bytes left");
+    // invalidCallback("LightControlOn: Too many bytes left");
     return state::Invalid{};
   }
-  lightControlOnCallback();
+
+  message->second.give();
+  Serial.println("ON");
+  lightActionQueue.push({LightController::LightActionType::ON, etl::nullopt});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlOff)
     -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("LightControlOff: Too many bytes left");
+    // invalidCallback("LightControlOff: Too many bytes left");
     return state::Invalid{};
   }
-  lightControlOffCallback();
+
+  message->second.give();
+  Serial.println("OFF");
+  lightActionQueue.push({LightController::LightActionType::OFF, etl::nullopt});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlToggle)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlToggle: Too many bytes left");
+    // invalidCallback("LightControlToggle: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlToggle: Stream not available");
+    // invalidCallback("LightControlToggle: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlToggleCallback(id.value());
+  message->second.give();
+  Serial.println("Toggle");
+  lightActionQueue.push({LightController::LightActionType::TOGGLE, id});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlIncrease)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlIncrease: Too many bytes left");
+    // invalidCallback("LightControlIncrease: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlIncrease: Stream not available");
+    // invalidCallback("LightControlIncrease: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlIncreaseCallback(id.value());
+  message->second.give();
+  Serial.println("Increase");
+  lightActionQueue.push({LightController::LightActionType::INCREASE, id});
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::LightControlDecrease)
     -> State {
   if (stream.available_bytes() > 1) {
-    invalidCallback("LightControlDecrease: Too many bytes left");
+    // invalidCallback("LightControlDecrease: Too many bytes left");
     return state::Invalid{};
   } else if (!stream.available<uint8_t>()) {
-    invalidCallback("LightControlDecrease: Stream not available");
+    // invalidCallback("LightControlDecrease: Stream not available");
     return state::Invalid{};
   }
 
   auto id = stream.read<uint8_t>();
-
-  lightControlDecreaseCallback(id.value());
+  message->second.give();
+  Serial.println("Decrease");
+  lightActionQueue.push({LightController::LightActionType::DECREASE, id});
   return state::Complete{};
 }
 
 auto message::parser::StateVisitor::operator()(state::Command) -> State {
   if (!stream.available<uint8_t>()) {
-    invalidCallback("Command: Stream not available");
+    // invalidCallback("Command: Stream not available");
     return state::Invalid{};
   }
 
   auto mode = stream.read<uint8_t>();
   if (!mode.has_value()) {
-    invalidCallback("Command: Mode has no value");
+    // invalidCallback("Command: Mode has no value");
     return state::Invalid{};
   }
 
@@ -363,59 +334,62 @@ auto message::parser::StateVisitor::operator()(state::Command) -> State {
     return state::CommandHelp{};
   case static_cast<uint8_t>(protocol::Command::Version):
     return state::CommandVersion{};
+  case static_cast<uint8_t>(protocol::Command::Reset):
+    return state::CommandReset{};
   default:
-    invalidCallback("Command: Invalid index");
+    // invalidCallback("Command: Invalid index");
     return state::Invalid{};
   }
 }
 auto message::parser::StateVisitor::operator()(state::CommandRequestLightData)
     -> State {
-  commandRequestLightDataCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(
     state::CommandEnterConsoleFlashing) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandEnterConsoleFlashing: Too many bytes left");
+    // invalidCallback("CommandEnterConsoleFlashing: Too many bytes left");
     return state::Invalid{};
   }
-  commandEnterConsoleFlashingCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(
     state::CommandExitConsoleFlashing) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandExitConsoleFlashing: Too many bytes left");
+    // invalidCallback("CommandExitConsoleFlashing: Too many bytes left");
     return state::Invalid{};
   }
-  commandExitConsoleFlashingCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandPairBluetooth)
     -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandPairBluetooth: Too many bytes left");
+    // invalidCallback("CommandPairBluetooth: Too many bytes left");
     return state::Invalid{};
   }
-  commandPairBluetoothCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandHelp) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandHelp: Too many bytes left");
+    // invalidCallback("CommandHelp: Too many bytes left");
     return state::Invalid{};
   }
-
-  commandHelpCallback();
-  return state::Complete{};
+  return state::Invalid{};
 }
 auto message::parser::StateVisitor::operator()(state::CommandVersion) -> State {
   if (stream.available_bytes() > 0) {
-    invalidCallback("CommandVersion: Too many bytes left");
+    // invalidCallback("CommandVersion: Too many bytes left");
     return state::Invalid{};
   }
+  return state::Invalid{};
+}
 
-  commandVersionCallback();
+auto message::parser::StateVisitor::operator()(state::CommandReset) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandVersion: Too many bytes left");
+    return state::Invalid{};
+  }
+  ESP.restart();
   return state::Complete{};
 }
 
@@ -423,55 +397,35 @@ auto message::parser::StateVisitor::operator()(state::Complete) -> State {
   return state::Complete{};
 }
 auto message::parser::StateVisitor::operator()(state::Invalid) -> State {
-  return state::Invalid{};
+  message->second.give();
+  return state::Complete{};
 }
 
 auto message::parser::parse(
-    etl::byte_stream_reader &&reader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback) -> void {
-  StateVisitor visitor{etl::move(reader),
-                       invalidCallback,
-                       messageLightDataCallback,
-                       messageInfoCallback,
-                       messageWarningCallback,
-                       messageErrorCallback,
-                       messageSuccessCallback,
-                       settingsSetBaudCallback,
-                       settingsWifiPasswordCallback,
-                       settingsWifiSSIDCallback,
-                       lightControlOnCallback,
-                       lightControlOffCallback,
-                       lightControlToggleCallback,
-                       lightControlIncreaseCallback,
-                       lightControlDecreaseCallback,
-                       commandRequestLightDataCallback,
-                       commandEnterConsoleFlashingCallback,
-                       commandExitConsoleFlashingCallback,
-                       commandPairBluetoothCallback,
-                       commandHelpCallback,
-                       commandVersionCallback};
+    message::Message *message,
+    freertos::Queue<LightController::LightAction> &lightActionQueue) -> void {
+  StateVisitor visitor{message, lightActionQueue};
   State state{state::ModeSelection{}};
-  while (!etl::holds_alternative<state::Invalid>(state) &&
-         !etl::holds_alternative<state::Complete>(state)) {
+  while (!etl::holds_alternative<state::Complete>(state)) {
     state = etl::visit(visitor, state);
   }
 }
+
+message::parser::Parser::Parser(
+    freertos::Queue<LightController::LightAction> &lightActionQueue,
+    UBaseType_t queueCapacity, UBaseType_t priority, BaseType_t coreID)
+    : parserTask{exec,  "parse received data", stackDepth, this, priority,
+                 coreID},
+      messages{queueCapacity}, lightActionQueue{lightActionQueue} {}
+
+void message::parser::Parser::exec(void *parserPtr) {
+  Parser *parser = static_cast<Parser *>(parserPtr);
+
+  while (true) {
+    auto message = parser->messages.pop();
+    Serial.println("starting parse");
+    if (message.has_value()) {
+      parse(message.value(), parser->lightActionQueue);
+    }
+  }
+}

Firmware/src/Messages/Parser.h

@@ -1,5 +1,9 @@
 #pragma once
 #include "Protocol.h"
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <LightController/Controller.h>
+#include <Messages/Message.h>
 #include <etl/byte_stream.h>
 #include <etl/function.h>
 #include <etl/span.h>
@@ -34,6 +38,7 @@ struct CommandExitConsoleFlashing {};
 struct CommandPairBluetooth {};
 struct CommandHelp {};
 struct CommandVersion {};
+struct CommandReset {};
 struct Complete {};
 } // namespace state
 
@@ -47,55 +52,16 @@ using State = etl::variant<
     state::LightControlIncrease, state::LightControlDecrease, state::Command,
     state::CommandRequestLightData, state::CommandEnterConsoleFlashing,
     state::CommandExitConsoleFlashing, state::CommandPairBluetooth,
-    state::CommandHelp, state::CommandVersion, state::Complete>;
+    state::CommandHelp, state::CommandVersion, state::CommandReset,
+    state::Complete>;
 
-void parse(
-    etl::byte_stream_reader &&reader,
-    etl::function<void, etl::string_view> invalidCallback,
-    etl::function<void, etl::span<const char>> messageLightDataCallback,
-    etl::function<void, etl::span<const char>> messageInfoCallback,
-    etl::function<void, etl::span<const char>> messageWarningCallback,
-    etl::function<void, etl::span<const char>> messageErrorCallback,
-    etl::function<void, etl::span<const char>> messageSuccessCallback,
-    etl::function<void, uint32_t> settingsSetBaudCallback,
-    etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-    etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-    etl::function<void, void> lightControlOnCallback,
-    etl::function<void, void> lightControlOffCallback,
-    etl::function<void, uint8_t> lightControlToggleCallback,
-    etl::function<void, uint8_t> lightControlIncreaseCallback,
-    etl::function<void, uint8_t> lightControlDecreaseCallback,
-    etl::function<void, void> commandRequestLightDataCallback,
-    etl::function<void, void> commandEnterConsoleFlashingCallback,
-    etl::function<void, void> commandExitConsoleFlashingCallback,
-    etl::function<void, void> commandPairBluetoothCallback,
-    etl::function<void, void> commandHelpCallback,
-    etl::function<void, void> commandVersionCallback);
+void parse(message::Message *message,
+           freertos::Queue<LightController::LightAction> &lightActionQueue);
 
 class StateVisitor {
 public:
-  StateVisitor(
-      etl::byte_stream_reader &&sreader,
-      etl::function<void, etl::string_view> invalidCallback,
-      etl::function<void, etl::span<const char>> messageLightDataCallback,
-      etl::function<void, etl::span<const char>> messageInfoCallback,
-      etl::function<void, etl::span<const char>> messageWarningCallback,
-      etl::function<void, etl::span<const char>> messageErrorCallback,
-      etl::function<void, etl::span<const char>> messageSuccessCallback,
-      etl::function<void, uint32_t> settingsSetBaudCallback,
-      etl::function<void, etl::span<const char>> settingsWifiPasswordCallback,
-      etl::function<void, etl::span<const char>> settingsWifiSSIDCallback,
-      etl::function<void, void> lightControlOnCallback,
-      etl::function<void, void> lightControlOffCallback,
-      etl::function<void, uint8_t> lightControlToggleCallback,
-      etl::function<void, uint8_t> lightControlIncreaseCallback,
-      etl::function<void, uint8_t> lightControlDecreaseCallback,
-      etl::function<void, void> commandRequestLightDataCallback,
-      etl::function<void, void> commandEnterConsoleFlashingCallback,
-      etl::function<void, void> commandExitConsoleFlashingCallback,
-      etl::function<void, void> commandPairBluetoothCallback,
-      etl::function<void, void> commandHelpCallback,
-      etl::function<void, void> commandVersionCallback);
+  StateVisitor(message::Message *message,
+               freertos::Queue<LightController::LightAction> &lightActionQueue);
   State operator()(state::ModeSelection);
   State operator()(state::Message);
   State operator()(state::MessageLightData);
@@ -122,33 +88,29 @@ public:
   State operator()(state::CommandPairBluetooth);
   State operator()(state::CommandHelp);
   State operator()(state::CommandVersion);
+  State operator()(state::CommandReset);
 
   State operator()(state::Complete);
   State operator()(state::Invalid);
 
 private:
   etl::byte_stream_reader stream;
-  // TODO: callback function parameter types
-  etl::function<void, etl::string_view> invalidCallback;
-  etl::function<void, etl::span<const char>> messageLightDataCallback;
-  etl::function<void, etl::span<const char>> messageInfoCallback;
-  etl::function<void, etl::span<const char>> messageWarningCallback;
-  etl::function<void, etl::span<const char>> messageErrorCallback;
-  etl::function<void, etl::span<const char>> messageSuccessCallback;
-  etl::function<void, uint32_t> settingsSetBaudCallback;
-  etl::function<void, etl::span<const char>> settingsWifiPasswordCallback;
-  etl::function<void, etl::span<const char>> settingsWifiSSIDCallback;
-  etl::function<void, void> lightControlOnCallback;
-  etl::function<void, void> lightControlOffCallback;
-  etl::function<void, uint8_t> lightControlToggleCallback;
-  etl::function<void, uint8_t> lightControlIncreaseCallback;
-  etl::function<void, uint8_t> lightControlDecreaseCallback;
-  etl::function<void, void> commandRequestLightDataCallback;
-  etl::function<void, void> commandEnterConsoleFlashingCallback;
-  etl::function<void, void> commandExitConsoleFlashingCallback;
-  etl::function<void, void> commandPairBluetoothCallback;
-  etl::function<void, void> commandHelpCallback;
-  etl::function<void, void> commandVersionCallback;
+  message::Message *message;
+  freertos::Queue<LightController::LightAction> &lightActionQueue;
+};
+
+class Parser {
+public:
+  Parser(freertos::Queue<LightController::LightAction> &lightActionQueue,
+         UBaseType_t queueCapacity, UBaseType_t priority, BaseType_t coreID);
+  freertos::Queue<message::Message *> &getMessageQueue() { return messages; }
+
+private:
+  static void exec(void *parserPtr);
+  freertos::Task parserTask;
+  static const uint32_t stackDepth = 2048;
+  freertos::Queue<message::Message *> messages;
+  freertos::Queue<LightController::LightAction> &lightActionQueue;
 };
 } // namespace parser
 } // namespace message

Firmware/src/Messages/Protocol.h

@@ -47,6 +47,7 @@ enum class Command : uint8_t {
   PairBluetooth = 0x3,
   Help = 0x4,
   Version = 0x5,
+  Reset = 0x6,
 };
 
 using Submode = etl::variant<Mode, Message, Settings, LightControl, Command>;