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



LiquidCrystal lcd(12,11,10,9,8,7); /*screen setup*/

Adafruit_MPU6050 mpu;

#define BUTTON_PIN1 4
#define BUTTON_PIN2 6 /* for reset button*/
#define LED_PIN 2
void setup() {
 Serial.begin(115200);
  lcd.begin(16,2);
  lcd.print("Device Ready");
 Serial.println("To Start press the orange button");
 Serial.println("To pause press the purple button");
 pinMode(LED_PIN, OUTPUT); /*initialize digital pin LED_PIN as an output*/
 pinMode(BUTTON_PIN1, INPUT_PULLUP); /*initialize BUTTON_PIN1 as an input*/
 pinMode(BUTTON_PIN2, INPUT_PULLUP); /*initialize BUTTON_PIN2 as an input*/
 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("");
 delay(100);

}
int lastState = HIGH;
void loop() {
   
 int value = digitalRead((BUTTON_PIN1));
 if (lastState != value) {
    lastState = value;

    if (value == HIGH) {
      Serial.println(" button released");
      lcd.clear();
      lcd.print("program stopped");
  }

    if (value == LOW) {
      Serial.println(" button pressed");
      int exitflag = 0;

        while (exitflag ==0 && (digitalRead(BUTTON_PIN2)==HIGH)) { /*while reset button is off*/
        
        int value2 = digitalRead((BUTTON_PIN2)); {
        if (value2 == LOW) {
        lcd.clear();
        
        exitflag = 1;
      
        }
        
        } 
        float ax[10];
        float sumx = 0;
        float ay[10];
        float sumy = 0;
        float az[10];
        float sumz = 0;    
        float avgx = 0;
        float avgy = 0;
        float avgz = 0;


        for(int i = 0; i<10; i++){        

        sensors_event_t a, g, temp;
    
        /* reading acceleration values */
        mpu.getEvent(&a, &g, &temp);
        
        Serial.print(a.acceleration.x);
        Serial.print(",");
        Serial.print(a.acceleration.y);
        Serial.print(",");
        Serial.print(a.acceleration.z);
        Serial.println("");
        delay(200);
        
        /*storing accelerations in array*/

        ax[i] = a.acceleration.x;
        ay[i] = a.acceleration.y;
        az[i] = a.acceleration.z;
        
        }
        /*summing all acclerations to find average*/
        for (int i = 0; i < 10; i++){
        sumx = sumx + ax[i];
        sumy = sumy + ay[i];
        sumz = sumz + az[i];
        }

        int SIGN = 1;
        float mu = 0.01;

        /*averaging accelerations*/
        avgx = sumx/10;
        avgy = sumy/10;
        avgz = sumz/10;

        if (avgz < 0 ) {
          int SIGN = -1;
        }
          
        /*pitch calculation*/
        int pitch;
        pitch = (180/PI)*atan(-avgx/sqrt(avgy*avgy + avgz*avgz));
        /*roll clculation*/
        int roll;
        roll = (180/PI)*atan(avgy/SIGN*avgz*sqrt(avgz*avgz + mu*avgx*avgx));
       
       /*printing pitch and roll on lcd screen*/
       lcd.clear();
       lcd.setCursor(0,0);
       lcd.print("Pitch =");
       lcd.print(pitch);
       lcd.setCursor(0,1);
       lcd.println("Roll =");
       lcd.println(roll);

       /* printing pitch and roll in window*/
        Serial.print(" Pitch = ");
        Serial.print(pitch);
        Serial.print("\n");
        Serial.print(" Roll = ");
        Serial.print(roll);
        Serial.println("");
        delay(100);

        

        /* led on when aligned*/
        if (roll == 0 && pitch == 45){
          digitalWrite(LED_PIN,HIGH);
          Serial.print("Accelerometer aligned\n");
          Serial.print("To restart press the orange button\n");
          exitflag = 1;
        
        }
         else{
           digitalWrite(LED_PIN,LOW);
         exitflag=0;
        }
        



    }
  }
}
}