#include <LiquidCrystal_I2C.h>
#include <Wire.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
#define trig 2
#define echo 3
float gx, gy, anglex, angley;
float currentTime, previousTime, elapsedTime;
float duration, distance, vdistance, velocity;
void setup() {
  Wire.begin();
  Serial.begin(9600);           //turns on arduino board and bps (transmittion speed)
  lcd.init();                   //initialises lcd
  lcd.backlight();              //turns on lcd back light
  pinMode(trig, OUTPUT);        //trig pin on ultrasonic is output
  pinMode(echo, INPUT);         //echo pin on ultrasonic is input
  ////////////////////// turn on the mpu6050
  Wire.beginTransmission(0x68);// initiates the i2c connection of the mpu6050
  Wire.write(0x6B); // power management register 
  Wire.write(0); // 1 in this register == sleep mode on, 0 in this register == sleep mode off
  Wire.endTransmission(true);
////////////////////////////
}

void loop() {
  //////////////////////distance from ultrasonic
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = ((duration/2) * 0.000343 ); //xby speed of sound 
    Serial.print("raw Ultrasonic sensor: ");
    Serial.println(duration);
    Serial.print("Distance to floor: ");
    Serial.println(distance);
  
/////////////////////////getting angles
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime)/1000;
  Wire.beginTransmission(0x68);
  Wire.write(0x43); // 0x43 high byte gyro x, 0x44 low byte gyro x, 0x45 high byte gyro y, 0x46 low byte gyro y, 
  Wire.endTransmission(false); 
  Wire.requestFrom(0x68, 4,true);
  gx = (Wire.read() << 8 | Wire.read()) / 131.0; //div by 130 to get angular velocity in deg/sec (factory standard)
  gy = (Wire.read() << 8 | Wire.read()) / 131.0;
  anglex = anglex + (gx*elapsedTime)*3.1415/180;
  angley = angley + (gy*elapsedTime)*3.1415/180;
    Serial.print("angle x: ");
    Serial.println(anglex);
    Serial.print("angle y: ");
    Serial.println(angley);

////////////////////////distance --> vertical distance
  vdistance = cos(anglex)*cos(angley)*distance;

////////////////////////calculating velocity using vSQ = uSQ +2as
  velocity = sqrt(0 + 2*9.81*vdistance);

/////////////////////////print display on lcd
  lcd.setCursor(0,0);
  lcd.print("S: ");
  lcd.print(vdistance);
  lcd.print(" m");

  lcd.setCursor(0,1);
  lcd.print("V: ");
  lcd.print(velocity);
  lcd.print(" m/s");

  delay(200);
}