#include <Adafruit_BNO08x.h>

#define BNO08X_I2C_ADDR 0x4A

Adafruit_BNO08x  bno08x;
sh2_SensorValue_t sensorValue;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  Serial.println("BNO08x Test - Accelerometer & Gyroscope");

  if (!bno08x.begin_I2C(BNO08X_I2C_ADDR)) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }

  Serial.println("BNO08x Found!");

  // Enable Accelerometer (500Hz)
  if (!bno08x.enableReport(SH2_ACCELEROMETER)) {
    Serial.println("Could not enable accelerometer");
  }
  
  // Enable Gyroscope (500Hz)
  if (!bno08x.enableReport(SH2_GYROSCOPE_CALIBRATED)) {
    Serial.println("Could not enable gyroscope");
  }
}

void loop() {
  if (bno08x.wasReset()) {
    Serial.println("Sensor was reset");
    bno08x.enableReport(SH2_ACCELEROMETER);
    bno08x.enableReport(SH2_GYROSCOPE_CALIBRATED);
  }
  
  if (!bno08x.getSensorEvent(&sensorValue)) {
    return;
  }

  switch (sensorValue.sensorId) {
    case SH2_ACCELEROMETER:
      Serial.print("Accel - X: "); Serial.print(sensorValue.un.accelerometer.x);
      Serial.print(" Y: "); Serial.print(sensorValue.un.accelerometer.y);
      Serial.print(" Z: "); Serial.println(sensorValue.un.accelerometer.z);
      break;
      
    case SH2_GYROSCOPE_CALIBRATED:
      Serial.print("Gyro  - X: "); Serial.print(sensorValue.un.gyroscope.x);
      Serial.print(" Y: "); Serial.print(sensorValue.un.gyroscope.y);
      Serial.print(" Z: "); Serial.println(sensorValue.un.gyroscope.z);
      break;
  }
  delay(100);
}