#include <LiquidCrystal_I2C.h>
#include <Adafruit_Sensor.h>     // librarys used
#include <Wire.h>
#include <UltrasonicHCSR04.h>
#include <MPU6050_light.h>
#define PIN_TRIG 3  // defines where trigger and echo are pinned
#define PIN_ECHO 2  // for the ultrasonic sensor

float yaw, pitch, Adj, Hyp, time, g, Vel, Adjm; // defined vairiables

LiquidCrystal_I2C lcd(0x27,A4,A5); // these initiallise the components
UltrasonicHCSR04 ultrasonic(PIN_TRIG, PIN_ECHO);
MPU6050 mpu(Wire); //uses 12c to comunicate with arduino
   
void setup() {
  Serial.begin(9600); //baud rate 9600
  lcd.init();
  lcd.backlight();
  Wire.begin();
  mpu.begin();
  mpu.calcOffsets(true, true); // callabrates MPU
  pinMode(PIN_TRIG, OUTPUT); //sets trigger as output
  pinMode(PIN_ECHO, INPUT); //sets echo as input
  
}

void loop() {
  
  digitalWrite(PIN_TRIG, HIGH); 
  delayMicroseconds(10); //sends 10microsecond to trig to start impulse
  digitalWrite(PIN_TRIG, LOW);
 
  time = pulseIn(PIN_ECHO, HIGH); //records time taken for impuses to return
  Hyp = (time / 58.5); //work out the distance based on time
  
  mpu.update(); // updates the readings of mpu
  pitch = mpu.getAngleX(); //stors as pitch and yaw
  yaw = mpu.getAngleY();
  
  mpu.setFilterGyroCoef(1); //ignores the accelorometer
  Adj = Hyp * cos(pitch *(PI/180)) * cos(yaw *(PI/180)); //using triganometrics to calculat vertical hieght, angle in degs but cos works in rads so *pi/180
  g = 9.80665; // value of g acceloration due to gravity
  Adjm = Adj/100; // converting vertical hieght into m to be able to give m/s
  
  Serial.print("\nx:");    // prints angles, distances and velocity to serial
  Serial.print(mpu.getAngleX()); // mainly for my benefit to check the maths is correct
  Serial.print("\ty:");
  Serial.print(mpu.getAngleY());
  Serial.print("\th:");
  Serial.print(Adjm);
  Serial.print("\tv:");
  Serial.print(Vel);

  if (Adjm<0){ // negative value wont work in square root also means angle>90
    lcd.clear(); // clears screen prints error returns to the start of the function
    lcd.setCursor(0,0);
    lcd.print("ERROR");
    delay(1000);
    return;
  }
  
  Vel = sqrt(2*g*Adjm); //uses rearanged SUVAT equation v^2=u^2 + 2as intitial velocity=0
  
  lcd.clear(); // clears the lcd 
  lcd.setCursor(0,0); //will start at those coordinates
  lcd.print("Impact velocity ");
  lcd.setCursor(0,1); // prints on the 2nd line
  lcd.print(Vel); //prints velocity calculated
  lcd.print("\t m/s"); //units
  delay(1000); //repeats every second
}