#include <Wire.h>

#define BMI160_ADDR 0x68
#define REG_CHIP_ID 0x00
#define REG_CMD     0x7E
#define REG_GYRO    0x0C
#define REG_ACCEL   0x12

void writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(BMI160_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void readRegisters(uint8_t reg, uint8_t *buffer, uint8_t len) {
  Wire.beginTransmission(BMI160_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);   // repeated start
  Wire.requestFrom(BMI160_ADDR, len);

  for (int i = 0; i < len; i++) {
    if (Wire.available()) {
      buffer[i] = Wire.read();
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(1000);

  Serial.println("BMI160 Test Starting...");
  Wire.setSDA(D4);
  Wire.setSCL(D5);
  Wire.begin();
  Serial.println(Wire.requestFrom(0x68, 1));

  // Check chip ID
  uint8_t chip_id;
  readRegisters(REG_CHIP_ID, &chip_id, 1);

  Serial.print("Chip ID: 0x");
  Serial.println(chip_id, HEX);

  if (chip_id != 0xD1) {
    Serial.println("ERROR: BMI160 not detected!");
    while (1);
  }

  // Put device in normal mode
  writeRegister(REG_CMD, 0x11);

  Serial.println("BMI160 Ready.");
}

void loop() {
  uint8_t buffer[6];

  // Read accelerometer (6 bytes)
  readRegisters(REG_ACCEL, buffer, 6);

  int16_t ax = (buffer[1] << 8) | buffer[0];
  int16_t ay = (buffer[3] << 8) | buffer[2];
  int16_t az = (buffer[5] << 8) | buffer[4];

  float ax_g = ax / 16384.0;
  float ay_g = ay / 16384.0;
  float az_g = az / 16384.0;

  // Read gyroscope (6 bytes)
  readRegisters(REG_GYRO, buffer, 6);

  int16_t gx = (buffer[1] << 8) | buffer[0];
  int16_t gy = (buffer[3] << 8) | buffer[2];
  int16_t gz = (buffer[5] << 8) | buffer[4];

  float gx_dps = gx / 131.0;
  float gy_dps = gy / 131.0;
  float gz_dps = gz / 131.0;

  Serial.print("ACC [g]  X:");
  Serial.print(ax_g, 3);
  Serial.print(" Y:");
  Serial.print(ay_g, 3);
  Serial.print(" Z:");
  Serial.print(az_g, 3);

  Serial.print("   |   GYRO [dps] X:");
  Serial.print(gx_dps, 2);
  Serial.print(" Y:");
  Serial.print(gy_dps, 2);
  Serial.print(" Z:");
  Serial.println(gz_dps, 2);

  delay(100);
}