#include <Adafruit_BNO08x.h>
#include <Wire.h>

Adafruit_BNO08x bno;
sh2_SensorValue_t sensorValue;

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

  Wire.begin(5, 6);

  if (!bno.begin_I2C(0x4A, &Wire)) {
    Serial.println("BNO08x not found, check wiring!");
    while (1) delay(10);
  }

  bno.enableReport(SH2_MAGNETIC_FIELD_CALIBRATED);
  Serial.println("Ready! Wave the board in a figure-8 to calibrate.");
}

void loop() {
  if (bno.getSensorEvent(&sensorValue)) {
    if (sensorValue.sensorId == SH2_MAGNETIC_FIELD_CALIBRATED) {

      float x = sensorValue.un.magneticField.x;
      float y = sensorValue.un.magneticField.y;

      float angle = atan2(y, x) * 180.0 / PI;

      Serial.print("X: "); Serial.print(x);
      Serial.print("  Y: "); Serial.print(y);
      Serial.print("  Angle: "); Serial.println(angle);
    }
  }
}