update movement set

2026-07-08 17:47:21 +02:00 · 2026-01-27 22:17:44 -05:00
parent 3c6f8b2c86
commit c6bcff20c3
9 changed files with 278 additions and 92 deletions
--- a/components/flatbuffers/MovementSetBuilder.cpp
+++ b/components/flatbuffers/MovementSetBuilder.cpp
@@ -6,40 +6,72 @@
 #ifdef MOVEMENTSETBUILDER

 namespace Flatbuffers {
-    SerializedMessage MovementSetBuilder::build_movement_set(const std::vector<flatbuffers::Offset<Movement::MovementEntry>>& set){
-
+    SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input){
        builder_.Clear();

-        const auto set_vector = builder_.CreateVector(set);
+        std::vector<flatbuffers::Offset<Movement::MovementVector>> map_entries;
+        map_entries.reserve(input.size());

-        const auto message = Movement::CreateMovementSet(
+        // 1. Collect and sort keys (DO NOT sort offsets)
+        std::vector<uint8_t> keys;
+        keys.reserve(input.size());
+
+        for (const auto& [key, _] : input) {
+        keys.push_back(key);
+        }
+
+        std::sort(keys.begin(), keys.end());
+
+        // 2. Build MovementVector entries in key order
+        for (uint8_t key : keys) {
+        const auto& entries = input.at(key);
+
+        std::vector<flatbuffers::Offset<Movement::MovementEntry>> fb_entries;
+        fb_entries.reserve(entries.size());
+
+        for (const auto& e : entries) {
+            fb_entries.push_back(
+                Movement::CreateMovementEntry(
                    builder_,
-            static_cast<uint8_t>(set.size()),
-            set_vector
+                    e.board_id,
+                    e.module_type,
+                    e.value_action,
+                    &e.condition,
+                    e.ack,
+                    e.ack_ttl_ms,
+                    e.post_delay_ms
+                )
+            );
+        }
+
+        auto movements_vector = builder_.CreateVector(fb_entries);
+
+        map_entries.push_back(
+            Movement::CreateMovementVector(
+                builder_,
+                key,
+                movements_vector
+            )
+        );
+        }
+
+        // 3. Build the map vector
+        auto map_vector = builder_.CreateVector(map_entries);
+
+        auto message = Movement::CreateMovementSet(
+        builder_,
+        map_vector
        );

        builder_.Finish(message);

-        return {builder_.GetBufferPointer(), builder_.GetSize()};
-    }
-
-    flatbuffers::Offset<Movement::MovementEntry> MovementSetBuilder::build_movement_entry(uint16_t board_id, uint8_t module_type, uint16_t value_action,
-            const Movement::ConditionBlob& condition, uint8_t ack, uint16_t ack_ttl_ms, uint16_t post_delay_ms){
-        return Movement::CreateMovementEntry(
-            builder_,
-            board_id,
-            module_type,
-            value_action,
-            &condition,     
-            ack,
-            ack_ttl_ms,
-            post_delay_ms
-        );
+        return { builder_.GetBufferPointer(), builder_.GetSize() };
    }

    Movement::ConditionBlob MovementSetBuilder::build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use){
        return Movement::ConditionBlob(value, cond, module_type, in_use);
    }
+
 }

 #endif //MOVEMENTSETBUILDER
--- a/components/flatbuffers/TopologyBuilder.cpp
+++ b/components/flatbuffers/TopologyBuilder.cpp
@@ -31,20 +31,6 @@ namespace Flatbuffers{
        return { builder_.GetBufferPointer(), builder_.GetSize() };
    }
    
-    flatbuffers::Offset<Topology::NeighbourBlob> TopologyBuilder::build_neighbour_info(
-        uint16_t board_id,
-        const std::vector<Topology::ChannelBoardConn>& connections
-    ) {
-        auto conn_vec = builder_.CreateVectorOfStructs(connections);
-
-        return Topology::CreateNeighbourBlob(
-            builder_,
-            board_id,
-            static_cast<uint8_t>(connections.size()),
-            conn_vec
-        );
-    }
-
    Topology::ChannelBoardConn TopologyBuilder::build_connections(uint8_t channel, uint16_t board_id){
        return Topology::ChannelBoardConn(channel, board_id);
    }
--- a/components/flatbuffers/include/MovementSetBuilder.h
+++ b/components/flatbuffers/include/MovementSetBuilder.h
@@ -11,14 +11,22 @@
 #include "flatbuffers_generated/Movement_generated.h"
 #include "flatbuffers/flatbuffers.h"

+struct MovementEntryInput {
+    uint16_t board_id;
+    uint8_t module_type;
+    uint16_t value_action;
+    Movement::ConditionBlob condition;
+    uint8_t ack;
+    uint16_t ack_ttl_ms;
+    uint16_t post_delay_ms;
+};
+
 namespace Flatbuffers {
    class MovementSetBuilder {
    public:
        MovementSetBuilder() : builder_(MAX_MOVEMENTS_IN_SET*sizeof(Movement::MovementEntry)) {}

-        SerializedMessage build_movement_set(const std::vector<flatbuffers::Offset<Movement::MovementEntry>>& set);
-        flatbuffers::Offset<Movement::MovementEntry> build_movement_entry(uint16_t board_id, uint8_t module_type, uint16_t value_action,
-            const Movement::ConditionBlob& condition, uint8_t ack, uint16_t ack_ttl_ms, uint16_t post_delay_ms);
+        SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input);
        Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use);

    private:
--- a/components/flatbuffers/include/TopologyBuilder.h
+++ b/components/flatbuffers/include/TopologyBuilder.h
@@ -18,7 +18,6 @@ namespace Flatbuffers {
        Topology::ChannelBoardConn build_connections(uint8_t channel, uint16_t board_id);
        
    private:
-        flatbuffers::Offset<Topology::NeighbourBlob> build_neighbour_info(uint16_t board_id, const std::vector<Topology::ChannelBoardConn>& connections);
        flatbuffers::FlatBufferBuilder builder_;
    };
 }
--- a/components/flatbuffers/include/flatbuffers_generated/Movement_generated.h
+++ b/components/flatbuffers/include/flatbuffers_generated/Movement_generated.h
@@ -20,6 +20,9 @@ struct ConditionBlob;
 struct MovementEntry;
 struct MovementEntryBuilder;

+struct MovementVector;
+struct MovementVectorBuilder;
+
 struct MovementSet;
 struct MovementSetBuilder;

@@ -165,14 +168,20 @@ inline ::flatbuffers::Offset<MovementEntry> CreateMovementEntry(
  return builder_.Finish();
 }

-struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
-  typedef MovementSetBuilder Builder;
+struct MovementVector FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef MovementVectorBuilder Builder;
  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
-    VT_NUM_MOVEMENTS = 4,
+    VT_KEY = 4,
    VT_MOVEMENTS = 6
  };
-  uint8_t num_movements() const {
-    return GetField<uint8_t>(VT_NUM_MOVEMENTS, 0);
+  uint8_t key() const {
+    return GetField<uint8_t>(VT_KEY, 0);
+  }
+  bool KeyCompareLessThan(const MovementVector * const o) const {
+    return key() < o->key();
+  }
+  int KeyCompareWithValue(uint8_t _key) const {
+    return static_cast<int>(key() > _key) - static_cast<int>(key() < _key);
  }
  const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements() const {
    return GetPointer<const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>> *>(VT_MOVEMENTS);
@@ -180,7 +189,7 @@ struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
  template <bool B = false>
  bool Verify(::flatbuffers::VerifierTemplate<B> &verifier) const {
    return VerifyTableStart(verifier) &&
-           VerifyField<uint8_t>(verifier, VT_NUM_MOVEMENTS, 1) &&
+           VerifyField<uint8_t>(verifier, VT_KEY, 1) &&
           VerifyOffset(verifier, VT_MOVEMENTS) &&
           verifier.VerifyVector(movements()) &&
           verifier.VerifyVectorOfTables(movements()) &&
@@ -188,15 +197,72 @@ struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
  }
 };

+struct MovementVectorBuilder {
+  typedef MovementVector Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_key(uint8_t key) {
+    fbb_.AddElement<uint8_t>(MovementVector::VT_KEY, key, 0);
+  }
+  void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements) {
+    fbb_.AddOffset(MovementVector::VT_MOVEMENTS, movements);
+  }
+  explicit MovementVectorBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<MovementVector> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<MovementVector>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<MovementVector> CreateMovementVector(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    uint8_t key = 0,
+    ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements = 0) {
+  MovementVectorBuilder builder_(_fbb);
+  builder_.add_movements(movements);
+  builder_.add_key(key);
+  return builder_.Finish();
+}
+
+inline ::flatbuffers::Offset<MovementVector> CreateMovementVectorDirect(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    uint8_t key = 0,
+    const std::vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements = nullptr) {
+  auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset<Movement::MovementEntry>>(*movements) : 0;
+  return Movement::CreateMovementVector(
+      _fbb,
+      key,
+      movements__);
+}
+
+struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef MovementSetBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_MOVEMENT_MAP = 4
+  };
+  const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>> *movement_map() const {
+    return GetPointer<const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>> *>(VT_MOVEMENT_MAP);
+  }
+  template <bool B = false>
+  bool Verify(::flatbuffers::VerifierTemplate<B> &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyOffset(verifier, VT_MOVEMENT_MAP) &&
+           verifier.VerifyVector(movement_map()) &&
+           verifier.VerifyVectorOfTables(movement_map()) &&
+           verifier.EndTable();
+  }
+};
+
 struct MovementSetBuilder {
  typedef MovementSet Table;
  ::flatbuffers::FlatBufferBuilder &fbb_;
  ::flatbuffers::uoffset_t start_;
-  void add_num_movements(uint8_t num_movements) {
-    fbb_.AddElement<uint8_t>(MovementSet::VT_NUM_MOVEMENTS, num_movements, 0);
-  }
-  void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements) {
-    fbb_.AddOffset(MovementSet::VT_MOVEMENTS, movements);
+  void add_movement_map(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>>> movement_map) {
+    fbb_.AddOffset(MovementSet::VT_MOVEMENT_MAP, movement_map);
  }
  explicit MovementSetBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
        : fbb_(_fbb) {
@@ -211,23 +277,19 @@ struct MovementSetBuilder {

 inline ::flatbuffers::Offset<MovementSet> CreateMovementSet(
    ::flatbuffers::FlatBufferBuilder &_fbb,
-    uint8_t num_movements = 0,
-    ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements = 0) {
+    ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>>> movement_map = 0) {
  MovementSetBuilder builder_(_fbb);
-  builder_.add_movements(movements);
-  builder_.add_num_movements(num_movements);
+  builder_.add_movement_map(movement_map);
  return builder_.Finish();
 }

 inline ::flatbuffers::Offset<MovementSet> CreateMovementSetDirect(
    ::flatbuffers::FlatBufferBuilder &_fbb,
-    uint8_t num_movements = 0,
-    const std::vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements = nullptr) {
-  auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset<Movement::MovementEntry>>(*movements) : 0;
+    std::vector<::flatbuffers::Offset<Movement::MovementVector>> *movement_map = nullptr) {
+  auto movement_map__ = movement_map ? _fbb.CreateVectorOfSortedTables<Movement::MovementVector>(movement_map) : 0;
  return Movement::CreateMovementSet(
      _fbb,
-      num_movements,
-      movements__);
+      movement_map__);
 }

 inline const Movement::MovementSet *GetMovementSet(const void *buf) {
--- a/components/movements/MovementSetManager.cpp
+++ b/components/movements/MovementSetManager.cpp
@@ -9,38 +9,85 @@ MovementSetManager::MovementSetManager(){
        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to open nvs for %s", MOVEMENTS_NVS_NAMESPACE);
        return;
    }
+
+    if (!topology_manager->is_ready()){
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Topology Manager is not ready!");
+        return;
+    }
+
+    ready = true;
 };

 MovementSetManager::~MovementSetManager(){
    nvs_close(handle);
 }

-// esp_err_t MovementSetManager::add_movement(struct MovementEntry& entry, uint8_t& index){
-//     return ESP_OK;
-// }
+esp_err_t MovementSetManager::add_movement(Movement::MovementEntry& entry, uint8_t& index){
+    if (!ready){
+        return ESP_ERR_INVALID_STATE;
+    }

-// esp_err_t MovementSetManager::remove_movement(uint8_t index){
-//     return ESP_OK;
-// }
+    if (entry.board_id() == PC_ADDR || entry.board_id() == BROADCAST_ADDR){
+        return ESP_ERR_INVALID_ARG;
+    }

-// esp_err_t MovementSetManager::update_movement(struct MovementEntry& entry, uint8_t index){
-//     return ESP_OK;
-// }
+    if (movements.find(index) != movements.end()){
+        for (Movement::MovementEntry& e : movements[index]){
+            if (e.board_id() == entry.board_id()){
+                return ESP_ERR_INVALID_ARG; //only unique board ids per index
+            }
+        }
+    }
    
-// esp_err_t MovementSetManager::get_curr_movement_set(struct MovementSet& set){
-//     return ESP_OK;
-// }
+    movements[index].push_back(entry);

-// esp_err_t MovementSetManager::verify_movement_set(){
-//     return ESP_OK;
-// }
+    return ESP_OK;
+}

-// esp_err_t MovementSetManager::get_nvs_movement_set(struct MovementSet& set){
-//     return ESP_OK;
-// }
+esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){
+    if (board_id == PC_ADDR || board_id == BROADCAST_ADDR){
+        return ESP_ERR_INVALID_ARG;
+    }

-// esp_err_t MovementSetManager::write_nvs_movement_set(){
-//     return ESP_OK;
-// }
+    if (movements.find(index) == movements.end()){
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    bool found = false;
+
+    for (auto it = movements[index].begin(); it != movements[index].end(); ++it) {
+        if (it->board_id() == board_id){
+            movements[index].erase(it);
+            found = true;
+            break;
+        }
+    }
+
+    if (!found){
+        return ESP_ERR_NOT_FOUND;
+    }
+
+    return ESP_OK;
+}
+
+esp_err_t MovementSetManager::update_movement(Movement::MovementEntry& entry, uint8_t index){
+    return ESP_OK;
+}
+
+esp_err_t MovementSetManager::get_curr_movement_set(Movement::MovementSet& set){
+    return ESP_OK;
+}
+
+esp_err_t MovementSetManager::verify_movement_set(){
+    return ESP_OK;
+}
+
+esp_err_t MovementSetManager::get_nvs_movement_set(Movement::MovementSet& set){
+    return ESP_OK;
+}
+
+esp_err_t MovementSetManager::write_nvs_movement_set(){
+    return ESP_OK;
+}

 #endif //MOVEMENTS
--- a/components/movements/TopologyManager.cpp
+++ b/components/movements/TopologyManager.cpp
@@ -17,6 +17,10 @@ TopologyManager::~TopologyManager(){
    nvs_close(handle);
 }

+bool TopologyManager::is_ready(){
+    return ready;
+}
+
 /**
 * @brief Creates a `Topology::NeighbourBlob` based on the input parameters and adds the blob to the internal topology map
 * 
@@ -72,6 +76,14 @@ esp_err_t TopologyManager::add_board_to_topology(const std::vector<std::pair<uin
    return ESP_OK;
 }

+/**
+ * @brief Removes the board associated with `board_id`
+ * 
+ * @warning This function does not remove any existing links on other boards on the topology
+ * 
+ * @param board_id 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
    if (!ready){
        return ESP_ERR_INVALID_STATE;
@@ -86,6 +98,13 @@ esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
    return ESP_OK;
 }

+/**
+ * @brief Returns a vector of connections that represents the channel connection to a board id from `curr_board_id`
+ * 
+ * @param connections 
+ * @param curr_board_id 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::get_board_in_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id){
    if (!ready){
        return ESP_ERR_INVALID_STATE;
@@ -106,12 +125,23 @@ esp_err_t TopologyManager::get_board_in_topology(std::vector<std::pair<uint8_t,
    return ESP_OK;
 }

+/**
+ * @brief Verifies the topology
+ * 
+ * Conditions to fail:
+ * 
+ * 1. Any board that is referenced in the topology does not have a reciprocal connection (eg. A -> B and B -> A)
+ * 
+ * 2. The topology contains 2 separate graphs (a board in the topology should be in the same graph with some sort of path to all other boards)
+ * 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::verify_topology(){
    if (!ready){
        return ESP_ERR_INVALID_STATE;
    }

-    if (topology.size() == 0){
+    if (topology.size() == 0 || topology.size() >= RIP_MAX_ROUTES){
        return ESP_ERR_INVALID_STATE;
    }

@@ -187,6 +217,12 @@ esp_err_t TopologyManager::verify_topology(){
    return ESP_OK;
 }

+/**
+ * @brief Gets the current topology stored in the manager
+ * 
+ * @param topology 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::get_curr_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology){
    if (!ready){
        return ESP_ERR_INVALID_STATE;
@@ -204,6 +240,11 @@ esp_err_t TopologyManager::get_curr_topology(std::unordered_map<uint16_t, std::v
    return ESP_OK;
 }

+/**
+ * @brief Writes the topology stored in the manager onto the board's NVS
+ * 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::write_nvs_topology(){
    if (!ready){
        return ESP_ERR_INVALID_STATE;
@@ -237,6 +278,12 @@ esp_err_t TopologyManager::write_nvs_topology(){
    return ESP_OK;
 }

+/**
+ * @brief Read's the board NVS to retreve the saved topology (if it exists)
+ * 
+ * @param topology 
+ * @return esp_err_t 
+ */
 esp_err_t TopologyManager::get_nvs_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology){
    if (!ready) {
        return ESP_ERR_INVALID_STATE;
--- a/components/movements/include/MovementSetManager.h
+++ b/components/movements/include/MovementSetManager.h
@@ -3,6 +3,8 @@
 #include "TopologyManager.h"
 #include <memory>
 #include <vector>
+#include "MovementSetBuilder.h"
+#include "unordered_map"

 #ifdef MOVEMENTS
 #pragma once
@@ -11,18 +13,20 @@ class MovementSetManager {
    public: 
        MovementSetManager();
        ~MovementSetManager();
-        // esp_err_t add_movement(struct MovementEntry& entry, uint8_t& index);
-        // esp_err_t remove_movement(uint8_t index);
-        // esp_err_t update_movement(struct MovementEntry& entry, uint8_t index);
-        // esp_err_t get_curr_movement_set(struct MovementSet& set);
-        // esp_err_t verify_movement_set();
-        // esp_err_t get_nvs_movement_set(struct MovementSet& set);
-        // esp_err_t write_nvs_movement_set();
+        esp_err_t add_movement(Movement::MovementEntry& entry, uint8_t& index);
+        esp_err_t remove_movement(uint8_t index, uint8_t board_id);
+        esp_err_t update_movement(Movement::MovementEntry& entry, uint8_t index);
+        esp_err_t get_curr_movement_set(Movement::MovementSet& set);
+        esp_err_t verify_movement_set();
+        esp_err_t get_nvs_movement_set(Movement::MovementSet& set);
+        esp_err_t write_nvs_movement_set();

    private:
-        std::unique_ptr<TopologyManager> topology_manager;
-        // std::vector<struct MovementEntry> movements;
+        std::unique_ptr<TopologyManager> topology_manager = std::make_unique<TopologyManager>();
+        std::unordered_map<uint8_t, std::vector<Movement::MovementEntry>> movements;
        nvs_handle_t handle;
+        bool ready = false;
+        Flatbuffers::MovementSetBuilder builder;
 };

 #endif //MOVEMENTS
--- a/components/movements/include/TopologyManager.h
+++ b/components/movements/include/TopologyManager.h
@@ -20,6 +20,7 @@ class TopologyManager {
    public:
        TopologyManager();
        ~TopologyManager();
+        bool is_ready();
        esp_err_t add_board_to_topology(const std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);
        esp_err_t remove_board_from_topology(uint16_t board_id);
        esp_err_t get_board_in_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);