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

Adafruit_MPU6050 mpu;

int ledping =4;  //led pin
int ledpinr =3;

int buzzer  =2; // buzzer pin 

 
int servopin1 = 9;  // Servo pin
int servopin2 = 10;
int servopin3 = 11;
int servopin4 = 12;
int servopin6 = 7; 
int servopin5 = 8; 
int servopin7 = 6;
int servopin8 = 5;

Servo servo1;   // Servo object 
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;

int pos = 0; //position

MPU6050 mpu6050(Wire);

long timer = 0;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10);
   Serial.println("Adafruit MPU6050 test of the MDCMakersLab Vertical Landing Rocket!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found! God Help Us... Make sure the rocket doesn't explode!");
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  servo1.attach(servopin1);   //attach object to pin 
  servo2.attach(servopin2);
  servo3.attach(servopin3);
  servo4.attach(servopin4);
  servo5.attach(servopin5);
  servo6.attach(servopin6);
  servo7.attach(servopin7);
  servo8.attach(servopin8);


  pinMode(4,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(2,OUTPUT);

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

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

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

void loop() {

  digitalWrite(ledpinr, HIGH);
  digitalWrite(ledping, HIGH);
  digitalWrite(buzzer, HIGH);
  mpu6050.update();
  sensors_event_t a, g, temp;
	mpu.getEvent(&a, &g, &temp);
  if(millis() - timer >250){


    Serial.print("angleX : ");            // print 
    Serial.print(mpu6050.getAngleX());
    Serial.print("  angleY : ");
    Serial.println(mpu6050.getAngleY());
	 
   /* 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("");

    timer = millis();

  }
       //x-axis / roll

  if( mpu6050.getAngleX() == -1<0< 1){                  
   servo2.write(90);    //set to 90 degrees
    }
   servo2.write(90 - mpu6050.getAngleY()); //unstable angle

   if( mpu6050.getAngleX() == 0){
      servo1.write(90); //set to 90 degrees
    }
   servo1.write(90 + mpu6050.getAngleY()); //unstable angle

      if( mpu6050.getAngleX() == 0){
      servo6.write(10); //set to 90 degrees
    }
   servo6.write(10 + mpu6050.getAngleY()); //unstable angle

       //y-axis / pitch 

   if(mpu6050.getAngleY() == 0){        
    servo3.write(90);//set to 90 degrees
    }
    servo3.write(90 +mpu6050.getAngleX()); //unstable angle

   if(mpu6050.getAngleY() == 0){
    servo4.write(90); //set to 90 degrees
    }
    servo4.write(90 - mpu6050.getAngleX());//unstable angle

    if(mpu6050.getAngleY() == 0){
    servo5.write(10); //set to 90 degrees
    }
    servo5.write(10 - mpu6050.getAngleX());//unstable angle
   
    if(a.acceleration.z == 0){
    servo7.write(45);
    }
    else{
    return servo7.write(0);
    }

    if(a.acceleration.z == 0){
    servo8.write(45);
    }
}