From 9986b0736294a2173d5bfd6dd8a48f70908aee87 Mon Sep 17 00:00:00 2001
From: Johnathon Slightham <slighthamj@gmail.com>
Date: Tue, 10 Mar 2026 16:15:02 -0400
Subject: [PATCH] Support multiple bots on the same network

---
 .../flatbuffers/RobotConfigurationBuilder.h   |  1 +
 include/lib_c_control.h                       |  3 +-
 include/libcontrol.h                          |  6 ++++
 src/lib_c_control.cpp                         | 20 ++++++++++++-
 src/libcontrol.cpp                            | 29 ++++++++++++++++++-
 5 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/include/flatbuffers/RobotConfigurationBuilder.h b/include/flatbuffers/RobotConfigurationBuilder.h
index 90a4fb7..f655532 100644
--- a/include/flatbuffers/RobotConfigurationBuilder.h
+++ b/include/flatbuffers/RobotConfigurationBuilder.h
@@ -15,6 +15,7 @@ struct ModuleInstance {
     uint8_t id;
     ModuleType type;
     int angle;
+    uint8_t leader;
 };
 
 struct ModuleConnectionInstance {
diff --git a/include/lib_c_control.h b/include/lib_c_control.h
index b9f3de0..7f73b40 100644
--- a/include/lib_c_control.h
+++ b/include/lib_c_control.h
@@ -19,7 +19,8 @@ LIB_API void cleanup();
 LIB_API int send_angle_control(int module_id, int angle);
 LIB_API int send_string_control(int module_id, const char *string);
 LIB_API double get_distance_control(int module_id);
-LIB_API char *get_configuration(int *size_out);
+LIB_API char *get_configuration(int *size_out, int leader_id);
+LIB_API const uint8_t *get_leaders(int *length);
 
 LIB_API bool remote_call_c(uint8_t function_tag, uint8_t module, const uint8_t *params,
                            uint16_t params_len, uint8_t *out_buffer, uint16_t out_buffer_capacity,
diff --git a/include/libcontrol.h b/include/libcontrol.h
index 61f5ac7..2ea6248 100644
--- a/include/libcontrol.h
+++ b/include/libcontrol.h
@@ -93,6 +93,9 @@ class RobotController {
    */
     void resetModules();
 
+    bool select_leader(uint8_t leader_id);
+    std::vector<uint8_t> get_leaders();
+
     /**
    * \brief Poll for devices accessible to the PC.
    *
@@ -103,6 +106,8 @@ class RobotController {
     std::optional<std::unique_ptr<std::vector<uint8_t>>>
     remote_call(uint8_t function_tag, uint8_t module, const std::vector<uint8_t> &parameters);
 
+    uint8_t get_selected_leader();
+
   private:
     std::shared_ptr<spdlog::logger> m_logger;
     std::unordered_map<uint8_t, std::shared_ptr<Module>> m_id_to_module{};
@@ -118,6 +123,7 @@ class RobotController {
     std::thread m_configuration_loop;
     std::thread m_sensor_loop;
     std::thread m_expiry_looop;
+    uint8_t m_selected_leader = 0;
 
     void metadata_loop();
     void transmit_loop();
diff --git a/src/lib_c_control.cpp b/src/lib_c_control.cpp
index b27036f..9146bb1 100644
--- a/src/lib_c_control.cpp
+++ b/src/lib_c_control.cpp
@@ -63,15 +63,27 @@ LIB_API double get_distance_control(int module_id) {
     return 0.0;
 }
 
-LIB_API char *get_configuration(int *size_out) {
+LIB_API char *get_configuration(int *size_out, int leader_id) {
     std::vector<Flatbuffers::ModuleInstance> modules_vec{};
     std::vector<Flatbuffers::ModuleConnectionInstance> connections_vec{};
+    std::unordered_set<uint8_t> modules_send{};
+
+    robot_controller->select_leader(leader_id);
 
     for (const auto &module : robot_controller->getModuleList()) {
+        if (robot_controller->get_selected_leader() != module.leader) {
+            continue;
+        }
         modules_vec.emplace_back(module);
+        modules_send.emplace(module.id);
     }
 
     for (const auto &connection : robot_controller->getConnections()) {
+        if (!modules_send.contains(connection.from_module_id) ||
+            !modules_send.contains(connection.to_module_id)) {
+            continue;
+        }
+
         connections_vec.emplace_back(connection);
     }
 
@@ -156,3 +168,9 @@ LIB_API bool remote_call_c(uint8_t function_tag, uint8_t module, const uint8_t *
 
     return true;
 }
+
+LIB_API const uint8_t *get_leaders(int *length) {
+    static const auto leaders = robot_controller->get_leaders();
+    *length = leaders.size();
+    return leaders.data();
+}
diff --git a/src/libcontrol.cpp b/src/libcontrol.cpp
index fc6703b..b8e8d42 100644
--- a/src/libcontrol.cpp
+++ b/src/libcontrol.cpp
@@ -113,7 +113,7 @@ std::vector<Flatbuffers::ModuleInstance> RobotController::getModuleList() {
     std::vector<Flatbuffers::ModuleInstance> out;
     std::shared_lock lock(m_module_lock);
     for (auto const &[key, value] : m_id_to_module) {
-        out.push_back({key, value->get_type()});
+        out.push_back({key, value->get_type(), value->get_leader()});
     }
     return out;
 }
@@ -201,6 +201,9 @@ void RobotController::transmit_loop() {
         std::this_thread::sleep_for(CONTROL_MESSAGE_FREQUENCY);
         std::shared_lock lock(m_module_lock);
         for (const auto [id, module] : m_id_to_module) {
+            if (m_selected_leader != module->get_leader()) {
+                continue;
+            }
             auto out = module->get_actuation_message();
 
             if (out.size() > 0) {
@@ -297,3 +300,27 @@ RobotController::remote_call(uint8_t function_tag, uint8_t module,
                              const std::vector<uint8_t> &parameters) {
     return m_messaging_interface->remote_call(function_tag, module, parameters);
 }
+
+std::vector<uint8_t> RobotController::get_leaders() {
+    std::shared_lock lock(m_module_lock);
+    std::unordered_set<uint8_t> out{};
+
+    for (const auto m : map_to_values(m_id_to_module)) {
+        out.insert(m->get_leader());
+    }
+
+    return {out.begin(), out.end()};
+}
+
+bool RobotController::select_leader(uint8_t leader_id) {
+    std::shared_lock lock(m_module_lock);
+    if (m_id_to_module.contains(leader_id)) {
+        m_selected_leader = leader_id;
+        return true;
+    }
+    return false;
+}
+
+uint8_t RobotController::get_selected_leader() {
+    return m_selected_leader;
+}