#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <stdio.h>
#include <math.h>
Adafruit_MPU6050 mpu;
  const int PIN_RED   = 5;
  const int PIN_GREEN = 6;
  const int PIN_BLUE  = 7;

#define BUTTON_PIN 4

void setup() {                                             // SETUP 
pinMode(BUTTON_PIN, INPUT_PULLUP);
  Serial.begin(115200);
  Serial.println("Push the button to start");
  Serial.println("Green indicates readings are not alligned");
  Serial.println("Blue indicates readings are alligned");
 if (!mpu.begin()) {
 Serial.println("Failed to find MPU6050 chip");
 while (1) {
 delay(10);
}
 mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
 mpu.setGyroRange(MPU6050_RANGE_250_DEG);
 mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
 Serial.println("");
  pinMode(PIN_RED,   OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE,  OUTPUT);
 delay(100);


 }
}
int lastState = HIGH;

void loop() {                                               // LOOP
                               


  int value = digitalRead((BUTTON_PIN));
 if (value == LOW) {

 
 Serial.println("Button pushed");
 
 

   double ax[10];
   double ay[10];
   double az[10];

   

   int i;
   double axmean;
   double aymean;
   double azmean;
   for(i=0;i<10,i++;){
      sensors_event_t a, g, temp, gyro;
      mpu.getEvent(&a, &g, &temp);
    ax[i] = a.acceleration.x;
    ay[i] = a.acceleration.y;
    az[i] = a.acceleration.z;


   }

    Serial.println(ax[i]);
    Serial.println(ay[i]);
    Serial.println(az[i]);

   axmean = (ax[0]+ax[1]+ax[2]+ax[3]+ax[4]+ax[5]+ax[6]+ax[7]+ax[8]+ax[9])/10;
   aymean = (ay[0]+ay[1]+ay[2]+ay[3]+ay[4]+ay[5]+ay[6]+ay[7]+ay[8]+ay[9])/10;
   azmean = (az[0]+az[1]+az[2]+az[3]+az[4]+az[5]+az[6]+az[7]+az[8]+az[9])/10;

  Serial.println(axmean);
  Serial.println(aymean);
  Serial.println(azmean);

   int signz;
   if (azmean<0){
     signz=-1;
   }else {
     signz=1;
   }
   Serial.println("signz is : \n");
   Serial.println(signz);
   double Roll = atan((-axmean)/(sqrt((aymean)*(aymean)+(azmean)*(azmean))));
   double Pitch = atan((aymean)/((signz)*(sqrt((azmean)*(azmean)+(0.01*(axmean)*(axmean))))));
   if (Roll==0 && Pitch==45) {
  
  Serial.println("Readings are alligned");
  Serial.println("Roll is: \n");
  Serial.println("Pitch is: \n");
  analogWrite(PIN_RED,   1000);
  analogWrite(PIN_GREEN, 1000);
  analogWrite(PIN_BLUE,  0);
 
  }
  else if (Roll!=0 && Pitch!=45); {

  Serial.println("Readings are not alligned");
  Serial.println("Roll is: \n");
  Serial.println("Pitch is: \n");
  analogWrite(PIN_RED, 1000);
  analogWrite(PIN_GREEN, 0);
  analogWrite(PIN_BLUE, 1000);
 }
}
delay(200);
}