diff --git a/modules/control/Firmware/include/README b/modules/control/Firmware/include/README
new file mode 100644
index 0000000..194dcd4
--- /dev/null
+++ b/modules/control/Firmware/include/README
@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
diff --git a/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.cpp b/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.cpp
deleted file mode 100644
index e572651..0000000
--- a/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <BluetoothCommunicator.h>
-#include <BluetoothSerial.h>
-
-BluetoothCommunicator::BluetoothCommunicator(BluetoothSerial &p_out, int timeout, __SIZE_TYPE__ bufferSize) : StreamCommunicator(p_out, bufferSize)
-{
-    p_out.begin("Heliox");
-    p_out.setTimeout(timeout);
-}
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.h b/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.h
deleted file mode 100644
index d8ec316..0000000
--- a/modules/control/Firmware/lib/BluetoothCommunicator/BluetoothCommunicator.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <StreamCommunicator.h>
-#include "BluetoothSerial.h"
-
-class BluetoothCommunicator : public StreamCommunicator
-{
-public:
-    BluetoothCommunicator(BluetoothSerial &p_out, int timeout, __SIZE_TYPE__ bufferSize);
-};
diff --git a/modules/control/Firmware/lib/Communicator/Communicator.cpp b/modules/control/Firmware/lib/Communicator/Communicator.cpp
deleted file mode 100644
index 3cd6a57..0000000
--- a/modules/control/Firmware/lib/Communicator/Communicator.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-#include <Communicator.h>
-#include <cstring>
-#include <WString.h>
-
-Communicator::Communicator(__SIZE_TYPE__ bufferSize){
-    this->messageBuffer = new char[bufferSize];
-    this->bufferSize = bufferSize;
-}
-
-void Communicator::sendMessage(int *values, __SIZE_TYPE__ numberOfValues)
-{
-
-}
-
-void Communicator::sendMessage(const char message[])
-{
-    
-}
-
-char *Communicator::receiveMessage()
-{
-
-}
-
-__SIZE_TYPE__ Communicator::calculateMessageOutSize(__SIZE_TYPE__ numberOfValues)
-{
-    return numberOfValues + (numberOfValues - 1) + 1;
-}
-
-void Communicator::parseIDs(const int values[], __SIZE_TYPE__ numberOfValues, char *output)
-{
-    String out = "";
-    __SIZE_TYPE__ outputSize = calculateMessageOutSize(numberOfValues);
-    __SIZE_TYPE__ outputCharPointer = 0;
-
-    for (__SIZE_TYPE__ i = 0; i < numberOfValues; i++)
-    {
-        out += values[i];
-        outputCharPointer++;
-        if (outputCharPointer < outputSize - 1)
-        {
-            out += ',';
-            outputCharPointer++;
-        }
-    }
-    strcpy(output, out.c_str());
-}
-
-char *Communicator::getBuffer()
-{
-    return messageBuffer;
-}
-
-void Communicator::clearBuffer()
-{
-    memset(getBuffer(), '\0', getBufferSize());
-}
-
-int Communicator::getBufferSize()
-{
-    return this->bufferSize;
-}
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/Communicator/Communicator.h b/modules/control/Firmware/lib/Communicator/Communicator.h
deleted file mode 100644
index c0680e6..0000000
--- a/modules/control/Firmware/lib/Communicator/Communicator.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef _COMMUNICATOR_INCLUDED_
-#define _COMMUNICATOR_INCLUDED_
-
-class Communicator
-{
-protected:
-    char *messageBuffer;
-    __SIZE_TYPE__ bufferSize;
-    __SIZE_TYPE__ calculateMessageOutSize(__SIZE_TYPE__ numberOfValues);
-    void parseIDs(const int values[], __SIZE_TYPE__ numberOfValues, char *out);
-
-public:
-    Communicator(__SIZE_TYPE__ bufferSize);
-    virtual void sendMessage(int *values, __SIZE_TYPE__ numberOfValues);
-    virtual void sendMessage(const char message[]);
-    virtual char *receiveMessage();
-    char *getBuffer();
-    void clearBuffer();
-    int getBufferSize();
-};
-#endif
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.cpp b/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.cpp
deleted file mode 100644
index 46676a2..0000000
--- a/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <I2CCommunicator.h>
-#include <Wire.h>
-
-I2CCommunicator::I2CCommunicator(TwoWire &w_out, int slaveAddr, int timeout, __SIZE_TYPE__ bufferSize) : StreamCommunicator(w_out, bufferSize)
-{
-    w_out.begin(slaveAddr);
-    w_out.setTimeout(timeout);
-}
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.h b/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.h
deleted file mode 100644
index 7523faa..0000000
--- a/modules/control/Firmware/lib/I2CCommunicator/I2CCommunicator.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <StreamCommunicator.h>
-#include <Wire.h>
-
-class I2CCommunicator : public StreamCommunicator
-{
-public:
-    I2CCommunicator(TwoWire &w_out, int slaveAddr, int timeout, __SIZE_TYPE__ bufferSize);
-};
diff --git a/modules/control/Firmware/lib/LightController/LightController.cpp b/modules/control/Firmware/lib/LightController/LightController.cpp
deleted file mode 100644
index 9336386..0000000
--- a/modules/control/Firmware/lib/LightController/LightController.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-#include <LightController.h>
-#include <string.h>
-#include <Arduino.h>
-#include <EEPROM.h>
-#include <analogWrite.h>
-
-LightController::LightController(const int bjtPins[], __SIZE_TYPE__ bjtCount)
-{
-    this->bjtCount = bjtCount;
-    this->bjtPins = bjtPins;
-    this->bjtState = new int[bjtCount];
-    EEPROM.begin(64);
-    initializePins();
-    initializeState();
-}
-
-void LightController::initializeState()
-{
-    initializeStateDefaultValues();
-    restoreState();
-}
-
-void LightController::initializePins()
-{
-    for (__SIZE_TYPE__ i = 0; i < bjtCount; i++)
-    {
-        pinMode(bjtPins[i], OUTPUT);
-    }
-}
-
-void LightController::initializeStateDefaultValues()
-{
-    for (__SIZE_TYPE__ i = 0; i < bjtCount; i++)
-    {
-        bjtState[i] = 20;
-    }
-}
-
-void LightController::restoreState()
-{
-    for (__SIZE_TYPE__ i = 0; i < bjtCount; i++)
-    {
-        bjtState[i] = EEPROM.readInt(i * sizeof(bjtState[i]));
-        commitPinState(i);
-    }
-}
-
-void LightController::updateState(const char data[], int steps)
-{
-    for (__SIZE_TYPE__ i = 0; i < bjtCount; i++)
-    {
-        parseRelativeState(data, i, steps);
-        setAbsoluteState(data, i);
-        commitState(i);
-    }
-}
-
-void LightController::parseRelativeState(const char data[], int index, int steps)
-{
-    char numChar[2];
-    itoa(index, numChar, 10);
-
-    if (data[0] == numChar[0])
-    {
-        if (data[1] == 't')
-        {
-            if (bjtState[index] != 0)
-            {
-                bjtState[index] = 0;
-            }
-            else
-            {
-                bjtState[index] = 255;
-            }
-        }
-        else if (data[1] == 'i')
-        {
-            bjtState[index] = clampState(bjtState[index] + steps);
-        }
-        else if (data[1] == 'd')
-        {
-            bjtState[index] = clampState(bjtState[index] - steps);
-        }
-    }
-}
-
-void LightController::setAbsoluteState(const char data[], int index)
-{
-    if (strcmp(data, "off") == 0)
-    {
-        bjtState[index] = 0;
-    }
-
-    if (strcmp(data, "on") == 0)
-    {
-        bjtState[index] = 255;
-    }
-}
-
-int LightController::clampState(int stateValue)
-{
-    int clampedState = stateValue;
-    if (stateValue > 255)
-    {
-        clampedState = 255;
-    }
-    else if (stateValue < 0)
-    {
-        clampedState = 0;
-    }
-    return clampedState;
-}
-
-void LightController::commitState(int index)
-{
-    commitPinState(index);
-    EEPROM.writeInt(index * sizeof(bjtState[index]), bjtState[index]);
-    EEPROM.commit();
-}
-
-void LightController::commitPinState(int index)
-{
-    analogWrite(bjtPins[index], bjtState[index]);
-}
-
-int LightController::getBjtCount()
-{
-    return bjtCount;
-}
-const int *LightController::getBjtPins()
-{
-    return bjtPins;
-}
-int *LightController::getBjtState()
-{
-    return bjtState;
-}
diff --git a/modules/control/Firmware/lib/LightController/LightController.h b/modules/control/Firmware/lib/LightController/LightController.h
deleted file mode 100644
index 6fbded9..0000000
--- a/modules/control/Firmware/lib/LightController/LightController.h
+++ /dev/null
@@ -1,25 +0,0 @@
-class LightController
-{
-protected:
-    __SIZE_TYPE__ bjtCount;
-    const int *bjtPins;
-    int *bjtState;
-
-private:
-    void setAbsoluteState(const char data[], int index);
-    void parseRelativeState(const char data[], int index, int steps);
-    int clampState(int stateValue);
-    void commitState(int index);
-    void commitPinState(int index);
-    void initializeStateDefaultValues();
-    void restoreState();
-    void initializeState();
-    void initializePins();
-
-public:
-    LightController(const int bjtPins[], __SIZE_TYPE__ bjtCount);
-    void updateState(const char data[], int steps);
-    int getBjtCount();
-    const int *getBjtPins();
-    int *getBjtState();
-};
diff --git a/modules/control/Firmware/lib/README b/modules/control/Firmware/lib/README
new file mode 100644
index 0000000..6debab1
--- /dev/null
+++ b/modules/control/Firmware/lib/README
@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html
diff --git a/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.cpp b/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.cpp
deleted file mode 100644
index 0ddac36..0000000
--- a/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <SerialCommunicator.h>
-#include <HardwareSerial.h>
-
-SerialCommunicator::SerialCommunicator(HardwareSerial &p_out, int baudRate, int timeout, __SIZE_TYPE__ bufferSize) : StreamCommunicator(p_out, bufferSize)
-{
-    p_out.begin(baudRate);
-    p_out.setTimeout(timeout);
-}
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.h b/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.h
deleted file mode 100644
index 9c2c050..0000000
--- a/modules/control/Firmware/lib/SerialCommunicator/SerialCommunicator.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#include <StreamCommunicator.h>
-#include <HardwareSerial.h>
-
-class SerialCommunicator : public StreamCommunicator
-{
-public:
-    SerialCommunicator(HardwareSerial &p_out, int baudRate, int timeout, __SIZE_TYPE__ bufferSize);
-};
diff --git a/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.cpp b/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.cpp
deleted file mode 100644
index d0e7dcc..0000000
--- a/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-#include <StreamCommunicator.h>
-
-StreamCommunicator::StreamCommunicator(Stream &s_out, __SIZE_TYPE__ bufferSize) : Communicator(bufferSize), stream(s_out)
-{
-}
-
-void StreamCommunicator::sendMessage(int *values, __SIZE_TYPE__ numberOfValues)
-{
-    char message[calculateMessageOutSize(numberOfValues)];
-    parseIDs(values, numberOfValues, message);
-    sendMessage(message);
-}
-
-void StreamCommunicator::sendMessage(const char message[])
-{
-    stream.println(message);
-}
-
-char *StreamCommunicator::receiveMessage()
-{
-    if (stream.available())
-    {
-        clearBuffer();
-        stream.readBytesUntil('\n', getBuffer(), getBufferSize());
-    }
-    return getBuffer();
-}
diff --git a/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.h b/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.h
deleted file mode 100644
index 6ed298f..0000000
--- a/modules/control/Firmware/lib/StreamCommunicator/StreamCommunicator.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "Stream.h"
-#include "Communicator.h"
-
-#ifndef _STREAM_COMMUNICATOR_INCLUDED_
-#define _STREAM_COMMUNICATOR_INCLUDED_
-
-class StreamCommunicator: public Communicator
-{
-protected:
-    Stream &stream;
-
-public:
-    StreamCommunicator(Stream &s_out, __SIZE_TYPE__ bufferSize);
-    void sendMessage(int *values, __SIZE_TYPE__ numberOfValues) override;
-    void sendMessage(const char message[]) override;
-    char *receiveMessage() override;
-};
-#endif
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.cpp b/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.cpp
deleted file mode 100644
index 2925fca..0000000
--- a/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-#include <WebsocketCommunicator.h>
-#include <ESPAsyncWebServer.h>
-
-WebsocketCommunicator::WebsocketCommunicator(AsyncWebSocket &socket, AsyncWebServer &server, __SIZE_TYPE__ bufferSize) : Communicator(bufferSize), socket(socket), server(server)
-{
-    AwsEventHandler onEvent = [this](AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type,
-                                     void *arg, uint8_t *data, size_t len)
-    {
-        switch (type)
-        {
-        case WS_EVT_CONNECT:
-            Serial.printf("WebSocket client #%u connected from %s\n", client->id(), client->remoteIP().toString().c_str());
-            break;
-        case WS_EVT_DISCONNECT:
-            Serial.printf("WebSocket client #%u disconnected\n", client->id());
-            break;
-        case WS_EVT_DATA:
-            handleMessage(arg, data, len);
-            break;
-        case WS_EVT_PONG:
-        case WS_EVT_ERROR:
-            break;
-        }
-    };
-    msgRead = false;
-    socket.onEvent(onEvent);
-    server.addHandler(&socket);
-}
-
-void WebsocketCommunicator::sendMessage(int *values, __SIZE_TYPE__ numberOfValues)
-{
-    char message[calculateMessageOutSize(numberOfValues)];
-    parseIDs(values, numberOfValues, message);
-    sendMessage(message);
-}
-
-void WebsocketCommunicator::sendMessage(const char message[])
-{
-    socket.textAll(message);
-}
-
-char *WebsocketCommunicator::receiveMessage()
-{
-    msgRead = true;
-    return getBuffer();
-}
-
-void WebsocketCommunicator::clearBufferSafely()
-{
-    if (msgRead)
-    {
-        clearBuffer();
-    }
-}
-
-void WebsocketCommunicator::handleMessage(void *arg, uint8_t *data, size_t len)
-{
-    msgRead = false;
-    int effectiveLen = len < bufferSize ? len : bufferSize;
-    strncpy(messageBuffer, (char *)data, effectiveLen);
-}
\ No newline at end of file
diff --git a/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.h b/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.h
deleted file mode 100644
index b7b620d..0000000
--- a/modules/control/Firmware/lib/WebsocketCommunicator/WebsocketCommunicator.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#include <Communicator.h>
-#include <ESPAsyncWebServer.h>
-
-class WebsocketCommunicator : public Communicator
-{
-private:
-    void handleMessage(void *arg, uint8_t *data, size_t len);
-    bool msgRead;
-
-protected:
-    AsyncWebSocket &socket;
-    AsyncWebServer &server;
-
-public:
-    WebsocketCommunicator(AsyncWebSocket &socket, AsyncWebServer &server, __SIZE_TYPE__ bufferSize);
-    void sendMessage(int *values, __SIZE_TYPE__ numberOfValues) override;
-    void sendMessage(const char message[]) override;
-    char *receiveMessage() override;
-    void clearBufferSafely();
-};
\ No newline at end of file
diff --git a/modules/control/Firmware/platformio.ini b/modules/control/Firmware/platformio.ini
index ca399d9..a810fe6 100644
--- a/modules/control/Firmware/platformio.ini
+++ b/modules/control/Firmware/platformio.ini
@@ -1,28 +1,21 @@
-; PlatformIO Project Configuration File
-;
-;   Build options: build flags, source filter
-;   Upload options: custom upload port, speed and extra flags
-;   Library options: dependencies, extra library storages
-;   Advanced options: extra scripting
-;
-; Please visit documentation for the other options and examples
-; https://docs.platformio.org/page/projectconf.html
-
-[platformio]
-default_envs = Upload_UART
-
-[env]
-platform = espressif32
-board = az-delivery-devkit-v4
-framework = arduino
-lib_deps = 
-	erropix/ESP32 AnalogWrite@^0.2
-	ESP Async WebServer
-board_build.partitions = huge_app.csv
-
-[env:Upload_UART]
-upload_port = COM9
-monitor_port = COM9
-
-[env:CI_Build]
-build_flags = -D DEBUG=1
\ No newline at end of file
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32dev]
+platform = espressif32
+board = esp32dev
+framework = arduino
+board_build.partitions = huge_app.csv
+lib_deps = etlcpp/Embedded Template Library@^20.35.12
+monitor_speed=115200
+build_flags=-Wall
+check_tool = clangtidy
+check_flags =
+    clangtidy: --checks=-*,clang-diagnostic-*,-clang-diagnostic-unused-value,clang-analyzer-*,-*,bugprone-*,performance-*,readability-*,-readability-magic-numbers,-readability-braces-around-statements,-readability-inconsistent-declaration-parameter-name,-readability-named-parameter --fix
diff --git a/modules/control/Firmware/src/Communication/Serial/SerialReceiver.h b/modules/control/Firmware/src/Communication/Serial/SerialReceiver.h
new file mode 100644
index 0000000..457e59f
--- /dev/null
+++ b/modules/control/Firmware/src/Communication/Serial/SerialReceiver.h
@@ -0,0 +1,76 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <FreeRTOS/Util.h>
+#include <Messages/Message.h>
+#include <Util/CircularBuffer.h>
+#include <etl/string.h>
+
+namespace comm {
+namespace serial {
+template <size_t bufferSize = 20> class SerialReceiver {
+public:
+  SerialReceiver(HardwareSerial &port,
+                 freertos::Queue<message::Message *> &messageQueue,
+                 UBaseType_t priority, BaseType_t coreID)
+      : port{port}, messageQueue{messageQueue},
+        serialReceiverTask{
+            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) {
+    freertos::sleep(20); // wait until Serial port has been initialized
+    SerialReceiver<bufferSize> *receiver =
+        static_cast<SerialReceiver<bufferSize> *>(receiverPtr);
+
+    while (true) {
+      receiver->magicNumberBuf.clear();
+      receiver->magicNumberBuf.resize(receiver->magicNumber.length());
+      size_t availableBytes = receiver->port.available();
+      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("Received Magic Number:");
+        receiver->port.println(receiver->magicNumberBuf == receiver->magicNumber
+                                   ? "correct"
+                                   : "incorrect");
+
+        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);
+          }
+        }
+      }
+    }
+  }
+
+  HardwareSerial &port;
+  freertos::Queue<message::Message *> &messageQueue;
+  etl::string<4> magicNumberBuf;
+  const etl::string<2> magicNumber{"HX"};
+  util::CircularBuffer<message::Message, bufferSize> buffer;
+  freertos::Task serialReceiverTask;
+  static const uint32_t stackDepth = 2048;
+};
+} // namespace serial
+} // namespace comm
diff --git a/modules/control/Firmware/src/Communication/Serial/SerialSender.cpp b/modules/control/Firmware/src/Communication/Serial/SerialSender.cpp
new file mode 100644
index 0000000..b687f31
--- /dev/null
+++ b/modules/control/Firmware/src/Communication/Serial/SerialSender.cpp
@@ -0,0 +1,24 @@
+#include <Communication/Serial/SerialSender.h>
+
+comm::serial::SerialSender::SerialSender(HardwareSerial &port,
+                                         UBaseType_t queueCapacity,
+                                         UBaseType_t priority,
+                                         BaseType_t coreID)
+    : port{port}, messages{queueCapacity}, SerialSenderTask{
+                                               exec,       "send serial data",
+                                               stackDepth, this,
+                                               priority,   coreID} {}
+
+void comm::serial::SerialSender::exec(void *senderPtr) {
+  SerialSender *sender = static_cast<SerialSender *>(senderPtr);
+  freertos::sleep(20); // wait until Serial port has been initialized
+  while (true) {
+    if (!sender->port.availableForWrite()) {
+      continue;
+    }
+    auto message = sender->messages.pop();
+    if (message.has_value()) {
+      sender->port.write(message.value().data(), message.value().size());
+    }
+  }
+}
\ No newline at end of file
diff --git a/modules/control/Firmware/src/Communication/Serial/SerialSender.h b/modules/control/Firmware/src/Communication/Serial/SerialSender.h
new file mode 100644
index 0000000..1ed80c0
--- /dev/null
+++ b/modules/control/Firmware/src/Communication/Serial/SerialSender.h
@@ -0,0 +1,27 @@
+#pragma once
+#include <Arduino.h>
+#include <FreeRTOS/Queue.h>
+#include <FreeRTOS/Task.h>
+#include <FreeRTOS/Util.h>
+#include <Messages/Message.h>
+#include <Util/CircularBuffer.h>
+#include <etl/span.h>
+#include <etl/string.h>
+
+namespace comm {
+namespace serial {
+class SerialSender {
+public:
+  SerialSender(HardwareSerial &port, UBaseType_t queueCapacity,
+               UBaseType_t priority, BaseType_t coreID);
+  freertos::Queue<etl::span<uint8_t>> &getMessageQueue() { return messages; }
+
+private:
+  static void exec(void *senderPtr);
+  HardwareSerial &port;
+  freertos::Queue<etl::span<uint8_t>> messages;
+  freertos::Task SerialSenderTask;
+  static const uint32_t stackDepth = 2048;
+};
+} // namespace serial
+} // namespace comm
diff --git a/modules/control/Firmware/include/Credentials/CredentialManager.h b/modules/control/Firmware/src/Credentials/CredentialManager.h
similarity index 71%
rename from modules/control/Firmware/include/Credentials/CredentialManager.h
rename to modules/control/Firmware/src/Credentials/CredentialManager.h
index 3d78db0..fce49f9 100644
--- a/modules/control/Firmware/include/Credentials/CredentialManager.h
+++ b/modules/control/Firmware/src/Credentials/CredentialManager.h
@@ -1,5 +1,5 @@
 #ifndef DEBUG
-#include <Credentials/Credentials.h>
+#include "Credentials/Credentials.h"
 #endif
 #ifdef DEBUG
 #define WIFI_SSID "ssid"
diff --git a/modules/control/Firmware/src/Credentials/Credentials.h b/modules/control/Firmware/src/Credentials/Credentials.h
new file mode 100644
index 0000000..e69de29
diff --git a/modules/control/Firmware/include/Credentials/Credentials.h.template b/modules/control/Firmware/src/Credentials/Credentials.h.template
similarity index 100%
rename from modules/control/Firmware/include/Credentials/Credentials.h.template
rename to modules/control/Firmware/src/Credentials/Credentials.h.template
diff --git a/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.cpp b/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.cpp
new file mode 100644
index 0000000..a0fb7d6
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.cpp
@@ -0,0 +1,26 @@
+#include <FreeRTOS/BinarySemaphore.h>
+#include <etl/utility.h>
+
+freertos::BinarySemaphore::BinarySemaphore()
+    : handle{xSemaphoreCreateBinary()} {}
+freertos::BinarySemaphore::BinarySemaphore(BinarySemaphore &&other) noexcept {
+  handle = etl::move(other.handle);
+}
+freertos::BinarySemaphore::~BinarySemaphore() { vSemaphoreDelete(handle); }
+auto freertos::BinarySemaphore::operator=(
+    const BinarySemaphore &&other) noexcept -> BinarySemaphore & {
+  handle = etl::move(other.handle);
+  return *this;
+}
+auto freertos::BinarySemaphore::take(TickType_t timeout) -> bool {
+  BaseType_t success = xSemaphoreTake(handle, timeout);
+  return success == pdTRUE;
+}
+auto freertos::BinarySemaphore::give() -> bool {
+  BaseType_t success = xSemaphoreGive(handle);
+  return success == pdTRUE;
+}
+
+auto freertos::BinarySemaphore::getCount() -> size_t {
+  return uxSemaphoreGetCount(handle);
+}
\ No newline at end of file
diff --git a/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.h b/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.h
new file mode 100644
index 0000000..08fb38d
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/BinarySemaphore.h
@@ -0,0 +1,20 @@
+#pragma once
+#include <Arduino.h>
+
+namespace freertos {
+class BinarySemaphore {
+public:
+  BinarySemaphore();
+  BinarySemaphore(const BinarySemaphore &other) = delete;
+  BinarySemaphore(BinarySemaphore &&other) noexcept;
+  ~BinarySemaphore();
+  BinarySemaphore &operator=(const BinarySemaphore &other) = delete;
+  BinarySemaphore &operator=(const BinarySemaphore &&other) noexcept;
+  bool take(TickType_t timeout = portMAX_DELAY);
+  bool give();
+  size_t getCount();
+
+private:
+  SemaphoreHandle_t handle;
+};
+} // namespace freertos
diff --git a/modules/control/Firmware/src/FreeRTOS/Mutex.cpp b/modules/control/Firmware/src/FreeRTOS/Mutex.cpp
new file mode 100644
index 0000000..b1786a1
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Mutex.cpp
@@ -0,0 +1,20 @@
+#include "Mutex.h"
+#include <etl/utility.h>
+
+freertos::Mutex::Mutex() : handle{xSemaphoreCreateMutex()} {}
+freertos::Mutex::Mutex(Mutex &&other) noexcept {
+  handle = etl::move(other.handle);
+}
+freertos::Mutex::~Mutex() { vSemaphoreDelete(handle); }
+auto freertos::Mutex::operator=(const Mutex &&other) noexcept -> Mutex & {
+  handle = etl::move(other.handle);
+  return *this;
+}
+auto freertos::Mutex::lock(TickType_t timeout) -> bool{
+  BaseType_t success = xSemaphoreTake(handle, timeout);
+  return success == pdTRUE;
+}
+auto freertos::Mutex::unlock() -> bool {
+  BaseType_t success = xSemaphoreGive(handle);
+  return success == pdTRUE;
+}
\ No newline at end of file
diff --git a/modules/control/Firmware/src/FreeRTOS/Mutex.h b/modules/control/Firmware/src/FreeRTOS/Mutex.h
new file mode 100644
index 0000000..77ec4b8
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Mutex.h
@@ -0,0 +1,19 @@
+#pragma once
+#include <Arduino.h>
+
+namespace freertos {
+class Mutex {
+public:
+  Mutex();
+  Mutex(const Mutex &other) = delete;
+  Mutex(Mutex &&other) noexcept;
+  ~Mutex();
+  Mutex &operator=(const Mutex &other) = delete;
+  Mutex &operator=(const Mutex &&other) noexcept;
+  bool lock(TickType_t timeout = portMAX_DELAY);
+  bool unlock();
+
+private:
+  SemaphoreHandle_t handle;
+};
+} // namespace freertos
diff --git a/modules/control/Firmware/src/FreeRTOS/Queue.h b/modules/control/Firmware/src/FreeRTOS/Queue.h
new file mode 100644
index 0000000..99f2fa0
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Queue.h
@@ -0,0 +1,44 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/optional.h>
+namespace freertos {
+template <typename T> class Queue {
+public:
+  Queue(UBaseType_t capacity) : queueCapacity{capacity} {
+    handle = xQueueCreate(capacity, sizeof(T));
+  }
+  Queue(const Queue &other) = delete;
+  Queue(Queue &&other)
+      : handle{etl::move(other.handle)}, queueCapacity{
+                                             etl::move(other.queueCapacity)} {}
+  ~Queue() { vQueueDelete(handle); }
+  Queue &operator=(const Queue &other) = delete;
+  Queue &operator=(const Queue &&other) {
+    handle = etl::move(other.handle);
+    queueCapacity = etl::move(other.queueCapacity);
+  }
+
+  etl::optional<T> peek(const TickType_t ticksToWait = portMAX_DELAY) {
+    T buf;
+    BaseType_t status = xQueuePeek(handle, &buf, ticksToWait);
+    return status == pdTRUE ? etl::make_optional<T>(buf) : etl::nullopt;
+  }
+  size_t size() { return uxQueueMessagesWaiting(handle); }
+  size_t available() { return uxQueueSpacesAvailable(handle); }
+  size_t capacity() { return queueCapacity; }
+  bool push(const T item, const TickType_t ticksToWait = portMAX_DELAY) {
+    return xQueueSend(handle, &item, ticksToWait) == pdTRUE;
+  }
+  etl::optional<T> pop(const TickType_t ticksToWait = portMAX_DELAY) {
+    T buf;
+    BaseType_t status = xQueueReceive(handle, &buf, ticksToWait);
+    return status == pdTRUE ? etl::make_optional<T>(buf) : etl::nullopt;
+  }
+  void clear() { xQueueReset(handle); }
+  bool isValid() { return handle != NULL; }
+
+private:
+  QueueHandle_t handle;
+  UBaseType_t queueCapacity;
+};
+} // namespace freertos
diff --git a/modules/control/Firmware/src/FreeRTOS/Task.cpp b/modules/control/Firmware/src/FreeRTOS/Task.cpp
new file mode 100644
index 0000000..acd1a89
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Task.cpp
@@ -0,0 +1,43 @@
+#include "Task.h"
+
+freertos::Task::Task(TaskFunction_t taskFunction, const etl::string_view name,
+                     const uint32_t stackDepth, void *const params,
+                     UBaseType_t priority, const BaseType_t coreID)
+    : name{name}, stackDepth{stackDepth}, priority{priority}, coreID{coreID} {
+  status = xTaskCreatePinnedToCore(taskFunction, name.data(), stackDepth,
+                                   params, priority, &handle, coreID);
+}
+
+freertos::Task::Task(Task &&other) noexcept
+    : name{etl::move(other.name)}, handle{etl::move(other.handle)},
+      stackDepth{etl::move(other.stackDepth)},
+      priority{etl::move(other.priority)}, coreID{etl::move(other.coreID)} {}
+
+freertos::Task::~Task() {
+  if (status == pdPASS) {
+    vTaskDelete(handle);
+  }
+}
+
+auto freertos::Task::operator=(const Task &&other) noexcept -> Task & {
+  name = etl::move(other.name);
+  handle = etl::move(other.handle);
+  stackDepth = etl::move(other.stackDepth);
+  priority = etl::move(other.priority);
+  coreID = etl::move(other.coreID);
+  return *this;
+}
+
+auto freertos::Task::getName() const -> etl::string_view { return name; }
+
+auto freertos::Task::getStackDepth() const -> uint32_t { return stackDepth; }
+
+auto freertos::Task::getPriority() const -> UBaseType_t { return priority; }
+
+auto freertos::Task::setPriority(const UBaseType_t newPriority) -> void {
+  vTaskPrioritySet(handle, newPriority);
+}
+
+auto freertos::Task::getCoreID() const -> BaseType_t { return coreID; }
+
+auto freertos::Task::isValid() const -> bool { return status == pdPASS; }
\ No newline at end of file
diff --git a/modules/control/Firmware/src/FreeRTOS/Task.h b/modules/control/Firmware/src/FreeRTOS/Task.h
new file mode 100644
index 0000000..fce288e
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Task.h
@@ -0,0 +1,32 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/string.h>
+
+namespace freertos {
+class Task {
+public:
+  Task(TaskFunction_t taskFunction, const etl::string_view name,
+       const uint32_t stackDepth, void *const params, UBaseType_t priority,
+       const BaseType_t coreID);
+  Task(const Task &other) = delete;
+  Task(Task &&other) noexcept;
+  ~Task();
+  Task &operator=(const Task &other) = delete;
+  Task &operator=(const Task &&other) noexcept;
+
+  etl::string_view getName() const;
+  uint32_t getStackDepth() const;
+  UBaseType_t getPriority() const;
+  void setPriority(const UBaseType_t newPriority);
+  BaseType_t getCoreID() const;
+  bool isValid() const;
+
+private:
+  etl::string_view name;
+  TaskHandle_t handle;
+  uint32_t stackDepth;
+  UBaseType_t priority;
+  BaseType_t coreID;
+  BaseType_t status;
+};
+} // namespace freertos
diff --git a/modules/control/Firmware/src/FreeRTOS/Util.cpp b/modules/control/Firmware/src/FreeRTOS/Util.cpp
new file mode 100644
index 0000000..2f9babb
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Util.cpp
@@ -0,0 +1,3 @@
+#include "Util.h"
+void freertos::sleep(int time) { vTaskDelay(time * portTICK_PERIOD_MS); }
+void freertos::sleepForever() { vTaskDelay(portMAX_DELAY); }
\ No newline at end of file
diff --git a/modules/control/Firmware/src/FreeRTOS/Util.h b/modules/control/Firmware/src/FreeRTOS/Util.h
new file mode 100644
index 0000000..c715747
--- /dev/null
+++ b/modules/control/Firmware/src/FreeRTOS/Util.h
@@ -0,0 +1,11 @@
+#pragma once
+#include <Arduino.h>
+
+namespace freertos {
+/// @brief Blocks the task for the specified amount of time
+/// @param time in milliseconds
+void sleep(int time);
+
+/// @brief Sleep for the maximum delay possible
+void sleepForever();
+} // namespace freertos
diff --git a/modules/control/Firmware/src/LightController/Controller.h b/modules/control/Firmware/src/LightController/Controller.h
new file mode 100644
index 0000000..a9c1a3d
--- /dev/null
+++ b/modules/control/Firmware/src/LightController/Controller.h
@@ -0,0 +1,126 @@
+#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>
+
+namespace LightController {
+enum class LightActionType { INCREASE, DECREASE, TOGGLE, ON, OFF };
+using LightAction = etl::pair<LightActionType, etl::optional<uint8_t>>;
+
+template <int... PIN> class Controller {
+public:
+  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, 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; }
+  constexpr size_t lightCount() { return lightPins.size(); }
+  const etl::span<int> getPins() { return lightPins; }
+
+private:
+  static void exec(void *controllerPtr) {
+    Controller *controller{static_cast<Controller *>(controllerPtr)};
+
+    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() < 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() < controller->lightStatesFinal.size() &&
+              controller->lightStatesFinal[index.value()] >= 0) {
+            controller->lightStatesFinal[index.value()] -= controller->step;
+          }
+          break;
+        case LightActionType::TOGGLE:
+          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:
+          controller->lightStatesCurrent.second.lock();
+          controller->lightStatesFinal.fill(255);
+          controller->lightStatesCurrent.first.fill(255);
+          controller->lightStatesCurrent.second.unlock();
+          break;
+        case LightActionType::OFF:
+          controller->lightStatesCurrent.second.lock();
+          controller->lightStatesFinal.fill(0);
+          controller->lightStatesCurrent.first.fill(0);
+          controller->lightStatesCurrent.second.unlock();
+          break;
+        default:
+          break;
+        }
+      }
+    }
+  }
+
+  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
diff --git a/modules/control/Firmware/src/Messages/Composer.cpp b/modules/control/Firmware/src/Messages/Composer.cpp
new file mode 100644
index 0000000..87d2428
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Composer.cpp
@@ -0,0 +1,15 @@
+#include "Composer.h"
+
+bool message::composer::sendMessage(protocol::Message type,
+                                    etl::string_view content) {
+  if (content.length() > protocol::MAX_PAYLOAD_LEN) {
+    return false;
+  }
+  uint8_t length =
+      protocol::HEADER_LEN + static_cast<uint8_t>(content.length());
+  // Serial.write("HX");
+  // Serial.write(length);
+  // Serial.write(protocol::to_underlying(type));
+  // Serial.write(content.data());
+  return true;
+};
diff --git a/modules/control/Firmware/src/Messages/Composer.h b/modules/control/Firmware/src/Messages/Composer.h
new file mode 100644
index 0000000..dcbd61f
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Composer.h
@@ -0,0 +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
diff --git a/modules/control/Firmware/src/Messages/Message.h b/modules/control/Firmware/src/Messages/Message.h
new file mode 100644
index 0000000..af77ffc
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Message.h
@@ -0,0 +1,13 @@
+#pragma once
+#include <Arduino.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::BinarySemaphore>;
+} // namespace message
diff --git a/modules/control/Firmware/src/Messages/Parser.cpp b/modules/control/Firmware/src/Messages/Parser.cpp
new file mode 100644
index 0000000..500b448
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Parser.cpp
@@ -0,0 +1,431 @@
+#include "Parser.h"
+#include <FreeRTOS/Util.h>
+#include <PinMap/PinMap.h>
+#include <etl/vector.h>
+
+message::parser::StateVisitor::StateVisitor(
+    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");
+    return state::Invalid{};
+  }
+
+  auto mode = stream.read<uint8_t>();
+  if (!mode.has_value()) {
+    // invalidCallback("ModeSelection: Mode has no value");
+    return state::Invalid{};
+  }
+
+  switch (mode.value()) {
+  case static_cast<uint8_t>(protocol::Mode::Message):
+    return state::Message{};
+  case static_cast<uint8_t>(protocol::Mode::Settings):
+    return state::Settings{};
+  case static_cast<uint8_t>(protocol::Mode::LightControl):
+    return state::LightControl{};
+  case static_cast<uint8_t>(protocol::Mode::Command):
+    return state::Command{};
+  default:
+    // 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");
+    return state::Invalid{};
+  }
+
+  auto mode = stream.read<uint8_t>();
+  if (!mode.has_value()) {
+    // invalidCallback("Message: Mode has no value");
+    return state::Invalid{};
+  }
+
+  switch (mode.value()) {
+  case static_cast<uint8_t>(protocol::Message::LightData):
+    return state::MessageLightData{};
+  case static_cast<uint8_t>(protocol::Message::Info):
+    return state::MessageInfo{};
+  case static_cast<uint8_t>(protocol::Message::Warning):
+    return state::MessageWarning{};
+  case static_cast<uint8_t>(protocol::Message::Error):
+    return state::MessageError{};
+  case static_cast<uint8_t>(protocol::Message::Success):
+    return state::MessageSuccess{};
+  default:
+    // invalidCallback("Message: Invalid index");
+    return state::Invalid{};
+  }
+}
+
+auto message::parser::StateVisitor::operator()(state::MessageLightData)
+    -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::MessageInfo) -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::MessageWarning) -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::MessageError) -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::MessageSuccess) -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+
+auto message::parser::StateVisitor::operator()(state::Settings) -> State {
+  if (!stream.available<uint8_t>()) {
+    // invalidCallback("Settings: Stream not available");
+    return state::Invalid{};
+  }
+
+  auto mode = stream.read<uint8_t>();
+  if (!mode.has_value()) {
+    // invalidCallback("Settings: Mode has no value");
+    return state::Invalid{};
+  }
+
+  switch (mode.value()) {
+  case static_cast<uint8_t>(protocol::Settings::SetBaud):
+    return state::SettingsSetBaud{};
+  case static_cast<uint8_t>(protocol::Settings::SetWifiPassword):
+    return state::SettingsWifiPassword{};
+  case static_cast<uint8_t>(protocol::Settings::SetWifiSSID):
+    return state::SettingsWifiSSID{};
+  default:
+    // 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");
+    return state::Invalid{};
+  }
+
+  auto baud = stream.read<uint32_t>();
+
+  if (!baud.has_value()) {
+    // invalidCallback("SettingsSetBaud: Baud has no value");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::SettingsWifiPassword)
+    -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::SettingsWifiSSID)
+    -> State {
+  if (!stream.available<char>()) {
+    // 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");
+    return state::Invalid{};
+  }
+
+  return state::Invalid{};
+}
+
+auto message::parser::StateVisitor::operator()(state::LightControl) -> State {
+  if (!stream.available<uint8_t>()) {
+    // invalidCallback("LightControl: Stream not available");
+    return state::Invalid{};
+  }
+
+  auto mode = stream.read<uint8_t>();
+  if (!mode.has_value()) {
+    // invalidCallback("LightControl: Mode has no value");
+    return state::Invalid{};
+  }
+
+  switch (mode.value()) {
+  case static_cast<uint8_t>(protocol::LightControl::On):
+    return state::LightControlOn{};
+  case static_cast<uint8_t>(protocol::LightControl::Off):
+    return state::LightControlOff{};
+  case static_cast<uint8_t>(protocol::LightControl::Toggle):
+    return state::LightControlToggle{};
+  case static_cast<uint8_t>(protocol::LightControl::Increase):
+    return state::LightControlIncrease{};
+  case static_cast<uint8_t>(protocol::LightControl::Decrease):
+    return state::LightControlDecrease{};
+  default:
+    // 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");
+    return state::Invalid{};
+  }
+
+  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");
+    return state::Invalid{};
+  }
+
+  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");
+    return state::Invalid{};
+  } else if (!stream.available<uint8_t>()) {
+    // invalidCallback("LightControlToggle: Stream not available");
+    return state::Invalid{};
+  }
+
+  auto id = stream.read<uint8_t>();
+  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");
+    return state::Invalid{};
+  } else if (!stream.available<uint8_t>()) {
+    // invalidCallback("LightControlIncrease: Stream not available");
+    return state::Invalid{};
+  }
+
+  auto id = stream.read<uint8_t>();
+  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");
+    return state::Invalid{};
+  } else if (!stream.available<uint8_t>()) {
+    // invalidCallback("LightControlDecrease: Stream not available");
+    return state::Invalid{};
+  }
+
+  auto id = stream.read<uint8_t>();
+  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");
+    return state::Invalid{};
+  }
+
+  auto mode = stream.read<uint8_t>();
+  if (!mode.has_value()) {
+    // invalidCallback("Command: Mode has no value");
+    return state::Invalid{};
+  }
+
+  switch (mode.value()) {
+  case static_cast<uint8_t>(protocol::Command::RequestLightData):
+    return state::CommandRequestLightData{};
+  case static_cast<uint8_t>(protocol::Command::EnterConsoleFlashing):
+    return state::CommandEnterConsoleFlashing{};
+  case static_cast<uint8_t>(protocol::Command::ExitConsoleFlashing):
+    return state::CommandExitConsoleFlashing{};
+  case static_cast<uint8_t>(protocol::Command::PairBluetooth):
+    return state::CommandPairBluetooth{};
+  case static_cast<uint8_t>(protocol::Command::Help):
+    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");
+    return state::Invalid{};
+  }
+}
+auto message::parser::StateVisitor::operator()(state::CommandRequestLightData)
+    -> State {
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(
+    state::CommandEnterConsoleFlashing) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandEnterConsoleFlashing: Too many bytes left");
+    return state::Invalid{};
+  }
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(
+    state::CommandExitConsoleFlashing) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandExitConsoleFlashing: Too many bytes left");
+    return state::Invalid{};
+  }
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::CommandPairBluetooth)
+    -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandPairBluetooth: Too many bytes left");
+    return state::Invalid{};
+  }
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::CommandHelp) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandHelp: Too many bytes left");
+    return state::Invalid{};
+  }
+  return state::Invalid{};
+}
+auto message::parser::StateVisitor::operator()(state::CommandVersion) -> State {
+  if (stream.available_bytes() > 0) {
+    // invalidCallback("CommandVersion: Too many bytes left");
+    return state::Invalid{};
+  }
+  return state::Invalid{};
+}
+
+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{};
+}
+
+auto message::parser::StateVisitor::operator()(state::Complete) -> State {
+  return state::Complete{};
+}
+auto message::parser::StateVisitor::operator()(state::Invalid) -> State {
+  message->second.give();
+  return state::Complete{};
+}
+
+auto message::parser::parse(
+    message::Message *message,
+    freertos::Queue<LightController::LightAction> &lightActionQueue) -> void {
+  StateVisitor visitor{message, lightActionQueue};
+  State state{state::ModeSelection{}};
+  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);
+    }
+  }
+}
\ No newline at end of file
diff --git a/modules/control/Firmware/src/Messages/Parser.h b/modules/control/Firmware/src/Messages/Parser.h
new file mode 100644
index 0000000..dac6ef5
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Parser.h
@@ -0,0 +1,116 @@
+#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>
+#include <etl/string_view.h>
+#include <etl/variant.h>
+
+namespace message {
+namespace parser {
+namespace state {
+struct Invalid {};
+struct ModeSelection {};
+struct Message {};
+struct MessageLightData {};
+struct MessageInfo {};
+struct MessageWarning {};
+struct MessageError {};
+struct MessageSuccess {};
+struct Settings {};
+struct SettingsSetBaud {};
+struct SettingsWifiPassword {};
+struct SettingsWifiSSID {};
+struct LightControl {};
+struct LightControlOn {};
+struct LightControlOff {};
+struct LightControlToggle {};
+struct LightControlIncrease {};
+struct LightControlDecrease {};
+struct Command {};
+struct CommandRequestLightData {};
+struct CommandEnterConsoleFlashing {};
+struct CommandExitConsoleFlashing {};
+struct CommandPairBluetooth {};
+struct CommandHelp {};
+struct CommandVersion {};
+struct CommandReset {};
+struct Complete {};
+} // namespace state
+
+using State = etl::variant<
+    state::Invalid, state::ModeSelection, state::Message,
+    state::MessageLightData, state::MessageInfo, state::MessageWarning,
+    state::MessageError, state::MessageSuccess, state::Settings,
+    state::SettingsSetBaud, state::SettingsWifiPassword,
+    state::SettingsWifiSSID, state::LightControl, state::LightControlOn,
+    state::LightControlOff, state::LightControlToggle,
+    state::LightControlIncrease, state::LightControlDecrease, state::Command,
+    state::CommandRequestLightData, state::CommandEnterConsoleFlashing,
+    state::CommandExitConsoleFlashing, state::CommandPairBluetooth,
+    state::CommandHelp, state::CommandVersion, state::CommandReset,
+    state::Complete>;
+
+void parse(message::Message *message,
+           freertos::Queue<LightController::LightAction> &lightActionQueue);
+
+class StateVisitor {
+public:
+  StateVisitor(message::Message *message,
+               freertos::Queue<LightController::LightAction> &lightActionQueue);
+  State operator()(state::ModeSelection);
+  State operator()(state::Message);
+  State operator()(state::MessageLightData);
+  State operator()(state::MessageInfo);
+  State operator()(state::MessageWarning);
+  State operator()(state::MessageError);
+  State operator()(state::MessageSuccess);
+
+  State operator()(state::Settings);
+  State operator()(state::SettingsSetBaud);
+  State operator()(state::SettingsWifiPassword);
+  State operator()(state::SettingsWifiSSID);
+
+  State operator()(state::LightControl);
+  State operator()(state::LightControlOn);
+  State operator()(state::LightControlOff);
+  State operator()(state::LightControlToggle);
+  State operator()(state::LightControlIncrease);
+  State operator()(state::LightControlDecrease);
+  State operator()(state::Command);
+  State operator()(state::CommandRequestLightData);
+  State operator()(state::CommandEnterConsoleFlashing);
+  State operator()(state::CommandExitConsoleFlashing);
+  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;
+  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
diff --git a/modules/control/Firmware/src/Messages/Protocol.h b/modules/control/Firmware/src/Messages/Protocol.h
new file mode 100644
index 0000000..0587849
--- /dev/null
+++ b/modules/control/Firmware/src/Messages/Protocol.h
@@ -0,0 +1,55 @@
+#pragma once
+#include <etl/type_def.h>
+#include <etl/type_traits.h>
+#include <etl/variant.h>
+#include <etl/visitor.h>
+#include <type_traits>
+
+namespace message {
+namespace protocol {
+static const uint8_t HEADER_LEN = 5;
+static const uint8_t MAX_TOTAL_LEN = 255;
+static const uint8_t MAX_PAYLOAD_LEN = MAX_TOTAL_LEN - HEADER_LEN;
+
+enum class Mode : uint8_t {
+  Message = 0x0,
+  Settings = 0x1,
+  LightControl = 0x2,
+  Command = 0x3
+};
+
+enum class Message : uint8_t {
+  LightData = 0x0,
+  Info = 0x1,
+  Warning = 0x2,
+  Error = 0x3,
+  Success = 0x5,
+};
+
+enum class Settings : uint8_t {
+  SetBaud = 0x0,
+  SetWifiPassword = 0x1,
+  SetWifiSSID = 0x2,
+};
+
+enum class LightControl : uint8_t {
+  On = 0x0,
+  Off = 0x1,
+  Toggle = 0x2,
+  Increase = 0x3,
+  Decrease = 0x4,
+};
+
+enum class Command : uint8_t {
+  RequestLightData = 0x0,
+  EnterConsoleFlashing = 0x1,
+  ExitConsoleFlashing = 0x2,
+  PairBluetooth = 0x3,
+  Help = 0x4,
+  Version = 0x5,
+  Reset = 0x6,
+};
+
+using Submode = etl::variant<Mode, Message, Settings, LightControl, Command>;
+} // namespace protocol
+} // namespace message
diff --git a/modules/control/Firmware/include/PinMap/PinMap.h b/modules/control/Firmware/src/PinMap/PinMap.h
similarity index 100%
rename from modules/control/Firmware/include/PinMap/PinMap.h
rename to modules/control/Firmware/src/PinMap/PinMap.h
diff --git a/modules/control/Firmware/src/Util/CircularBuffer.h b/modules/control/Firmware/src/Util/CircularBuffer.h
new file mode 100644
index 0000000..c879dc6
--- /dev/null
+++ b/modules/control/Firmware/src/Util/CircularBuffer.h
@@ -0,0 +1,21 @@
+#pragma once
+#include <Arduino.h>
+#include <etl/array.h>
+
+namespace util {
+template <typename T, size_t size> class CircularBuffer {
+public:
+  T &peek() { return buffer[index]; }
+
+  /// @brief Warning: Does not deconstruct entry! If necessary use
+  /// etl::circular_buffer instead!
+  T &pop() {
+    index = (index + 1) % buffer.size();
+    return buffer[index];
+  }
+
+private:
+  etl::array<T, size> buffer;
+  size_t index;
+};
+} // namespace util
diff --git a/modules/control/Firmware/src/main.cpp b/modules/control/Firmware/src/main.cpp
index 0cdfaa9..58b344c 100644
--- a/modules/control/Firmware/src/main.cpp
+++ b/modules/control/Firmware/src/main.cpp
@@ -1,109 +1,9 @@
 #include <Arduino.h>
-#include <LightController.h>
-#include <SerialCommunicator.h>
-#include <BluetoothCommunicator.h>
-#include <WebsocketCommunicator.h>
-#include <PinMap/PinMap.h>
-#include <esp_spi_flash.h>
-#include <WiFi.h>
-#include <ESPAsyncWebServer.h>
-#include <Credentials/CredentialManager.h>
 
-const int STEPS = 5;
-const int WIFI_TIMEOUT = 20;
-const int bjtCount = 4;
-const int bjtPin[bjtCount] = {SIG1A, SIG1B, SIG2A, SIG2B};
-
-BluetoothSerial bt;
-AsyncWebServer server(80);
-AsyncWebSocket ws("/ws");
-
-Communicator *computer;
-Communicator *phone;
-WebsocketCommunicator *websocket;
-LightController *light;
-
-void websocketTask(void *parameter)
-{
-    while (true)
-    {
-        websocket->sendMessage(light->getBjtState(), light->getBjtCount());
-
-        vTaskDelay(100 / portTICK_PERIOD_MS);
-    }
-    vTaskDelete(NULL);
+void setup() {
+  // put your setup code here, to run once:
 }
 
-void registerWebSocketTask()
-{
-    xTaskCreate(websocketTask, "websocketTask", 10000, NULL, 1, NULL);
-}
-
-void connectWifi(int timeout)
-{
-    int secondsPassed = 0;
-    WiFi.setHostname("Heliox");
-    WiFi.begin(WIFI_SSID, WIFI_PW);
-    Serial.print("Connecting to WiFi");
-    while (WiFi.status() != WL_CONNECTED)
-    {
-        if (secondsPassed < timeout)
-        {
-            Serial.print(".");
-            delay(1000);
-            secondsPassed++;
-        }
-        else
-        {
-            Serial.println("");
-            Serial.println("WiFi timed out");
-            return;
-        }
-    }
-
-    Serial.println("");
-    Serial.println(WiFi.localIP());
-    digitalWrite(LEDB, HIGH);
-}
-
-void setup()
-{
-    pinMode(LEDB, OUTPUT);
-    digitalWrite(LEDB, LOW);
-    computer = new SerialCommunicator(Serial, 9600, 5, 50);
-    phone = new BluetoothCommunicator(bt, 5, 50);
-    light = new LightController(bjtPin, bjtCount);
-    websocket = new WebsocketCommunicator(ws, server, 50);
-
-    connectWifi(WIFI_TIMEOUT);
-    server.begin();
-    registerWebSocketTask();
-}
-
-void computerCycle()
-{
-    light->updateState(computer->receiveMessage(), STEPS);
-    computer->sendMessage(light->getBjtState(), light->getBjtCount());
-    computer->clearBuffer();
-}
-
-void phoneCycle()
-{
-    light->updateState(phone->receiveMessage(), STEPS);
-    phone->sendMessage(light->getBjtState(), light->getBjtCount());
-    phone->clearBuffer();
-}
-
-void websocketCycle()
-{
-    light->updateState(websocket->receiveMessage(), STEPS);
-    websocket->clearBufferSafely();
-}
-
-void loop()
-{
-    computerCycle();
-    phoneCycle();
-    websocketCycle();
-    ws.cleanupClients();
+void loop() {
+  // put your main code here, to run repeatedly:
 }
\ No newline at end of file
diff --git a/modules/control/Firmware/test/README b/modules/control/Firmware/test/README
index e7d1588..9b1e87b 100644
--- a/modules/control/Firmware/test/README
+++ b/modules/control/Firmware/test/README
@@ -1,11 +1,11 @@
-
-This directory is intended for PlatformIO Unit Testing and project tests.
-
-Unit Testing is a software testing method by which individual units of
-source code, sets of one or more MCU program modules together with associated
-control data, usage procedures, and operating procedures, are tested to
-determine whether they are fit for use. Unit testing finds problems early
-in the development cycle.
-
-More information about PlatformIO Unit Testing:
-- https://docs.platformio.org/page/plus/unit-testing.html
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
diff --git a/modules/debugger/debugger.py b/modules/debugger/debugger.py
new file mode 100644
index 0000000..ea9a5f4
--- /dev/null
+++ b/modules/debugger/debugger.py
@@ -0,0 +1,10 @@
+import serial
+import time
+ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200)
+ser.write(b'HX\x02\x02\x00')
+time.sleep(1)
+ser.write(b'HX\x02\x02\x01')
+print(f"Listening for messages on {ser.name}:")
+while True:
+    print(ser.readline())
+ser.close()
diff --git a/modules/debugger/poetry.lock b/modules/debugger/poetry.lock
new file mode 100644
index 0000000..3a0dd7f
--- /dev/null
+++ b/modules/debugger/poetry.lock
@@ -0,0 +1,48 @@
+# This file is automatically @generated by Poetry 1.4.0 and should not be changed by hand.
+
+[[package]]
+name = "autopep8"
+version = "2.0.2"
+description = "A tool that automatically formats Python code to conform to the PEP 8 style guide"
+category = "dev"
+optional = false
+python-versions = ">=3.6"
+files = [
+    {file = "autopep8-2.0.2-py2.py3-none-any.whl", hash = "sha256:86e9303b5e5c8160872b2f5ef611161b2893e9bfe8ccc7e2f76385947d57a2f1"},
+    {file = "autopep8-2.0.2.tar.gz", hash = "sha256:f9849cdd62108cb739dbcdbfb7fdcc9a30d1b63c4cc3e1c1f893b5360941b61c"},
+]
+
+[package.dependencies]
+pycodestyle = ">=2.10.0"
+
+[[package]]
+name = "pycodestyle"
+version = "2.10.0"
+description = "Python style guide checker"
+category = "dev"
+optional = false
+python-versions = ">=3.6"
+files = [
+    {file = "pycodestyle-2.10.0-py2.py3-none-any.whl", hash = "sha256:8a4eaf0d0495c7395bdab3589ac2db602797d76207242c17d470186815706610"},
+    {file = "pycodestyle-2.10.0.tar.gz", hash = "sha256:347187bdb476329d98f695c213d7295a846d1152ff4fe9bacb8a9590b8ee7053"},
+]
+
+[[package]]
+name = "pyserial"
+version = "3.5"
+description = "Python Serial Port Extension"
+category = "main"
+optional = false
+python-versions = "*"
+files = [
+    {file = "pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0"},
+    {file = "pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb"},
+]
+
+[package.extras]
+cp2110 = ["hidapi"]
+
+[metadata]
+lock-version = "2.0"
+python-versions = "^3.11"
+content-hash = "e62c345d7dc7473afcf2d0b5bdf8471dec4b4834efc156b4cef53daeb59c89ff"
diff --git a/modules/debugger/pyproject.toml b/modules/debugger/pyproject.toml
new file mode 100644
index 0000000..405fd46
--- /dev/null
+++ b/modules/debugger/pyproject.toml
@@ -0,0 +1,18 @@
+[tool.poetry]
+name = "debugger"
+version = "0.1.0"
+description = ""
+authors = ["GHOSCHT <31184695+GHOSCHT@users.noreply.github.com>"]
+readme = "README.md"
+
+[tool.poetry.dependencies]
+python = "^3.11"
+pyserial = "^3.5"
+
+
+[tool.poetry.group.dev.dependencies]
+autopep8 = "^2.0.2"
+
+[build-system]
+requires = ["poetry-core"]
+build-backend = "poetry.core.masonry.api"