#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <LiquidCrystal_I2C.h>
// Start Up the MPU and LCD screen
Adafruit_MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// confirm which pins are wired to the ultrasonic sensor to measure the duration
const int trigPin = 2;
const int echoPin = 5;

float pitch = 0.0, yaw = 0.0; // Define the Yaw and pitch which both can affect the angle
unsigned long previousTime = 0; //Start a time from 0 and assign it to the variable previous time

void setup() {
  Serial.begin(115200); //Set debugging rate (baud rate)

  lcd.begin(16, 2);//  Set the parameters of the LCD screen 
  lcd.setCursor(0, 0); //Move to the top left of the screen and print initializing as its being setup
  lcd.print("Initializing...");

  if (!mpu.begin()) {               //BEgin set up of the gyroscope
    lcd.setCursor(0, 1);
  }

  // Define which pin of our sensor is inputting data pack into the system and which is this output
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  lcd.setCursor(0, 1);    //print a message at the bottom left of the LCD stating that set up is complete
  lcd.print("Ready!");
  delay(1000);

 
  previousTime = millis(); // begin a timer ( in milliseconds)
}

void loop() {
  // use the function to obtain a value from the sensor for the distance
  float distance = findDistance();

  sensors_event_t gyro, temp, accl; // obtain values from the MPU for each element 
  mpu.getEvent(&accl, &gyro, &temp); // assign each value to a variable

  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previousTime) / 1000.0; // find the change in time (in seconds)
  previousTime = currentTime; //start the timer again from this point

  pitch += gyro.gyro.x * deltaTime; // Use gyroscope data to find the pitch
  yaw += gyro.gyro.y * deltaTime;  // Use gyroscope data to find the yaw
  
  Serial.print("Pitch:");  //Print the pitch and yaw in the bar below 
  Serial.println(pitch);
  Serial.print("Yaw:");
  Serial.println(yaw);
  // Calculate the actual distance to the round relative to rotation
  float correctedDistance = distance/100 * cos(radians(pitch)) * cos(radians(yaw));

  // use the forumla v=sqrt(2gh) to calculate velocity on impact
  float velocity = sqrt(2 * 9.81 * correctedDistance);

  // Display on the LCD the real distance on the top line 
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Dist: ");
  lcd.print(correctedDistance, 2);
  lcd.print(" m");
  // print on the line beneath the calculated velocity in m/s
  lcd.setCursor(0, 1);
  lcd.print("Vel: "); 
  lcd.print(velocity, 2);
  lcd.print(" m/s");

  delay(500);
}

// Function to get a value for distance from the centre 
float findDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float distance = (duration * 0.0342) / 2.0; // 
  return distance;
}