movement set read write to nvs working

.gitignore

@@ -5,5 +5,6 @@ sdkconfig.old
 .vscode/*
 /.cache
 test/build/*
+build_movement
 build_A
 build_B

components/flatbuffers/CMakeLists.txt

@@ -3,6 +3,7 @@ idf_component_register(SRCS "MPIMessageBuilder.cpp"
     "TopologyMessageBuilder.cpp" 
     "SensorMessageBuilder.cpp" 
     "TopologyBuilder.cpp"
+    "MovementSetBuilder.cpp"
     REQUIRES "constants"
     INCLUDE_DIRS "include"
 )

components/flatbuffers/MovementSetBuilder.cpp

@@ -6,7 +6,7 @@
 #ifdef MOVEMENTSETBUILDER
 
 namespace Flatbuffers {
-    SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input){
+    SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryData>>& input){
         builder_.Clear();
 
         std::vector<flatbuffers::Offset<Movement::MovementVector>> map_entries;

components/flatbuffers/include/MovementSetBuilder.h

@@ -11,22 +11,36 @@
 #include "flatbuffers_generated/Movement_generated.h"
 #include "flatbuffers/flatbuffers.h"
 
-struct MovementEntryInput {
+struct MovementEntryData {
     uint16_t board_id;
     uint8_t module_type;
     uint16_t value_action;
-    Movement::ConditionBlob condition;
+    Movement::ConditionBlob condition; // struct is fine to reuse
     uint8_t ack;
     uint16_t ack_ttl_ms;
     uint16_t post_delay_ms;
+
+    bool operator==(const MovementEntryData& other) const {
+    return board_id == other.board_id &&
+            module_type == other.module_type &&
+            value_action == other.value_action &&
+            condition.value() == other.condition.value() &&
+            condition.cond() == other.condition.cond() &&
+            condition.module_type() == other.condition.module_type() &&
+            condition.in_use() == other.condition.in_use() &&
+            ack == other.ack &&
+            ack_ttl_ms == other.ack_ttl_ms &&
+            post_delay_ms == other.post_delay_ms;
+    }
 };
 
+
 namespace Flatbuffers {
     class MovementSetBuilder {
     public:
         MovementSetBuilder() : builder_(MAX_MOVEMENTS_IN_SET*sizeof(Movement::MovementEntry)) {}
 
-        SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input);
+        SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryData>>& input);
         Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use);
 
     private:

components/movements/CMakeLists.txt

@@ -1,4 +1,4 @@
-idf_component_register(SRCS "MovementManager.cpp" "MovementManager.cpp" "TopologyManager.cpp"
+idf_component_register(SRCS "MovementManager.cpp" "MovementSetManager.cpp" "TopologyManager.cpp"
                        PRIV_REQUIRES esp_event nvs_flash rmt dataLink flatbuffers
                     #    REQUIRES esp_timer
                        INCLUDE_DIRS "include")

components/movements/MovementSetManager.cpp

@@ -10,11 +10,6 @@ MovementSetManager::MovementSetManager(){
         return;
     }
 
-    if (!topology_manager->is_ready()){
-        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Topology Manager is not ready!");
-        return;
-    }
-
     ready = true;
 };
 
@@ -22,28 +17,51 @@ MovementSetManager::~MovementSetManager(){
     nvs_close(handle);
 }
 
-esp_err_t MovementSetManager::add_movement(Movement::MovementEntry& entry, uint8_t& index){
+/**
+ * @brief Adds a movement entry into the movements map
+ * 
+ * @param entry Movement entry information
+ * @param index Sequence number of the movements
+ * @return esp_err_t 
+ */
+esp_err_t MovementSetManager::add_movement(MovementEntryData& entry, uint8_t index){
     if (!ready){
         return ESP_ERR_INVALID_STATE;
     }
 
-    if (entry.board_id() == PC_ADDR || entry.board_id() == BROADCAST_ADDR){
+    if (entry.board_id == PC_ADDR || entry.board_id == BROADCAST_ADDR){
         return ESP_ERR_INVALID_ARG;
     }
 
     if (movements.find(index) != movements.end()){
-        for (Movement::MovementEntry& e : movements[index]){
+        for (MovementEntryData& e : movements[index]){
-            if (e.board_id() == entry.board_id()){
+            if (e.board_id == entry.board_id){
                 return ESP_ERR_INVALID_ARG; //only unique board ids per index
             }
         }
     }
 
+    if (entry.post_delay_ms == 0){
+        //we don't want 0ms delay in between the movements, so we will add the default delay
+        entry.post_delay_ms = DEFAULT_DELAY_MS;
+    }
+
+    if (entry.ack != static_cast<uint8_t>(ACK_VALUES::NO_ACK) && entry.ack_ttl_ms == 0){
+        entry.ack_ttl_ms = DEFAULT_ACK_TTL_MS;
+    }
+    
     movements[index].push_back(entry);
 
     return ESP_OK;
 }
 
+/**
+ * @brief Removes a movement entry from the sequence, given the board id
+ * 
+ * @param index Sequence number
+ * @param board_id 
+ * @return esp_err_t 
+ */
 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;
@@ -53,10 +71,14 @@ esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){
         return ESP_ERR_INVALID_ARG;
     }
 
+    if (!ready){
+        return ESP_ERR_INVALID_STATE;
+    }
+
     bool found = false;
 
     for (auto it = movements[index].begin(); it != movements[index].end(); ++it) {
-        if (it->board_id() == board_id){
+        if (it->board_id == board_id){
             movements[index].erase(it);
             found = true;
             break;
@@ -70,23 +92,116 @@ esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){
     return ESP_OK;
 }
 
-esp_err_t MovementSetManager::update_movement(Movement::MovementEntry& entry, uint8_t index){
+esp_err_t MovementSetManager::get_curr_movement_set(std::unordered_map<uint8_t, std::vector<MovementEntryData>>& set){
+    if (!ready){
+        return ESP_ERR_INVALID_STATE;
+    }
+    
+    set = movements;
     return ESP_OK;
 }
 
-esp_err_t MovementSetManager::get_curr_movement_set(Movement::MovementSet& set){
+esp_err_t MovementSetManager::get_nvs_movement_set(std::unordered_map<uint8_t, std::vector<MovementEntryData>>& set){
-    return ESP_OK;
+    if (!ready){
+        return ESP_ERR_INVALID_STATE;
     }
 
-esp_err_t MovementSetManager::verify_movement_set(){
+    size_t size = 0;
-    return ESP_OK;
+    esp_err_t res = nvs_get_u32(handle, MOVEMENTS_NVS_SET_SIZE_KEY, reinterpret_cast<uint32_t*>(&size));
+
+    if (res != ESP_OK) {
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to read data size");
+        return res;
     }
 
-esp_err_t MovementSetManager::get_nvs_movement_set(Movement::MovementSet& set){
+    std::vector<uint8_t> buffer(size);
+
+    res = nvs_get_blob(handle, MOVEMENTS_NVS_SET_KEY, buffer.data(), &size);
+
+    if (res != ESP_OK) {
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to read blob");
+        return res;
+    }
+
+    // Verify the FlatBuffer
+    flatbuffers::Verifier verifier(buffer.data(), buffer.size());
+    if (!Movement::VerifyMovementSetBuffer(verifier)) {
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "FlatBuffer verification failed");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    // Parse root
+    const Movement::MovementSet* movement_set = Movement::GetMovementSet(buffer.data());
+
+    movements.clear();
+
+    const auto* map = movement_set->movement_map();
+    if (!map) {
+        return ESP_OK; // empty set
+    }
+
+    // Iterate map entries
+    for (const Movement::MovementVector* mv : *map) {
+        uint8_t key = mv->key();
+
+        auto& vec = movements[key];
+        vec.clear();
+
+        const auto* entries = mv->movements();
+        if (!entries) {
+            continue;
+        }
+
+        vec.reserve(entries->size());
+
+        // Must iterate over Movement::MovementEntry* (FlatBuffers table)
+        for (const Movement::MovementEntry* e : *entries) {
+            vec.push_back({
+                e->board_id(),
+                e->module_type(),
+                e->value_action(),
+                *e->condition(),   // copy struct
+                e->ack(),
+                e->ack_ttl_ms(),
+                e->post_delay_ms()
+            });
+        }
+    }
+
+    set = movements;
+    
     return ESP_OK;
 }
 
 esp_err_t MovementSetManager::write_nvs_movement_set(){
+    if (!ready){
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    Flatbuffers::SerializedMessage m = builder.build_movement_set(movements);
+
+    esp_err_t res = nvs_set_u32(handle, MOVEMENTS_NVS_SET_SIZE_KEY, static_cast<uint32_t>(m.size));
+
+    res = nvs_commit(handle);
+
+    if (res != ESP_OK) {
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to commit data size");
+        return res;
+    }
+
+    res = nvs_set_blob(handle, MOVEMENTS_NVS_SET_KEY, m.data, m.size);
+
+    if (res != ESP_OK){
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to write to nvs");
+        return res;
+    }
+
+    res = nvs_commit(handle);
+    if (res != ESP_OK) {
+        ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to commit topology");
+        return res;
+    }
+    
     return ESP_OK;
 }
 

components/movements/include/MovementManager.h

@@ -7,6 +7,8 @@
 #define MOVEMENTS_NVS_KEY "key"
 #define MOVEMENTS_NVS_TOPOLOGY_KEY "topology"
 #define MOVEMENTS_NVS_TOPOLOGY_DATA_SIZE_KEY "data_size"
+#define MOVEMENTS_NVS_SET_KEY "mvt_set"
+#define MOVEMENTS_NVS_SET_SIZE_KEY "mvt_data_size"
 #define MOVEMENTS_DEBUG_TAG "movements"
 
 class MovementManager {

components/movements/include/MovementSetManager.h

@@ -1,29 +1,47 @@
 #include "MovementManager.h"
 #include "nvs_flash.h"
-#include "TopologyManager.h"
 #include <memory>
 #include <vector>
 #include "MovementSetBuilder.h"
 #include "unordered_map"
+#include "constants/rmt.h"
+#include "constants/datalink.h"
 
 #ifdef MOVEMENTS
 #pragma once
 
+#define DEFAULT_DELAY_MS 50 //some random value that i am using to default to for the delay after a movement has been completed
+#define DEFAULT_ACK_TTL_MS 100
+#define CONDITION_BLOB_NOT_IN_USE 0
+#define CONDITION_BLOB_IN_USE 1
+
+enum class ACK_VALUES : uint8_t {
+    NO_ACK, //no acks required
+    ACK, //acks required to be sent and received properly within ttl
+    ACK_NO_FAIL, //acks required to be send but not necessarily required to be received properly within ttl
+};
+
+enum class CONDITION_VALUES : uint8_t {
+    LT,
+    LEQ,
+    GT,
+    GEQ,
+    EQ, 
+    NE,
+};
+
 class MovementSetManager {
     public: 
         MovementSetManager();
         ~MovementSetManager();
-        esp_err_t add_movement(Movement::MovementEntry& entry, uint8_t& index);
+        esp_err_t add_movement(MovementEntryData& 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(std::unordered_map<uint8_t, std::vector<MovementEntryData>>& set);
-        esp_err_t get_curr_movement_set(Movement::MovementSet& set);
+        esp_err_t get_nvs_movement_set(std::unordered_map<uint8_t, std::vector<MovementEntryData>>& 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::make_unique<TopologyManager>();
+        std::unordered_map<uint8_t, std::vector<MovementEntryData>> movements;
-        std::unordered_map<uint8_t, std::vector<Movement::MovementEntry>> movements;
         nvs_handle_t handle;
         bool ready = false;
         Flatbuffers::MovementSetBuilder builder;

main/README.md

@@ -42,6 +42,8 @@ To run the sample code, add the compiler variable `PP_MOVE=1` when compiling.
 
 You can use the `SRC_BOARD` compiler variable to set a custom board id.
 
+Sample usage: `idf.py -B ./build_movement build -D SRC_BOARD=101 -D PP_MOVE=1`
+
 The sample topology used to test:
 
 ![Sample Topology Used for Testing](images/sample_topology.png)

main/main_preprogrammed_test.cpp

@@ -10,6 +10,9 @@
 
 #include "constants/datalink.h"
 #include "TopologyManager.h"
+#include "MovementSetManager.h"
+#include "MovementSetBuilder.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
 
 #include <esp_netif.h>
 #include <esp_event.h>
@@ -238,6 +241,80 @@ extern "C" [[noreturn]] void app_main(void) {
 
     ESP_LOGI(MOVEMENT_DEBUG_TAG, "Current topology is correct");
 
+    //Testing Movements
+
+    MovementSetManager movement_set_mgr = MovementSetManager();
+
+    std::unordered_map<uint8_t, std::vector<MovementEntryData>> movements = {
+        {0, {
+            {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+            {67, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+        }},
+        {1, {
+            {198, (uint8_t)ModuleType_BATTERY, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}
+        }},
+        {2, {
+            {56, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+            {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+            {26, (uint8_t)ModuleType_SERVO_2, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+        }},
+        {3, {
+            {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+            {67, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0},
+        }},
+    };
+
+    for (auto& [index, movement_vector] : movements){
+        for (auto& movement : movement_vector){
+            res = movement_set_mgr.add_movement(movement, index);
+            if (res != ESP_OK){
+                ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to insert movement for board id %d at index %d", movement.board_id, index);
+                restart();
+            }
+        }
+    }
+
+    res = movement_set_mgr.write_nvs_movement_set();
+
+    if (res != ESP_OK){
+        ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to write movement set to nvs");
+        restart();
+    }
+
+    std::unordered_map<uint8_t, std::vector<MovementEntryData>> movements_test;
+
+    res = movement_set_mgr.get_nvs_movement_set(movements_test);
+
+    if (res != ESP_OK){
+        ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to get movement set from nvs");
+        restart();
+    }
+
+    for (const auto& [key, vec] : movements) {
+        auto it = movements_test.find(key);
+        if (it == movements_test.end()) {
+            ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Missing key %d", key);
+            restart();
+        }
+
+        const auto& vec2 = it->second;
+        if (vec.size() != vec2.size()) {
+            ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Size mismatch at key %d: %d vs %d", key, vec.size(), vec2.size());
+            restart();
+        }
+
+        for (size_t i = 0; i < vec.size(); ++i) {
+            if (!(vec[i] == vec2[i])) {
+                ESP_LOGE(MOVEMENTS_DEBUG_TAG,
+                        "Mismatch at key %d index %d: board_id %d vs %d",
+                        key, i, vec[i].board_id, vec2[i].board_id);
+                restart();
+            }
+        }
+    }
+
+    ESP_LOGI(MOVEMENTS_DEBUG_TAG, "Successfully got movement set from nvs correctly");
+
     restart();
 
     while(true){