#pragma once

#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 FREQUENCY           100.0f
#define IMU_DT					    1.0f / FREQUENCY

#define GYRO_FS_SEL_2000		3 << 1
#define GYRO_DLPFCFG_73			3 << 3
#define GYRO_FCHOICE_ON			1
#define GYRO_FCHOICE_OFF		0

#define ACCEL_FS_SEL_4			1 << 1
#define ACCEL_FS_SEL_8			2 << 1
#define ACCEL_DLPFCFG_69		3 << 3
#define ACCEL_FCHOICE_ON		1
#define ACCEL_FCHOICE_OFF		0

static volatile uint8_t i2c_busy = 0;
static uint8_t i2c_buf[16];
static uint8_t i2c_len = 0;
static uint8_t i2c_reg = 0;
static uint8_t i2c_addr = 0;

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

/*typedef struct I2C_Request
{
    uint8_t addr;
    uint8_t reg;
    uint8_t *buf;
    uint8_t len;

    void (*callback)(uint8_t*);

    struct I2C_Request* next;
} I2C_Request;

static I2C_Request* i2c_head = 0;
static uint8_t i2c_busy = 0;*/

/*typedef struct I2C_Request
{
    void (*Callback)(uint8_t* data, uint8_t size);

    uint8_t* Buffer;
    uint8_t Size;

    uint8_t Address;
    uint8_t Write;
    uint8_t Read;

    struct I2C_Request* Next;
} I2C_Request;*/

static void (*i2c_callback)(uint8_t* buf) = 0;

void imu_pow_init();

void i2c_gpio_init();

void i2c1_init();

void imu_init();

void imu_tim6_init(const uint16_t freq);

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

//void i2c_enqueue(I2C_Request* req);
void i2c_start_next();

void imu_get_async(void (*cb)(uint8_t* data, uint8_t size));

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

void imu_read_raw(imu_raw_t* data);

static void i2c_wait_idle(I2C_TypeDef* i2c);

#endif