/* MPU6000 + ICM-42688-P dual driver — auto-detects based on WHO_AM_I */
#include "stm32f7xx_hal.h"
#include "config.h"
#include "icm42688.h"

static SPI_HandleTypeDef hspi1;
static uint8_t imu_type = 0; /* 0=unknown, 1=MPU6000, 2=ICM42688 */

/* MPU6000 registers */
#define MPU_REG_SMPLRT_DIV    0x19
#define MPU_REG_CONFIG        0x1A
#define MPU_REG_GYRO_CONFIG   0x1B
#define MPU_REG_ACCEL_CONFIG  0x1C
#define MPU_REG_ACCEL_XOUT_H  0x3B
#define MPU_REG_PWR_MGMT_1    0x6B
#define MPU_REG_PWR_MGMT_2    0x6C
#define MPU_REG_WHO_AM_I      0x75
#define MPU6000_WHO            0x68

/* ICM-42688-P registers */
#define ICM_REG_DEVICE_CONFIG  0x11
#define ICM_REG_TEMP_DATA1     0x1D
#define ICM_REG_PWR_MGMT0      0x4E
#define ICM_REG_GYRO_CONFIG0   0x4F
#define ICM_REG_ACCEL_CONFIG0  0x50
#define ICM_REG_WHO_AM_I       0x75
#define ICM_REG_BANK_SEL       0x76
#define ICM42688_WHO            0x47

static void cs_low(void)  { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_RESET); }
static void cs_high(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_SET); }

static void wreg(uint8_t reg, uint8_t val) {
    uint8_t tx[2] = { reg & 0x7F, val };
    uint8_t rx[2];
    cs_low();
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
    cs_high();
    HAL_Delay(1);
}

static uint8_t rreg(uint8_t reg) {
    uint8_t tx[2] = { reg | 0x80, 0x00 };
    uint8_t rx[2] = {0, 0};
    cs_low();
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
    cs_high();
    /* DCache coherency: invalidate rx so CPU reads SPI-written SRAM, not stale cache.
     * No-op when DCache is disabled; required if DCache is on (e.g. SCB_EnableDCache). */
    SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
    return rx[1];
}

static uint8_t trace[16];
static int trace_idx = 0;
static void tr(uint8_t v) { if (trace_idx < 16) trace[trace_idx++] = v; }

static int init_mpu6000(void) {
    /* Reset */
    wreg(MPU_REG_PWR_MGMT_1, 0x80);
    HAL_Delay(100);

    /* Wake up, use PLL with X gyro ref */
    wreg(MPU_REG_PWR_MGMT_1, 0x01);
    HAL_Delay(10);

    /* Sample rate = 1kHz (divider=0) */
    wreg(MPU_REG_SMPLRT_DIV, 0x00);

    /* DLPF = 42Hz (config=3) */
    wreg(MPU_REG_CONFIG, 0x03);

    /* Gyro: ±2000°/s (FS_SEL=3) */
    wreg(MPU_REG_GYRO_CONFIG, 0x18);

    /* Accel: ±16g (AFS_SEL=3) */
    wreg(MPU_REG_ACCEL_CONFIG, 0x18);

    /* Enable all axes */
    wreg(MPU_REG_PWR_MGMT_2, 0x00);
    HAL_Delay(50);

    /* Verify */
    uint8_t pwr = rreg(MPU_REG_PWR_MGMT_1);
    tr(pwr);  /* Should be 0x01 */
    
    return (pwr == 0x01) ? 0 : -200 - (int)pwr;
}

int icm42688_init(void) {
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_SPI1_CLK_ENABLE();

    GPIO_InitTypeDef gpio = {0};
    gpio.Pin = GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_NOPULL;
    gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    gpio.Alternate = GPIO_AF5_SPI1;
    HAL_GPIO_Init(GPIOA, &gpio);

    /* CS on PA4 for MAMBA */
    gpio.Pin = MPU_CS_PIN;
    gpio.Mode = GPIO_MODE_OUTPUT_PP;
    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
    gpio.Pull = GPIO_PULLUP;
    HAL_GPIO_Init(MPU_CS_PORT, &gpio);
    cs_high();

    /* DCache: main.c does NOT call SCB_EnableDCache(), so DCache is currently OFF.
     * If DCache is ever enabled, all SPI rx buffers need SCB_InvalidateDCache_by_Addr()
     * (already added in rreg() and icm42688_read()) and USB buffers must remain mapped
     * non-cacheable via MPU Region 0 in usbd_conf.c. */

    hspi1.Instance = SPI1;
    hspi1.Init.Mode = SPI_MODE_MASTER;
    hspi1.Init.Direction = SPI_DIRECTION_2LINES;
    hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
    hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
    hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
    hspi1.Init.NSS = SPI_NSS_SOFT;
    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
    hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
    hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
    hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    if (HAL_SPI_Init(&hspi1) != HAL_OK) return -1;

    HAL_Delay(200);
    /* Wake from sleep first - MPU6000 needs this before WHO_AM_I */
    wreg(0x6B, 0x80); /* Reset */
    HAL_Delay(100);
    wreg(0x6B, 0x01); /* Wake, PLL */
    HAL_Delay(50);

    /* Retry WHO_AM_I up to 3 times: a single SPI glitch returning 0x00
     * would otherwise abort init and prevent calibration from ever running. */
    uint8_t who = 0;
    for (int attempt = 0; attempt < 3 && who == 0; attempt++) {
        if (attempt > 0) HAL_Delay(10);
        who = rreg(MPU_REG_WHO_AM_I);
    }
    tr(who);  /* trace[0] */

    int ret;
    if (who == MPU6000_WHO) {
        imu_type = 1;
        ret = init_mpu6000();
    } else if (who == ICM42688_WHO) {
        imu_type = 2;
        ret = -99; /* TODO: ICM init */
    } else {
        /* who==0 means no SPI response — must not return 0 (false success) */
        ret = (who != 0) ? -(int)who : -128;
    }

    /* Speed up SPI for reads */
    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
    HAL_SPI_Init(&hspi1);

    tr((uint8_t)imu_type);  /* trace[last] */
    return ret;
}

void icm42688_read(icm42688_data_t *d) {
    if (imu_type == 1) {
        /* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */
        uint8_t tx[15] = {0};
        uint8_t rx[15] = {0};   /* zero-init: failed SPI transfers return 0, not garbage */
        tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80;
        cs_low();
        HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100);
        cs_high();
        /* DCache coherency: force CPU to read SPI-written SRAM, not a stale cache line.
         * No-op when DCache is disabled; critical if SCB_EnableDCache() is called. */
        SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));

        d->ax = (int16_t)((rx[1] << 8) | rx[2]);
        d->ay = (int16_t)((rx[3] << 8) | rx[4]);
        d->az = (int16_t)((rx[5] << 8) | rx[6]);
        int16_t temp_raw = (int16_t)((rx[7] << 8) | rx[8]);
        d->temp_x10 = (int16_t)((temp_raw + 12421) / 34);  /* MPU6000 formula */
        d->gx = (int16_t)((rx[9] << 8) | rx[10]);
        d->gy = (int16_t)((rx[11] << 8) | rx[12]);
        d->gz = (int16_t)((rx[13] << 8) | rx[14]);
    }
}

void icm42688_get_trace(uint8_t *out, int *len) {
    *len = trace_idx;
    for (int i = 0; i < trace_idx; i++) out[i] = trace[i];
}