// Include necessary libraries
#include <Wire.h>
#include <LiquidCrystal.h>
#include <Adafruit_MPU6050.h>

// Define pins for the ultrasonic sensor
#define TRIG_PIN 9
#define ECHO_PIN 10

// defining what pins the lcd is in
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Adafruit_MPU6050 mpu;

// Defining g
float GRAVITY = 9.8; // m/s²

void setup() {
  // Initialize serial communication
  Serial.begin(9600); // Conecting to computer at baud rate of 9600
  Wire.begin(); //communicating to sensor

  // getting lcd ready to process/print data (16,2 is the dimensions)
  lcd.begin(16, 2);
  lcd.print("Initializing...");
  
  // 
  pinMode(TRIG_PIN, OUTPUT); //starts measurement
  pinMode(ECHO_PIN, INPUT); //recieves data
  
  // making sure mpu and lcd are going to work. for and print statements for in not
  if (mpu.begin()) {
    lcd.setCursor(0, 1);
    lcd.print("MPU6050 Ready");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("MPU Error");
  }
  
  delay(2000);
  lcd.clear();

}

void loop() {

  long Start_Time = millis();

  // Measure distance to the floor using the ultrasonic sensor
  long duration; //(long stores larger values)
  float distance;
  int angleX, angleY, angleZ;

  digitalWrite(TRIG_PIN, LOW); // turns of any previous data that may have being on
  delayMicroseconds(2); //does the above for 2 milliseconds
  digitalWrite(TRIG_PIN, HIGH); //gets data 
  delayMicroseconds(10); // does the above for 10 milliseconds 
  digitalWrite(TRIG_PIN, LOW); // turns off pulse

  // Recieving echo data for 25 milliseconds
  duration = pulseIn(ECHO_PIN, HIGH, 25000);
  
  // Calculate the distance in meters from duration
  distance = duration * 0.0342 / 2 / 100;
  
  // Calculate velocity using the formula v = sqrt(2gh)
  float velocity = sqrt(2 * GRAVITY * distance);

  sensors_event_t accel_event, gyro_event; //defining events
  mpu.getAccelerometerSensor()->getEvent(&accel_event); //getting accelleration and saving it as an event
  mpu.getGyroSensor()->getEvent(&gyro_event); //getting gyro and saving it as an event

  // saving gyro measurements to individual x,y,z variables and incorporating them to the new velocity
  float gyroX, gyroY, gyroZ;
  gyroX = gyro_event.gyro.x;
  gyroY = gyro_event.gyro.y;
  gyroZ = gyro_event.gyro.z;
  float adjusted_velocity = velocity + sqrt(gyroX * gyroX + gyroY * gyroY + gyroZ);

  long Current_Time = millis();
  float dt = (Current_Time - Start_Time) / 1000;
  
  angleX = (gyroX * dt) * PI / 180;
  angleY = (gyroY * dt) * PI / 180;
  angleZ = (gyroZ * dt) * PI / 180;

  // printing to lcd
  lcd.setCursor(0, 0);
  lcd.print("Dist: ");
  lcd.print(distance);
  lcd.print(" m");

  
  // printing to monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print(" m, Velocity: ");
  Serial.print(adjusted_velocity);
  Serial.println(" m/s");

  Serial.print(" Rotation X: "); 
  Serial.print(angleX); 
  Serial.println(" rad");
  Serial.print(" Rotation Y: "); 
  Serial.print(angleY); 
  Serial.println(" rad");
  Serial.print(" Rotation Z: "); 
  Serial.print(angleZ); 
  Serial.println(" rad");


  // printing data to lcd
  lcd.setCursor(0, 1);
  lcd.print("Vel: ");
  lcd.print(adjusted_velocity);
  lcd.print(" m/s");

  delay(1000); // Wait for 1 seconds before the next measurement
}