#include <LiquidCrystal.h>
#include <math.h>
LiquidCrystal lcd(12,11,10,9,8,7);

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

Adafruit_MPU6050 mpu;

#define PIN_TRIG 3
#define PIN_ECHO 2

void setup(void) {
  Serial.begin(115200);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

   while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  delay(100);
}

sensors_event_t event;

void loop(){
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(20);
  digitalWrite(PIN_TRIG, LOW);
  int duration = pulseIn(PIN_ECHO, HIGH);
  int distance = duration/58.4;

  mpu.getGyroSensor()->getEvent(&event); 
  float angle = max(abs(event.gyro.x),abs(event.gyro.z));
  float VerticalDistance = abs(cos(angle)*distance);
  float velocity = sqrt(2*9.81*(VerticalDistance/100));
  
  lcd.begin(16,2);
  lcd.print(velocity);
  lcd.print( "m/s");

  delay(1000);
}