From 481dfaf8e57d8419a63a03d27a0cb07872a15c6e Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Tue, 8 Jul 2025 22:28:46 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=A4=B9=20Update=20adapters=20with=20bette?=
 =?UTF-8?q?r=20handling?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/include/adapters/comm_base.hpp | 35 +++++++++++++++++++++-------
 esp32/include/adapters/websocket.hpp |  9 -------
 esp32/src/adapters/websocket.cpp     | 10 +++++---
 3 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/esp32/include/adapters/comm_base.hpp b/esp32/include/adapters/comm_base.hpp
index 8b8a026..ca0b17b 100644
--- a/esp32/include/adapters/comm_base.hpp
+++ b/esp32/include/adapters/comm_base.hpp
@@ -18,9 +18,14 @@ class CommBase {
     std::array<Bits, NTopics> subs_;
     portMUX_TYPE mux_ portMUX_INITIALIZER_UNLOCKED;
 
-    std::array<void*, static_cast<size_t>(Topic::COUNT)> subscriptionHandle {};
+    std::array<void*, NTopics> subscriptionHandle {};
 
-    static constexpr size_t idx(Topic t) { return static_cast<size_t>(t); }
+    static constexpr size_t invalid = SIZE_MAX;
+
+    static constexpr size_t idx(Topic t) {
+        size_t i = static_cast<size_t>(t);
+        return i < NTopics ? i : invalid;
+    }
 
     template <Topic T>
     void encode(JsonDocument& d, const typename TopicTraits<T>::Msg& m) {
@@ -36,33 +41,45 @@ class CommBase {
     template <class Msg>
     auto& getHandle(Topic topic) {
         using H = typename EventBus<Msg>::Handle;
-        return *static_cast<H*>(subscriptionHandle[static_cast<size_t>(topic)]);
+        static H dummy;
+        auto* p = static_cast<H*>(subscriptionHandle[size_t(topic)]);
+        return p ? *p : dummy;
     }
 
     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));
+        subscriptionHandle[size_t(topic)] = new H(std::move(h));
     }
 
   public:
     void subscribe(Topic t, size_t cid) {
+        size_t i = idx(t);
+        if (i == invalid) return;
         portENTER_CRITICAL(&mux_);
-        subs_[idx(t)].set(cid);
+        subs_[i].set(cid);
         portEXIT_CRITICAL(&mux_);
     }
 
     void unsubscribe(Topic t, size_t cid) {
+        size_t i = idx(t);
+        if (i == invalid) return;
         portENTER_CRITICAL(&mux_);
-        subs_[idx(t)].reset(cid);
+        subs_[i].reset(cid);
         portEXIT_CRITICAL(&mux_);
     }
 
-    bool has(Topic t) const { return subs_[idx(t)].any(); }
+    bool has(Topic t) const {
+        size_t i = idx(t);
+        return i == invalid ? false : subs_[i].any();
+    }
 
     template <Topic T>
     void emit(const typename TopicTraits<T>::Msg& m) {
-        if (!has(T)) return;
+        constexpr size_t i = idx(T);
+        if (i == invalid) return;
+        if (!subs_[i].any()) return;
+
         JsonDocument doc;
         encode<T>(doc, m);
         String out;
@@ -71,7 +88,7 @@ class CommBase {
 #else
         serializeJson(doc, out);
 #endif
-        auto& b = subs_[idx(T)];
+        auto& b = subs_[i];
         for (size_t cid = 0; cid < MaxCid; ++cid)
             if (b.test(cid)) send(cid, out.c_str(), out.length());
     }
diff --git a/esp32/include/adapters/websocket.hpp b/esp32/include/adapters/websocket.hpp
index 56061b6..55caf01 100644
--- a/esp32/include/adapters/websocket.hpp
+++ b/esp32/include/adapters/websocket.hpp
@@ -10,12 +10,6 @@
 #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;
@@ -28,9 +22,6 @@ class EventSocket : public CommBase<> {
     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);
diff --git a/esp32/src/adapters/websocket.cpp b/esp32/src/adapters/websocket.cpp
index 90cf02e..f2c9c53 100644
--- a/esp32/src/adapters/websocket.cpp
+++ b/esp32/src/adapters/websocket.cpp
@@ -4,6 +4,13 @@ EventSocket::EventSocket() {
     _socket.onOpen((std::bind(&EventSocket::onWSOpen, this, std::placeholders::_1)));
     _socket.onClose(std::bind(&EventSocket::onWSClose, this, std::placeholders::_1));
     _socket.onFrame(std::bind(&EventSocket::onFrame, this, std::placeholders::_1, std::placeholders::_2));
+
+#define X(e, t)                                                                  \
+    setHandle<t>(Topic::e, EventBus<t>::subscribe([this](const t* d, size_t n) { \
+                     if (n) this->emit<Topic::e>(d[0]);                          \
+                 }));
+    TOPIC_LIST
+#undef X
 }
 
 void EventSocket::onWSOpen(PsychicWebSocketClient* client) {
@@ -93,9 +100,6 @@ void EventSocket::send(size_t clientId, const char* data, size_t len) {
 #else
     client->sendMessage(HTTPD_WS_TYPE_TEXT, data, len);
 #endif
-
-    // ESP_LOGV("EventSocket", "Sending to client %zu: %.*s", clientId, len, data);
-    // client->sendMessage(data);
 }
 
 EventSocket socket;
\ No newline at end of file