AR-Autopilot/firmware/ar_autopilot_v1/src/hal/nvs_config.cpp

// =============================================================================
//  hal/nvs_config.cpp -- NVS-backed calibration store (Sprint 7)
// =============================================================================

#include "nvs_config.h"

#include <nvs.h>
#include <nvs_flash.h>
#include <Arduino.h>

#include "../system/ar_log.h"

namespace arautopilot::hal {

namespace {
constexpr const char* TAG      = "AR/NVS";
constexpr const char* NS       = "ar_cal";
}  // namespace

bool nvs_config_init() {
    esp_err_t err = nvs_flash_init();
    if (err == ESP_ERR_NVS_NO_FREE_PAGES ||
        err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        AR_LOGW(TAG, "NVS partition needs erase (err=0x%x) -- factory-resetting", err);
        nvs_flash_erase();
        err = nvs_flash_init();
    }
    if (err != ESP_OK) {
        AR_LOGE(TAG, "nvs_flash_init failed: 0x%x", err);
        return false;
    }
    AR_LOGI(TAG, "NVS initialised (namespace: %s)", NS);
    return true;
}

CalibrationData nvs_config_load() {
    CalibrationData cal;  // defaults in struct definition

    nvs_handle_t h;
    if (nvs_open(NS, NVS_READONLY, &h) != ESP_OK) {
        AR_LOGI(TAG, "No calibration in NVS -- using factory defaults");
        return cal;
    }

    // Helper lambdas: read or keep default on missing key.
    auto get_i16 = [&](const char* key, int16_t& out) {
        int16_t v;
        if (nvs_get_i16(h, key, &v) == ESP_OK) out = v;
    };
    auto get_float = [&](const char* key, float& out) {
        // NVS has no native float; store as uint32 via memcpy.
        uint32_t raw;
        if (nvs_get_u32(h, key, &raw) == ESP_OK) {
            memcpy(&out, &raw, sizeof(float));
        }
    };
    auto get_u8 = [&](const char* key, uint8_t& out) {
        uint8_t v;
        if (nvs_get_u8(h, key, &v) == ESP_OK) out = v;
    };

    get_i16("adc_port",  cal.adc_port);
    get_i16("adc_stbd",  cal.adc_stbd);
    get_float("rud_port", cal.rudder_port_deg);
    get_float("rud_stbd", cal.rudder_stbd_deg);
    get_float("ou_kp",    cal.outer_kp);
    get_float("ou_ki",    cal.outer_ki);
    get_float("ou_kd",    cal.outer_kd);
    uint8_t comm = 0;
    get_u8("commissioned", comm);
    cal.commissioned = (comm == 1);

    nvs_close(h);
    AR_LOGI(TAG, "Calibration loaded (commissioned=%d adc=%d..%d)",
            (int)cal.commissioned, cal.adc_port, cal.adc_stbd);
    return cal;
}

bool nvs_config_save(const CalibrationData& cal) {
    nvs_handle_t h;
    if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) {
        AR_LOGE(TAG, "nvs_open for write failed");
        return false;
    }

    auto put_float = [&](const char* key, float v) {
        uint32_t raw;
        memcpy(&raw, &v, sizeof(float));
        nvs_set_u32(h, key, raw);
    };

    nvs_set_i16(h, "adc_port",  cal.adc_port);
    nvs_set_i16(h, "adc_stbd",  cal.adc_stbd);
    put_float("rud_port", cal.rudder_port_deg);
    put_float("rud_stbd", cal.rudder_stbd_deg);
    put_float("ou_kp",    cal.outer_kp);
    put_float("ou_ki",    cal.outer_ki);
    put_float("ou_kd",    cal.outer_kd);
    nvs_set_u8(h, "commissioned", cal.commissioned ? 1u : 0u);

    esp_err_t err = nvs_commit(h);
    nvs_close(h);
    if (err != ESP_OK) {
        AR_LOGE(TAG, "nvs_commit failed: 0x%x", err);
        return false;
    }
    AR_LOGI(TAG, "Calibration saved to NVS");
    return true;
}

bool nvs_config_erase() {
    nvs_handle_t h;
    if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) return false;
    esp_err_t err = nvs_erase_all(h);
    nvs_commit(h);
    nvs_close(h);
    AR_LOGW(TAG, "NVS namespace '%s' erased (factory reset)", NS);
    return err == ESP_OK;
}

}  // namespace arautopilot::hal