Fix RIP bugs, add in UART

components/rpc/CommunicationRouter.cpp

@@ -19,6 +19,7 @@
 #define TAG "CommunicationRouter"
 #define MAX_RX_BUFFER_SIZE 1024
 #define WIRELESS_DEQUEUE_TIMEOUT_MS 3000
+#define LEADER_ELECTION_PERIOD_MS 3000
 
 CommunicationRouter::~CommunicationRouter() {
     vTaskDelete(m_router_thread);
@@ -35,6 +36,16 @@ CommunicationRouter::~CommunicationRouter() {
     }
 }
 
+[[noreturn]] void CommunicationRouter::leader_election_thread(void *args) {
+    const auto that = static_cast<CommunicationRouter *>(args);
+
+    while (true) {
+        that->m_last_leader_updated = std::chrono::system_clock::now();
+        that->update_leader();
+        vTaskDelay(pdMS_TO_TICKS(LEADER_ELECTION_PERIOD_MS));
+    }
+}
+
 [[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
     const auto that = static_cast<CommunicationRouter *>(args);
 
@@ -61,21 +72,45 @@ void CommunicationRouter::update_leader() {
     size_t table_size = RIP_MAX_ROUTES;
     this->m_data_link_manager->get_routing_table(table, &table_size);
 
-    // Leader election (just get the highest id in rip)
+    // Compute our own sum_of_hops from our local routing table (row 0 in the
+    // internal table carries this, but it is also returned in the public table
+    // as the self-entry). We reconstruct it here directly from the peer rows.
+    uint32_t self_sum = 0;
     std::vector<int> connected_module_ids;
-    uint8_t max = m_module_id;
-    for (int i = 0; i < table_size; i++) {
-        const auto id = table[i].info.board_id;
-        connected_module_ids.emplace_back(id);
-        if (id > max) { // todo: change this to be correct
-            max = id;
+    for (size_t i = 0; i < table_size; i++) {
+        if (table[i].info.board_id == m_module_id) continue; // skip self-row if present
+        connected_module_ids.emplace_back(table[i].info.board_id);
+        self_sum += table[i].info.hops;
+    }
+    const uint8_t self_sum_clamped = (self_sum > 255) ? 255 : static_cast<uint8_t>(self_sum);
+
+    // Leader election: elect the module with the lowest sum_of_hops (most central).
+    // Each module advertises its own sum_of_hops as the third byte of every RIP row,
+    // so every peer in the routing table carries the score that module computed
+    // locally from its own perspective — the true minimum total hop distance.
+    //
+    // Ties are broken by higher board_id for determinism.
+    uint8_t best_id    = m_module_id;
+    uint8_t best_score = self_sum_clamped;
+
+    for (size_t i = 0; i < table_size; i++) {
+        const auto& row = table[i];
+        if (row.info.board_id == m_module_id) continue;
+
+        const uint8_t score = row.info.sum_of_hops;
+        if (score == 0) continue; // a remote board advertising sum_of_hops=0 is invalid — never trust it
+        if (score < best_score || (score == best_score && row.info.board_id > best_id)) {
+            best_score = score;
+            best_id    = row.info.board_id;
         }
     }
 
+    const uint8_t elected = best_id;
+
     // Leader has changed, we may need to change PC connection state
-    if (this->m_leader != max) {
-        ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
-        if (max == m_module_id) {
+    if (this->m_leader != elected) {
+        ESP_LOGI(TAG, "Leader has changed from %d to %d (sum_of_hops=%d)", this->m_leader, elected, best_score);
+        if (elected == m_module_id) {
             m_pc_connection->connect();
             m_lossless_server->startup();
             m_lossy_server->startup();
@@ -86,7 +121,7 @@ void CommunicationRouter::update_leader() {
         }
     }
 
-    this->m_leader = max;
+    this->m_leader = elected;
 
     if (this->m_leader == m_module_id) {
         this->m_discovery_service->set_connected_boards(connected_module_ids);