const float g = 9.81;  // Gravitational acceleration
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>

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

MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  Wire.begin();
  mpu.initialize();

  lcd.begin(20, 4);  // 20x4 LCD initialization
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Check MPU6050 connection
  if (mpu.testConnection()) {
    lcd.setCursor(0, 1);
    lcd.print("Gyroscope Connected");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("Gyroscope Error");
  }
  delay(2000);
  lcd.clear();
}

void loop() {
  // Measure distance with HC-SR04
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  long pulseT = pulseIn(ECHO_PIN, HIGH);  // Measure echo time
  float distance = (pulseT * 0.034) / 2;  // Convert to cm

  // Read accelerometer data for tilt calculation
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calculate pitch angle (degrees)
  float pitchAngle = atan2(ay, sqrt(ax * ax + az * az)) * 180.0 / PI;

  // Correct the vertical height based on tilt
  float verticalHeight = distance * cos(pitchAngle * PI / 180.0);

  // Calculate free-fall velocity (m/s)
  float velocity = sqrt(2 * g * verticalHeight / 100.0);  // Convert cm to m

  // Display data on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Height: ");
  lcd.print(verticalHeight, 2);
  lcd.print(" cm");

  lcd.setCursor(0, 1);
  lcd.print("Velocity: ");
  lcd.print(velocity, 2);
  lcd.print(" m/s");

  lcd.setCursor(0, 2);
  lcd.print("Pitch: ");
  lcd.print(pitchAngle, 2);
  lcd.print(" deg");

  delay(1000);
}