#pragma once

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

#include "driver/i2c.h"
#include "esp_err.h"
#include "freertos/FreeRTOS.h"

#ifdef __cplusplus
extern "C" {
#endif

#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_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_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;
} bmi160_t;

typedef struct {
    struct {
        int16_t x;
        int16_t y;
        int16_t z;
    } acc, gyr;
} 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);
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value);

#ifdef __cplusplus
}
#endif