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

Adafruit_MPU6050 mpu;
Servo myservo1, myservo2;

float pitch_2, roll_2;
float A , B;
float alpha = 0.98; // Complementary filter coefficient

//Timers 
//float timeStep = 0.01;
float elapsedTime, currentTime, previousTime;

void setup(void) {
  myservo1.attach(10);
  myservo2.attach(9);

  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_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }


  Serial.println("");
  delay(100);
}

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

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");
  delay(500);

  // Calculate Roll
  int roll = (atan2(a.acceleration.y, a.acceleration.z )*180.0)/M_PI;
  
  Serial.print(" Roll = ");
  Serial.print(roll);

  // Calculate Pitch 
  int pitch = -(atan2(a.acceleration.x, sqrt(a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z))*180.0)/M_PI;
  
  Serial.print(" Pitch = ");
  Serial.print(pitch);

  Serial.println();

  // Calculate pitch_2

  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds

  pitch_2 = pitch_2 +  g.gyro.y * elapsedTime;

  Serial.print(" Pitch_2 = ");
  Serial.print(pitch_2);

  // Calculate roll_2
  roll_2 = roll_2 +  g.gyro.x * elapsedTime;

  Serial.print(" Roll_2 = ");
  Serial.print(roll_2);

  Serial.println();

  // Apply complementary filter
  A = alpha * (pitch_2 +  g.gyro.y * elapsedTime) + (1 - alpha) * pitch;
  B = alpha * (roll_2 +  g.gyro.x * elapsedTime) + (1 - alpha) * roll;

  Serial.print(" New Pitch = ");
  Serial.print(A);
  Serial.print(" New Roll = ");
  Serial.print(B);

  Serial.println();
  
  myservo1.write( A );
  myservo2.write( B );

}