/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
 *
 * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
 * Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
 * the mandatory sector erase by reading them into RAM before erasing.
 */

#include "imu_cal_flash.h"
#include "pid_flash.h"
#include "stm32f7xx_hal.h"
#include <string.h>

bool imu_cal_flash_load(float *pitch_offset, float *roll_offset)
{
    const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;

    if (p->magic != IMU_CAL_FLASH_MAGIC) return false;

    /* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */
    if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false;
    if (p->roll_offset  < -90.0f || p->roll_offset  > 90.0f) return false;

    *pitch_offset = p->pitch_offset;
    *roll_offset  = p->roll_offset;
    return true;
}

/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'.
 * Flash must be unlocked by caller. */
static HAL_StatusTypeDef write_words(uint32_t addr,
                                     const void *src,
                                     uint32_t len)
{
    const uint32_t *p = (const uint32_t *)src;
    for (uint32_t i = 0; i < len / 4u; i++) {
        HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
                                                  addr, p[i]);
        if (rc != HAL_OK) return rc;
        addr += 4u;
    }
    return HAL_OK;
}

bool imu_cal_flash_save(float pitch_offset, float roll_offset)
{
    /* Snapshot PID records BEFORE erasing so we can restore them */
    pid_flash_t       pid_snap;
    pid_sched_flash_t sched_snap;
    memcpy(&pid_snap,   (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap));
    memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap));

    HAL_StatusTypeDef rc;

    rc = HAL_FLASH_Unlock();
    if (rc != HAL_OK) return false;

    /* Erase sector 7 (covers all three records) */
    FLASH_EraseInitTypeDef erase = {
        .TypeErase    = FLASH_TYPEERASE_SECTORS,
        .Sector       = PID_FLASH_SECTOR,
        .NbSectors    = 1,
        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
    };
    uint32_t sector_error = 0;
    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
        HAL_FLASH_Lock();
        return false;
    }

    /* Write new IMU calibration record at 0x0807FF00 */
    imu_cal_flash_t cal;
    memset(&cal, 0xFF, sizeof(cal));
    cal.magic        = IMU_CAL_FLASH_MAGIC;
    cal.pitch_offset = pitch_offset;
    cal.roll_offset  = roll_offset;

    rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal));
    if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }

    /* Restore PID gain schedule if it was valid */
    if (sched_snap.magic == PID_SCHED_MAGIC) {
        rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap));
        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
    }

    /* Restore single-PID record if it was valid */
    if (pid_snap.magic == PID_FLASH_MAGIC) {
        rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap));
        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
    }

    HAL_FLASH_Lock();

    /* Verify readback */
    const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
    return (v->magic        == IMU_CAL_FLASH_MAGIC &&
            v->pitch_offset == pitch_offset &&
            v->roll_offset  == roll_offset);
}