#include "bmi270.h"
#include "FreeRTOS.h"
#include "i2c.h"
#include "pins.h"
#include <stdint.h>

void bmi270_init(void) {
  uint8_t id;
  i2c_read_reg(IMU_I2C_ADDRESS, BMI270_CHIP_ID, &id);

  if (id != 0x24)
    return;

  // soft reset
  i2c_write_reg(IMU_I2C_ADDRESS, 0x7E, 0xB6);
  vTaskDelay(pdMS_TO_TICKS(10));

  // power on accel + gyro
  i2c_write_reg(IMU_I2C_ADDRESS, BMI270_PWR_CTRL, 0x0E);

  // accel config (100 Hz-ish)
  i2c_write_reg(IMU_I2C_ADDRESS, BMI270_ACC_CONF, 0xA8);

  // gyro config
  i2c_write_reg(IMU_I2C_ADDRESS, BMI270_GYR_CONF, 0xA9);
}

void bmi270_read(bmi270_data_t *d) {
  uint8_t buf[12];

  i2c_read_reg_multi(IMU_I2C_ADDRESS, BMI270_ACC_X_L, buf, 12);

  d->ax = (int16_t)(buf[1] << 8 | buf[0]);
  d->ay = (int16_t)(buf[3] << 8 | buf[2]);
  d->az = (int16_t)(buf[5] << 8 | buf[4]);

  d->gx = (int16_t)(buf[7] << 8 | buf[6]);
  d->gy = (int16_t)(buf[9] << 8 | buf[8]);
  d->gz = (int16_t)(buf[11] << 8 | buf[10]);
}