/*
 * mpu6000.c — IMU Sensor Fusion for MPU6000
 *
 * Wraps the icm42688 SPI driver (which auto-detects MPU6000) and applies
 * a complementary filter to produce a stable pitch estimate.
 *
 * Hardware: MAMBA F722S, MPU6000 on SPI1, CW270 alignment
 * Config:   Gyro ±2000°/s (init_mpu6000 sets FS_SEL=3)
 *           Accel ±16g    (init_mpu6000 sets AFS_SEL=3)
 */

#include "mpu6000.h"
#include "icm42688.h"
#include "stm32f7xx_hal.h"
#include <math.h>

/* Scale factors matching init_mpu6000() config in icm42688.c */
#define GYRO_SCALE    (1.0f / 16.384f)   /* LSB to °/s  — ±2000°/s range */
#define ACCEL_SCALE   (1.0f / 2048.0f)   /* LSB to g    — ±16g range     */

/*
 * Complementary filter coefficient.
 * 0.98 = trust gyro integration, 0.02 = accel correction for drift.
 * Tune higher (0.99) for noisier environments.
 */
#define COMP_ALPHA    0.98f

/* Filter state */
static float s_pitch = 0.0f;
static uint32_t s_last_tick = 0;

bool mpu6000_init(void) {
    int ret = icm42688_init();
    if (ret == 0) {
        s_pitch = 0.0f;
        s_last_tick = HAL_GetTick();
    }
    return (ret == 0);
}

void mpu6000_read(IMUData *data) {
    icm42688_data_t raw;
    icm42688_read(&raw);

    /* Compute dt from wall clock — robust to loop jitter */
    uint32_t now = HAL_GetTick();
    uint32_t elapsed_ms = now - s_last_tick;
    if (elapsed_ms == 0) elapsed_ms = 1;         /* min 1ms to avoid divide-by-zero */
    if (elapsed_ms > 100) elapsed_ms = 100;       /* clamp: don't integrate stale data */
    float dt = elapsed_ms * 0.001f;
    s_last_tick = now;

    /* Convert raw to physical units */
    float ax = raw.ax * ACCEL_SCALE;   /* g  */
    float az = raw.az * ACCEL_SCALE;   /* g  */

    /*
     * Gyro pitch axis with CW270 alignment: pitch rate = gx.
     * Sign may need inverting depending on mounting orientation —
     * verify during bench test (positive nose-up should be positive).
     */
    float gyro_pitch_rate = raw.gx * GYRO_SCALE;  /* °/s */

    /*
     * Accel pitch angle (degrees).
     * CW270 alignment: pitch = atan2(ax, az).
     * Valid while ax² + az² ≈ 1g (i.e., low linear acceleration).
     */
    float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f);

    /*
     * Complementary filter:
     *   pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch
     *
     * Gyro integration tracks fast dynamics; accel correction
     * prevents long-term drift.
     */
    s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt)
            + (1.0f - COMP_ALPHA) * accel_pitch;

    data->pitch      = s_pitch;
    data->pitch_rate = gyro_pitch_rate;
    data->accel_x    = ax;
    data->accel_z    = az;
}