#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>
#include<Adafruit_MPU6050.h>

#define TRIGGER_PIN 13  // Pin connected to the trigger
#define ECHO_PIN 12     // Pin connected to the echo
#define MAX_DISTANCE 200000 // Maximum distance to measure in cm



Adafruit_MPU6050 mpu;// Initialises /starts up the MPU sensor
LiquidCrystal_I2C lcd(0x27, 20, 4);// defines the lcd screen with its specific code, and width and height screen.

float height, dist, gyx, gyz, Angle=0, velocity=0, elapsed;//defining variables outside main functions
long pulse, curtime, prevtime;// defining long interger values 

void setup() {


  lcd.begin(20, 4);  // 20x4 LCD initialization
  lcd.backlight(); //turns on backlight of screen (makes more readable)
  lcd.setCursor(0, 0);//sets cursor to the top left
  lcd.print("Initializing"); //just prints a start up message

  pinMode(TRIGGER_PIN, OUTPUT);// assigns trigger pin to output/ send pulse out.
  pinMode(ECHO_PIN, INPUT);//assigns echo pin as input /recieve pulse from trig pin

  // Check MPU6050 connection
  if (!mpu.begin())//starts up mpu device
  {
    lcd.setCursor(0,1);// sets cursor to left hand side of secound row
    lcd.print("MPU input failed");//prints message to the user 
    while(1);// infinte loop till MPU starts
  }
  delay(2000);//waits 2 secounds
  lcd.clear();// clears the screen of messages 
  Serial.begin(9600); //starts serial communication

 prevtime= millis();//sets previous time to time that the sensors start
}
void loop() {

  // Measure distance with HC-SR04 sensor
  digitalWrite(TRIGGER_PIN, LOW);// sets pulse to 0/off.
  delayMicroseconds(2); //waits 2 microseconds 
  digitalWrite(TRIGGER_PIN, HIGH);//sets/ outputs pulseto on 
  delayMicroseconds(10);//pulses for 10 microsecounds
  digitalWrite(TRIGGER_PIN, LOW);//turns pulse back to off

  pulse = pulseIn(ECHO_PIN, HIGH);  // Measure time taken for pulse to return
  dist = (pulse * 0.034) / 2;  // converts pulse time to distance

  // Gets MPU data
  sensors_event_t a, g, temp;// defines MPU data in the adafruit format
  mpu.getEvent(&a, &g, &temp); //stores the MPU data 
  curtime = millis();//takes time reading after sensor data for max accuracy
  elapsed = (curtime - prevtime) / 1000.0;// finds elapsed time and converts it to secounds
  prevtime = curtime;// sets previous time equal to current time for next reading
  
  //intergrating gyroscope values
  gyx += g.gyro.x * elapsed; 
  gyz += g.gyro.z * elapsed;

  //height calculations 
  Angle = atan(sqrt(tan(gyz)*tan(gyz)+tan(gyx)*tan(gyx)));//pythagoras thereom to find angle
  height = (dist * cos (Angle))/100;//trigonometry to find vertical height +converts it to metres
  velocity= sqrt(2 * 9.81 * height);// suvat for verlocity calculations  


  // prints infomation on lcd screen
  lcd.setCursor(0, 0);
  lcd.print("Height: ");
  lcd.print(height);
  lcd.print("m");
  lcd.setCursor(0,1);
  lcd.print("Velocity: ");
  lcd.print(velocity);
  lcd.print("m/s");
  lcd.setCursor(0, 2);
  lcd.print("Pitch: ");
  lcd.print(gyx);
  lcd.print("° ");
  lcd.setCursor(0, 3);
  lcd.print("Yaw: ");
  lcd.print(gyz);
  lcd.print("°");

  // reduces amouunt of updates by delaying restart by 2 secounds
  delay(2000);
}