From 44af33921bf6588a170a6ee7e6461c5197bdb878 Mon Sep 17 00:00:00 2001 From: superkor Date: Tue, 27 Jan 2026 22:17:44 -0500 Subject: [PATCH] update movement set --- components/flatbuffers/MovementSetBuilder.cpp | 76 +++++++++---- components/flatbuffers/TopologyBuilder.cpp | 16 +-- .../flatbuffers/include/MovementSetBuilder.h | 14 ++- .../flatbuffers/include/TopologyBuilder.h | 1 - .../Movement_generated.h | 102 ++++++++++++++---- components/movements/MovementSetManager.cpp | 89 +++++++++++---- components/movements/TopologyManager.cpp | 49 ++++++++- .../movements/include/MovementSetManager.h | 22 ++-- .../movements/include/TopologyManager.h | 1 + 9 files changed, 278 insertions(+), 92 deletions(-) diff --git a/components/flatbuffers/MovementSetBuilder.cpp b/components/flatbuffers/MovementSetBuilder.cpp index 02d5515..8b9c571 100644 --- 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>& set){ - + SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map>& input){ builder_.Clear(); - const auto set_vector = builder_.CreateVector(set); + std::vector> map_entries; + map_entries.reserve(input.size()); - const auto message = Movement::CreateMovementSet( - builder_, - static_cast(set.size()), - set_vector + // 1. Collect and sort keys (DO NOT sort offsets) + std::vector 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> fb_entries; + fb_entries.reserve(entries.size()); + + for (const auto& e : entries) { + fb_entries.push_back( + Movement::CreateMovementEntry( + builder_, + 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 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 diff --git a/components/flatbuffers/TopologyBuilder.cpp b/components/flatbuffers/TopologyBuilder.cpp index 8ea5319..5d07a55 100644 --- a/components/flatbuffers/TopologyBuilder.cpp +++ b/components/flatbuffers/TopologyBuilder.cpp @@ -30,21 +30,7 @@ namespace Flatbuffers{ return { builder_.GetBufferPointer(), builder_.GetSize() }; } - - flatbuffers::Offset TopologyBuilder::build_neighbour_info( - uint16_t board_id, - const std::vector& connections - ) { - auto conn_vec = builder_.CreateVectorOfStructs(connections); - - return Topology::CreateNeighbourBlob( - builder_, - board_id, - static_cast(connections.size()), - conn_vec - ); - } - + Topology::ChannelBoardConn TopologyBuilder::build_connections(uint8_t channel, uint16_t board_id){ return Topology::ChannelBoardConn(channel, board_id); } diff --git a/components/flatbuffers/include/MovementSetBuilder.h b/components/flatbuffers/include/MovementSetBuilder.h index cbf5af6..eb63a2c 100644 --- 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>& set); - flatbuffers::Offset 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>& input); Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use); private: diff --git a/components/flatbuffers/include/TopologyBuilder.h b/components/flatbuffers/include/TopologyBuilder.h index f996818..773d8ef 100644 --- 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 build_neighbour_info(uint16_t board_id, const std::vector& connections); flatbuffers::FlatBufferBuilder builder_; }; } diff --git a/components/flatbuffers/include/flatbuffers_generated/Movement_generated.h b/components/flatbuffers/include/flatbuffers_generated/Movement_generated.h index b6e8010..58c2499 100644 --- 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 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(VT_NUM_MOVEMENTS, 0); + uint8_t key() const { + return GetField(VT_KEY, 0); + } + bool KeyCompareLessThan(const MovementVector * const o) const { + return key() < o->key(); + } + int KeyCompareWithValue(uint8_t _key) const { + return static_cast(key() > _key) - static_cast(key() < _key); } const ::flatbuffers::Vector<::flatbuffers::Offset> *movements() const { return GetPointer> *>(VT_MOVEMENTS); @@ -180,7 +189,7 @@ struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { template bool Verify(::flatbuffers::VerifierTemplate &verifier) const { return VerifyTableStart(verifier) && - VerifyField(verifier, VT_NUM_MOVEMENTS, 1) && + VerifyField(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(MovementVector::VT_KEY, key, 0); + } + void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> movements) { + fbb_.AddOffset(MovementVector::VT_MOVEMENTS, movements); + } + explicit MovementVectorBuilder(::flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreateMovementVector( + ::flatbuffers::FlatBufferBuilder &_fbb, + uint8_t key = 0, + ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> movements = 0) { + MovementVectorBuilder builder_(_fbb); + builder_.add_movements(movements); + builder_.add_key(key); + return builder_.Finish(); +} + +inline ::flatbuffers::Offset CreateMovementVectorDirect( + ::flatbuffers::FlatBufferBuilder &_fbb, + uint8_t key = 0, + const std::vector<::flatbuffers::Offset> *movements = nullptr) { + auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset>(*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_map() const { + return GetPointer> *>(VT_MOVEMENT_MAP); + } + template + bool Verify(::flatbuffers::VerifierTemplate &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(MovementSet::VT_NUM_MOVEMENTS, num_movements, 0); - } - void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> movements) { - fbb_.AddOffset(MovementSet::VT_MOVEMENTS, movements); + void add_movement_map(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> 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 CreateMovementSet( ::flatbuffers::FlatBufferBuilder &_fbb, - uint8_t num_movements = 0, - ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> movements = 0) { + ::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset>> 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 CreateMovementSetDirect( ::flatbuffers::FlatBufferBuilder &_fbb, - uint8_t num_movements = 0, - const std::vector<::flatbuffers::Offset> *movements = nullptr) { - auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset>(*movements) : 0; + std::vector<::flatbuffers::Offset> *movement_map = nullptr) { + auto movement_map__ = movement_map ? _fbb.CreateVectorOfSortedTables(movement_map) : 0; return Movement::CreateMovementSet( _fbb, - num_movements, - movements__); + movement_map__); } inline const Movement::MovementSet *GetMovementSet(const void *buf) { diff --git a/components/movements/MovementSetManager.cpp b/components/movements/MovementSetManager.cpp index 53d288e..6ab6fa0 100644 --- 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 + } + } + } + + movements[index].push_back(entry); -// esp_err_t MovementSetManager::get_curr_movement_set(struct MovementSet& set){ -// return ESP_OK; -// } + return ESP_OK; +} -// esp_err_t MovementSetManager::verify_movement_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::get_nvs_movement_set(struct MovementSet& set){ -// return ESP_OK; -// } + if (movements.find(index) == movements.end()){ + return ESP_ERR_INVALID_ARG; + } -// esp_err_t MovementSetManager::write_nvs_movement_set(){ -// return ESP_OK; -// } + 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 \ No newline at end of file diff --git a/components/movements/TopologyManager.cpp b/components/movements/TopologyManager.cpp index 3031258..96431a4 100644 --- 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>& 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 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>>& topology){ if (!ready){ return ESP_ERR_INVALID_STATE; @@ -204,6 +240,11 @@ esp_err_t TopologyManager::get_curr_topology(std::unordered_map>>& topology){ if (!ready) { return ESP_ERR_INVALID_STATE; diff --git a/components/movements/include/MovementSetManager.h b/components/movements/include/MovementSetManager.h index db2b8f7..c286d1a 100644 --- a/components/movements/include/MovementSetManager.h +++ b/components/movements/include/MovementSetManager.h @@ -3,6 +3,8 @@ #include "TopologyManager.h" #include #include +#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 topology_manager; - // std::vector movements; + std::unique_ptr topology_manager = std::make_unique(); + std::unordered_map> movements; nvs_handle_t handle; + bool ready = false; + Flatbuffers::MovementSetBuilder builder; }; #endif //MOVEMENTS \ No newline at end of file diff --git a/components/movements/include/TopologyManager.h b/components/movements/include/TopologyManager.h index 94ebf6d..f4abedc 100644 --- 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>& 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>& connections, uint16_t curr_board_id);