#include <Wire.h>           // Include all the random libraries i need
// Initialize LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Create an object for the MPU6050 IMU to get accelerometer and gyroscope data
MPU6050 mpu;

// Define the pins for the HC-SR04 Ultrasonic Sensor
const int trigPin = 9;  
const int echoPin = 10; 

// Declare variables for pitch, roll, and yaw values from the MPU6050
int pitch, roll, yaw;

void setup() {
  // Initialize serial communication at 9600 baud rate
  Serial.begin(9600);
  
  // Initialize the LCD
  lcd.begin(16, 2);           // Set up the LCD with 16 columns and 2 rows
  lcd.print("Initializing..."); // Display a message to show that the system is starting

  // Initialize the MPU6050 (MPU6050 is an I2C sensor)
  Wire.begin();               // Initialize I2C communication
  mpu.initialize();           // Initialize the MPU6050 sensor

  
  pinMode(trigPin, OUTPUT);   
  pinMode(echoPin, INPUT);    
}

void loop() {
  // Read rotation data (pitch, roll, and yaw) from the MPU6050 sensor
  mpu.getRotation(&pitch, &roll, &yaw);  

  // Variables to hold the duration of the pulse and the calculated distance from the ultrasonic sensor
  long duration, distance;

  // Generate a pulse on the trigger pin to start the ultrasonic sensor
  digitalWrite(trigPin, LOW);               
  delayMicroseconds(2);                       
  // All examples I saw said to wait for a short time
  digitalWrite(trigPin, HIGH);              
  delayMicroseconds(10);                 
  digitalWrite(trigPin, LOW);          
  
  // Read the pulse duration from the echo pin, which represents the time it took for the sound to bounce back
  duration = pulseIn(echoPin, HIGH);         // Measure the duration of the pulse from the echo pin

  // Calculate the distance using the time it took for the pulse to travel back and forth
  distance = (duration / 2) * 0.0344;  // Calculate distance in cm. Duration is divided by 2 because it travels to the ground and back

  // I apolagise in advance for this maths
  // Compensate for the wierd angle by multiplying the raw distance by the cosine of the pitch angle
  // This works because when the sensor is tilted, the effective distance to the floor increases
  float adjustedDistance = distance * cos(radians(pitch));

  // Calculate the velocity of impact using basic physics equations.
  // v = sqrt(2 * g * h)
  // g = acceleration due to gravity - on earth its 9.81
  // h = height from which the person would fall (in meters)
  // Since the distance is in centimeters, we convert it to meters by dividing by 100
  float velocity = sqrt(2 * 9.81 * adjustedDistance / 100);
  // divide it by 100 at the end becaus ewe want it in meters per second

  // Display the adjusted distance and velocity on the LCD
  lcd.clear();                    // Clear the previous display content
  lcd.setCursor(0, 0);            // Move the cursor to the first row, first column
  lcd.print("Dist: ");            // Display the label for distance
  lcd.print(adjustedDistance);    // Display the adjusted distance in cm
  lcd.print(" cm");               // Label for unit (cm)

  lcd.setCursor(0, 1);            // Move the cursor to the second row, first column
  lcd.print("Vel: ");             // Display the label for velocity
  lcd.print(velocity);            // Display the calculated impact velocity in m/s
  lcd.print(" m/s");              // Label for unit (m/s)

  delay(500); // Wait for half a second before updating the display again
}