#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
  
Adafruit_BNO055 bno = Adafruit_BNO055(55);

void setup(void) 
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Test"); Serial.println("");
  
  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  
  delay(1000);
    
  bno.setExtCrystalUse(true);
}

void loop(void) 
{
  /* Get a new sensor event */ 
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  imu::Vector<3> mag = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

  // float heading = 0.f;
  float mag_x = mag.x();
  float mag_y = mag.y();
  float mag_z = mag.z();
  // heading = -atan2(mag_x, mag_y) * 180 / M_PI;
  // if (heading < 0){  
  //   heading += 360;
  // }
  
  /* Display the floating point data */
  Serial.print("heading|");
  Serial.print(euler.x());
  Serial.print("|MAG|");
  Serial.print(mag_x);
  Serial.print(";");
  Serial.print(mag_y);
  Serial.print(";");
  Serial.print(mag_z);
  Serial.println();
  delay(100);
}