#include <Adafruit_MPU6050.h> //imported libraries used to communicate with components
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define trigPin 3 //trigger pin for the ultrasonic sensor, sends the pulse
#define echoPin 2 //echo pin for the ultrasonic sensor, listens for the reflected pulse

Adafruit_MPU6050 imu = Adafruit_MPU6050(); //instantiates an object for the IMU unit
LiquidCrystal_I2C lcd(0x27,20,4); //instantiates an object for the LCD

float correctedDistance; //stores the correct distance after pitch and yaw have been taken into account
float deltaTime; //time difference between loop iterations used to convert rotation to angles
float pitch = 0.0; //pitch angle
float yaw = 0.0; //yaw angle
unsigned long previousTimeMillis = 0; //used for calculating delta time

float getDistance(float pitch, float yaw) //function that calculates the distance of the worker to the ground, it takes two arguments
{
  digitalWrite(trigPin, HIGH); //sends a pulse to the ultrasonic sensors trigger to start the measurement
  delayMicroseconds(10); //keeps the trigger pulse on for 10ms to initiate the sensor
  digitalWrite(trigPin, LOW); //turns off the trigger after 10ms

  float time = pulseIn(echoPin, HIGH); //records the time it takes for the signal to hit the floor and bounce back
  float distance = (time * 0.0344 / 2); //converts the time into a distance in cm and halves as the journey distance is there and back
  return (distance * cos(pitch) * cos(yaw)); //corrects the distance based on the pitch and yaw of the gryo sensor
}



void setup() {
  Serial.begin(9600); //initialises serial communication at 9600 baud rate. Used to send debug information to the serial monitor which is useful for debugging
  lcd.init(); //initialises the lcd screen
  lcd.backlight(); //turns on the backlight of the lcd
  pinMode(trigPin, OUTPUT); //configures the pins connected to the ultrasonic sensor
  pinMode(echoPin, INPUT); //configures the pins connected to the ultrasonic sensor
  imu.begin(); //initialises the IMU sensor to read accelerometer data
  previousTimeMillis = millis(); //initialises the timestamp for deltaTime calculation
}


void loop() {
  unsigned long currentTimeMillis = millis(); //gets the current time in milliseconds
  deltaTime = (currentTimeMillis - previousTimeMillis) / 1000.0; //calculate the deltaTime in seconds
  previousTimeMillis = currentTimeMillis; //update previous time for next loop

  lcd.clear(); //clears the lcd before updating it with new data
  
  sensors_event_t a, g, temp;
  imu.getEvent(&a, &g, &temp); //reads the accelerometer, gyroscope and temperature data

  pitch = g.gyro.x * deltaTime; //calculates the pitch angle from the gyroscope
  yaw = g.gyro.z * deltaTime;  //calculates the yaw angle from the gyroscope

  correctedDistance = getDistance(pitch, yaw); //calls the getDistance() function to calculate the corrected distance

  float velocity = sqrt(2 * 9.81 * correctedDistance); //calculates the fall velocity

  lcd.print("Velocity:"); //prints "velocity" to the LCD screen
  lcd.setCursor(0,1); //sets the cursor to the second line of the LCD
  lcd.print(velocity); //prints the calculated velocity to the LCD
  lcd.print(" cm/s");

  delay(1000); //waits for 1000ms before continuing the loop again

}