#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>
#define ECHO_PIN 2
#define TRIG_PIN 3 

Adafruit_MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 16, 2);

long duration;
int distance;

float roll = 0;
float pitch = 0;
float yaw = 0;
float velocity = 0.0;
float acceleration = 0.0;


void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);

lcd.begin(16, 2);
lcd.print("Starting up...");


mpu.begin();

pinMode(2, INPUT);
pinMode(3, OUTPUT);

delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:
sensors_event_t acc, gcc, temp;
mpu.getEvent(&acc, &gcc, &temp);

float ax = 0.0;
float ay = 0.0;
float az = 0.0;

//As we are calculating acceleration from pitch(x), roll(y) and yaw(z), we need to calculate the magnitude of this acceleration
acceleration = sqrt((ax * ax) + (ay * ay) + (az * az));

unsigned long time = millis();

velocity = acceleration * time;

pitch = atan2(ay, sqrt((ax * ax) + (az * az))) * 180.0 / PI; //converting radians to degrees for simpler calculations

roll = atan2(-ax, az) * 180.0 / PI;

yaw = atan2(ay, sqrt((ax * ax) + (az * az))) * 180.0 / PI;

//Measuring distance now
digitalWrite(3, LOW);
delayMicroseconds(2);
digitalWrite(3, HIGH);
delayMicroseconds(10);

//calculating the distance from the ground using the ultrasonic sensor. Speed of sound converted to cm/us.
duration = pulseIn(2, HIGH);
distance = duration * 0.0344 / 2;

lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Distance: ");
lcd.print(distance);
lcd.print(" cm");

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

Serial.clear();
Serial.print(" Pitch: ");
Serial.print(pitch);
Serial.print(" degrees");
Serial.print(" Roll: ");
Serial.print(roll);
Serial.print(" degrees");
Serial.print(" Yaw: ");
Serial.print(yaw);
Serial.print(" degrees");

delay(2000); //adding delay to make display readable.
}