// Connor Lappage
// ENGI 1171 COMPUTING B ASSESSMENT

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#define BUTTON_PIN 4

Adafruit_MPU6050 mpu;

// Setup loop
void setup(void) {
Serial.begin(115200);
 pinMode(BUTTON_PIN, INPUT_PULLUP);

  // Initialisation
  if (!mpu.begin()) {
  Serial.println("Failed to find MPU6050 chip"); // Indication to user that initialisation has failed
  while (1) {
  delay(10);
  }
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("\n\n");
  delay(100);

  // Start screen and program instructions
  Serial.println("Device ready to start");
  Serial.println("Press button to start alignment measurement");
  Serial.println("LED will illuminate when device is aligned");
  Serial.println("You can press the button at any point to stop measurement");
}
  
  // Main loop and setting inital variable values
int lastState = HIGH;
void loop() {
 int cont = 1;
 int SIGN = 1;
 int value = digitalRead((BUTTON_PIN));

delay (50);
  if (value == LOW) { // Begin measurement if button pressed (value = LOW when button pressed)
    Serial.println("\n\n\n\n\n\n");
    Serial.println("Measuring..."); // Indication to the user that the measurement is being taken
    while (cont == 1) { // cont variable controls whether measurement is being taken (=1) or not (=0)
      
     // Creation of variables used in pitch and roll calculations
     double array_ax[10];
     double array_ay[10];
     double array_az[10];
     double ax = 0.0;
     double ay = 0.0;
     double az = 0.0;
     
     // Get 10 new sensor events with readings
     for (int i=0; i < 10; ++i) {

     sensors_event_t a, g, temp;
     mpu.getEvent(&a, &g, &temp);

     array_ax[i]=a.acceleration.x;
     ax = ax + array_ax[i];

     array_ay[i]=a.acceleration.y;
     ay = ay + array_ay[i];

     array_az[i]=a.acceleration.z;
     az = az + array_az[i];

     delay(10);
     }

     // Calculating average of the 10 readings
     double average_ax = ax/10; 
     double average_ay = ay/10; 
     double average_az = az/10; 
     
     // Calculating the SIGN variable used in the pitch calculations
     if (average_az >= 0) {
     SIGN = 1; 
     } else {
     SIGN = -1;
     }

     // Pitch and roll calculations
     double roll = (atan((- average_ax)/(sqrt((average_ay)*(average_ay)+(average_az)*(average_az)))))*(180/PI);

     double pitch = (atan((average_ay)/(SIGN*sqrt((average_az)*(average_az)+(0.01*(average_ax)*(average_ax))))))*(180/PI);

     // If statement controlling LED illumination when device is aligned
     if ( pitch>=43 && pitch<=47 && roll>=-2 && roll<=2 ) {
     digitalWrite(LED_BUILTIN, HIGH); // LED ON
     cont = 0; 
     Serial.println("\n\n\n\n\n\n");
     Serial.println("Device aligned!"); // Additional indication to the user that the device is aligned
     Serial.println("Press button to start alignment measurement again");
     } else {
     digitalWrite(LED_BUILTIN, LOW); // LED OFF
     }

     // Pausing the program using the button
     value = digitalRead((BUTTON_PIN));
     if (value == LOW) {
       while (value == LOW) {
         delay(25);
         value = digitalRead((BUTTON_PIN));
       }
        cont = 0;
        Serial.println("\n\n\n\n\n\n");
        Serial.println("Measurement paused"); // Indication to the user that the measurement has been paused
        Serial.println("Press button to resume measurement"); // Instructions on what to do next
      }

    }
  }
}