#include <Wire.h>              // defining studios 
#include <LiquidCrystal_I2C.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>

Adafruit_MPU6050 mpu; // defining which parts where used 
LiquidCrystal_I2C lcd(0x27, 16, 2);  // defingin
const int echo = 10; // defiining what pins are where 
const int trig = 9;

float g = 9.81; // sets gravity as value of g 

void setup() {
  lcd.init();
  lcd.backlight();  // starts the lcd 

  Serial.begin(9600); // baud rate amoubnt of bit processes per seconf 
  pinMode(trig, OUTPUT); // defines which pin is a out put and input 
  pinMode(echo, INPUT);

  mpu.begin();
}

void loop() { // start loop
  // starts new measurement by turning pins on hight or low voltage 
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);


  long duration = pulseIn(echo, HIGH);   
  
  
  float InitialH = duration * 0.034 / 2.0; // speed of sound devided by distance as it pulse needs to be sent out thean it needs to return 

  
  sensors_event_t event; // 
  mpu.getGyroSensor()->getEvent(&event);
  

  float Yaw = event.gyro.x;  // gets calues from gyroscope and sets them as yaw and pitch
  float Pitch = event.gyro.z;

  float ObtainH = InitialH * cos(Yaw);  // corrects the height for the angle 
  float CalcH = ObtainH * cos(Pitch);   

 
  float velocity = sqrt(2 * g * CalcH); // using the equation v = square root of 2 x gravity x height
  


  lcd.clear(); // clears the lcd screen 
  lcd.setCursor(0, 0); 
  lcd.print("Speed= "); // prints the speed 
  lcd.print(velocity); 
  lcd.print("cm/s");



  
  delay(1000);  // how fast the code will run 
}