//setup
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
#define BUTTON_MEASURE 4
#define BUTTON_STOP 12
#define LED 13

void setup(void) {
 Serial.begin(115200);   
 pinMode(BUTTON_MEASURE, INPUT_PULLUP);
 pinMode(BUTTON_STOP, INPUT_PULLUP);
 pinMode(LED, OUTPUT);

 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);
 delay(100);
}

int lastState = HIGH;
void loop() {
 int value = digitalRead((BUTTON_MEASURE));

 //button pressing outputs
 if (value == HIGH) { 
 digitalWrite(LED, LOW );  
 Serial.println("Ready to Start - Press Button");
 delay(1000);
 }

 if (value == LOW) {
  int exitflag=0;
  while (exitflag==0) {

 //declaring variables
 double ax[10];
 double ay[10];
 double az[10];
 double theta;
 double phi;

 //storing measurements in arrays
 int i;
 for (i=0; i<10; i++){
  sensors_event_t a, g, temp; //getting measurements
  mpu.getEvent(&a, &g, &temp);
  
  ax[i]=a.acceleration.x; //storing measurements in array
  ay[i]=a.acceleration.y;
  az[i]=a.acceleration.z;
  }

 //calculating average of 10 readings
 double xa;
 double ya;
 double za;
 double xtotal=0;
 double ytotal=0;
 double ztotal=0;
 double state=0; //state of alignment 0=false 1=true

 for(i=0;i<10;i++){
    xtotal=xtotal+ax[i];
    ytotal=ytotal+ay[i];
    ztotal=ztotal+az[i];
  };

  xa=(xtotal/10);
  ya=(ytotal/10);
  za=(ztotal/10);
  
 //determination of SIGN   
 double SIGN; 
 if (za<0) {
  SIGN=-1;
 } else { SIGN=1;}

 //calculation and printing of pitch and roll angles
 theta = (180/PI)*atan((-1*xa)/(sqrt(ya*ya+za*za))); //calculation + printing of angles
 phi = (180/PI)*atan(ya/(SIGN*(sqrt(za*za+(0.01*xa*xa)))));
 Serial.print("Roll = ");
 Serial.print(theta);
 Serial.println("");
 Serial.print("Pitch = ");
 Serial.print(phi);
 Serial.println("");
 delay(1000);

 //turn on LED when aligned, stop taking measurements
 if ((44.00<=phi && phi<=46.00 )&&(-1.00<=theta && theta<=1.00)){  
    digitalWrite(LED, HIGH);
    state=1;
    while (state==1){
      value = digitalRead((BUTTON_STOP)); 
      if (value == LOW){
        exitflag=1;
        state=0;
        delay(2000);
    }
    }
  
 //interruprt measurements when button pressed
 } else {
    digitalWrite(LED, LOW);
    value = digitalRead((BUTTON_STOP)); 
      if (value == LOW) {
        exitflag=1;
        delay(1000);
  }
  }
  }
  }
}