#ifndef IMU_H
#define IMU_H

#include "stm32g431xx.h"

#define ICM_ADDR                0x68
#define REG_PWR_MGMT_1          0x06
#define REG_BANK_SEL            0x7F
#define REG_GYRO_CONFIG_1       0x01
#define REG_ACCEL_CONFIG        0x14
#define IMU_RATE_HZ             1000                    // 1 ms
#define IMU_DT                  0.002f   // 2 ms

typedef struct
{
  int16_t ax, ay, az; // lsb
  int16_t gx, gy, gz; // lsb
} imu_raw_t;

void i2c_gpio_init();

void i2c1_init();

void icm_init();

void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);

void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);

void icm_read_raw(imu_raw_t* data);

static void i2c_wait_idle(I2C_TypeDef* i2c);

#endif