🎨 Moving project to use event bus

esp32/include/adapters/bluetooth.hpp

@@ -14,21 +14,8 @@ class BluetoothService : public CommBase<> {
     BLECharacteristic* rxCharacteristic {nullptr};
     bool connected {false};
 
-  protected:
-    template <typename Msg>
-    using EventBusHandle = typename EventBus<Msg>::Handle;
-
-    template <typename Msg>
-    EventBusHandle<Msg>& getHandle(Topic topic) {
-        return *static_cast<EventBusHandle<Msg>*>(subscriptionHandle[static_cast<size_t>(topic)]);
-    }
-
-    template <typename Msg>
-    void setHandle(Topic topic, EventBusHandle<Msg>&& handle) {
-        subscriptionHandle[static_cast<size_t>(topic)] = new EventBusHandle<Msg>(std::move(handle));
-    }
-
-    std::array<void*, static_cast<size_t>(Topic::COUNT)> subscriptionHandle {};
+  public:
+    void begin(const char* name);
 
   private:
     void handleReceive(const std::string& data);
@@ -53,8 +40,4 @@ class BluetoothService : public CommBase<> {
             if (!v.empty()) svc->handleReceive(v);
         }
     };
-
-  public:
-    void begin(const char* name);
-    void loop() {}
 };

esp32/include/adapters/comm_base.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include "event_bus.hpp"
 #include <ArduinoJson.h>
 #include <array>
 #include <bitset>
@@ -17,6 +18,8 @@ class CommBase {
     std::array<Bits, NTopics> subs_;
     portMUX_TYPE mux_ portMUX_INITIALIZER_UNLOCKED;
 
+    std::array<void*, static_cast<size_t>(Topic::COUNT)> subscriptionHandle {};
+
     static constexpr size_t idx(Topic t) { return static_cast<size_t>(t); }
 
     template <Topic T>
@@ -30,6 +33,18 @@ class CommBase {
   protected:
     virtual void send(size_t cid, const char* data, size_t len) = 0;
 
+    template <class Msg>
+    auto& getHandle(Topic topic) {
+        using H = typename EventBus<Msg>::Handle;
+        return *static_cast<H*>(subscriptionHandle[static_cast<size_t>(topic)]);
+    }
+
+    template <class Msg>
+    void setHandle(Topic topic, typename EventBus<Msg>::Handle&& h) {
+        using H = typename EventBus<Msg>::Handle;
+        subscriptionHandle[static_cast<size_t>(topic)] = new H(std::move(h));
+    }
+
   public:
     void subscribe(Topic t, size_t cid) {
         portENTER_CRITICAL(&mux_);

esp32/include/adapters/websocket.hpp

@@ -0,0 +1,42 @@
+#ifndef Socket_h
+#define Socket_h
+
+#include <PsychicHttp.h>
+#include <ArduinoJson.h>
+#include <map>
+#include <list>
+#include <functional>
+
+#include "event_bus.hpp"
+#include "adapters/comm_base.hpp"
+#include "topic.hpp"
+// #include "msgs/motion_input_msg.hpp"
+// #include "msgs/motion_angles_msg.hpp"
+// #include "msgs/motion_position_msg.hpp"
+// #include "msgs/motion_mode_msg.hpp"
+
+// typedef std::function<void(JsonObject &root, int originId)> EventCallback;
+
+class EventSocket : public CommBase<> {
+    PsychicWebSocketHandler _socket;
+
+  public:
+    EventSocket();
+    PsychicWebSocketHandler *getHandler() { return &_socket; }
+
+  private:
+    void send(size_t clientId, const char *data, size_t len) override;
+
+    void handleReceive(const std::string &data);
+    // void handleTypedMessage(const std::string &data);
+    // void handleLegacyMessage(const std::string &data, int originId);
+    // void handleEventCallbacks(String event, JsonObject &jsonObject, int originId);
+
+    void onWSOpen(PsychicWebSocketClient *client);
+    void onWSClose(PsychicWebSocketClient *client);
+    esp_err_t onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *frame);
+};
+
+extern EventSocket socket;
+
+#endif

esp32/include/ap_service.h

@@ -0,0 +1,38 @@
+#include <template/stateful_service.h>
+#include <template/stateful_endpoint.h>
+#include <template/stateful_persistence.h>
+#include <settings/ap_settings.h>
+#include <utils/timing.h>
+#include <WiFi.h>
+
+class APService : public StatefulService<APSettings> {
+  public:
+    APService();
+    ~APService();
+
+    void begin();
+    void loop();
+    void recoveryMode();
+
+    esp_err_t getStatus(PsychicRequest *request);
+    void status(JsonObject &root);
+    APNetworkStatus getAPNetworkStatus();
+
+    StatefulHttpEndpoint<APSettings> endpoint;
+
+  private:
+    PsychicHttpServer *_server;
+    FSPersistence<APSettings> _persistence;
+
+    DNSServer *_dnsServer;
+
+    volatile unsigned long _lastManaged;
+    volatile boolean _reconfigureAp;
+    volatile boolean _recoveryMode = false;
+
+    void reconfigureAP();
+    void manageAP();
+    void startAP();
+    void stopAP();
+    void handleDNS();
+};

esp32/include/event_bus.hpp

@@ -3,6 +3,7 @@
 #include <bitset>
 #include <functional>
 #include <optional>
+#include <atomic>
 
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
@@ -102,11 +103,12 @@ class EventBus {
 
   public:
     class Handle {
-        size_t index;
+        size_t index {MaxSubs};
         friend class EventBus;
         explicit Handle(size_t i) : index(i) {}
 
       public:
+        Handle() = default;
         Handle(const Handle&) = delete;
         Handle& operator=(const Handle&) = delete;
         Handle(Handle&& h) noexcept : index(h.index) { h.index = MaxSubs; }

esp32/include/features.h

@@ -0,0 +1,92 @@
+#ifndef Features_h
+#define Features_h
+
+#include <WiFi.h>
+#include <ArduinoJson.h>
+#include <PsychicHttp.h>
+
+#define FT_ENABLED(feature) feature
+
+// upload firmware feature off by default
+#ifndef USE_UPLOAD_FIRMWARE
+#define USE_UPLOAD_FIRMWARE 0
+#endif
+
+// download firmware feature off by default
+#ifndef USE_DOWNLOAD_FIRMWARE
+#define USE_DOWNLOAD_FIRMWARE 0
+#endif
+
+// ESP32 sleep states off by default
+#ifndef USE_SLEEP
+#define USE_SLEEP 0
+#endif
+
+// ESP32 camera off by default
+#ifndef USE_CAMERA
+#define USE_CAMERA 0
+#endif
+
+// ESP32 IMU on by default
+#ifndef USE_MPU6050
+#define USE_MPU6050 0
+#endif
+
+// ESP32 IMU on by default
+#ifndef USE_BNO055
+#define USE_BNO055 1
+#endif
+
+// ESP32 magnetometer on by default
+#ifndef USE_HMC5883
+#define USE_HMC5883 0
+#endif
+
+// ESP32 barometer off by default
+#ifndef USE_BMP180
+#define USE_BMP180 0
+#endif
+
+// ESP32 SONAR off by default
+#ifndef USE_USS
+#define USE_USS 0
+#endif
+
+// PCA9685 Servo controller on by default
+#ifndef USE_PCA9685
+#define USE_PCA9685 1
+#endif
+
+// ESP32 GPS off by default
+#ifndef USE_GPS
+#define USE_GPS 0
+#endif
+
+// ESP32 MDNS on by default
+#ifndef USE_MDNS
+#define USE_MDNS 1
+#endif
+
+// ESP32 MSGPACK on by default
+#ifndef USE_MSGPACK
+#define USE_MSGPACK 1
+#endif
+
+// ESP32 JSON off by default
+#ifndef USE_JSON
+#define USE_JSON 0
+#endif
+
+static_assert(!(USE_JSON == 1 && USE_MSGPACK == 1), "Cannot set both USE_JSON and USE_MSGPACK to 1 simultaneously");
+
+namespace feature_service {
+
+void printFeatureConfiguration();
+
+void features(JsonObject &root);
+
+esp_err_t getFeatures(PsychicRequest *request);
+
+} // namespace feature_service
+
+#endif

esp32/include/filesystem.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <PsychicHttp.h>
+
+#include <LittleFS.h>
+
+#define ESPFS LittleFS
+
+#define AP_SETTINGS_FILE "/config/apSettings.json"
+#define CAMERA_SETTINGS_FILE "/config/cameraSettings.json"
+#define FS_CONFIG_DIRECTORY "/config"
+#define DEVICE_CONFIG_FILE "/config/peripheral.json"
+#define WIFI_SETTINGS_FILE "/config/wifiSettings.json"
+#define SERVO_SETTINGS_FILE "/config/servoSettings.json"
+#define MDNS_SETTINGS_FILE "/config/mdnsSettings.json"
+
+namespace FileSystem {
+extern PsychicUploadHandler *uploadHandler;
+
+String listFiles(const String &directory, bool isRoot = true);
+bool deleteFile(const char *filename);
+bool editFile(const char *filename, const char *content);
+esp_err_t uploadFile(PsychicRequest *request, const String &filename, uint64_t index, uint8_t *data, size_t len,
+                     bool last);
+
+esp_err_t getFiles(PsychicRequest *request);
+esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json);
+esp_err_t handleEdit(PsychicRequest *request, JsonVariant &json);
+
+esp_err_t mkdir(PsychicRequest *request, JsonVariant &json);
+} // namespace FileSystem

esp32/include/firmware_download_service.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <Arduino.h>
+
+#include <WiFi.h>
+#include <ArduinoJson.h>
+#include <event_bus.hpp>
+#include <PsychicHttp.h>
+
+#include <HTTPClient.h>
+#include <HTTPUpdate.h>
+#include <task_manager.h>
+
+#define EVENT_DOWNLOAD_OTA "otastatus"
+#define OTA_TASK_STACK_SIZE 9216
+
+class DownloadFirmwareService {
+  public:
+    DownloadFirmwareService();
+
+    esp_err_t handleDownloadUpdate(PsychicRequest *request, JsonVariant &json);
+
+  private:
+};

esp32/include/firmware_upload_service.h

@@ -0,0 +1,32 @@
+#ifndef FirmwareUploadService_h
+#define FirmwareUploadService_h
+
+#include <Arduino.h>
+
+#include <Update.h>
+#include <WiFi.h>
+
+#include <PsychicHttp.h>
+#include <system_service.h>
+#include <event_bus.hpp>
+
+enum FileType { ft_none = 0, ft_firmware = 1, ft_md5 = 2 };
+
+class FirmwareUploadService {
+  public:
+    FirmwareUploadService();
+
+    void begin();
+
+    PsychicUploadHandler *getHandler() { return &uploadHandler; }
+
+  private:
+    PsychicUploadHandler uploadHandler;
+    esp_err_t handleUpload(PsychicRequest *request, const String &filename, uint64_t index, uint8_t *data, size_t len,
+                           bool final);
+    esp_err_t uploadComplete(PsychicRequest *request);
+    esp_err_t handleError(PsychicRequest *request, int code);
+    esp_err_t handleEarlyDisconnect();
+};
+
+#endif // end FirmwareUploadService_h

esp32/include/gait/bezier_state.h

@@ -0,0 +1,135 @@
+#pragma once
+
+#include <gait/state.h>
+#include <utils/math_utils.h>
+#include <array>
+#include <functional>
+
+class BezierState : public GaitState {
+  private:
+    float phase_time = 0.0f;
+    static constexpr float PHASE_OFFSET[4] = {0.f, 0.5f, 0.5f, 0.f};
+    static constexpr float STAND_OFFSET = 0.75f;
+    static constexpr uint8_t BEZIER_POINTS = 12;
+    float step_length = 0.0f;
+    static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = {
+        combinatorial_constexpr(11, 0),  // 1
+        combinatorial_constexpr(11, 1),  // 11
+        combinatorial_constexpr(11, 2),  // 55
+        combinatorial_constexpr(11, 3),  // 165
+        combinatorial_constexpr(11, 4),  // 330
+        combinatorial_constexpr(11, 5),  // 462
+        combinatorial_constexpr(11, 6),  // 462
+        combinatorial_constexpr(11, 7),  // 330
+        combinatorial_constexpr(11, 8),  // 165
+        combinatorial_constexpr(11, 9),  // 55
+        combinatorial_constexpr(11, 10), // 11
+        combinatorial_constexpr(11, 11)  // 1
+    };
+
+    alignas(32) static constexpr float BEZIER_STEPS[12] = {-1.0f, -1.4f, -1.5f, -1.5f, -1.5f, 0.0f,
+                                                           0.0f,  0.0f,  1.5f,  1.5f,  1.4f,  1.0f};
+
+    alignas(32) static constexpr float BEZIER_HEIGHTS[12] = {0.0f, 0.0f, 0.9f, 0.9f, 0.9f, 0.9f,
+                                                             0.9f, 1.1f, 1.1f, 1.1f, 0.0f, 0.0f};
+
+  public:
+    const char *name() const override { return "Bezier"; }
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        this->mapCommand(command);
+        step_length = std::hypot(gait_state.step_x, gait_state.step_z);
+        if (gait_state.step_x < 0.0f) {
+            step_length = -step_length;
+        }
+        updatePhase(dt);
+        updateFeetPositions(body_state);
+    }
+
+  protected:
+    void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); }
+
+    void updateFeetPositions(body_state_t &body_state) {
+        for (int i = 0; i < 4; ++i) {
+            updateFootPosition(body_state, i);
+        }
+    }
+
+    void updateFootPosition(body_state_t &body_state, const int index) {
+        body_state.feet[index][0] = this->default_feet_pos[index][0];
+        body_state.feet[index][1] = this->default_feet_pos[index][1];
+        body_state.feet[index][2] = this->default_feet_pos[index][2];
+        const float leg_phase = std::fmod(phase_time + PHASE_OFFSET[index], 1.0f);
+        const bool contact = leg_phase <= STAND_OFFSET;
+        contact ? standController(body_state, index, leg_phase / 0.75)
+                : swingController(body_state, index, (leg_phase - 0.75) / (1 - 0.75));
+    }
+
+    void standController(body_state_t &body_state, const int index, const float phase) {
+        controller(index, body_state, phase, stanceCurve, &gait_state.step_depth);
+    }
+
+    void swingController(body_state_t &body_state, const int index, const float phase) {
+        controller(index, body_state, phase, bezierCurve, &gait_state.step_height);
+    }
+
+    void controller(const int index, body_state_t &body_state, const float phase,
+                    std::function<void(float, float, float *, float, float *)> curve, float *arg) {
+        float delta_pos[3] = {0};
+        float delta_rot[3] = {0};
+
+        float length = step_length / 2.0f;
+        float angle = std::atan2(gait_state.step_z, step_length) * 2;
+        curve(length, angle, arg, phase, delta_pos);
+
+        length = gait_state.step_angle * 2.0f;
+        angle = yawArc(default_feet_pos[index], body_state.feet[index]);
+        curve(length, angle, arg, phase, delta_rot);
+
+        body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2;
+        if (step_length || gait_state.step_angle) body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2;
+        body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2;
+    }
+
+    static void stanceCurve(const float length, const float angle, const float *depth, const float phase,
+                            float *point) {
+        float step = length * (1.0f - 2.0f * phase);
+        point[0] += step * std::cos(angle);
+        point[2] += step * std::sin(angle);
+
+        if (length != 0.0f) {
+            point[1] = -*depth * std::cos((M_PI * (point[0] + point[2])) / (2.f * length));
+        }
+    }
+
+    static void bezierCurve(const float length, const float angle, const float *height, const float phase,
+                            float *point) {
+        const float X_POLAR = std::cos(angle);
+        const float Z_POLAR = std::sin(angle);
+
+        float phase_power = 1.0f;
+        float inv_phase_power = std::pow(1.0f - phase, 11);
+        const float one_minus_phase = 1.0f - phase;
+
+        for (int i = 0; i < 12; i++) {
+            float b = COMBINATORIAL_VALUES[i] * phase_power * inv_phase_power;
+            point[0] += b * BEZIER_STEPS[i] * length * X_POLAR;
+            point[1] += b * BEZIER_HEIGHTS[i] * *height;
+            point[2] += b * BEZIER_STEPS[i] * length * Z_POLAR;
+
+            phase_power *= phase;
+            inv_phase_power /= one_minus_phase;
+        }
+    }
+
+    static float yawArc(const float feet_pos[4], const float *current_pos) {
+        const float foot_mag = std::hypot(feet_pos[0], feet_pos[2]);
+        const float foot_dir = std::atan2(feet_pos[2], feet_pos[0]);
+        const float offsets[] = {current_pos[0] - feet_pos[0], current_pos[1] - feet_pos[1],
+                                 current_pos[2] - feet_pos[2]};
+        const float offset_mag = std::hypot(offsets[0], offsets[2]);
+        const float offset_mod = std::atan2(offset_mag, foot_mag);
+
+        return M_PI_2 + foot_dir + offset_mod;
+    }
+};

esp32/include/gait/crawl_state.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <gait/phase_state_base.h>
+
+class EightPhaseWalkState : public PhaseGaitState {
+  protected:
+    const char *name() const override { return "Eight phase walk"; }
+
+    int num_phases() const override { return 8; }
+
+    float phase_speed_factor() const override { return 4; }
+
+    float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
+
+  public:
+    EightPhaseWalkState() {
+        uint8_t contact[4][8] = {
+            {1, 0, 1, 1, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 0, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 0}, {1, 1, 1, 0, 1, 1, 1, 1}};
+        float shift_values[4][3] = {{-0.05f, 0, -0.2f}, {0.25f, 0, 0.2f}, {-0.05f, 0, 0.2f}, {0.25f, 0, -0.2f}};
+        for (uint8_t i = 0; i < 4; ++i) {
+            for (uint8_t j = 0; j < 8; ++j) {
+                contact_phases[i][j] = contact[i][j];
+            }
+            for (uint8_t j = 0; j < 3; ++j) {
+                shifts[i][j] = shift_values[i][j];
+            }
+        }
+    }
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        return PhaseGaitState::step(body_state, command, dt);
+    }
+};

esp32/include/gait/four_phase_trot_state.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <gait/phase_state_base.h>
+
+class FourPhaseWalkState : public PhaseGaitState {
+  protected:
+    const char *name() const override { return "Four phase walk"; }
+
+    int num_phases() const override { return 4; }
+
+    float phase_speed_factor() const override { return 6; }
+
+    float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
+
+  public:
+    FourPhaseWalkState() {
+        uint8_t contact[4][4] = {{1, 0, 1, 1}, {1, 1, 1, 0}, {1, 1, 1, 0}, {1, 0, 1, 1}};
+        for (int i = 0; i < 4; ++i) {
+            for (int j = 0; j < 4; ++j) {
+                contact_phases[i][j] = contact[i][j];
+            }
+        }
+    }
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        return PhaseGaitState::step(body_state, command, dt);
+    }
+};

esp32/include/gait/idle_state.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <gait/state.h>
+
+class IdleState : public GaitState {
+  protected:
+    const char *name() const override { return "Idle"; }
+};

esp32/include/gait/phase_state_base.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <gait/state.h>
+
+class PhaseGaitState : public GaitState {
+  protected:
+    int phase = 0;
+    float phase_time = 0;
+    virtual int num_phases() const = 0;
+    virtual float phase_speed_factor() const = 0;
+    virtual float swing_stand_ratio() const = 0;
+    float dt = 0.02f;
+
+    uint8_t contact_phases[4][8];
+    float shifts[4][3];
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        mapCommand(command);
+        this->dt = dt;
+        updatePhase();
+        updateBodyPosition(body_state);
+        updateFeetPositions(body_state);
+    }
+
+    void updatePhase() {
+        phase_time += dt * phase_speed_factor() * gait_state.step_velocity;
+
+        if (phase_time >= 1.0f) {
+            phase += 1;
+            if (phase == num_phases()) phase = 0;
+            phase_time = 0;
+        }
+    }
+
+    void updateBodyPosition(body_state_t &body_state) {
+        if (num_phases() == 4) return;
+
+        const auto &shift = shifts[phase / 2];
+        body_state.xm += (shift[0] - body_state.xm) * dt * 4;
+        body_state.zm += (shift[2] - body_state.zm) * dt * 4;
+    }
+
+    void updateFeetPositions(body_state_t &body_state) {
+        for (int i = 0; i < 4; ++i) {
+            updateFootPosition(body_state, i);
+        }
+    }
+
+    void updateFootPosition(body_state_t &body_state, int index) {
+        bool contact = contact_phases[index][phase];
+        contact ? stand(body_state, index) : swing(body_state, index);
+    }
+
+    void stand(body_state_t &body_state, int index) {
+        float delta_pos[3] = {-gait_state.step_x * dt * swing_stand_ratio(), 0,
+                              -gait_state.step_z * dt * swing_stand_ratio()};
+
+        body_state.feet[index][0] += delta_pos[0];
+        body_state.feet[index][1] = default_feet_pos[index][1];
+        body_state.feet[index][2] += delta_pos[2];
+    }
+
+    void swing(body_state_t &body_state, int index) {
+        float delta_pos[3] = {gait_state.step_x * dt, 0, gait_state.step_z * dt};
+
+        if (std::fabs(gait_state.step_x) < 0.01) {
+            delta_pos[0] = (default_feet_pos[index][0] - body_state.feet[index][0]) * dt * 8;
+        }
+
+        if (std::fabs(gait_state.step_z) < 0.01) {
+            delta_pos[2] = (default_feet_pos[index][2] - body_state.feet[index][2]) * dt * 8;
+        }
+
+        body_state.feet[index][0] += delta_pos[0];
+        body_state.feet[index][1] = default_feet_pos[index][1] + std::sin(phase_time * M_PI) * gait_state.step_height;
+        body_state.feet[index][2] += delta_pos[2];
+    }
+};

esp32/include/gait/rest_state.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <gait/state.h>
+
+class RestState : public GaitState {
+  protected:
+    const char *name() const override { return "Rest"; }
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        body_state.omega = 0;
+        body_state.phi = 0;
+        body_state.psi = 0;
+        body_state.xm = 0;
+        body_state.ym = getDefaultHeight() / 2;
+        body_state.zm = 0;
+        body_state.updateFeet(default_feet_pos);
+    }
+};

esp32/include/gait/stand_state.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <gait/state.h>
+
+class StandState : public GaitState {
+  protected:
+    const char *name() const override { return "Stand"; }
+
+    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+        body_state.omega = 0;
+        body_state.phi = command.rx / 8;
+        body_state.psi = command.ry / 8;
+        body_state.xm = command.ly / 2 / 100;
+        body_state.zm = command.lx / 2 / 100;
+        body_state.updateFeet(default_feet_pos);
+    }
+};

esp32/include/gait/state.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <kinematics.h>
+
+struct gait_state_t {
+    float step_height;
+    float step_x;
+    float step_z;
+    float step_angle;
+    float step_velocity;
+    float step_depth;
+};
+
+struct ControllerCommand {
+    int stop;
+    float lx, ly, rx, ry, h, s, s1;
+};
+
+class GaitState {
+  protected:
+    virtual const char *name() const = 0;
+    float default_feet_pos[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
+    gait_state_t gait_state = {0.4, 0, 0, 0, 1, 0.002};
+
+    void mapCommand(ControllerCommand command) {
+        this->gait_state.step_height = 0.4 + (command.s1 / 128 + 1) / 2;
+        this->gait_state.step_x = command.ly / 128;
+        this->gait_state.step_z = -command.lx / 128;
+        this->gait_state.step_velocity = command.s / 128 + 1;
+        this->gait_state.step_angle = command.rx / 128;
+        this->gait_state.step_depth = 0.002;
+    }
+
+  public:
+    virtual float getDefaultHeight() const { return 0.5f; }
+
+    virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
+
+    virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
+
+    virtual void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) {
+        this->mapCommand(command);
+    }
+};

esp32/include/global.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <esp32-hal.h>
+
+#if CONFIG_IDF_TARGET_ESP32 // ESP32/PICO-D4
+#include "esp32/rom/rtc.h"
+#ifndef ESP_PLATFORM
+#define ESP_PLATFORM "ESP32"
+#endif
+#elif CONFIG_IDF_TARGET_ESP32S2
+#include "esp32/rom/rtc.h"
+#ifndef ESP_PLATFORM
+#define ESP_PLATFORM "ESP32-S2"
+#endif
+#elif CONFIG_IDF_TARGET_ESP32C3
+#include "esp32c3/rom/rtc.h"
+#ifndef ESP_PLATFORM
+#define ESP_PLATFORM "ESP32-C3"
+#endif
+#elif CONFIG_IDF_TARGET_ESP32S3
+#include "esp32s3/rom/rtc.h"
+#ifndef ESP_PLATFORM
+#define ESP_PLATFORM "ESP32-S3"
+#endif
+#else
+#error Target CONFIG_IDF_TARGET is not supported
+#endif
+
+#ifndef ARDUINO_VERSION
+#ifndef STRINGIFY
+#define STRINGIFY(s) #s
+#endif
+#define ARDUINO_VERSION_STR(major, minor, patch) "v" STRINGIFY(major) "." STRINGIFY(minor) "." STRINGIFY(patch)
+#define ARDUINO_VERSION \
+    ARDUINO_VERSION_STR(ESP_ARDUINO_VERSION_MAJOR, ESP_ARDUINO_VERSION_MINOR, ESP_ARDUINO_VERSION_PATCH)
+#endif
+
+/*
+ * I2C software connection
+ */
+#ifndef SDA_PIN
+#define SDA_PIN SDA
+#endif
+#ifndef SCL_PIN
+#define SCL_PIN SCL
+#endif
+#ifndef I2C_FREQUENCY
+#define I2C_FREQUENCY 100000UL
+#endif

esp32/include/kinematics.h

@@ -0,0 +1,167 @@
+#ifndef Kinematics_h
+#define Kinematics_h
+
+#include <utils/math_utils.h>
+
+struct alignas(16) body_state_t {
+    float omega, phi, psi, xm, ym, zm;
+    float feet[4][4];
+
+    void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
+
+    bool operator==(const body_state_t &other) const {
+        if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
+            !IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
+            !IS_ALMOST_EQUAL(zm, other.zm)) {
+            return false;
+        }
+        return arrayEqual(feet, other.feet, 0.1f);
+    }
+};
+
+class Kinematics {
+  private:
+#if defined(SPOTMICRO_ESP32)
+    static constexpr float l1 = 60.5f / 100.0f;
+    static constexpr float l2 = 10.0f / 100.0f;
+    static constexpr float l3 = 111.2f / 100.0f;
+    static constexpr float l4 = 118.5f / 100.0f;
+
+    static constexpr float L = 207.5f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
+#elif defined(SPOTMICRO_YERTLE)
+    static constexpr float l1 = 35.0f / 100.0f;
+    static constexpr float l2 = 0.0f;
+    static constexpr float l3 = 130.0f / 100.0f;
+    static constexpr float l4 = 130.0f / 100.0f;
+
+    static constexpr float L = 240.0f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
+#else
+#error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE"
+#endif
+
+    static constexpr float mountOffsets[4][3] = {
+        {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}};
+
+    static constexpr float invMountRot[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
+
+    alignas(16) float rot[3][3] = {0};
+    alignas(16) float inv_rot[3][3] = {0};
+    alignas(16) float inv_trans[3] = {0};
+
+    body_state_t currentState;
+
+  public:
+    static constexpr float default_feet_positions[4][4] = {
+        {mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1},
+        {mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1},
+        {mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1},
+        {mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 1},
+    };
+
+    esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
+        esp_err_t ret = ESP_OK;
+
+        if (currentState == body_state) return ESP_OK;
+
+        currentState.omega = body_state.omega;
+        currentState.phi = body_state.phi;
+        currentState.psi = body_state.psi;
+        currentState.xm = body_state.xm;
+        currentState.ym = body_state.ym;
+        currentState.zm = body_state.zm;
+        currentState.updateFeet(body_state.feet);
+
+        float roll = body_state.omega * DEG2RAD_F;
+        float pitch = body_state.phi * DEG2RAD_F;
+        float yaw = body_state.psi * DEG2RAD_F;
+        euler2R(roll, pitch, yaw, rot);
+        inverse(rot, inv_rot);
+
+        inv_trans[0] =
+            -inv_rot[0][0] * currentState.xm - inv_rot[0][1] * currentState.ym - inv_rot[0][2] * currentState.zm;
+        inv_trans[1] =
+            -inv_rot[1][0] * currentState.xm - inv_rot[1][1] * currentState.ym - inv_rot[1][2] * currentState.zm;
+        inv_trans[2] =
+            -inv_rot[2][0] * currentState.xm - inv_rot[2][1] * currentState.ym - inv_rot[2][2] * currentState.zm;
+
+        for (int i = 0; i < 4; i++) {
+            float wx = currentState.feet[i][0];
+            float wy = currentState.feet[i][1];
+            float wz = currentState.feet[i][2];
+
+            float bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0];
+            float by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1];
+            float bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2];
+
+            float mx = mountOffsets[i][0];
+            float my = mountOffsets[i][1];
+            float mz = mountOffsets[i][2];
+
+            float px = bx - mx;
+            float py = by - my;
+            float pz = bz - mz;
+
+            float lx = invMountRot[0][0] * px + invMountRot[0][1] * py + invMountRot[0][2] * pz;
+            float ly = invMountRot[1][0] * px + invMountRot[1][1] * py + invMountRot[1][2] * pz;
+            float lz = invMountRot[2][0] * px + invMountRot[2][1] * py + invMountRot[2][2] * pz;
+
+            float xLocal = (i % 2 == 1) ? -lx : lx;
+            legIK(xLocal, ly, lz, result + i * 3);
+        }
+
+        return ret;
+    }
+
+    inline void euler2R(float roll, float pitch, float yaw, float rot[3][3]) {
+        float cos_roll = std::cos(roll);
+        float sin_roll = std::sin(roll);
+        float cos_pitch = std::cos(pitch);
+        float sin_pitch = std::sin(pitch);
+        float cos_yaw = std::cos(yaw);
+        float sin_yaw = std::sin(yaw);
+
+        rot[0][0] = cos_pitch * cos_yaw;
+        rot[0][1] = -sin_yaw * cos_pitch;
+        rot[0][2] = sin_pitch;
+        rot[1][0] = sin_roll * sin_pitch * cos_yaw + sin_yaw * cos_roll;
+        rot[1][1] = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
+        rot[1][2] = -sin_roll * cos_pitch;
+        rot[2][0] = sin_roll * sin_yaw - sin_pitch * cos_roll * cos_yaw;
+        rot[2][1] = sin_roll * cos_yaw + sin_pitch * sin_yaw * cos_roll;
+        rot[2][2] = cos_roll * cos_pitch;
+    }
+
+    inline void inverse(float rot[3][3], float inv_rot[3][3]) {
+        inv_rot[0][0] = rot[0][0];
+        inv_rot[0][1] = rot[1][0];
+        inv_rot[0][2] = rot[2][0];
+        inv_rot[1][0] = rot[0][1];
+        inv_rot[1][1] = rot[1][1];
+        inv_rot[1][2] = rot[2][1];
+        inv_rot[2][0] = rot[0][2];
+        inv_rot[2][1] = rot[1][2];
+        inv_rot[2][2] = rot[2][2];
+    }
+
+    inline void legIK(float x, float y, float z, float result[3]) {
+        float F = sqrt(max(0.0f, x * x + y * y - l1 * l1));
+        float G = F - l2;
+        float H = sqrt(G * G + z * z);
+
+        float theta1 = -atan2f(y, x) - atan2f(F, -l1);
+        float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4);
+        float theta3 = acosf(max(-1.0f, min(1.0f, D)));
+        float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3));
+        result[0] = RAD_TO_DEG_F(theta1);
+        result[1] = RAD_TO_DEG_F(theta2);
+#if defined(SPOTMICRO_ESP32)
+        result[2] = RAD_TO_DEG_F(theta3);
+#elif defined(SPOTMICRO_YERTLE)
+        result[2] = RAD_TO_DEG_F(theta3 + theta2);
+#endif
+    }
+};
+
+#endif

esp32/include/mdns_service.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <PsychicHttp.h>
+#include <ESPmDNS.h>
+#include <template/stateful_service.h>
+#include <template/stateful_endpoint.h>
+#include <template/stateful_persistence.h>
+#include <settings/mdns_settings.h>
+#include <utils/timing.h>
+
+class MDNSService : public StatefulService<MDNSSettings> {
+  private:
+    FSPersistence<MDNSSettings> _persistence;
+    bool _started;
+
+    void reconfigureMDNS();
+    void startMDNS();
+    void stopMDNS();
+    void addServices();
+
+  public:
+    MDNSService();
+    ~MDNSService();
+
+    void begin();
+
+    esp_err_t getStatus(PsychicRequest *request);
+    void getStatus(JsonObject &root);
+
+    static esp_err_t queryServices(PsychicRequest *request, JsonVariant &json);
+
+    StatefulHttpEndpoint<MDNSSettings> endpoint;
+};

esp32/include/motion.h

@@ -0,0 +1,178 @@
+#ifndef MotionService_h
+#define MotionService_h
+
+#include <event_bus.hpp>
+#include <topic.hpp>
+#include <kinematics.h>
+#include <peripherals/servo_controller.h>
+#include <utils/timing.h>
+#include <utils/math_utils.h>
+
+#include <gait/state.h>
+#include <gait/crawl_state.h>
+#include <gait/bezier_state.h>
+
+#define DEFAULT_STATE false
+#define ANGLES_EVENT "angles"
+#define INPUT_EVENT "input"
+#define POSITION_EVENT "position"
+#define MODE_EVENT "mode"
+
+enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK };
+
+class MotionService {
+  public:
+    MotionService(ServoController* servoController) : _servoController(servoController) {}
+
+    void begin() {
+        setupEventBusSubscriptions();
+        body_state.updateFeet(kinematics.default_feet_positions);
+    }
+
+    void anglesEvent(const MotionAnglesMsg& msg) {
+        for (int i = 0; i < 12; i++) {
+            angles[i] = msg.angles[i];
+        }
+        syncAngles();
+    }
+
+    void positionEvent(const MotionPositionMsg& msg) {
+        body_state.omega = msg.omega;
+        body_state.phi = msg.phi;
+        body_state.psi = msg.psi;
+        body_state.xm = msg.xm;
+        body_state.ym = msg.ym;
+        body_state.zm = msg.zm;
+    }
+
+    void handleInput(const MotionInputMsg& msg) {
+        command.lx = msg.lx;
+        command.ly = msg.ly;
+        command.rx = msg.rx;
+        command.ry = msg.ry;
+        command.h = msg.h;
+        command.s = msg.s;
+        command.s1 = msg.s1;
+
+        body_state.ym = (command.h + 127.f) * 0.35f / 100;
+
+        switch (motionState) {
+            case MOTION_STATE::STAND: {
+                body_state.phi = command.rx / 8;
+                body_state.psi = command.ry / 8;
+                body_state.xm = command.ly / 2 / 100;
+                body_state.zm = command.lx / 2 / 100;
+                body_state.updateFeet(kinematics.default_feet_positions);
+                break;
+            }
+        }
+    }
+
+    void handleMode(const MotionModeMsg& msg) {
+        motionState = (MOTION_STATE)msg.mode;
+        ESP_LOGV("MotionService", "Mode %d", motionState);
+
+        motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
+
+        MotionModeMsg response;
+        response.mode = msg.mode;
+        EventBus<MotionModeMsg>::publishAsync(response, _modeHandle);
+    }
+
+    void emitAngles() {
+        MotionAnglesMsg anglesMsg;
+        for (int i = 0; i < 12; i++) {
+            anglesMsg.angles[i] = angles[i];
+        }
+        EventBus<MotionAnglesMsg>::publishAsync(anglesMsg, _anglesHandle);
+    }
+
+    void syncAngles() {
+        emitAngles();
+        _servoController->setAngles(angles);
+    }
+
+    bool updateMotion() {
+        switch (motionState) {
+            case MOTION_STATE::DEACTIVATED: return false;
+            case MOTION_STATE::IDLE: return false;
+            case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break;
+            case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
+            case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
+            case MOTION_STATE::CRAWL:
+                crawlGait->step(body_state, command);
+                kinematics.calculate_inverse_kinematics(body_state, new_angles);
+                break;
+            case MOTION_STATE::WALK:
+                walkGait->step(body_state, command);
+                kinematics.calculate_inverse_kinematics(body_state, new_angles);
+                break;
+        }
+        return update_angles(new_angles, angles);
+    }
+
+    bool update_angles(float new_angles[12], float angles[12], bool useLerp = true) {
+        bool updated = false;
+        for (int i = 0; i < 12; i++) {
+            float new_angle = useLerp ? lerp(angles[i], new_angles[i] * dir[i], 0.3) : new_angles[i] * dir[i];
+            if (!isEqual(new_angle, angles[i], 0.1)) {
+                angles[i] = new_angle;
+                updated = true;
+            }
+        }
+        return updated;
+    }
+
+    float* getAngles() { return angles; }
+
+  private:
+    void setupEventBusSubscriptions() {
+        _inputHandle = EventBus<MotionInputMsg>::subscribe([this](const MotionInputMsg* msg, size_t n) {
+            if (n > 0) handleInput(msg[0]);
+        });
+
+        _modeHandle = EventBus<MotionModeMsg>::subscribe([this](const MotionModeMsg* msg, size_t n) {
+            if (n > 0) handleMode(msg[0]);
+        });
+
+        _anglesHandle = EventBus<MotionAnglesMsg>::subscribe([this](const MotionAnglesMsg* msg, size_t n) {
+            if (n > 0) anglesEvent(msg[0]);
+        });
+
+        _positionHandle = EventBus<MotionPositionMsg>::subscribe([this](const MotionPositionMsg* msg, size_t n) {
+            if (n > 0) positionEvent(msg[0]);
+        });
+    }
+
+    EventBus<MotionInputMsg>::Handle _inputHandle;
+    EventBus<MotionModeMsg>::Handle _modeHandle;
+    EventBus<MotionAnglesMsg>::Handle _anglesHandle;
+    EventBus<MotionPositionMsg>::Handle _positionHandle;
+
+    ServoController* _servoController;
+    Kinematics kinematics;
+    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
+
+    friend class GaitState;
+
+    std::unique_ptr<GaitState> crawlGait = std::make_unique<EightPhaseWalkState>();
+    std::unique_ptr<GaitState> walkGait = std::make_unique<BezierState>();
+
+    MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
+    unsigned long _lastUpdate;
+
+    body_state_t body_state = {0, 0, 0, 0, 0, 0};
+    float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+    float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
+#if defined(SPOTMICRO_ESP32)
+    float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+#elif defined(SPOTMICRO_YERTLE)
+    float rest_angles[12] = {0, 45, -45, 0, 45, -45, 0, 45, -45, 0, 45, -45};
+    float calibration_angles[12] = {0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0};
+#endif
+};
+
+#endif

esp32/include/peripherals/barometer.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_Sensor.h>
+
+class Barometer {
+  public:
+    Barometer() : _bmp(10085) {}
+
+    bool initialize() {
+        bmp_success = _bmp.begin();
+        return bmp_success;
+    }
+
+    bool readBarometer() {
+        if (!bmp_success) return false;
+        _bmp.getTemperature(&temperature);
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        pressure = event.pressure;
+        altitude = _bmp.pressureToAltitude(seaLevelPressure, pressure);
+        return true;
+    }
+
+    float getPressure() { return pressure; }
+
+    float getAltitude() { return altitude; }
+
+    float getTemperature() { return temperature; }
+
+    void readBarometer(JsonObject& root) {
+        if (!bmp_success) return;
+        root["pressure"] = round2(getPressure());
+        root["altitude"] = round2(getAltitude());
+        root["bmp_temp"] = round2(getTemperature());
+    }
+
+    bool active() { return bmp_success; }
+
+  private:
+    Adafruit_BMP085_Unified _bmp;
+    bool bmp_success {false};
+    float pressure {0};
+    float altitude {0};
+    float temperature {0};
+
+    const float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+};

esp32/include/peripherals/camera_pins.h

@@ -0,0 +1,337 @@
+#if defined(CAMERA_MODEL_WROVER_KIT)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 21
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y9_GPIO_NUM 35
+#define Y8_GPIO_NUM 34
+#define Y7_GPIO_NUM 39
+#define Y6_GPIO_NUM 36
+#define Y5_GPIO_NUM 19
+#define Y4_GPIO_NUM 18
+#define Y3_GPIO_NUM 5
+#define Y2_GPIO_NUM 4
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_ESP_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 4
+#define SIOD_GPIO_NUM 18
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 36
+#define Y8_GPIO_NUM 37
+#define Y7_GPIO_NUM 38
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 35
+#define Y4_GPIO_NUM 14
+#define Y3_GPIO_NUM 13
+#define Y2_GPIO_NUM 34
+#define VSYNC_GPIO_NUM 5
+#define HREF_GPIO_NUM 27
+#define PCLK_GPIO_NUM 25
+
+#define LED_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_M5STACK_PSRAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_V2_PSRAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 22
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_WIDE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 22
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#define LED_GPIO_NUM 2
+
+#elif defined(CAMERA_MODEL_M5STACK_ESP32CAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 17
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_UNITCAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_AI_THINKER)
+#define PWDN_GPIO_NUM 32
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 0
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y9_GPIO_NUM 35
+#define Y8_GPIO_NUM 34
+#define Y7_GPIO_NUM 39
+#define Y6_GPIO_NUM 36
+#define Y5_GPIO_NUM 21
+#define Y4_GPIO_NUM 19
+#define Y3_GPIO_NUM 18
+#define Y2_GPIO_NUM 5
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+// 4 for flash led or 33 for normal led
+#define LED_GPIO_NUM 4
+
+#elif defined(CAMERA_MODEL_TTGO_T_JOURNAL)
+#define PWDN_GPIO_NUM 0
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 17
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_XIAO_ESP32S3)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 10
+#define SIOD_GPIO_NUM 40
+#define SIOC_GPIO_NUM 39
+
+#define Y9_GPIO_NUM 48
+#define Y8_GPIO_NUM 11
+#define Y7_GPIO_NUM 12
+#define Y6_GPIO_NUM 14
+#define Y5_GPIO_NUM 16
+#define Y4_GPIO_NUM 18
+#define Y3_GPIO_NUM 17
+#define Y2_GPIO_NUM 15
+#define VSYNC_GPIO_NUM 38
+#define HREF_GPIO_NUM 47
+#define PCLK_GPIO_NUM 13
+
+#elif defined(CAMERA_MODEL_ESP32_CAM_BOARD)
+// The 18 pin header on the board has Y5 and Y3 swapped
+#define USE_BOARD_HEADER 0
+#define PWDN_GPIO_NUM 32
+#define RESET_GPIO_NUM 33
+#define XCLK_GPIO_NUM 4
+#define SIOD_GPIO_NUM 18
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 36
+#define Y8_GPIO_NUM 19
+#define Y7_GPIO_NUM 21
+#define Y6_GPIO_NUM 39
+#if USE_BOARD_HEADER
+#define Y5_GPIO_NUM 13
+#else
+#define Y5_GPIO_NUM 35
+#endif
+#define Y4_GPIO_NUM 14
+#if USE_BOARD_HEADER
+#define Y3_GPIO_NUM 35
+#else
+#define Y3_GPIO_NUM 13
+#endif
+#define Y2_GPIO_NUM 34
+#define VSYNC_GPIO_NUM 5
+#define HREF_GPIO_NUM 27
+#define PCLK_GPIO_NUM 25
+
+#elif defined(CAMERA_MODEL_ESP32S3_CAM_LCD)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 40
+#define SIOD_GPIO_NUM 17
+#define SIOC_GPIO_NUM 18
+
+#define Y9_GPIO_NUM 39
+#define Y8_GPIO_NUM 41
+#define Y7_GPIO_NUM 42
+#define Y6_GPIO_NUM 12
+#define Y5_GPIO_NUM 3
+#define Y4_GPIO_NUM 14
+#define Y3_GPIO_NUM 47
+#define Y2_GPIO_NUM 13
+#define VSYNC_GPIO_NUM 21
+#define HREF_GPIO_NUM 38
+#define PCLK_GPIO_NUM 11
+
+#elif defined(CAMERA_MODEL_ESP32S2_CAM_BOARD)
+// The 18 pin header on the board has Y5 and Y3 swapped
+#define USE_BOARD_HEADER 0
+#define PWDN_GPIO_NUM 1
+#define RESET_GPIO_NUM 2
+#define XCLK_GPIO_NUM 42
+#define SIOD_GPIO_NUM 41
+#define SIOC_GPIO_NUM 18
+
+#define Y9_GPIO_NUM 16
+#define Y8_GPIO_NUM 39
+#define Y7_GPIO_NUM 40
+#define Y6_GPIO_NUM 15
+#if USE_BOARD_HEADER
+#define Y5_GPIO_NUM 12
+#else
+#define Y5_GPIO_NUM 13
+#endif
+#define Y4_GPIO_NUM 5
+#if USE_BOARD_HEADER
+#define Y3_GPIO_NUM 13
+#else
+#define Y3_GPIO_NUM 12
+#endif
+#define Y2_GPIO_NUM 14
+#define VSYNC_GPIO_NUM 38
+#define HREF_GPIO_NUM 4
+#define PCLK_GPIO_NUM 3
+
+#elif defined(CAMERA_MODEL_ESP32S3_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 15
+#define SIOD_GPIO_NUM 4
+#define SIOC_GPIO_NUM 5
+
+#define Y2_GPIO_NUM 11
+#define Y3_GPIO_NUM 9
+#define Y4_GPIO_NUM 8
+#define Y5_GPIO_NUM 10
+#define Y6_GPIO_NUM 12
+#define Y7_GPIO_NUM 18
+#define Y8_GPIO_NUM 17
+#define Y9_GPIO_NUM 16
+
+#define VSYNC_GPIO_NUM 6
+#define HREF_GPIO_NUM 7
+#define PCLK_GPIO_NUM 13
+
+#elif defined(CAMERA_MODEL_ESP32S3_WROVER)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 21
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y2_GPIO_NUM 4
+#define Y3_GPIO_NUM 5
+#define Y4_GPIO_NUM 18
+#define Y5_GPIO_NUM 19
+#define Y6_GPIO_NUM 36
+#define Y7_GPIO_NUM 39
+#define Y8_GPIO_NUM 34
+#define Y9_GPIO_NUM 35
+
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_ESP32S3_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 15
+#define SIOD_GPIO_NUM 4
+#define SIOC_GPIO_NUM 5
+
+#define Y2_GPIO_NUM 11
+#define Y3_GPIO_NUM 9
+#define Y4_GPIO_NUM 8
+#define Y5_GPIO_NUM 10
+#define Y6_GPIO_NUM 12
+#define Y7_GPIO_NUM 18
+#define Y8_GPIO_NUM 17
+#define Y9_GPIO_NUM 16
+
+#define VSYNC_GPIO_NUM 6
+#define HREF_GPIO_NUM 7
+#define PCLK_GPIO_NUM 13
+
+#else
+#error "Camera model not selected"
+#endif

esp32/include/peripherals/camera_service.h

@@ -0,0 +1,51 @@
+#ifndef CameraService_h
+#define CameraService_h
+
+#include <ArduinoJson.h>
+#include <PsychicHttp.h>
+#include <WiFi.h>
+#include <async_worker.h>
+
+#include <features.h>
+#include <task_manager.h>
+#include <template/stateful_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_endpoint.h>
+
+#include <settings/camera_settings.h>
+
+namespace Camera {
+
+#include <esp_camera.h>
+
+#if USE_CAMERA
+#include <peripherals/camera_pins.h>
+#endif
+
+#define PART_BOUNDARY "frame"
+
+#define EVENT_CAMERA_SETTINGS "CameraSettings"
+
+camera_fb_t *safe_camera_fb_get();
+sensor_t *safe_sensor_get();
+void safe_sensor_return();
+
+class CameraService : public StatefulService<CameraSettings> {
+  public:
+    CameraService();
+
+    esp_err_t begin();
+
+    esp_err_t cameraStill(PsychicRequest *request);
+    esp_err_t cameraStream(PsychicRequest *request);
+
+    StatefulHttpEndpoint<CameraSettings> endpoint;
+
+  private:
+    EventEndpoint<CameraSettings> _eventEndpoint;
+    FSPersistence<CameraSettings> _persistence;
+    void updateCamera();
+};
+} // namespace Camera
+
+#endif // end CameraService_h

esp32/include/peripherals/imu.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#if FT_ENABLED(USE_MPU6050)
+#include <MPU6050_6Axis_MotionApps612.h>
+#endif
+
+#if FT_ENABLED(USE_BNO055)
+#include <Adafruit_BNO055.h>
+#endif
+
+class IMU {
+  public:
+    IMU()
+#if FT_ENABLED(USE_BNO055)
+        : _imu(55, 0x29)
+#endif
+    {
+    }
+    bool initialize() {
+#if FT_ENABLED(USE_MPU6050)
+        _imu.initialize();
+        imu_success = _imu.testConnection();
+        devStatus = _imu.dmpInitialize();
+        if (!imu_success) return false;
+        _imu.setDMPEnabled(true);
+        _imu.setI2CMasterModeEnabled(false);
+        _imu.setI2CBypassEnabled(true);
+        _imu.setSleepEnabled(false);
+#endif
+#if FT_ENABLED(USE_BNO055)
+        imu_success = _imu.begin();
+        if (!imu_success) {
+            return false;
+        }
+        _imu.setExtCrystalUse(true);
+#endif
+        return true;
+    }
+
+    bool readIMU() {
+        if (!imu_success) return false;
+#if FT_ENABLED(USE_MPU6050)
+        bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
+        _imu.dmpGetQuaternion(&q, fifoBuffer);
+        _imu.dmpGetGravity(&gravity, &q);
+        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] *= 180 / M_PI;
+        ypr[1] *= 180 / M_PI;
+        ypr[2] *= 180 / M_PI;
+        return updated;
+#endif
+#if FT_ENABLED(USE_BNO055)
+        sensors_event_t event;
+        _imu.getEvent(&event);
+        ypr[0] = (float)event.orientation.x;
+        ypr[1] = (float)event.orientation.y;
+        ypr[2] = (float)event.orientation.z;
+#endif
+        return true;
+    }
+
+    float getTemperature() { return imu_success ? imu_temperature : -1; }
+
+    float getAngleX() { return imu_success ? ypr[0] : 0; }
+
+    float getAngleY() { return imu_success ? ypr[1] : 0; }
+
+    float getAngleZ() { return imu_success ? ypr[2] : 0; }
+
+    void readIMU(JsonObject& root) {
+        if (!imu_success) return;
+        root["x"] = round2(getAngleX());
+        root["y"] = round2(getAngleY());
+        root["z"] = round2(getAngleZ());
+    }
+
+    bool active() { return imu_success; }
+
+  private:
+#if FT_ENABLED(USE_MPU6050)
+    MPU6050 _imu;
+    uint8_t devStatus {false};
+    Quaternion q;
+    uint8_t fifoBuffer[64];
+    VectorFloat gravity;
+#endif
+#if FT_ENABLED(USE_BNO055)
+    Adafruit_BNO055 _imu;
+#endif
+    bool imu_success {false};
+    float ypr[3];
+    float imu_temperature {-1};
+};

esp32/include/peripherals/led_service.h

@@ -0,0 +1,56 @@
+#ifndef LEDService_h
+#define LEDService_h
+
+#include <FastLED.h>
+
+#ifndef WS2812_PIN
+#define WS2812_PIN 12
+#endif
+
+#ifndef WS2812_NUM_LEDS
+#define WS2812_NUM_LEDS 1 + 12
+#endif
+
+#define COLOR_ORDER GRB
+#define CHIPSET WS2811
+
+class LEDService {
+  private:
+    CRGB leds[WS2812_NUM_LEDS];
+    CRGBPalette16 currentPalette;
+    TBlendType currentBlending;
+
+    int _brightness = 255;
+    int direction = 1;
+
+  public:
+    LEDService() {
+        FastLED.addLeds<CHIPSET, WS2812_PIN, COLOR_ORDER>(leds, WS2812_NUM_LEDS).setCorrection(TypicalLEDStrip);
+        currentPalette = OceanColors_p;
+        currentBlending = LINEARBLEND;
+    }
+    ~LEDService() {}
+
+    void loop() {
+        EXECUTE_EVERY_N_MS(1000 / 60, {
+            if (_brightness >= 200) direction = -5;
+            if (_brightness <= 50) direction = 5;
+            _brightness += direction;
+            if (WiFi.isConnected()) {
+                fillFromPallette(OceanColors_p, 0);
+            } else {
+                fillFromPallette(ForestColors_p, 128);
+            }
+            FastLED.show();
+        });
+    }
+
+    void fillFromPallette(CRGBPalette16 colorPalette, uint8_t colorIndex) {
+        CRGB color = ColorFromPalette(colorPalette, colorIndex, _brightness, currentBlending);
+        for (int i = 0; i < WS2812_NUM_LEDS; ++i) {
+            leds[i] = color;
+        }
+    }
+};
+
+#endif

esp32/include/peripherals/magnetometer.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#include <Adafruit_HMC5883_U.h>
+#include <Adafruit_Sensor.h>
+
+class Magnetometer {
+  public:
+    Magnetometer() : _mag(12345) {}
+
+    bool initialize() {
+        mag_success = _mag.begin();
+        return mag_success;
+    }
+
+    bool readMagnetometer() {
+        if (!mag_success) return false;
+        sensors_event_t event;
+        bool updated = _mag.getEvent(&event);
+        if (!updated) return false;
+        ypr[0] = event.magnetic.x;
+        ypr[1] = event.magnetic.y;
+        ypr[2] = event.magnetic.z;
+        heading = atan2(event.magnetic.y, event.magnetic.x);
+        heading += declinationAngle;
+        if (heading < 0) heading += 2 * PI;
+        if (heading > 2 * PI) heading -= 2 * PI;
+        heading *= 180 / M_PI;
+        return true;
+    }
+
+    float getMagX() { return mag_success ? ypr[0] : 0; }
+
+    float getMagY() { return mag_success ? ypr[1] : 0; }
+
+    float getMagZ() { return mag_success ? ypr[2] : 0; }
+
+    float getHeading() { return heading; }
+
+    void readMagnetometer(JsonObject& root) {
+        if (!mag_success) return;
+        root["heading"] = round2(getHeading());
+    }
+
+    bool active() { return mag_success; }
+
+  private:
+    Adafruit_HMC5883_Unified _mag;
+    bool mag_success {false};
+    float ypr[3];
+    float heading {0};
+
+    const float declinationAngle = 0.22;
+};

esp32/include/peripherals/peripherals.h

@@ -0,0 +1,237 @@
+#ifndef Peripherals_h
+#define Peripherals_h
+
+#include <template/stateful_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_service.h>
+#include <utils/math_utils.h>
+#include <utils/timing.h>
+#include <filesystem.h>
+#include <features.h>
+#include <settings/peripherals_settings.h>
+#include <template/stateful_endpoint.h>
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+
+#include <NewPing.h>
+#include <peripherals/imu.h>
+#include <peripherals/magnetometer.h>
+#include <peripherals/barometer.h>
+
+#define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
+
+#define EVENT_I2C_SCAN "i2cScan"
+
+#define I2C_INTERVAL 250
+#define MAX_ESP_IMU_SIZE 500
+#define EVENT_IMU "imu"
+
+/*
+ * OLED Settings
+ */
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 64
+#define SCREEN_RESET -1
+
+/*
+ * Ultrasonic Sensor Settings
+ */
+#define MAX_DISTANCE 200
+
+class Peripherals : public StatefulService<PeripheralsConfiguration> {
+  public:
+    Peripherals()
+        : endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
+          _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
+                         EVENT_CONFIGURATION_SETTINGS),
+          _persistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, DEVICE_CONFIG_FILE) {
+        _accessMutex = xSemaphoreCreateMutex();
+        addUpdateHandler([&](const String &originId) { updatePins(); }, false);
+    };
+
+    void begin() {
+        _eventEndpoint.begin();
+        _persistence.readFromFS();
+
+        // socket.onEvent(EVENT_I2C_SCAN, [&](JsonObject &root, int originId) {
+        //     scanI2C();
+        //     emitI2C();
+        // });
+
+        // socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
+        //     scanI2C();
+        //     emitI2C(originId, sync);
+        // });
+
+        updatePins();
+
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+#endif
+#if FT_ENABLED(USE_HMC5883)
+        if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
+#endif
+#if FT_ENABLED(USE_BMP180)
+        if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+#endif
+#if FT_ENABLED(USE_USS)
+        _left_sonar = new NewPing(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
+        _right_sonar = new NewPing(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
+#endif
+    };
+
+    void loop() {
+        EXECUTE_EVERY_N_MS(_updateInterval, {
+            beginTransaction();
+            emitIMU();
+            readSonar();
+            emitSonar();
+            endTransaction();
+        });
+    }
+
+    void updatePins() {
+        if (i2c_active) {
+            Wire.end();
+        }
+
+        if (state().sda != -1 && state().scl != -1) {
+            Wire.begin(state().sda, state().scl, state().frequency);
+            i2c_active = true;
+        }
+    }
+
+    void emitI2C(const String &originId = "", bool sync = false) {
+        char output[150];
+        JsonDocument doc;
+        JsonObject root = doc.to<JsonObject>();
+        root["sda"] = state().sda;
+        root["scl"] = state().scl;
+        JsonArray addresses = root["addresses"].to<JsonArray>();
+        for (auto &address : addressList) {
+            addresses.add(address);
+        }
+        serializeJson(root, output);
+        ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
+        // socket.emit(EVENT_I2C_SCAN, output, originId.c_str(), sync);
+    }
+
+    void scanI2C(uint8_t lower = 1, uint8_t higher = 127) {
+        addressList.clear();
+        for (uint8_t address = lower; address < higher; address++) {
+            Wire.beginTransmission(address);
+            if (Wire.endTransmission() == 0) {
+                addressList.emplace_back(address);
+                ESP_LOGI("Peripherals", "I2C device found at address 0x%02X", address);
+            }
+        }
+        uint8_t nDevices = addressList.size();
+        if (nDevices == 0)
+            ESP_LOGI("Peripherals", "No I2C devices found");
+        else
+            ESP_LOGI("Peripherals", "Scan complete - Found %d devices", nDevices);
+    }
+
+    /* IMU FUNCTIONS */
+    bool readIMU() {
+        bool updated = false;
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        beginTransaction();
+        updated = _imu.readIMU();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    bool readMag() {
+        bool updated = false;
+#if FT_ENABLED(USE_HMC5883)
+        beginTransaction();
+        updated = _mag.readMagnetometer();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    bool readBMP() {
+        bool updated = false;
+#if FT_ENABLED(USE_BMP180)
+        beginTransaction();
+        updated = _bmp.readBarometer();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    void readSonar() {
+#if FT_ENABLED(USE_USS)
+        _left_distance = _left_sonar->ping_cm();
+        delay(50);
+        _right_distance = _right_sonar->ping_cm();
+#endif
+    }
+
+    float leftDistance() { return _left_distance; }
+    float rightDistance() { return _right_distance; }
+
+    StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
+
+    void emitIMU() {
+        doc.clear();
+        JsonObject root = doc.to<JsonObject>();
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        _imu.readIMU(root);
+#endif
+#if FT_ENABLED(USE_HMC5883)
+        _mag.readMagnetometer(root);
+#endif
+#if FT_ENABLED(USE_BMP180)
+        _bmp.readBarometer(root);
+#endif
+        serializeJson(doc, message);
+        // socket.emit(EVENT_IMU, message);
+    }
+
+    void emitSonar() {
+#if FT_ENABLED(USE_USS)
+        char output[16];
+        snprintf(output, sizeof(output), "[%.1f,%.1f]", _left_distance, _right_distance);
+        // socket.emit("sonar", output);
+#endif
+    }
+
+  private:
+    EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
+    FSPersistence<PeripheralsConfiguration> _persistence;
+
+    SemaphoreHandle_t _accessMutex;
+    inline void beginTransaction() { xSemaphoreTakeRecursive(_accessMutex, portMAX_DELAY); }
+
+    inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); }
+
+    JsonDocument doc;
+    char message[MAX_ESP_IMU_SIZE];
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    IMU _imu;
+#endif
+#if FT_ENABLED(USE_HMC5883)
+    Magnetometer _mag;
+#endif
+#if FT_ENABLED(USE_BMP180)
+    Barometer _bmp;
+#endif
+#if FT_ENABLED(USE_USS)
+    NewPing *_left_sonar;
+    NewPing *_right_sonar;
+#endif
+    float _left_distance {MAX_DISTANCE};
+    float _right_distance {MAX_DISTANCE};
+
+    std::list<uint8_t> addressList;
+    bool i2c_active = false;
+    unsigned long _updateInterval {I2C_INTERVAL};
+};
+
+#endif

esp32/include/peripherals/servo_controller.h

@@ -0,0 +1,145 @@
+#ifndef ServoController_h
+#define ServoController_h
+
+#include <Adafruit_PWMServoDriver.h>
+#include <event_bus.hpp>
+#include <template/stateful_persistence.h>
+#include <template/stateful_service.h>
+#include <template/stateful_endpoint.h>
+#include <utils/math_utils.h>
+#include <settings/servo_settings.h>
+
+/*
+ * Servo Settings
+ */
+#ifndef FACTORY_SERVO_PWM_FREQUENCY
+#define FACTORY_SERVO_PWM_FREQUENCY 50
+#endif
+
+#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
+#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
+#endif
+
+#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
+#define EVENT_SERVO_STATE "servoState"
+
+enum class SERVO_CONTROL_STATE { DEACTIVATED, PWM, ANGLE };
+
+class ServoController : public StatefulService<ServoSettings> {
+  public:
+    ServoController()
+        : endpoint(ServoSettings::read, ServoSettings::update, this),
+          _persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
+
+    void begin() {
+        // socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
+        //                [&](JsonObject &root, int originId) { servoEvent(root, originId); });
+        // socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
+        _persistence.readFromFS();
+
+        initializePCA();
+        // socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
+        //     is_active = root["active"] | false;
+        //     is_active ? activate() : deactivate();
+        // });
+    }
+
+    void pcaWrite(int index, int value) {
+        if (value < 0 || value > 4096) {
+            ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
+            return;
+        }
+        _pca.setPWM(index, 0, value);
+    }
+
+    void activate() {
+        if (is_active) return;
+        control_state = SERVO_CONTROL_STATE::ANGLE;
+        is_active = true;
+        _pca.wakeup();
+    }
+
+    void deactivate() {
+        if (!is_active) return;
+        is_active = false;
+        control_state = SERVO_CONTROL_STATE::DEACTIVATED;
+        _pca.sleep();
+    }
+
+    void stateUpdate(JsonObject &root, int originId) {
+        bool active = root["active"].as<bool>();
+        ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
+        active ? activate() : deactivate();
+    }
+
+    void servoEvent(JsonObject &root, int originId) {
+        control_state = SERVO_CONTROL_STATE::PWM;
+        int8_t servo_id = root["servo_id"];
+        uint16_t pwm = root["pwm"].as<uint16_t>();
+        if (servo_id < 0) {
+            uint16_t pwms[12];
+            std::fill_n(pwms, 12, pwm);
+            _pca.setMultiplePWM(pwms, 12);
+        } else {
+            _pca.setPWM(servo_id, 0, pwm);
+        }
+        ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
+    }
+
+    void syncAngles(const String &originId) {
+        char output[100];
+        snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
+                 angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
+                 angles[10], angles[11]);
+        // socket.emit("angles", output, String(originId).c_str());
+    }
+
+    void updateActiveState() { is_active ? activate() : deactivate(); }
+
+    void setAngles(float new_angles[12]) {
+        control_state = SERVO_CONTROL_STATE::ANGLE;
+        for (int i = 0; i < 12; i++) {
+            target_angles[i] = new_angles[i];
+        }
+    }
+
+    void calculatePWM() {
+        uint16_t pwms[12];
+        for (int i = 0; i < 12; i++) {
+            angles[i] = lerp(angles[i], target_angles[i], 0.05);
+            auto &servo = state().servos[i];
+            float angle = servo.direction * angles[i] + servo.centerAngle;
+            uint16_t pwm = angle * servo.conversion + servo.centerPwm;
+            if (pwm < 125 || pwm > 600) {
+                continue;
+            }
+            pwms[i] = pwm;
+        }
+        _pca.setMultiplePWM(pwms, 12);
+    }
+
+    void updateServoState() {
+        if (control_state == SERVO_CONTROL_STATE::ANGLE) calculatePWM();
+    }
+
+    StatefulHttpEndpoint<ServoSettings> endpoint;
+
+  private:
+    void initializePCA() {
+        _pca.begin();
+        _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
+        _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
+        _pca.sleep();
+    }
+    FSPersistence<ServoSettings> _persistence;
+
+    Adafruit_PWMServoDriver _pca;
+
+    SERVO_CONTROL_STATE control_state = SERVO_CONTROL_STATE::DEACTIVATED;
+
+    bool is_active {false};
+    float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+};
+
+#endif

esp32/include/settings/ap_settings.h

@@ -0,0 +1,115 @@
+#pragma once
+
+#include <WiFi.h>
+#include <IPAddress.h>
+#include <ArduinoJson.h>
+#include <utils/json_utils.h>
+#include <utils/string_utils.h>
+#include <utils/ip_utils.h>
+#include <template/state_result.h>
+
+#include <DNSServer.h>
+#include <IPAddress.h>
+
+#ifndef FACTORY_AP_PROVISION_MODE
+#define FACTORY_AP_PROVISION_MODE AP_MODE_DISCONNECTED
+#endif
+
+#ifndef FACTORY_AP_SSID
+#define FACTORY_AP_SSID "ESP32-SvelteKit-#{unique_id}"
+#endif
+
+#ifndef FACTORY_AP_PASSWORD
+#define FACTORY_AP_PASSWORD "esp-sveltekit"
+#endif
+
+#ifndef FACTORY_AP_LOCAL_IP
+#define FACTORY_AP_LOCAL_IP "192.168.4.1"
+#endif
+
+#ifndef FACTORY_AP_GATEWAY_IP
+#define FACTORY_AP_GATEWAY_IP "192.168.4.1"
+#endif
+
+#ifndef FACTORY_AP_SUBNET_MASK
+#define FACTORY_AP_SUBNET_MASK "255.255.255.0"
+#endif
+
+#ifndef FACTORY_AP_CHANNEL
+#define FACTORY_AP_CHANNEL 1
+#endif
+
+#ifndef FACTORY_AP_SSID_HIDDEN
+#define FACTORY_AP_SSID_HIDDEN false
+#endif
+
+#ifndef FACTORY_AP_MAX_CLIENTS
+#define FACTORY_AP_MAX_CLIENTS 4
+#endif
+
+#define AP_MODE_ALWAYS 0
+#define AP_MODE_DISCONNECTED 1
+#define AP_MODE_NEVER 2
+
+#define MANAGE_NETWORK_DELAY 10000
+#define DNS_PORT 53
+
+enum APNetworkStatus { ACTIVE = 0, INACTIVE, LINGERING };
+
+class APSettings {
+  public:
+    uint8_t provisionMode;
+    String ssid;
+    String password;
+    uint8_t channel;
+    bool ssidHidden;
+    uint8_t maxClients;
+
+    IPAddress localIP;
+    IPAddress gatewayIP;
+    IPAddress subnetMask;
+
+    bool operator==(const APSettings &settings) const {
+        return provisionMode == settings.provisionMode && ssid == settings.ssid && password == settings.password &&
+               channel == settings.channel && ssidHidden == settings.ssidHidden && maxClients == settings.maxClients &&
+               localIP == settings.localIP && gatewayIP == settings.gatewayIP && subnetMask == settings.subnetMask;
+    }
+
+    static void read(APSettings &settings, JsonObject &root) {
+        root["provision_mode"] = settings.provisionMode;
+        root["ssid"] = settings.ssid;
+        root["password"] = settings.password;
+        root["channel"] = settings.channel;
+        root["ssid_hidden"] = settings.ssidHidden;
+        root["max_clients"] = settings.maxClients;
+        root["local_ip"] = settings.localIP.toString();
+        root["gateway_ip"] = settings.gatewayIP.toString();
+        root["subnet_mask"] = settings.subnetMask.toString();
+    }
+
+    static StateUpdateResult update(JsonObject &root, APSettings &settings) {
+        APSettings newSettings = {};
+        newSettings.provisionMode = root["provision_mode"] | FACTORY_AP_PROVISION_MODE;
+        switch (settings.provisionMode) {
+            case AP_MODE_ALWAYS:
+            case AP_MODE_DISCONNECTED:
+            case AP_MODE_NEVER: break;
+            default: newSettings.provisionMode = AP_MODE_DISCONNECTED;
+        }
+        newSettings.ssid = root["ssid"] | format(FACTORY_AP_SSID);
+        newSettings.password = root["password"] | FACTORY_AP_PASSWORD;
+        newSettings.channel = root["channel"] | FACTORY_AP_CHANNEL;
+        newSettings.ssidHidden = root["ssid_hidden"] | FACTORY_AP_SSID_HIDDEN;
+        newSettings.maxClients = root["max_clients"] | FACTORY_AP_MAX_CLIENTS;
+
+        JsonUtils::readIP(root, "local_ip", newSettings.localIP, FACTORY_AP_LOCAL_IP);
+        JsonUtils::readIP(root, "gateway_ip", newSettings.gatewayIP, FACTORY_AP_GATEWAY_IP);
+        JsonUtils::readIP(root, "subnet_mask", newSettings.subnetMask, FACTORY_AP_SUBNET_MASK);
+
+        if (newSettings == settings) {
+            return StateUpdateResult::UNCHANGED;
+        }
+        settings = newSettings;
+        return StateUpdateResult::CHANGED;
+    }
+};

esp32/include/settings/camera_settings.h

@@ -0,0 +1,109 @@
+#pragma once
+
+namespace Camera {
+
+#include <ArduinoJson.h>
+#include <template/state_result.h>
+#include <esp_camera.h>
+
+class CameraSettings {
+  public:
+    pixformat_t pixformat;
+    framesize_t framesize; // 0 - 10
+    uint8_t quality;       // 0 - 63
+    int8_t brightness;     //-2 - 2
+    int8_t contrast;       //-2 - 2
+    int8_t saturation;     //-2 - 2
+    int8_t sharpness;      //-2 - 2
+    uint8_t denoise;
+    gainceiling_t gainceiling;
+    uint8_t whitebal;
+    uint8_t special_effect; // 0 - 6
+    uint8_t wb_mode;        // 0 - 4
+    uint8_t awb;
+    uint8_t exposure_ctrl;
+    uint8_t awb_gain;
+    uint8_t gain_ctrl;
+    uint8_t aec;
+    uint8_t aec2;
+    int8_t ae_level;    //-2 - 2
+    uint16_t aec_value; // 0 - 1200
+    uint8_t agc;
+    uint8_t agc_gain; // 0 - 30
+    uint8_t bpc;
+    uint8_t wpc;
+    uint8_t raw_gma;
+    uint8_t lenc;
+    uint8_t hmirror;
+    uint8_t vflip;
+    uint8_t dcw;
+    uint8_t colorbar;
+
+    static void read(CameraSettings &settings, JsonObject &root) {
+        root["pixformat"] = settings.pixformat;
+        root["framesize"] = settings.framesize;
+        root["quality"] = settings.quality;
+        root["brightness"] = settings.brightness;
+        root["contrast"] = settings.contrast;
+        root["saturation"] = settings.saturation;
+        root["sharpness"] = settings.sharpness;
+        root["denoise"] = settings.denoise;
+        root["special_effect"] = settings.special_effect;
+        root["wb_mode"] = settings.wb_mode;
+        root["exposure_ctrl"] = settings.exposure_ctrl;
+        root["gain_ctrl"] = settings.gain_ctrl;
+        root["awb"] = settings.awb;
+        root["awb_gain"] = settings.awb_gain;
+        root["aec"] = settings.aec;
+        root["aec2"] = settings.aec2;
+        root["ae_level"] = settings.ae_level;
+        root["aec_value"] = settings.aec_value;
+        root["agc"] = settings.agc;
+        root["agc_gain"] = settings.agc_gain;
+        root["gainceiling"] = settings.gainceiling;
+        root["bpc"] = settings.bpc;
+        root["wpc"] = settings.wpc;
+        root["raw_gma"] = settings.raw_gma;
+        root["lenc"] = settings.lenc;
+        root["hmirror"] = settings.hmirror;
+        root["vflip"] = settings.vflip;
+        root["dcw"] = settings.dcw;
+        root["colorbar"] = settings.colorbar;
+    }
+
+    static StateUpdateResult update(JsonObject &root, CameraSettings &settings) {
+        settings.pixformat = root["pixformat"];
+        settings.framesize = root["framesize"];
+        settings.brightness = root["brightness"];
+        settings.contrast = root["contrast"];
+        settings.quality = root["quality"];
+        settings.contrast = root["contrast"];
+        settings.saturation = root["saturation"];
+        settings.sharpness = root["sharpness"];
+        settings.denoise = root["denoise"];
+        settings.exposure_ctrl = root["exposure_ctrl"];
+        settings.gain_ctrl = root["gain_ctrl"];
+        settings.special_effect = root["special_effect"];
+        settings.wb_mode = root["wb_mode"];
+        settings.awb = root["awb"];
+        settings.awb_gain = root["awb_gain"];
+        settings.aec = root["aec"];
+        settings.aec2 = root["aec2"];
+        settings.ae_level = root["ae_level"];
+        settings.aec_value = root["aec_value"];
+        settings.agc = root["agc"];
+        settings.agc_gain = root["agc_gain"];
+        settings.gainceiling = root["gainceiling"];
+        settings.bpc = root["bpc"];
+        settings.wpc = root["wpc"];
+        settings.raw_gma = root["raw_gma"];
+        settings.lenc = root["lenc"];
+        settings.hmirror = root["hmirror"];
+        settings.vflip = root["vflip"];
+        settings.dcw = root["dcw"];
+        settings.colorbar = root["colorbar"];
+
+        return StateUpdateResult::CHANGED;
+    };
+};
+} // namespace Camera

esp32/include/settings/mdns_settings.h

@@ -0,0 +1,139 @@
+#pragma once
+
+#include <ArduinoJson.h>
+#include <utils/json_utils.h>
+#include <utils/string_utils.h>
+#include <template/state_result.h>
+#include <filesystem.h>
+
+#ifndef FACTORY_MDNS_HOSTNAME
+#define FACTORY_MDNS_HOSTNAME "esp32"
+#endif
+
+#ifndef FACTORY_MDNS_INSTANCE
+#define FACTORY_MDNS_INSTANCE "ESP32 Device"
+#endif
+
+typedef struct {
+    String key;
+    String value;
+
+    void serialize(JsonObject &json) const {
+        json["key"] = key;
+        json["value"] = value;
+    }
+
+    bool deserialize(const JsonObject &json) {
+        key = json["key"].as<String>();
+        value = json["value"].as<String>();
+
+        return key.length() > 0;
+    }
+} mdns_txt_record_t;
+
+typedef struct {
+    String service;
+    String protocol;
+    uint16_t port;
+    std::vector<mdns_txt_record_t> txtRecords;
+
+    void serialize(JsonObject &json) const {
+        json["service"] = service;
+        json["protocol"] = protocol;
+        json["port"] = port;
+
+        if (txtRecords.size() > 0) {
+            JsonArray txtArray = json["txt_records"].to<JsonArray>();
+            for (const auto &txt : txtRecords) {
+                JsonObject txtObj = txtArray.add<JsonObject>();
+                txt.serialize(txtObj);
+            }
+        }
+    }
+
+    bool deserialize(const JsonObject &json) {
+        service = json["service"].as<String>();
+        protocol = json["protocol"].as<String>();
+        port = json["port"] | 0;
+
+        txtRecords.clear();
+        if (json["txt_records"].is<JsonArray>()) {
+            JsonArray txtArray = json["txt_records"];
+            for (JsonObject txtObj : txtArray) {
+                mdns_txt_record_t txt;
+                if (txt.deserialize(txtObj)) {
+                    txtRecords.push_back(txt);
+                }
+            }
+        }
+
+        return service.length() > 0 && protocol.length() > 0 && port > 0;
+    }
+} mdns_service_t;
+
+class MDNSSettings {
+  public:
+    String hostname;
+    String instance;
+    std::vector<mdns_service_t> services;
+    std::vector<mdns_txt_record_t> globalTxtRecords;
+
+    static void read(MDNSSettings &settings, JsonObject &root) {
+        root["hostname"] = settings.hostname;
+        root["instance"] = settings.instance;
+
+        JsonArray servicesArray = root["services"].to<JsonArray>();
+        for (const auto &service : settings.services) {
+            JsonObject serviceObj = servicesArray.add<JsonObject>();
+            service.serialize(serviceObj);
+        }
+
+        JsonArray txtArray = root["global_txt_records"].to<JsonArray>();
+        for (const auto &txt : settings.globalTxtRecords) {
+            JsonObject txtObj = txtArray.add<JsonObject>();
+            txt.serialize(txtObj);
+        }
+    }
+
+    static StateUpdateResult update(JsonObject &root, MDNSSettings &settings) {
+        settings.hostname = root["hostname"] | FACTORY_MDNS_HOSTNAME;
+        settings.instance = root["instance"] | FACTORY_MDNS_INSTANCE;
+
+        settings.services.clear();
+        if (root["services"].is<JsonArray>()) {
+            JsonArray servicesArray = root["services"];
+            for (JsonObject serviceObj : servicesArray) {
+                mdns_service_t service;
+                if (service.deserialize(serviceObj)) {
+                    settings.services.push_back(service);
+                }
+            }
+        }
+
+        if (settings.services.empty()) {
+            mdns_service_t httpService = {.service = "http", .protocol = "tcp", .port = 80};
+            settings.services.push_back(httpService);
+
+            mdns_service_t wsService = {.service = "ws", .protocol = "tcp", .port = 80};
+            settings.services.push_back(wsService);
+        }
+
+        settings.globalTxtRecords.clear();
+        if (root["global_txt_records"].is<JsonArray>()) {
+            JsonArray txtArray = root["global_txt_records"];
+            for (JsonObject txtObj : txtArray) {
+                mdns_txt_record_t txt;
+                if (txt.deserialize(txtObj)) {
+                    settings.globalTxtRecords.push_back(txt);
+                }
+            }
+        }
+
+        if (settings.globalTxtRecords.empty()) {
+            mdns_txt_record_t firmwareVersion = {.key = "Firmware Version", .value = APP_VERSION};
+            settings.globalTxtRecords.push_back(firmwareVersion);
+        }
+
+        return StateUpdateResult::CHANGED;
+    }
+};

esp32/include/settings/ntp_settings.h

@@ -0,0 +1,42 @@
+#include <Arduino.h>
+#include <template/state_result.h>
+#include <ArduinoJson.h>
+
+#ifndef FACTORY_NTP_ENABLED
+#define FACTORY_NTP_ENABLED true
+#endif
+
+#ifndef FACTORY_NTP_TIME_ZONE_LABEL
+#define FACTORY_NTP_TIME_ZONE_LABEL "Europe/London"
+#endif
+
+#ifndef FACTORY_NTP_TIME_ZONE_FORMAT
+#define FACTORY_NTP_TIME_ZONE_FORMAT "GMT0BST,M3.5.0/1,M10.5.0"
+#endif
+
+#ifndef FACTORY_NTP_SERVER
+#define FACTORY_NTP_SERVER "time.google.com"
+#endif
+
+class NTPSettings {
+  public:
+    bool enabled;
+    String tzLabel;
+    String tzFormat;
+    String server;
+
+    static void read(NTPSettings &settings, JsonObject &root) {
+        root["enabled"] = settings.enabled;
+        root["server"] = settings.server;
+        root["tz_label"] = settings.tzLabel;
+        root["tz_format"] = settings.tzFormat;
+    }
+
+    static StateUpdateResult update(JsonObject &root, NTPSettings &settings) {
+        settings.enabled = root["enabled"] | FACTORY_NTP_ENABLED;
+        settings.server = root["server"] | FACTORY_NTP_SERVER;
+        settings.tzLabel = root["tz_label"] | FACTORY_NTP_TIME_ZONE_LABEL;
+        settings.tzFormat = root["tz_format"] | FACTORY_NTP_TIME_ZONE_FORMAT;
+        return StateUpdateResult::CHANGED;
+    }
+};

esp32/include/settings/peripherals_settings.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <vector>
+#include <ArduinoJson.h>
+#include <template/state_result.h>
+
+/*
+ * I2C software connection
+ */
+#ifndef SDA_PIN
+#define SDA_PIN SDA
+#endif
+#ifndef SCL_PIN
+#define SCL_PIN SCL
+#endif
+#ifndef I2C_FREQUENCY
+#define I2C_FREQUENCY 1000000UL
+#endif
+
+class PinConfig {
+  public:
+    int pin;
+    String mode;
+    String type;
+    String role;
+
+    PinConfig(int p, String m, String t, String r) : pin(p), mode(m), type(t), role(r) {}
+};
+
+class PeripheralsConfiguration {
+  public:
+    int sda = SDA_PIN;
+    int scl = SCL_PIN;
+    long frequency = I2C_FREQUENCY;
+    std::vector<PinConfig> pins;
+
+    static void read(PeripheralsConfiguration &settings, JsonObject &root) {
+        root["sda"] = settings.sda;
+        root["scl"] = settings.scl;
+        root["frequency"] = settings.frequency;
+    }
+
+    static StateUpdateResult update(JsonObject &root, PeripheralsConfiguration &settings) {
+        settings.sda = root["sda"] | SDA_PIN;
+        settings.scl = root["scl"] | SCL_PIN;
+        settings.frequency = root["frequency"] | I2C_FREQUENCY;
+        return StateUpdateResult::CHANGED;
+    };
+};

esp32/include/settings/servo_settings.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <ArduinoJson.h>
+#include <template/state_result.h>
+
+typedef struct {
+    float centerPwm;
+    float direction;
+    float centerAngle;
+    float conversion;
+    const char *name;
+    // bool enable;
+} servo_settings_t;
+
+class ServoSettings {
+  public:
+    servo_settings_t servos[12] = {
+        {306, -1, 0, 2.2, "Servo1"}, {306, 1, -45, 2.1055555, "Servo2"},  {306, 1, 90, 1.96923, "Servo3"},
+        {306, -1, 0, 2.2, "Servo4"}, {306, -1, 45, 2.1055555, "Servo5"},  {306, -1, -90, 1.96923, "Servo6"},
+        {306, 1, 0, 2.2, "Servo7"},  {306, 1, -45, 2.1055555, "Servo8"},  {306, 1, 90, 1.96923, "Servo9"},
+        {306, 1, 0, 2.2, "Servo10"}, {306, -1, 45, 2.1055555, "Servo11"}, {306, -1, -90, 1.96923, "Servo12"}};
+    static void read(ServoSettings &settings, JsonObject &root) {
+        JsonArray servos = root["servos"].to<JsonArray>();
+        for (auto &servo : settings.servos) {
+            JsonObject newServo = servos.add<JsonObject>();
+            newServo["center_pwm"] = servo.centerPwm;
+            newServo["direction"] = servo.direction;
+            newServo["center_angle"] = servo.centerAngle;
+            newServo["conversion"] = servo.conversion;
+        }
+    }
+    static StateUpdateResult update(JsonObject &root, ServoSettings &settings) {
+        if (root["servos"].is<JsonArray>()) {
+            JsonArray servosJson = root["servos"];
+            int i = 0;
+            for (auto servo : servosJson) {
+                JsonObject servoObject = servo.as<JsonObject>();
+                uint8_t servoId = i; // servoObject["id"].as<uint8_t>();
+                settings.servos[servoId].centerPwm = servoObject["center_pwm"].as<float>();
+                settings.servos[servoId].centerAngle = servoObject["center_angle"].as<float>();
+                settings.servos[servoId].direction = servoObject["direction"].as<float>();
+                settings.servos[servoId].conversion = servoObject["conversion"].as<float>();
+                i++;
+            }
+        }
+        ESP_LOGI("ServoController", "Updating servo data");
+        return StateUpdateResult::CHANGED;
+    };
+};

esp32/include/settings/wifi_settings.h

@@ -0,0 +1,136 @@
+#pragma once
+
+#include <WiFi.h>
+#include <IPAddress.h>
+#include <ArduinoJson.h>
+#include <utils/json_utils.h>
+#include <utils/ip_utils.h>
+#include <utils/string_utils.h>
+#include <template/state_result.h>
+
+#ifndef FACTORY_WIFI_SSID
+#define FACTORY_WIFI_SSID ""
+#endif
+
+#ifndef FACTORY_WIFI_PASSWORD
+#define FACTORY_WIFI_PASSWORD ""
+#endif
+
+#ifndef FACTORY_WIFI_HOSTNAME
+#define FACTORY_WIFI_HOSTNAME "#{platform}-#{unique_id}"
+#endif
+
+#ifndef FACTORY_WIFI_RSSI_THRESHOLD
+#define FACTORY_WIFI_RSSI_THRESHOLD -80
+#endif
+
+typedef struct {
+    String ssid;
+    uint8_t bssid[6];
+    int32_t channel;
+    String password;
+    bool staticIPConfig;
+    IPAddress localIP;
+    IPAddress gatewayIP;
+    IPAddress subnetMask;
+    IPAddress dnsIP1;
+    IPAddress dnsIP2;
+    bool available;
+
+    void serialize(JsonObject &json) const {
+        json["ssid"] = ssid;
+        json["password"] = password;
+        json["static_ip_config"] = staticIPConfig;
+        if (staticIPConfig) {
+            JsonUtils::writeIP(json, "local_ip", localIP);
+            JsonUtils::writeIP(json, "gateway_ip", gatewayIP);
+            JsonUtils::writeIP(json, "subnet_mask", subnetMask);
+            JsonUtils::writeIP(json, "dns_ip_1", dnsIP1);
+            JsonUtils::writeIP(json, "dns_ip_2", dnsIP2);
+        }
+    }
+
+    bool deserialize(const JsonObject &json) {
+        String newSsid = json["ssid"].as<String>();
+        String newPassword = json["password"].as<String>();
+        if (newSsid.length() < 1 || newSsid.length() > 31 || newPassword.length() > 64) {
+            ESP_LOGE("WiFiSettings", "SSID or password length is invalid");
+            return false;
+        }
+        ssid = newSsid;
+        password = newPassword;
+        staticIPConfig = json["static_ip_config"] | false;
+        if (staticIPConfig) {
+            JsonUtils::readIP(json, "local_ip", localIP);
+            JsonUtils::readIP(json, "gateway_ip", gatewayIP);
+            JsonUtils::readIP(json, "subnet_mask", subnetMask);
+            JsonUtils::readIP(json, "dns_ip_1", dnsIP1);
+            JsonUtils::readIP(json, "dns_ip_2", dnsIP2);
+            if (IPUtils::isNotSet(dnsIP1) && IPUtils::isSet(dnsIP2)) {
+                dnsIP1 = dnsIP2;
+                dnsIP2 = INADDR_NONE;
+            }
+            if (IPUtils::isNotSet(localIP) || IPUtils::isNotSet(gatewayIP) || IPUtils::isNotSet(subnetMask)) {
+                staticIPConfig = false;
+                ESP_LOGW("WiFiSettings", "Invalid static IP configuration - falling back to DHCP");
+            }
+        }
+        available = false;
+        return true;
+    }
+
+} wifi_settings_t;
+
+inline wifi_settings_t createDefaultWiFiSettings() {
+    return wifi_settings_t {.ssid = FACTORY_WIFI_SSID,
+                            .password = FACTORY_WIFI_PASSWORD,
+                            .staticIPConfig = false,
+                            .localIP = INADDR_NONE,
+                            .gatewayIP = INADDR_NONE,
+                            .subnetMask = INADDR_NONE,
+                            .dnsIP1 = INADDR_NONE,
+                            .dnsIP2 = INADDR_NONE,
+                            .available = false};
+}
+
+class WiFiSettings {
+  public:
+    String hostname;
+    bool priorityBySignalStrength;
+    std::vector<wifi_settings_t> wifiSettings;
+    static void read(WiFiSettings &settings, JsonObject &root) {
+        root["hostname"] = settings.hostname;
+        root["priority_RSSI"] = settings.priorityBySignalStrength;
+        JsonArray wifiNetworks = root["wifi_networks"].to<JsonArray>();
+        for (const auto &wifi : settings.wifiSettings) {
+            JsonObject wifiNetwork = wifiNetworks.add<JsonObject>();
+            wifi.serialize(wifiNetwork);
+        }
+        ESP_LOGV("WiFiSettings", "WiFi Settings read");
+    }
+    static StateUpdateResult update(JsonObject &root, WiFiSettings &settings) {
+        settings.hostname = root["hostname"] | FACTORY_WIFI_HOSTNAME;
+        settings.priorityBySignalStrength = root["priority_RSSI"] | true;
+        settings.wifiSettings.clear();
+        if (root["wifi_networks"].is<JsonArray>()) {
+            JsonArray wifiNetworks = root["wifi_networks"];
+            int networkCount = 0;
+            for (JsonObject wifiNetwork : wifiNetworks) {
+                if (networkCount >= 5) {
+                    ESP_LOGE("WiFiSettings", "Too many wifi networks");
+                    break;
+                }
+                wifi_settings_t newSettings;
+                if (newSettings.deserialize(wifiNetwork)) {
+                    settings.wifiSettings.push_back(newSettings);
+                    networkCount++;
+                }
+            }
+        } else if (String(FACTORY_WIFI_SSID).length() > 0) {
+            ESP_LOGI("WiFiSettings", "No WiFi config found - using factory settings");
+            settings.wifiSettings.push_back(createDefaultWiFiSettings());
+        }
+        ESP_LOGV("WiFiSettings", "WiFi Settings updated");
+        return StateUpdateResult::CHANGED;
+    }
+};

esp32/include/spot.h

@@ -14,7 +14,8 @@
 #include <peripherals/servo_controller.h>
 #include <peripherals/led_service.h>
 #include <peripherals/camera_service.h>
-#include <event_socket.h>
+#include <event_bus.hpp>
+#include <adapters/websocket.hpp>
 #include <features.h>
 #include <motion.h>
 #include <task_manager.h>
@@ -80,7 +81,6 @@ class Spot {
     PsychicHttpServer _server;
     WiFiService _wifiService;
     APService _apService;
-    EventSocket _socket;
     MDNSService _mdnsService;
 #if FT_ENABLED(USE_UPLOAD_FIRMWARE)
     FirmwareUploadService _uploadFirmwareService;
@@ -103,7 +103,7 @@ class Spot {
     bool updatedMotion = false;
 
     const char *_appName = APP_NAME;
-    const u_int16_t _numberEndpoints = 120;
+    const u_int16_t _numberEndpoints = 130;
     const u_int32_t _maxFileUpload = 2300000; // 2.3 MB
     const uint16_t _port = 80;
 

esp32/include/system_service.h

@@ -0,0 +1,33 @@
+#ifndef SYSTEM_SERVICE_H
+#define SYSTEM_SERVICE_H
+
+#include <ESPmDNS.h>
+#include <PsychicHttp.h>
+#include <WiFi.h>
+#include <task_manager.h>
+#include <event_bus.hpp>
+#include <filesystem.h>
+#include <global.h>
+
+#define MAX_ESP_ANALYTICS_SIZE 2024
+#define EVENT_ANALYTICS "analytics"
+
+namespace system_service {
+esp_err_t handleReset(PsychicRequest *request);
+esp_err_t handleRestart(PsychicRequest *request);
+esp_err_t handleSleep(PsychicRequest *request);
+esp_err_t getStatus(PsychicRequest *request);
+esp_err_t getMetrics(PsychicRequest *request);
+
+void reset();
+void restart();
+void sleep();
+void status(JsonObject &root);
+void metrics(JsonObject &root);
+
+void emitMetrics();
+
+const char *resetReason(esp_reset_reason_t reason);
+} // namespace system_service
+
+#endif

esp32/include/task_manager.h

@@ -0,0 +1,184 @@
+#pragma once
+
+#include <esp_task_wdt.h>
+#include <map>
+#include <string>
+#include <vector>
+#include <Arduino.h>
+
+#define IDLE_STACK_SIZE 2048
+#define DEFAULT_STACK_SIZE 2048 + 512
+
+#define DELETE_TASK(handle) \
+    if (handle != nullptr) vTaskDelete(handle)
+
+struct task_t {
+    String name;
+    TaskHandle_t handle;
+    uint32_t stackSize;
+    UBaseType_t priority;
+    BaseType_t coreId;
+    bool pinned;
+    bool active; // blocked ('B'), ready ('R'), deleted ('D') or suspended ('S').
+};
+
+class IdleTask {
+  private:
+    float idle_ratio = 0.0;
+    unsigned long last_measurement;
+    const int kMillisPerLoop = 1;
+    const int kMillisPerCalc = 1000;
+    unsigned long counter = 0;
+
+  public:
+    IdleTask() : last_measurement(millis()) {}
+
+    void ProcessIdleTime() {
+        last_measurement = millis();
+        counter = 0;
+
+        for (;;) {
+            unsigned long current_time = millis();
+            unsigned long delta = current_time - last_measurement;
+            if (delta >= kMillisPerCalc) {
+                idle_ratio = static_cast<float>(counter) / delta;
+                last_measurement = current_time;
+                counter = 0;
+            } else {
+                esp_task_wdt_reset();
+                delayMicroseconds(kMillisPerLoop * 1000);
+                counter += kMillisPerLoop;
+            }
+        }
+    }
+
+    float GetCPUUsage() const {
+        if (millis() - last_measurement > kMillisPerCalc) return 100.0f;
+
+        return 100.0f - 100 * idle_ratio;
+    }
+
+    static void IdleTaskEntry(void *that) { static_cast<IdleTask *>(that)->ProcessIdleTime(); }
+};
+
+class TaskManager {
+  private:
+    std::map<const char *, task_t> _tasks;
+    IdleTask _taskIdle0;
+    IdleTask _taskIdle1;
+
+    TaskHandle_t _hIdle0;
+    TaskHandle_t _hIdle1;
+
+  public:
+    TaskManager() {}
+
+    void begin() {
+        createTask(IdleTask::IdleTaskEntry, "Idle Core 0", IDLE_STACK_SIZE, &_taskIdle0, 1, &_hIdle0, 0);
+        createTask(IdleTask::IdleTaskEntry, "Idle Core 1", IDLE_STACK_SIZE, &_taskIdle1, 1, &_hIdle1, 1);
+        esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(0));
+        esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(1));
+        esp_task_wdt_add(_hIdle0);
+        esp_task_wdt_add(_hIdle1);
+    }
+
+    std::vector<task_t> getTasks() {
+        update();
+        std::vector<task_t> tasks;
+        for (auto const &task : _tasks) tasks.push_back(task.second);
+        return tasks;
+    }
+
+    int getTaskCount() const { return _tasks.size(); }
+
+    int getKernelTaskCount() const { return uxTaskGetNumberOfTasks(); }
+
+    void update() {
+        for (auto task = _tasks.begin(); task != _tasks.end();) {
+            eTaskState state = eTaskGetState(task->second.handle);
+            if (state == eDeleted) {
+                task = _tasks.erase(task);
+            } else {
+                _tasks[task->first].priority = uxTaskPriorityGet(task->second.handle);
+                _tasks[task->first].coreId = xTaskGetAffinity(task->second.handle);
+                ++task;
+            }
+        }
+    }
+
+    float getCpuUsage(int iCore = -1) const {
+        if (iCore == 0)
+            return _taskIdle0.GetCPUUsage();
+        else if (iCore == 1)
+            return _taskIdle1.GetCPUUsage();
+        return (_taskIdle0.GetCPUUsage() + _taskIdle1.GetCPUUsage()) / 2;
+    }
+
+    BaseType_t createTask(void (*taskFunction)(void *), const char *name, uint32_t stackSize = 2048,
+                          void *params = nullptr, UBaseType_t priority = 2, TaskHandle_t *handle = nullptr,
+                          BaseType_t coreId = -1) {
+        TaskHandle_t localHandle;
+        if (handle == nullptr) handle = &localHandle;
+        BaseType_t res = coreId == -1
+                             ? xTaskCreate(taskFunction, name, stackSize, params, tskIDLE_PRIORITY + priority, handle)
+                             : xTaskCreatePinnedToCore(taskFunction, name, stackSize, params,
+                                                       tskIDLE_PRIORITY + priority, handle, coreId);
+        task_t task = {name, handle, stackSize, priority, coreId, coreId != -1, true};
+        if (res == pdPASS) _tasks[name] = task;
+        return res;
+    }
+
+    void suspendTask(const char *name) {
+        if (_tasks.find(name) != _tasks.end()) {
+            vTaskSuspend(_tasks[name].handle);
+            _tasks[name].active = false;
+        }
+    }
+
+    void resumeTask(const char *name) {
+        if (_tasks.find(name) != _tasks.end()) {
+            vTaskResume(_tasks[name].handle);
+            _tasks[name].active = true;
+        }
+    }
+
+    void notifyTask(const char *name, uint32_t notificationValue, eNotifyAction action = eSetValueWithOverwrite) {
+        if (_tasks.find(name) != _tasks.end()) xTaskNotify(_tasks[name].handle, notificationValue, action);
+    }
+
+    void deleteTask(const char *name) {
+        if (_tasks.find(name) != _tasks.end()) {
+            vTaskDelete(_tasks[name].handle);
+            _tasks.erase(name);
+        }
+    }
+};
+
+class CPUBurnerTask {
+  private:
+    float burn_ratio;
+    const int kMillisPerCalc = 1000;
+
+  public:
+    CPUBurnerTask(float ratio) : burn_ratio(ratio) {}
+
+    void BurnCPUTask() {
+        unsigned long burn_time = burn_ratio * kMillisPerCalc;
+        unsigned long idle_time = kMillisPerCalc - burn_time;
+
+        while (true) {
+            unsigned long start_time = millis();
+            while ((millis() - start_time) < burn_time) {
+                esp_task_wdt_reset();
+            }
+
+            delay(idle_time);
+        }
+    }
+
+    static void CPUBurnerTaskEntry(void *instance) { static_cast<CPUBurnerTask *>(instance)->BurnCPUTask(); }
+
+    void StartTask() { xTaskCreate(CPUBurnerTaskEntry, "CPUBurnerTask", 2048, this, tskIDLE_PRIORITY + 2, nullptr); }
+};
+
+extern TaskManager g_taskManager;

esp32/include/template/state_result.h

@@ -0,0 +1,7 @@
+#pragma once
+
+enum class StateUpdateResult {
+    CHANGED = 0, // The update changed the state and propagation should take place if required
+    UNCHANGED,   // The state was unchanged, propagation should not take place
+    ERROR        // There was a problem updating the state, propagation should not take place
+};

esp32/include/template/stateful_endpoint.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <PsychicHttp.h>
+#include <template/stateful_service.h>
+
+#include <functional>
+
+#define HTTP_ENDPOINT_ORIGIN_ID "http"
+#define HTTPS_ENDPOINT_ORIGIN_ID "https"
+
+template <class T>
+class StatefulHttpEndpoint {
+  protected:
+    JsonStateReader<T> _stateReader;
+    JsonStateUpdater<T> _stateUpdater;
+    StatefulService<T> *_statefulService;
+
+  public:
+    StatefulHttpEndpoint(JsonStateReader<T> stateReader, JsonStateUpdater<T> stateUpdater,
+                         StatefulService<T> *statefulService)
+        : _stateReader(stateReader), _stateUpdater(stateUpdater), _statefulService(statefulService) {}
+
+    esp_err_t handleStateUpdate(PsychicRequest *request, JsonVariant &json) {
+        if (!json.is<JsonObject>()) return request->reply(400);
+
+        JsonObject jsonObject = json.as<JsonObject>();
+        StateUpdateResult outcome = _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
+
+        if (outcome == StateUpdateResult::ERROR)
+            return request->reply(400);
+        else if ((outcome == StateUpdateResult::CHANGED)) {
+            // persist the changes to the FS
+            _statefulService->callUpdateHandlers(HTTP_ENDPOINT_ORIGIN_ID);
+        }
+
+        PsychicJsonResponse response = PsychicJsonResponse(request, false);
+        jsonObject = response.getRoot();
+
+        _statefulService->read(jsonObject, _stateReader);
+
+        return response.send();
+    }
+
+    esp_err_t getState(PsychicRequest *request) {
+        PsychicJsonResponse response = PsychicJsonResponse(request, false);
+        JsonObject jsonObject = response.getRoot();
+        _statefulService->read(jsonObject, _stateReader);
+        return response.send();
+    }
+};

esp32/include/template/stateful_persistence.h

@@ -0,0 +1,120 @@
+#ifndef FSPersistence_h
+#define FSPersistence_h
+
+/**
+ *   ESP32 SvelteKit
+ *
+ *   A simple, secure and extensible framework for IoT projects for ESP32 platforms
+ *   with responsive Sveltekit front-end built with TailwindCSS and DaisyUI.
+ *   https://github.com/theelims/ESP32-sveltekit
+ *
+ *   Copyright (C) 2018 - 2023 rjwats
+ *   Copyright (C) 2023 theelims
+ *   Copyright (C) 2024 runeharlyk
+ *
+ *   All Rights Reserved. This software may be modified and distributed under
+ *   the terms of the LGPL v3 license. See the LICENSE file for details.
+ **/
+
+#include <FS.h>
+#include <template/stateful_service.h>
+#include <filesystem.h>
+
+template <class T>
+class FSPersistence {
+  public:
+    FSPersistence(JsonStateReader<T> stateReader, JsonStateUpdater<T> stateUpdater, StatefulService<T> *statefulService,
+                  const char *filePath)
+        : _stateReader(stateReader),
+          _stateUpdater(stateUpdater),
+          _statefulService(statefulService),
+          _filePath(filePath),
+          _updateHandlerId(0) {
+        enableUpdateHandler();
+    }
+
+    void readFromFS() {
+        File settingsFile = _fs->open(_filePath, "r");
+
+        if (settingsFile) {
+            JsonDocument jsonDocument;
+            DeserializationError error = deserializeJson(jsonDocument, settingsFile);
+            if (error == DeserializationError::Ok && jsonDocument.is<JsonObject>()) {
+                JsonObject jsonObject = jsonDocument.as<JsonObject>();
+                _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
+                settingsFile.close();
+                return;
+            }
+            settingsFile.close();
+        }
+
+        // If we reach here we have not been successful in loading the config
+        // and hard-coded defaults are now applied. The settings are then
+        // written back to the file system so the defaults persist between
+        // resets. This last step is required as in some cases defaults contain
+        // randomly generated values which would otherwise be modified on reset.
+        applyDefaults();
+        writeToFS();
+    }
+
+    bool writeToFS() {
+        JsonDocument jsonDocument;
+        JsonObject jsonObject = jsonDocument.to<JsonObject>();
+        _statefulService->read(jsonObject, _stateReader);
+
+        mkdirs();
+
+        File file = _fs->open(_filePath, "w");
+
+        if (!file) return false;
+
+        serializeJson(jsonDocument, file);
+        file.close();
+        return true;
+    }
+
+    void disableUpdateHandler() {
+        if (_updateHandlerId) {
+            _statefulService->removeUpdateHandler(_updateHandlerId);
+            _updateHandlerId = 0;
+        }
+    }
+
+    void enableUpdateHandler() {
+        if (!_updateHandlerId) {
+            _updateHandlerId = _statefulService->addUpdateHandler([&](const String &originId) { writeToFS(); });
+        }
+    }
+
+  private:
+    JsonStateReader<T> _stateReader;
+    JsonStateUpdater<T> _stateUpdater;
+    StatefulService<T> *_statefulService;
+    FS *_fs {&ESPFS};
+    const char *_filePath;
+    size_t _bufferSize;
+    HandlerId _updateHandlerId;
+
+    // We assume we have a _filePath with format
+    // "/directory1/directory2/filename" We create a directory for each missing
+    // parent
+    void mkdirs() {
+        String path(_filePath);
+        int index = 0;
+        while ((index = path.indexOf('/', index + 1)) != -1) {
+            String segment = path.substring(0, index);
+            if (!_fs->exists(segment)) _fs->mkdir(segment);
+        }
+    }
+
+  protected:
+    // We assume the updater supplies sensible defaults if an empty object
+    // is supplied, this virtual function allows that to be changed.
+    virtual void applyDefaults() {
+        JsonDocument jsonDocument;
+        JsonObject jsonObject = jsonDocument.as<JsonObject>();
+        _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
+    }
+};
+
+#endif // end FSPersistence

esp32/include/template/stateful_service.h

@@ -0,0 +1,158 @@
+#pragma once
+
+#include <Arduino.h>
+#include <ArduinoJson.h>
+
+#include <list>
+#include <functional>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+#include <template/state_result.h>
+
+template <typename T>
+using JsonStateUpdater = std::function<StateUpdateResult(JsonObject &root, T &settings)>;
+
+template <typename T>
+using JsonStateReader = std::function<void(T &settings, JsonObject &root)>;
+
+using HandlerId = size_t;
+using StateUpdateCallback = std::function<void(const String &originId)>;
+using StateHookCallback = std::function<void(const String &originId, StateUpdateResult &result)>;
+
+class HandlerBase {
+  protected:
+    static inline HandlerId nextId_ = 1; // Start from 1, 0 is invalid
+    HandlerId id_;
+    bool allowRemove_;
+
+    HandlerBase(bool allowRemove) : id_(nextId_++), allowRemove_(allowRemove) {}
+
+  public:
+    HandlerId getId() const { return id_; }
+    bool isRemovable() const { return allowRemove_; }
+};
+
+class UpdateHandler : public HandlerBase {
+    StateUpdateCallback callback_;
+
+  public:
+    UpdateHandler(StateUpdateCallback callback, bool allowRemove)
+        : HandlerBase(allowRemove), callback_(std::move(callback)) {}
+
+    void invoke(const String &originId) const { callback_(originId); }
+};
+
+class HookHandler : public HandlerBase {
+    StateHookCallback callback_;
+
+  public:
+    HookHandler(StateHookCallback callback, bool allowRemove)
+        : HandlerBase(allowRemove), callback_(std::move(callback)) {}
+
+    void invoke(const String &originId, StateUpdateResult &result) const { callback_(originId, result); }
+};
+
+template <class T>
+class StatefulService {
+  public:
+    template <typename... Args>
+    StatefulService(Args &&...args) : state_(std::forward<Args>(args)...), mutex_(xSemaphoreCreateRecursiveMutex()) {}
+
+    HandlerId addUpdateHandler(StateUpdateCallback callback, bool allowRemove = true) {
+        if (!callback) return 0;
+
+        updateHandlers_.emplace_back(std::move(callback), allowRemove);
+        return updateHandlers_.back().getId();
+    }
+
+    void removeUpdateHandler(HandlerId id) {
+        updateHandlers_.remove_if(
+            [id](const UpdateHandler &handler) { return handler.isRemovable() && handler.getId() == id; });
+    }
+
+    HandlerId addHookHandler(StateHookCallback callback, bool allowRemove = true) {
+        if (!callback) return 0;
+
+        hookHandlers_.emplace_back(std::move(callback), allowRemove);
+        return hookHandlers_.back().getId();
+    }
+
+    void removeHookHandler(HandlerId id) {
+        hookHandlers_.remove_if(
+            [id](const HookHandler &handler) { return handler.isRemovable() && handler.getId() == id; });
+    }
+
+    StateUpdateResult update(std::function<StateUpdateResult(T &)> stateUpdater, const String &originId) {
+        lock();
+        StateUpdateResult result = stateUpdater(state_);
+        unlock();
+        notifyStateChange(originId, result);
+        return result;
+    }
+
+    StateUpdateResult updateWithoutPropagation(std::function<StateUpdateResult(T &)> stateUpdater) {
+        lock();
+        StateUpdateResult result = stateUpdater(state_);
+        unlock();
+        return result;
+    }
+
+    StateUpdateResult update(JsonObject &jsonObject, JsonStateUpdater<T> stateUpdater, const String &originId) {
+        lock();
+        StateUpdateResult result = stateUpdater(jsonObject, state_);
+        unlock();
+        notifyStateChange(originId, result);
+        return result;
+    }
+
+    StateUpdateResult updateWithoutPropagation(JsonObject &jsonObject, JsonStateUpdater<T> stateUpdater) {
+        lock();
+        StateUpdateResult result = stateUpdater(jsonObject, state_);
+        unlock();
+        return result;
+    }
+
+    void read(std::function<void(T &)> stateReader) {
+        lock();
+        stateReader(state_);
+        unlock();
+    }
+
+    void read(JsonObject &jsonObject, JsonStateReader<T> stateReader) {
+        lock();
+        stateReader(state_, jsonObject);
+        unlock();
+    }
+
+    void callUpdateHandlers(const String &originId) {
+        for (const UpdateHandler &updateHandler : updateHandlers_) {
+            updateHandler.invoke(originId);
+        }
+    }
+
+    void callHookHandlers(const String &originId, StateUpdateResult &result) {
+        for (const HookHandler &hookHandler : hookHandlers_) {
+            hookHandler.invoke(originId, result);
+        }
+    }
+
+    T &state() { return state_; }
+
+  private:
+    T state_;
+
+    inline void lock() { xSemaphoreTakeRecursive(mutex_, portMAX_DELAY); }
+    inline void unlock() { xSemaphoreGiveRecursive(mutex_); }
+
+    void notifyStateChange(const String &originId, StateUpdateResult &result) {
+        callHookHandlers(originId, result);
+        if (result == StateUpdateResult::CHANGED) {
+            callUpdateHandlers(originId);
+        }
+    }
+
+    SemaphoreHandle_t mutex_;
+    std::list<UpdateHandler> updateHandlers_;
+    std::list<HookHandler> hookHandlers_;
+};

esp32/include/template/stateful_socket.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <PsychicHttp.h>
+
+#include <event_bus.hpp>
+#include <template/stateful_service.h>
+
+template <class T>
+class EventEndpoint {
+  public:
+    EventEndpoint(JsonStateReader<T> stateReader, JsonStateUpdater<T> stateUpdater, StatefulService<T> *statefulService,
+                  const char *event)
+        : _stateReader(stateReader), _stateUpdater(stateUpdater), _statefulService(statefulService), _event(event) {
+        _statefulService->addUpdateHandler([&](const String &originId) { syncState(originId); }, false);
+    }
+
+    void begin() {
+        // socket.onEvent(_event,
+        //                std::bind(&EventEndpoint::updateState, this, std::placeholders::_1, std::placeholders::_2));
+        // socket.onSubscribe(_event,
+        //                    std::bind(&EventEndpoint::syncState, this, std::placeholders::_1, std::placeholders::_2));
+    }
+
+  private:
+    JsonStateReader<T> _stateReader;
+    JsonStateUpdater<T> _stateUpdater;
+    StatefulService<T> *_statefulService;
+    const char *_event;
+
+    void updateState(JsonObject &root, int originId) {
+        _statefulService->update(root, _stateUpdater, String(originId));
+    }
+
+    void syncState(const String &originId, bool sync = false) {
+        JsonDocument jsonDocument;
+        JsonObject root = jsonDocument.to<JsonObject>();
+        String output;
+        _statefulService->read(root, _stateReader);
+        serializeJson(root, output);
+        ESP_LOGV("EventEndpoint", "Syncing state: %s", output.c_str());
+        // socket.emit(_event, output.c_str(), originId.c_str(), sync);
+    }
+};

esp32/include/utils/ip_utils.h

@@ -0,0 +1,28 @@
+#ifndef IPUtils_h
+#define IPUtils_h
+
+/**
+ *   ESP32 SvelteKit
+ *
+ *   A simple, secure and extensible framework for IoT projects for ESP32 platforms
+ *   with responsive Sveltekit front-end built with TailwindCSS and DaisyUI.
+ *   https://github.com/theelims/ESP32-sveltekit
+ *
+ *   Copyright (C) 2018 - 2023 rjwats
+ *   Copyright (C) 2023 theelims
+ *
+ *   All Rights Reserved. This software may be modified and distributed under
+ *   the terms of the LGPL v3 license. See the LICENSE file for details.
+ **/
+
+#include <IPAddress.h>
+
+const IPAddress IP_NOT_SET = IPAddress(INADDR_NONE);
+
+class IPUtils {
+  public:
+    static bool isSet(const IPAddress &ip) { return ip != IP_NOT_SET; }
+    static bool isNotSet(const IPAddress &ip) { return ip == IP_NOT_SET; }
+};
+
+#endif // end IPUtils_h

esp32/include/utils/json_utils.h

@@ -0,0 +1,31 @@
+#ifndef JsonUtils_h
+#define JsonUtils_h
+
+#include <Arduino.h>
+#include <utils/ip_utils.h>
+#include <ArduinoJson.h>
+
+class JsonUtils {
+  public:
+    static void readIP(const JsonObject &root, const String &key, IPAddress &ip, const String &def) {
+        IPAddress defaultIp = {};
+        if (!defaultIp.fromString(def)) {
+            defaultIp = INADDR_NONE;
+        }
+        readIP(root, key, ip, defaultIp);
+    }
+
+    static void readIP(const JsonObject &root, const String &key, IPAddress &ip,
+                       const IPAddress &defaultIp = INADDR_NONE) {
+        if (!root[key].is<String>() || !ip.fromString(root[key].as<String>())) {
+            ip = defaultIp;
+        }
+    }
+
+    static void writeIP(JsonObject &root, const String &key, const IPAddress &ip) {
+        if (IPUtils::isSet(ip)) {
+            root[key] = ip.toString();
+        }
+    }
+};
+#endif // end JsonUtils

esp32/include/utils/math_utils.h

@@ -0,0 +1,84 @@
+#ifndef MATHUTILS_H
+#define MATHUTILS_H
+
+#include <dspm_mult.h>
+#include <cmath>
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0]))
+
+#define COPY_2D_ARRAY_4x4(dest, src) \
+    do {                             \
+        (dest)[0][0] = (src)[0][0];  \
+        (dest)[0][1] = (src)[0][1];  \
+        (dest)[0][2] = (src)[0][2];  \
+        (dest)[0][3] = (src)[0][3];  \
+        (dest)[1][0] = (src)[1][0];  \
+        (dest)[1][1] = (src)[1][1];  \
+        (dest)[1][2] = (src)[1][2];  \
+        (dest)[1][3] = (src)[1][3];  \
+        (dest)[2][0] = (src)[2][0];  \
+        (dest)[2][1] = (src)[2][1];  \
+        (dest)[2][2] = (src)[2][2];  \
+        (dest)[2][3] = (src)[2][3];  \
+        (dest)[3][0] = (src)[3][0];  \
+        (dest)[3][1] = (src)[3][1];  \
+        (dest)[3][2] = (src)[3][2];  \
+        (dest)[3][3] = (src)[3][3];  \
+    } while (0)
+
+#define MAT_MULT(A, B, result, rows, cols, result_cols) \
+    dspm_mult_f32_ae32((float *)(A), (float *)(B), (float *)(result), (rows), (cols), (result_cols))
+
+#define INT_TO_STRING(state, output)      \
+    do {                                  \
+        itoa((int)(state), (output), 10); \
+    } while (0)
+
+#define PI_F 3.1415927f
+
+#define DEG2RAD_F 0.0174532f
+
+#define RAD2DEG_F 57.2957795f
+
+#define RAD_TO_DEG_F(rad) ((rad) * RAD2DEG_F)
+
+#define DEG_TO_RAD_F(deg) ((deg) * DEG2RAD_F)
+
+#define COS_DEG_F(deg) (cosf(DEG_TO_RAD_F(deg)))
+
+#define SIN_DEG_F(deg) (sinf(DEG_TO_RAD_F(deg)))
+
+#define IS_EQUAL(a, b, epsilon) (std::fabs((a) - (b)) < (epsilon))
+
+#define IS_ALMOST_EQUAL(a, b) IS_EQUAL((a), (b), 0.001f)
+
+inline float lerp(float start, float end, float t) { return (1 - t) * start + t * end; }
+
+inline bool isEqual(float a, float b, float epsilon) { return std::fabs(a - b) < epsilon; }
+
+inline float round2(float value) { return (int)(value * 100 + 0.5) / 100.0; }
+
+inline bool arrayEqual(const float arr1[4][4], const float arr2[4][4], float epsilon = 1e-3) {
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            if (std::fabs(arr1[i][j] - arr2[i][j]) > epsilon) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+static constexpr float combinatorial_constexpr(const int n, int k) {
+    if (k < 0 || k > n) return 0.0f;
+    if (k == 0 || k == n) return 1.0f;
+    k = (k < (n - k)) ? k : (n - k);
+    float result = 1.0f;
+    for (int i = 0; i < k; ++i) {
+        result *= (n - i);
+        result /= (i + 1);
+    }
+    return result;
+}
+
+#endif

esp32/include/utils/string_utils.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <Arduino.h>
+
+constexpr static const char* PLATFORM = "esp32";
+
+static String getRandom() { return String(random(2147483647), HEX); }
+
+static String getUniqueId() {
+    uint8_t mac[6];
+    esp_read_mac(mac, ESP_MAC_WIFI_STA);
+    char macStr[13] = {0};
+    sprintf(macStr, "%02x%02x%02x%02x%02x%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+    return String(macStr);
+}
+
+static String replaceEach(String value, String pattern, String (*generateReplacement)()) {
+    while (true) {
+        int index = value.indexOf(pattern);
+        if (index == -1) {
+            break;
+        }
+        value = value.substring(0, index) + generateReplacement() + value.substring(index + pattern.length());
+    }
+    return value;
+}
+
+static String format(String value) {
+    value = replaceEach(value, "#{random}", getRandom);
+    value.replace("#{unique_id}", getUniqueId());
+    value.replace("#{platform}", PLATFORM);
+    return value;
+}

esp32/include/utils/timing.h

@@ -0,0 +1,42 @@
+#ifndef TIMING_H
+#define TIMING_H
+
+#define CONCAT(a, b) a##b
+
+#define UNIQUE_VAR(base) CONCAT(base, __LINE__)
+
+#define EXECUTE_EVERY_N_MS(n, code)                                                               \
+    do {                                                                                          \
+        static volatile unsigned long UNIQUE_VAR(lastExecution_) = 0;                             \
+        unsigned long currentMillis = millis();                                                   \
+        if (UNIQUE_VAR(lastExecution_) == 0 || currentMillis - UNIQUE_VAR(lastExecution_) >= n) { \
+            code;                                                                                 \
+            UNIQUE_VAR(lastExecution_) = currentMillis;                                           \
+        }                                                                                         \
+    } while (0)
+
+#define TIME_IT(code)                                                                       \
+    {                                                                                       \
+        uint32_t time_it_start = micros();                                                  \
+        code;                                                                               \
+        uint32_t time_it_elapsed = micros() - time_it_start;                                \
+        if (time_it_elapsed < 1000) {                                                       \
+            ESP_LOGI("Time It", "Time elapsed: %lu microseconds", time_it_elapsed);         \
+        } else if (time_it_elapsed < 1000000) {                                             \
+            ESP_LOGI("Time It", "Time elapsed: %lu milliseconds", time_it_elapsed / 1000);  \
+        } else {                                                                            \
+            ESP_LOGI("Time It", "Time elapsed: %.2f seconds", time_it_elapsed / 1000000.0); \
+        }                                                                                   \
+    }
+
+#define CALLS_PER_SECOND(name)                                            \
+    static unsigned long name##_count = 0;                                \
+    static unsigned long last_time = 0;                                   \
+    name##_count++;                                                       \
+    if (millis() - last_time >= 1000) {                                   \
+        Serial.printf("%s: %lu calls per second\n", #name, name##_count); \
+        name##_count = 0;                                                 \
+        last_time = millis();                                             \
+    }
+
+#endif

esp32/include/wifi_service.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <PsychicHttp.h>
+#include <IPAddress.h>
+#include <WiFi.h>
+#include <ESPmDNS.h>
+
+#include <filesystem.h>
+#include <utils/timing.h>
+#include <template/stateful_service.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_endpoint.h>
+#include <settings/wifi_settings.h>
+
+class WiFiService : public StatefulService<WiFiSettings> {
+  private:
+    static void getNetworks(JsonObject &root);
+    static void getNetworkStatus(JsonObject &root);
+    void onStationModeDisconnected(WiFiEvent_t event, WiFiEventInfo_t info);
+    void onStationModeStop(WiFiEvent_t event, WiFiEventInfo_t info);
+    static void onStationModeGotIP(WiFiEvent_t event, WiFiEventInfo_t info);
+
+    FSPersistence<WiFiSettings> _persistence;
+
+    void reconfigureWiFiConnection();
+    void manageSTA();
+    void connectToWiFi();
+    void configureNetwork(wifi_settings_t &network);
+
+    unsigned long _lastConnectionAttempt;
+    bool _stopping;
+
+    constexpr static uint16_t reconnectDelay {10000};
+
+  public:
+    WiFiService();
+    ~WiFiService();
+
+    void begin();
+    void loop();
+
+    void setupMDNS(const char *hostname);
+
+    const char *getHostname() { return state().hostname.c_str(); }
+
+    static esp_err_t handleScan(PsychicRequest *request);
+    static esp_err_t getNetworks(PsychicRequest *request);
+    static esp_err_t getNetworkStatus(PsychicRequest *request);
+
+    StatefulHttpEndpoint<WiFiSettings> endpoint;
+};