#include <Wire.h>
#include <LiquidCrystal.h>
#include <MPU6050.h>

// Constants
#define TRIG_PIN 2
#define ECHO_PIN 3
#define GRAVITY 9.81  // m/s^2 (acceleration due to gravity)

// Create objects
  // LCD address 0x27 for 20x4 display
MPU6050 mpu;
LiquidCrystal lcd(12,11,10,9,8,7);
// Variables
float distance = 0;
float pitch = 0;
float pitchangle =0;
float yaw = 0;
float yawangle =0;
float roll = 0;
float rollangle =0;
float heightAdjusted = 0;
float velocity = 0;

// Setup
void setup() {
  // Initialize communication
  Serial.begin(115200);
  Wire.begin();
  lcd.begin(20, 4);
  
  // Initialize MPU6050
  mpu.initialize();
  
  // Set trigger and echo pins as input/output
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  

}

// Measure Distance using Ultrasonic Sensor
float measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  float dist = (duration / 2.0) * 0.343;  // Calculate distance in cm from the distance sensor, 0.0343= speed of wave in air in mm/s
  return dist;
}

// Get IMU data (Pitch and Roll)
void getIMUData() {
  // Get raw data from MPU6050
  int16_t ax, ay, az, gx, gy, gz;//a's are from accelerometer & g's are from gyroscope
  mpu.getAcceleration(&ax, &ay, &az);
  mpu.getRotation(&gx, &gy, &gz);

  // Calculate pitch and yaw from accelerometer data
  yaw = atan2(ay, az) * 180 / PI;//pitch=x. rad to degrees
  pitch = atan2(ax, az) * 180 / PI;//yaw=y.  rad to degrees
  // Gyroscope data to calculate pitch and yaw
  yawangle = atan2(gy, az) * 180/ PI;
  pitchangle= atan2(gx, az) * 180/PI;
  rollangle= atan2(gz, az)* 180/PI;
}
// Adjusts height based on sensor orientation
float adjustHeightForOrientation(float measuredDistance) {
  // Adjusts the height by taking pitch and yaw into account.
  float adjustedDistance = measuredDistance / cos(radians(pitch));  // Apply pitch compensation
  adjustedDistance = adjustedDistance / cos(radians(yaw));        // Apply yaw compensation
  float adjustedDistanceangle = measuredDistance / cos(radians(pitchangle));  // Apply pitch compensation
  adjustedDistanceangle = adjustedDistance / cos(radians(yawangle));        // Apply yaw compensation
  return adjustedDistance;
}

// Calculate fall velocity
float calculateFallVelocity(float height) {
  // Velocity equation: v = sqrt(2 * g * h)
  return sqrt(2 * GRAVITY * height);
}

// Main loop
void loop() {
  // Measure distance from the proximity sensor
  distance = measureDistance();

  // Get IMU data (pitch and yaw)
  getIMUData();

  // Adjust the height based on orientation
  heightAdjusted = adjustHeightForOrientation(distance);

  // Calculate the fall velocity
  velocity = calculateFallVelocity(heightAdjusted);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("s:");
  lcd.print(heightAdjusted/10);
  lcd.print("cm");

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

  lcd.setCursor(0,2);
  lcd.print("Yaw: ");
  lcd.print(yawangle);
  lcd.print(" degrees");

  lcd.setCursor(0,3);
  lcd.print("Pitch: ");
  lcd.print(pitchangle);
  lcd.print(" degrees");

  // Wait for a bit before updating
  delay(1000);
}