#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h> 

// Acceleration Values = a.acceleration.y   ,  a.acceleration.x   , a.acceleratation.z
// gyro Values = 

Adafruit_MPU6050 mpu;

float ACCpitch;
float ACCroll;
float zAccelerationOld = 0;
float yAccelerationOld = 0;
float xAccelerationOld= 0;
float zAccelerationSmoothed;
float yAccelerationSmoothed;
float xAccelerationSmoothed;

float GYRpitch = 0;
float GYRroll= 0;
float dt; 
unsigned long oldMillis; 


void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

 oldMillis = millis(); 
}

void loop() {

  //Get new sensor events with the readings 
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  //Low Pass Filter
  zAccelerationSmoothed = zAccelerationOld * .75 + a.acceleration.z * .25;
  yAccelerationSmoothed = yAccelerationOld * .75 + a.acceleration.y * .25;
  xAccelerationSmoothed = xAccelerationOld * .75 + a.acceleration.x * .25;

  ACCpitch = atan2((xAccelerationSmoothed/9.81),(zAccelerationSmoothed/9.81))/2/3.1459*360;
  ACCroll = atan2((yAccelerationSmoothed/9.81),(zAccelerationSmoothed/9.81))/2/3.1459*360;
 
  dt = (millis() - oldMillis) / 1000.;
  oldMillis = millis();

  GYRpitch = GYRpitch + g.gyro.y*dt;
  GYRroll = GYRroll + g.gyro.x*dt;

  // Print out the values 
  Serial.print(GYRpitch);
  Serial.print("  , ");
  Serial.println(GYRroll);

  yAccelerationOld = yAccelerationSmoothed;
  yAccelerationOld = yAccelerationSmoothed;
  xAccelerationOld = xAccelerationSmoothed;


}