/* Get all possible data from MPU6050
 * Accelerometer values are given as multiple of the gravity [1g = 9.81 m/s²]
 * Gyro values are given in deg/s
 * Angles are given in degrees
 * Note that X and Y are tilt angles and not pitch/roll.
 *
 * License: MIT
 */

#include "Wire.h"
#include <MPU6050_light.h>

MPU6050 mpu(Wire);

unsigned long timer = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
  
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  mpu.calcOffsets(true,true); // gyro and accelero
  Serial.println("Done!\n");
  
}

void loop() {
  mpu.update();

  if(millis() - timer > 1000){ // print data every second
  
    float xAcc = mpu.getAccX();
    float yAcc = mpu.getAccY();
    float zAcc = mpu.getAccZ();

    float rollRad = mpu.getAngleX() * M_PI / 180;
    float pitchRad = mpu.getAngleY() * M_PI / 180;
    float yawRad = mpu.getAngleZ() * M_PI / 180;

    Serial.print(F("ACCELERO  X: "));Serial.print(xAcc);
    Serial.print("\tY: ");Serial.print(yAcc);
    Serial.print("\tZ: ");Serial.println(zAcc);
    
    float x = xAcc, y = yAcc, z = 0;
    float xAccNew = xAcc, yAccNew = yAcc, zAccNew = zAcc;
    xAccNew -= x * cos(yawRad) - y * sin(yawRad) ;
    yAccNew -= x * sin(yawRad) - y * cos(yawRad) ;

    x = xAccNew, z = zAccNew;
    xAccNew -=  x * cos(pitchRad) + z * sin(pitchRad) ;
    zAccNew -= - x * sin(pitchRad) + z * cos(pitchRad) ;

    y = yAccNew, z = zAccNew;
    yAccNew -=  y * cos(rollRad) - z * sin(rollRad);
    zAccNew -=  y * sin(rollRad) + z * cos(rollRad);

    Serial.print(F("ACCELERO  X: "));Serial.print(xAccNew);
    Serial.print("\tY: ");Serial.print(yAccNew);
    Serial.print("\tZ: ");Serial.println(zAccNew);

       
    Serial.print(F("ANGLE     X: "));Serial.print(rollRad);
    Serial.print("\tY: ");Serial.print(pitchRad);
    Serial.print("\tZ: ");Serial.println(yawRad);
    
    Serial.println(F("=====================================================\n"));
    timer = millis();

    //Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());


  /*Serial.print(F("GYRO      X: "));Serial.print(mpu.getGyroX());
    Serial.print("\tY: ");Serial.print(mpu.getGyroY());
    Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());
  
    Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
    Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());*/


  }

}