#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
   which provides a common 'type' for sensor data and some helper functions.

   To use this driver you will also need to download the Adafruit_Sensor
   library and include it in your libraries folder.

   You should also assign a unique ID to this sensor for use with
   the Adafruit Sensor API so that you can identify this particular
   sensor in any data logs, etc.  To assign a unique ID, simply
   provide an appropriate value in the constructor below (12345
   is used by default in this example).

   Connections
   ===========
   Connect SCL to analog 5
   Connect SDA to analog 4
   Connect VDD to 3.3-5V DC
   Connect GROUND to common ground

   History
   =======
   2015/MAR/03  - First release (KTOWN)
*/

/* Set the delay between fresh samples */
uint16_t BNO055_SAMPLERATE_DELAY_MS = 100;

// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);

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

  while (!Serial) delay(10);  // wait for serial port to open!

  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);
}

void loop(void)
{
  //could add VECTOR_ACCELEROMETER, VECTOR_MAGNETOMETER,VECTOR_GRAVITY...
  sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
  bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);

  uint8_t system, gyro, accel, mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);

  Serial.print("Calibration: Sys=");
  Serial.print(system);
  Serial.print(" Gyro=");
  Serial.print(gyro);
  Serial.print(" Accel=");
  Serial.print(accel);
  Serial.print(" Mag=");
  Serial.print(mag);
  Serial.print("  ");

  printEvent(&accelerometerData);
  printEvent(&orientationData);

  
  delay(BNO055_SAMPLERATE_DELAY_MS);
  Serial.println();
}

void printEvent(sensors_event_t* event) {
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;

    Serial.print("   \tAccx= ");
    Serial.print(x, 2);
    Serial.print("   \tAccy= ");
    Serial.print(y, 2);
    Serial.print("   \tAccz= ");
    Serial.print(z, 2);

    //######### Computation for Inclinometer Applications #########

    //Now let's calculate the magnitude g of the gravity vector
    //This is given by the equation: gravity = sqrt(x²+y²+z²)
    //The unit of gravity is the same as the units of x, y and z.

    float gravity;
    gravity = sqrt(sq(x) + sq(y) + sq(z));
    Serial.print("   \tgravity= ");
    Serial.print(gravity, 2);

    //Let's assume that the z-axis is pointing down. The values of z and gravity are the same.
    //If we incline the sensor on the y-axis, the value of y will increase, z will decrease and x will remain the same.
    //Let's call the angle of inclination theta.
    //We can calculate theta using four different formulas:
    //theta_asin = asin(y / gravity)
    //theta_acos = acos(z / gravity)
    //theta_atan = atan(y / z)
    //theta_atan2 = atan2(y, z) 
    //The unit of theta is radians. We can convert theta to degrees by multiplying by 180° and dividing by pi.

    double theta_asin;
    theta_asin = asin(y / gravity);
    theta_asin = theta_asin * 180/PI;

    double theta_acos;
    theta_acos = acos(z / gravity);
    theta_acos = theta_acos * 180/PI;  

    double theta_atan;
    theta_atan = atan(y / z);
    theta_atan = theta_atan * 180/PI;   

    double theta_atan2;
    theta_atan2 = atan2(y, z);
    theta_atan2 = theta_atan2 * 180/PI;   
    
    Serial.print("=    \ttheta_asin= ");
    Serial.print(theta_asin, 2);
    Serial.print("=    \ttheta_acos= ");
    Serial.print(theta_acos, 2);
    Serial.print("=    \ttheta_atan= ");
    Serial.print(theta_atan, 2);
    Serial.print("=   \ttheta_atan2= ");
    Serial.print(theta_atan2, 2);

  }
  else if (event->type == SENSOR_TYPE_ORIENTATION) {
    x = event->orientation.x;
    y = event->orientation.y;
    z = event->orientation.z;

    Serial.print("=    \tEuler_z= ");
    Serial.print(z, 2);
  }

  else {
    Serial.print("Unk:");
  }
 

}