/*******************************************
  ALIGNMENT SENSOR USING ACCELEROMETER DATA
	IN C, FOR ARDUINO UNO, WOKWI EMULATOR
	BY MATTHEW MOORE, FEBRUARY 2022

	ALL delay() TIMES IN ms

*******************************************/



#include <Adafruit_MPU6050.h>          // import libraries
#include <Adafruit_Sensor.h>
#include <Wire.h>
#define BUTTON_PIN 12          // button connected to pin 12
Adafruit_MPU6050 mpu;

void setup(){

 Serial.begin(115200);
 if (!mpu.begin()) {
 Serial.println("Failed to find MPU6050 chip");
 while (1) {
   delay(100);
  }
 }

 pinMode(BUTTON_PIN, INPUT_PULLUP);     // initialise button in pull-up format

 mpu.setAccelerometerRange(MPU6050_RANGE_16_G);    // set up the MPU6050
 mpu.setGyroRange(MPU6050_RANGE_250_DEG);
 mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void loop(){

 Serial.println("Device ready. Press button once to begin");
 Serial.println("Press and hold button during program to cancel measurements");

 int flag1 = 0;       // loop to check if button has been pressed to start program
 while (flag1 == 0){
   int buttonread = digitalRead(BUTTON_PIN);
   if (buttonread == LOW){
   flag1 = 1;
	 }
 }
 

 int flag2 = 0;
 while (flag2 == 0){    // while loop contains each set of 10 readings and then checks if device is aligned 
 
	 double xr[10];   // create 3x arrays to store acceleration readings in
	 double yr[10];
	 double zr[10];

	 double pitch;   // declare (average) pitch and roll outside of for loop
	 double roll;   
	 double lastpitch;
	 double lastroll;

	 int i;
	 for (i = 0; i < 10; i++){   // for loop to take 10 readings , i is the iteration variable

		 sensors_event_t a, g, temp;
		 mpu.getEvent(&a, &g, &temp);
		
     Serial.print("X: ");		            // print 10 sets of readings for x, y and z
		 Serial.print(a.acceleration.x);
		 Serial.print(",  Y:");
		 Serial.print(a.acceleration.y);
		 Serial.print(",  Z:");
		 Serial.print(a.acceleration.z);
		 Serial.println("");
		
		 xr[i] = a.acceleration.x;   // store each reading into the i th cell of respective array
		 yr[i] = a.acceleration.y;
		 zr[i] = a.acceleration.z;

		 delay(500);
	  };

	 int codebreak = digitalRead(BUTTON_PIN);  // break while loop if button is pressed, resetting program
	 if (codebreak == LOW){
		 break;
		 flag2 = 1;  
	 }

		double xavg = (xr[0]+xr[1]+xr[2]+xr[3]+xr[4]+xr[5]+xr[6]+xr[7]+xr[8]+xr[9])/10;  // find average x, y and z accleration
		double yavg = (yr[0]+yr[1]+yr[2]+yr[3]+yr[4]+yr[5]+yr[6]+yr[7]+yr[8]+yr[9])/10;
		double zavg = (zr[0]+zr[1]+zr[2]+zr[3]+zr[4]+zr[5]+zr[6]+zr[7]+zr[8]+zr[9])/10;
    
		roll = atan(-xavg / sqrt(yavg*yavg + zavg*zavg));        // calculate average roll using average x, y and z
    lastroll = atan(-xr[9] / sqrt(yr[9]*yr[9] + zr[9]*zr[9]));    // calculate the last measured roll

		if (zavg >=0){
		  pitch = atan(yavg / sqrt(zavg*zavg + 0.01*(yavg*yavg)));   // calculate average pitch, checking if positive or negative
		 }else{
			pitch = atan(- yavg / sqrt(zavg*zavg + 0.01*(yavg*yavg)));      // mu=0.01 used in pitch calculations
		}

		if (zr[10] >=0){
		  lastpitch = atan(yr[9] / sqrt(zr[9]*zr[9] + 0.01*(yr[9]*yr[9])));   // calculate the last measured pitch
		 }else{
			lastpitch = atan(- yr[9] / sqrt(zr[9]*zr[9] + 0.01*(yr[9]*yr[9])));      
		}
		
   if ((-0.1<roll<0.1) && (0.775<pitch<0.795) && (-0.1<lastroll<0.1) && (0.775<lastpitch<0.795)){        // check if average and last roll is close to 0 and average and last pitch is close to pi/4 rads (+- 0.1 rads deviation allowed)
		 Serial.println("Device aligned. Press button to reset");  
		 digitalWrite(LED_BUILTIN, HIGH);                                  // light up LED if aligned
     flag2 = 1;                                                        // break while loop 
	  }else{
			Serial.println("Device unaligned. Repeating measurements");
			delay(1000);
		}		
  }

 int buttoncheck = 0;                // check if button pressed in order to reset the program
 while (buttoncheck == 0){
   int buttonvalue = digitalRead((BUTTON_PIN));
   if (buttonvalue == LOW){
     buttoncheck = 1;
     digitalWrite(LED_BUILTIN, LOW);   // turn off LED before resetting program
	 }
 }

 Serial.println("Device reset");
 Serial.println("");
 delay(3000);  // longer delay to avoid program auto-restating if user keeps button held down
}