#pragma once

#include <stddef.h>
#include <stdint.h>

#include "driver/i2c.h"
#include "driver/gpio.h"
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"

#define BMI160_I2C_ADDRESS_LOW 0x68
#define BMI160_I2C_ADDRESS_HIGH 0x69

#define BMI160_REG_CHIP_ID 0x00
#define BMI160_REG_PMU_STATUS 0x03
#define BMI160_REG_DATA 0x04
#define BMI160_SIZE_REG_DATA 20
#define BMI160_REG_SENSORTIME 0x18
#define BMI160_SIZE_SENSORTIME 3
#define BMI160_REG_STATUS 0x1B
#define BMI160_REG_ACC_CONF 0x40
#define BMI160_REG_ACC_RANGE 0x41
#define BMI160_REG_GYR_CONF 0x42
#define BMI160_REG_GYR_RANGE 0x43
#define BMI160_REG_INT_EN0 0x50
#define BMI160_REG_INT_EN1 0x51
#define BMI160_REG_INT_EN2 0x52
#define BMI160_REG_INT_OUT_CTRL 0x53
#define BMI160_REG_INT_LATCH 0x54
#define BMI160_REG_INT_MAP0 0x55
#define BMI160_REG_INT_MAP1 0x56
#define BMI160_REG_INT_MAP2 0x57
#define BMI160_REG_CMD 0x7e

#define BMI160_CHIP_ID 0xD1

#define BMI160_DEFAULT_TIMEOUT_MS 100

typedef struct
{
    i2c_port_t i2c_port;
    uint8_t i2c_address;
    TickType_t timeout_ticks;
    gpio_num_t drdy_io;
    QueueHandle_t sample_queue;
    TaskHandle_t read_task;
} bmi160_t;

typedef struct
{
    struct
    {
        int32_t x; // Fixed point Q.12
        int32_t y; // Fixed point Q.12
        int32_t z; // Fixed point Q.12
    } acc, gyr;
    uint32_t time;
} bmi160_value_t;

esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address);
esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value);
esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len);
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value);
esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len);

esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle);
esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value);