update movement set

components/flatbuffers/MovementSetBuilder.cpp

@@ -6,16 +6,61 @@
 #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()),
+                    e.board_id,
-            set_vector
+                    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);
@@ -23,23 +68,10 @@ namespace Flatbuffers {
         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
-        );
-    }
-
     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

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);
     }

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);
+        SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input);
-        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);
         Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use);
 
     private:

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_;
     };
 }

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 {
+struct MovementVector FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
-  typedef MovementSetBuilder Builder;
+  typedef MovementVectorBuilder Builder;
   enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
-    VT_NUM_MOVEMENTS = 4,
+    VT_KEY = 4,
     VT_MOVEMENTS = 6
   };
-  uint8_t num_movements() const {
+  uint8_t key() const {
-    return GetField<uint8_t>(VT_NUM_MOVEMENTS, 0);
+    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) {
+  void add_movement_map(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>>> movement_map) {
-    fbb_.AddElement<uint8_t>(MovementSet::VT_NUM_MOVEMENTS, num_movements, 0);
+    fbb_.AddOffset(MovementSet::VT_MOVEMENT_MAP, movement_map);
-  }
-  void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements) {
-    fbb_.AddOffset(MovementSet::VT_MOVEMENTS, movements);
   }
   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::MovementVector>>> movement_map = 0) {
-    ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements = 0) {
   MovementSetBuilder builder_(_fbb);
-  builder_.add_movements(movements);
+  builder_.add_movement_map(movement_map);
-  builder_.add_num_movements(num_movements);
   return builder_.Finish();
 }
 
 inline ::flatbuffers::Offset<MovementSet> CreateMovementSetDirect(
     ::flatbuffers::FlatBufferBuilder &_fbb,
-    uint8_t num_movements = 0,
+    std::vector<::flatbuffers::Offset<Movement::MovementVector>> *movement_map = nullptr) {
-    const std::vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements = nullptr) {
+  auto movement_map__ = movement_map ? _fbb.CreateVectorOfSortedTables<Movement::MovementVector>(movement_map) : 0;
-  auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset<Movement::MovementEntry>>(*movements) : 0;
   return Movement::CreateMovementSet(
       _fbb,
-      num_movements,
+      movement_map__);
-      movements__);
 }
 
 inline const Movement::MovementSet *GetMovementSet(const void *buf) {

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){
+esp_err_t MovementSetManager::add_movement(Movement::MovementEntry& entry, uint8_t& index){
-//     return ESP_OK;
+    if (!ready){
-// }
+        return ESP_ERR_INVALID_STATE;
+    }
 
-// esp_err_t MovementSetManager::remove_movement(uint8_t index){
+    if (entry.board_id() == PC_ADDR || entry.board_id() == BROADCAST_ADDR){
-//     return ESP_OK;
+        return ESP_ERR_INVALID_ARG;
-// }
+    }
 
-// esp_err_t MovementSetManager::update_movement(struct MovementEntry& entry, uint8_t index){
+    if (movements.find(index) != movements.end()){
-//     return ESP_OK;
+        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){
+    movements[index].push_back(entry);
-//     return ESP_OK;
-// }
 
-// esp_err_t MovementSetManager::verify_movement_set(){
+    return ESP_OK;
-//     return ESP_OK;
+}
-// }
 
-// esp_err_t MovementSetManager::get_nvs_movement_set(struct MovementSet& set){
+esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){
-//     return ESP_OK;
+    if (board_id == PC_ADDR || board_id == BROADCAST_ADDR){
-// }
+        return ESP_ERR_INVALID_ARG;
+    }
 
-// esp_err_t MovementSetManager::write_nvs_movement_set(){
+    if (movements.find(index) == movements.end()){
-//     return ESP_OK;
+        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

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;

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 add_movement(Movement::MovementEntry& entry, uint8_t& index);
-        // esp_err_t remove_movement(uint8_t index);
+        esp_err_t remove_movement(uint8_t index, uint8_t board_id);
-        // esp_err_t update_movement(struct MovementEntry& entry, uint8_t index);
+        esp_err_t update_movement(Movement::MovementEntry& entry, uint8_t index);
-        // esp_err_t get_curr_movement_set(struct MovementSet& set);
+        esp_err_t get_curr_movement_set(Movement::MovementSet& set);
-        // esp_err_t verify_movement_set();
+        esp_err_t verify_movement_set();
-        // esp_err_t get_nvs_movement_set(struct MovementSet& set);
+        esp_err_t get_nvs_movement_set(Movement::MovementSet& set);
-        // esp_err_t write_nvs_movement_set();
+        esp_err_t write_nvs_movement_set();
 
     private:
-        std::unique_ptr<TopologyManager> topology_manager;
+        std::unique_ptr<TopologyManager> topology_manager = std::make_unique<TopologyManager>();
-        // std::vector<struct MovementEntry> movements;
+        std::unordered_map<uint8_t, std::vector<Movement::MovementEntry>> movements;
         nvs_handle_t handle;
+        bool ready = false;
+        Flatbuffers::MovementSetBuilder builder;
 };
 
 #endif //MOVEMENTS

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);