/*
 * bno055.c — Bosch BNO055 NDOF IMU driver over I2C1
 *
 * Issue #135: auto-detect alongside MPU6000; fallback + yaw augmentation.
 *
 * Calibration offsets are persisted in STM32F7 RTC backup registers
 * (BKP0R–BKP6R) and restored automatically on next power-on, so the
 * sensor re-enters a calibrated state immediately after reset.
 *
 * Internal temperature compensation:
 *   The BNO055 silicon compensates all three sensors (accel, gyro, mag)
 *   continuously against its onboard thermometer. No external correction
 *   is required. bno055_temperature() exposes the reading for telemetry.
 */

#include "bno055.h"
#include "i2c1.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
#include <math.h>

/* ---- I2C addresses (7-bit, shifted left 1 for HAL) ---- */
#define BNO055_ADDR_LOW    (0x28u << 1)
#define BNO055_ADDR_HIGH   (0x29u << 1)
#define BNO055_CHIP_ID_VAL  0xA0u

/* ---- Register map (page 0) ---- */
#define REG_CHIP_ID        0x00u
#define REG_PAGE_ID        0x07u
#define REG_GYRO_X_LSB    0x14u   /* 6 bytes: Gx, Gy, Gz */
#define REG_EULER_H_LSB   0x1Au   /* 6 bytes: H, R, P (each int16) */
#define REG_LIA_X_LSB     0x28u   /* 6 bytes: linear accel X,Y,Z */
#define REG_GRV_X_LSB     0x2Eu   /* 6 bytes: gravity X,Y,Z */
#define REG_TEMP          0x34u
#define REG_CALIB_STAT    0x35u
#define REG_OPR_MODE      0x3Du
#define REG_PWR_MODE      0x3Eu
#define REG_SYS_TRIGGER   0x3Fu
#define REG_TEMP_SOURCE   0x40u
#define REG_OFFSET_BASE   0x55u   /* 22 bytes: accel(6)+mag(6)+gyro(6)+radii(4) */

/* ---- Operating modes ---- */
#define MODE_CONFIG    0x00u
#define MODE_IMUPLUS   0x08u   /* 6DOF, no mag */
#define MODE_NDOF      0x0Cu   /* 9DOF with mag */

/* ---- RTC backup register magic for saved offsets ---- */
/*
 * Layout: BKP0R..BKP5R hold 22 bytes of calibration offsets (packed
 * 4 bytes per register; BKP5R only uses its low 2 bytes).
 * BKP6R holds the magic word to validate the stored data.
 */
#define BNO055_BKP_MAGIC   0xB055CA10u
#define BKP_REG(n)  (*(volatile uint32_t *)(&RTC->BKP0R + (n)))

/* ---- Scaling constants ---- */
/*
 * BNO055 default unit selection (UNIT_SEL=0x00):
 *   Euler angles: 1 degree = 16 LSB → divide by 16.0f
 *   Gyro rates:   1 °/s    = 16 LSB → divide by 16.0f
 *   Accel/LIA/GRV: 1 m/s²  = 100 LSB → divide by 100.0f
 */
#define EULER_SCALE   16.0f
#define GYRO_SCALE    16.0f
#define ACCEL_SCALE   100.0f
#define G_MS2          9.80665f     /* m/s² per g */

/* ---- Module state ---- */
static uint16_t s_addr      = 0;
static bool     s_present   = false;
static bool     s_offsets_ok = false;     /* true if offsets restored */
static uint8_t  s_calib     = 0;          /* cached CALIB_STAT */
static int8_t   s_temp      = 25;         /* cached temperature °C */
static uint16_t s_temp_ctr  = 0;          /* temp read divider */

/* ---- Low-level I2C helpers ---- */
static uint8_t rd(uint8_t reg)
{
    uint8_t v = 0;
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 100);
    return v;
}

static void wr(uint8_t reg, uint8_t val)
{
    HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
}

static void rdblk(uint8_t reg, uint8_t *buf, uint8_t len)
{
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 200);
}

static void wrblk(uint8_t reg, const uint8_t *buf, uint8_t len)
{
    HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, (uint8_t *)buf, len, 200);
}

/* ---- bno055_restore_offsets() ---- */
bool bno055_restore_offsets(void)
{
    __HAL_RCC_PWR_CLK_ENABLE();
    HAL_PWR_EnableBkUpAccess();

    if (BKP_REG(6) != BNO055_BKP_MAGIC) return false;

    /* Unpack 22 bytes from BKP0R..BKP5R */
    uint8_t offsets[22];
    for (int i = 0; i < 5; i++) {
        uint32_t w = BKP_REG(i);
        offsets[i * 4 + 0] = (uint8_t)(w & 0xFFu);
        offsets[i * 4 + 1] = (uint8_t)((w >>  8) & 0xFFu);
        offsets[i * 4 + 2] = (uint8_t)((w >> 16) & 0xFFu);
        offsets[i * 4 + 3] = (uint8_t)((w >> 24) & 0xFFu);
    }
    uint32_t w5 = BKP_REG(5);
    offsets[20] = (uint8_t)(w5 & 0xFFu);
    offsets[21] = (uint8_t)((w5 >> 8) & 0xFFu);

    /* Write offsets — must be in CONFIG mode */
    wr(REG_OPR_MODE, MODE_CONFIG);
    HAL_Delay(25);
    wr(REG_PAGE_ID, 0x00);
    wrblk(REG_OFFSET_BASE, offsets, 22);
    /* Caller switches back to NDOF after this returns */
    return true;
}

/* ---- bno055_save_offsets() ---- */
bool bno055_save_offsets(void)
{
    if (!s_present) return false;

    /* Switch to CONFIG mode to read offsets */
    wr(REG_OPR_MODE, MODE_CONFIG);
    HAL_Delay(25);
    wr(REG_PAGE_ID, 0x00);

    uint8_t offsets[22];
    rdblk(REG_OFFSET_BASE, offsets, 22);

    /* Restore to NDOF */
    wr(REG_OPR_MODE, MODE_NDOF);
    HAL_Delay(10);

    /* Pack into RTC backup registers */
    __HAL_RCC_PWR_CLK_ENABLE();
    HAL_PWR_EnableBkUpAccess();

    for (int i = 0; i < 5; i++) {
        BKP_REG(i) = ((uint32_t)offsets[i * 4 + 0])
                   | ((uint32_t)offsets[i * 4 + 1] <<  8)
                   | ((uint32_t)offsets[i * 4 + 2] << 16)
                   | ((uint32_t)offsets[i * 4 + 3] << 24);
    }
    BKP_REG(5) = (uint32_t)offsets[20] | ((uint32_t)offsets[21] << 8);
    BKP_REG(6) = BNO055_BKP_MAGIC;

    return true;
}

/* ---- bno055_init() ---- */
int bno055_init(void)
{
    /* Probe 0x28 (ADR=0) first, then 0x29 (ADR=1) */
    const uint16_t candidates[2] = { BNO055_ADDR_LOW, BNO055_ADDR_HIGH };
    s_present = false;
    for (int i = 0; i < 2; i++) {
        s_addr = candidates[i];
        if (rd(REG_CHIP_ID) == BNO055_CHIP_ID_VAL) {
            s_present = true;
            break;
        }
    }
    if (!s_present) return -1;

    /* Software reset via SYS_TRIGGER */
    wr(REG_PAGE_ID, 0x00);
    wr(REG_SYS_TRIGGER, 0x20u);
    HAL_Delay(700);   /* POR settle time from datasheet */

    /* Confirm chip ID after reset */
    if (rd(REG_CHIP_ID) != BNO055_CHIP_ID_VAL) {
        s_present = false;
        return -1;
    }

    wr(REG_PAGE_ID, 0x00);

    /* CONFIGMODE before any register writes */
    wr(REG_OPR_MODE, MODE_CONFIG);
    HAL_Delay(25);

    /* Normal power mode */
    wr(REG_PWR_MODE, 0x00u);
    HAL_Delay(10);

    /*
     * Unit selection (UNIT_SEL = 0x00, default):
     *   Euler = degrees, Gyro = dps, Accel = m/s², Temp = °C
     * No write needed — defaults match what we want.
     */

    /* Temperature source = gyro (more stable than accel) */
    wr(REG_TEMP_SOURCE, 0x01u);

    /* Try to restore saved calibration offsets */
    s_offsets_ok = bno055_restore_offsets();

    /* Enter NDOF fusion mode */
    wr(REG_OPR_MODE, MODE_NDOF);
    HAL_Delay(20);

    s_calib  = 0;
    s_temp   = 25;
    s_temp_ctr = 0;
    return 0;
}

/* ---- bno055_read() ---- */
/*
 * Performs two I2C burst reads (~3ms total at 100kHz):
 *   1. REG_GYRO_X_LSB  → 12 bytes: gyro(6) + euler(6)
 *   2. REG_LIA_X_LSB   → 12 bytes: linear_accel(6) + gravity(6)
 *
 * BNO055 coordinate frame (NDOF, no remapping):
 *   X: right, Y: forward, Z: up
 *   Euler H: yaw (0–360°, clockwise from north)
 *   Euler R: roll (–180°..+180°, right bank positive)
 *   Euler P: pitch (–90°..+90°, nose up positive)
 *   Gyro Y: pitch rate (deg/s, nose-up positive)
 *
 * Linear accel X (m/s²): forward-backward acceleration (gravity removed)
 * Gravity Z (m/s²): gravity component on Z axis (≈9.81 when level)
 */
void bno055_read(IMUData *data)
{
    uint8_t buf[12];

    /* ---- Burst 1: Gyro (0x14–0x19) + Euler (0x1A–0x1F) ---- */
    rdblk(REG_GYRO_X_LSB, buf, 12);

    /* Gyro: bytes 0..5 = [Gx_L, Gx_H, Gy_L, Gy_H, Gz_L, Gz_H]
     * Gy = pitch rate (Y axis = pitch axis in BNO055 frame) */
    float pitch_rate = (float)(int16_t)((uint16_t)buf[3] << 8 | buf[2]) / GYRO_SCALE;

    /* Euler: bytes 6..11 = [H_L, H_H, R_L, R_H, P_L, P_H] */
    float heading = (float)(int16_t)((uint16_t)buf[7]  << 8 | buf[6])  / EULER_SCALE;
    float roll    = (float)(int16_t)((uint16_t)buf[9]  << 8 | buf[8])  / EULER_SCALE;
    float pitch   = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / EULER_SCALE;

    /* ---- Burst 2: Linear accel (0x28–0x2D) + Gravity (0x2E–0x33) ---- */
    rdblk(REG_LIA_X_LSB, buf, 12);

    /* Linear accel X (forward, m/s²) — gravity-compensated */
    float lia_x_ms2 = (float)(int16_t)((uint16_t)buf[1] << 8 | buf[0]) / ACCEL_SCALE;

    /* Gravity Z (up axis, m/s²) — should be ≈+9.81 when level */
    float grv_z_ms2 = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / ACCEL_SCALE;

    /* Fill IMUData in same units as mpu6000_read() */
    data->pitch      = pitch;
    data->pitch_rate = pitch_rate;
    data->roll       = roll;
    data->yaw        = heading;
    data->accel_x    = lia_x_ms2 / G_MS2;   /* m/s² → g */
    data->accel_z    = grv_z_ms2 / G_MS2;   /* m/s² → g (≈1.0 when level) */

    /* Periodically update calibration status and temperature (~1Hz at 250Hz loop) */
    if (++s_temp_ctr >= 250u) {
        s_temp_ctr = 0;
        s_calib  = rd(REG_CALIB_STAT);
        s_temp   = (int8_t)rd(REG_TEMP);

        /*
         * Auto-save offsets once when full calibration is achieved for
         * the first time this session (sys=3, acc=3, gyr=3, don't wait
         * for mag=3 as it is harder to achieve and not critical for balance).
         */
        static bool s_saved = false;
        if (!s_saved) {
            uint8_t sys_cal = (s_calib & BNO055_CAL_SYS_MASK) >> 6;
            uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
            uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
            if (sys_cal >= 1u && acc_cal >= 3u && gyr_cal >= 3u) {
                bno055_save_offsets();
                s_saved = true;
            }
        }
    }
}

/* ---- bno055_is_ready() ---- */
bool bno055_is_ready(void)
{
    if (!s_present) return false;
    /* If we restored saved offsets, trust them immediately */
    if (s_offsets_ok) return true;
    /* Otherwise require gyro ≥ 2 and accel ≥ 2 */
    uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
    uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
    return (acc_cal >= 2u) && (gyr_cal >= 2u);
}

/* ---- bno055_calib_status() ---- */
uint8_t bno055_calib_status(void)
{
    return s_calib;
}

/* ---- bno055_temperature() ---- */
int8_t bno055_temperature(void)
{
    return s_temp;
}