#include "icm20948.h"

extern I2C_HandleTypeDef hi2c1;
#define ICM20948_ADDR (0x69 << 1)

uint8_t read_register(uint8_t reg) {
    uint8_t val;
    HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
    return val;
}

void write_register(uint8_t reg, uint8_t val) {
    HAL_I2C_Mem_Write(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
}

void icm20948_select_bank(uint8_t bank) {
    write_register(0x7F, bank << 4);  // REG_BANK_SEL (0x7F)
}

void icm20948_init(void) {
    icm20948_select_bank(0);           // BANK 0
    write_register(0x06, 0x01);        // PWR_MGMT_1: reset
    HAL_Delay(10);
    write_register(0x06, 0x01);        // Clock source auto

    icm20948_select_bank(2);           // BANK 2
   // write_register(0x14, 0x00);        // ACCEL_CONFIG: ±2g
    write_register(0x14, 0x04);  // ACCEL_CONFIG : ±8g (bits 2:1 = 10)

    icm20948_select_bank(0);           // Revenir en BANK 0 pour lecture
}

void icm20948_read_accel(float *ax, float *ay, float *az) {
    uint8_t data[6];
    HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x2D, 1, data, 6, HAL_MAX_DELAY);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    //*ax = (float)x / 16384.0f;
    //*ay = (float)y / 16384.0f;
    //*az = (float)z / 16384.0f;
    *ax = (float)x / 4096.0f;
    *ay = (float)y / 4096.0f;
    *az = (float)z / 4096.0f;

}

void icm20948_read_gyro(float *gx, float *gy, float *gz) {
    uint8_t data[6];
    icm20948_select_bank(0);
    HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x33, 1, data, 6, HAL_MAX_DELAY);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    *gx = (float)x / 131.0f; // sensibilité typique ±250 dps
    *gy = (float)y / 131.0f;
    *gz = (float)z / 131.0f;
}



void icm20948_mag_init(void) {
    icm20948_select_bank(3);
    // Configurer le bus maître I2C (ex : 400 kHz)
    write_register(0x01, 0x07); // I2C_MST_CTRL, 400 kHz
    // Configure la lecture du magnéto via I2C Master
    // Exemples de setup (registres slave 0, adresse, etc.)
    // ...
    icm20948_select_bank(0);
}

void icm20948_read_mag(float *mx, float *my, float *mz) {
    uint8_t data[6];
    // Lecture des registres magnéto via I2C Master
    // Par défaut les données du magnétomètre sont relayées en registres USER_MAG_DATA_X/Y/Z
    // ou on doit lire via I2C Master
    // Exemple simplifié :
    HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x31, 1, data, 6, HAL_MAX_DELAY);
    int16_t x = (data[1] << 8) | data[0]; // Attention à l’ordre des octets pour AK09916
    int16_t y = (data[3] << 8) | data[2];
    int16_t z = (data[5] << 8) | data[4];
    *mx = (float)x * 0.15f; // conversion en µT (selon datasheet)
    *my = (float)y * 0.15f;
    *mz = (float)z * 0.15f;
}