#include <LiquidCrystal_I2C.h>  //to control lcd screen with I2C 
#include <Adafruit_MPU6050.h>  //interact with MPU6050 sensor
#include <Adafruit_Sensor.h>  //abstraction layer 
#include <Wire.h>  //I2C communication

#define PIN_TRIG 3  //trigger pin location for ultrasonic sensor
#define PIN_ECHO 2  //echo pin location for ultrasonic sensor 
float cm;  //variable guarda distance in cm
float angle;  //angle do ultrasonic sensor com relation to floor

Adafruit_MPU6050 mpu;  //instance of the MPU6050 sensor
float gyroX, gyroY, gyroZ;  //variables of wdegrees/s of gyro rotation in x,y,z direction

float height;  //altura da pessoa in m
float velocity;  //when spat on the floor
//time integration of Wdegrees/s
unsigned long previousTime= 0;
unsigned long currentTime;
float deltaTime;  //time difference between refresh in seconds 

LiquidCrystal_I2C lcd(0x27, 20, 4); //size of lcd (20,4)

void setup() {
  Serial.begin(9600);// for debugging
//for lcd screen to start 
  lcd.begin(20,4); //start lcd
  lcd.backlight(); //Arose LCD backlight
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");

//to start the ultrasonic sensors pins 
  pinMode(PIN_TRIG, OUTPUT);  //trig as an output 
  pinMode(PIN_ECHO, INPUT);  //echo as an input

//To start the MPU6050 sensor
  while (!mpu.begin()) { //start the SENSOR
    Serial.println("Not good bro");
    lcd.clear(); //clear the lcd screen
    lcd.setCursor(0, 0);
    lcd.print("NAH BRO"); //if it dont work
    lcd.setCursor(0, 1);
    lcd.print("YOU GOOD JO");  //if it works
    delay(1000);  
  }
  lcd.clear();  // eliminates everything before showing new things
  lcd.setCursor(0, 0);  //puts it on first row
  lcd.print("YOU GOOD JO");  //Lets go it works!!!
  Serial.println("You good JO");  //shows that i am good on screen
  delay(1500);
// put your setup code here, to run once:
}
void loop() {
//measure distance of ultrasonic sensor
  digitalWrite(PIN_TRIG, LOW);  //make the trig pin low
  delayMicroseconds(2);  //WAIT FOR 2 MICROSECONDS
  digitalWrite(PIN_TRIG, HIGH);  //send a pulse to trig pin
  delayMicroseconds(10);  //WAIT FOR 10 MICROSECONDS
  digitalWrite(PIN_TRIG, LOW);  //stops sending pulse
  long duration= pulseIn(PIN_ECHO, HIGH);  //measure duration of echo pin pulse
  cm= (duration/2)/29.1;  //divide by 29.1 to get values in centimeters
  height= cm/100.0;   //convert to m

//information in MPU6050 sensor of accelerometer and gyroscope
  sensors_event_t a, g, temp, accelEvent;
  mpu.getEvent(&a, &g, &temp, &accelEvent);

  angle= atan2(a.acceleration.y,a.acceleration.z)*180/PI;  //value of angle from the floor degrees
  angle= atan2(-accelEvent.acceleration.x, sqrt(accelEvent.acceleration.y * accelEvent.acceleration.y + accelEvent.acceleration.z * accelEvent.acceleration.z)) * 180.0 / PI;
  angle= atan2(accelEvent.acceleration.y, accelEvent.acceleration.x) * 180.0 / PI;

//calculations of Wdegrees/s
  gyroX = g.gyro.x;  //rotation rate on x-axis
  gyroY = g.gyro.y;  //rotation rate on y-axis
  gyroZ = g.gyro.z;  //rotation rate on z-axis
//calculation of Wdegrees/s integration time difference
  currentTime= millis();  //time in milliseconds
  deltaTime= (currentTime - previousTime)/1000.0;  //conv. to sec
  previousTime= currentTime;  //new version of time 

  if(abs(gyroX)>100 || abs(gyroY)>100 || abs(gyroZ)>100) {
    velocity= sqrt(2*9.81*height);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Velocity:");
    lcd.print(velocity);  //
    lcd.print("m/s");
  }

  Serial.print("G-X:"); Serial.print(gyroX); Serial.print("deg/s");
  Serial.print("G-Y:"); Serial.print(gyroY); Serial.print("deg/s");
  Serial.print("G-Z:"); Serial.print(gyroZ); Serial.print("deg/s");
  Serial.print("Angle:"); Serial.println(angle); Serial.println("degrees"); //in degrees
  Serial.print("Velocity:"); Serial.print(velocity); Serial.println("m/s");
//shows the information of the ultrasonic sensor
  Serial.print("Distance:"); Serial.print(cm); Serial.println("cm");
  
//to display on the LCD
  lcd.clear();
  lcd.setCursor(0, 0);  //puts it in first row
  lcd.print("Vel:"); lcd.print(velocity);
  lcd.setCursor(0, 1);  //puts it in second row
  lcd.print("ANG:"); lcd.print(angle);
  lcd.setCursor(0, 3);  //puts it in fourth row
  lcd.print("Dist:"); lcd.print(cm);  //shows distance of ultrasonic sensor
  delay(1000);

  /*
  mpu.getAccelerometerSensor()->getEvent(&event); //retrieve accelerometer information
  lcd.clear(); //clear what was in before
  lcd.setCursor(0, 0); //puts it in first row
  lcd.print("Ac-X:");
  lcd.print(a.acceleration.x, 2);  //X-axis information in 2 decimal places
  lcd.setCursor(0, 1); //puts it in second row
  lcd.print("Ac-Y:");
  lcd.print(a.acceleration.y, 2); //Y-axis information in 2 decimal places
  lcd.setCursor(0, 2); //puts it in third row
  lcd.print("Ac-Z:");
  lcd.print(a.acceleration.z, 2);  //Z-axis information in 2 decimal places
  */
}