#include <LiquidCrystal.h> //LCD display library
#include <Adafruit_MPU6050.h> //IMU library
#include <Adafruit_Sensor.h> //Sensory library
#include <stdlib.h>


Adafruit_MPU6050 mpu; //Defines the IMU as "mpu"
LiquidCrystal lcd(12, 11, 10, 9, 8, 7); //deifines the lcd screen connections and name as lcd
#define ECHO_PIN 2 //defines pin 2 as the echo port of the ultrasound distance measurer
#define TRIG_PIN 3 //fines pin 3 as the trigger port

float x = 0, y = 0, z = 0; //defines rotaional angles
unsigned long last_time = 0; // defines the long non-negative variable "last-time"
int distance;
//run this fucntion as soon as program starts
void setup() {
  Serial.begin(115200); //sets out rate of processes per second
  pinMode(TRIG_PIN, OUTPUT); //sets the mode of the trigger pin to output
  pinMode(ECHO_PIN, INPUT); //sets mode of echo pin to input
  lcd.begin(20, 4); //initializes lcd screen with 20 characters per row and 4 rows

  mpu.begin(); //initializes the IMU
  mpu.setGyroRange(MPU6050_RANGE_500_DEG); //sets the range for the gyroscope in the IMU

}

//run this function infinetly after code has started and after void stepup() has finished running
void loop() {
//time elapsed
  unsigned long current_time = millis(); //defines the long non-negative variable "current_time" as the time passed since the code started running in milliseconds
  float delta_time = (current_time - last_time)/1000.0; //defines the varibale delta_time whihc is the time elapsed between each time the function runs in seconds
  last_time = current_time; //sets last_time as current_time

//measure IMU
  sensors_event_t accel, gyro, temp; //defines the variables accel, gyro and temp as sensory infromation from IMU
  mpu.getEvent(&accel, &gyro, &temp); // returns the values measured by the IMU to these variables

//calculate rotation
  x += (gyro.gyro.x) * delta_time; //calculates roll
  y += (gyro.gyro.y) * delta_time; //calculates pitch
  z += (gyro.gyro.z) * delta_time; //calculates yaw
  //All using the time and the gyroscpe readings which measures rotation of the device (in radians) per second

  lcd.setCursor(0,2); //start writing on 3rd row of lcd
  //prints rotation values on lcd
  lcd.print("X:");
  lcd.print(x);
  lcd.print(" Y:");
  lcd.print(y);
  lcd.setCursor(0,3);
  lcd.print("Z:");
  lcd.print(z);

  //prints rotation values on terminal
  Serial.println();
  Serial.println("Rotation: ");
  Serial.print("X: ");
  Serial.println(x);
  Serial.print("Y: ");
  Serial.println(y);
  Serial.print("Z: ");
  Serial.println(z);

  //send pulse
  digitalWrite(TRIG_PIN, HIGH); //Sends a high voltage to the tigger pin to activate it
  delayMicroseconds(10); //waits 10 micro seconds
  digitalWrite(ECHO_PIN, LOW); //stop high voltage to deactivate echo pin
  digitalWrite(TRIG_PIN, LOW); //stop high voltage to deactivate trigger pin

//distance to ground in relation to the angle
  int pulse = pulseIn(ECHO_PIN, HIGH); //defines pulse variable as the time the echo pin stays high (detecting the pulse form trigger pin)
  if (x == 0 && (z > 0 || z < 0)){
     distance = (pulse/58) * cos(z);
  }
  if(z == 0 && (x > 0 || x < 0)){
     distance = (pulse/58) * cos(x);
  }
  if (x == 0 && z == 0){
     distance = (pulse/58); //defines "distance" as the distance measured by the ultrasound sensor
  }
  if((x < 0 || x > 0) && (z < 0 || z > 0)){
    distance = (pulse/58) * cos(x) * cos(z);
  }
  distance = abs(distance);
  Serial.print("Distance from the ground is: "); //prints text on terminal
  Serial.println(distance); //prints the distance on terminal
  lcd.setCursor(0,0); //sets start wrtting point on lcd as 0,0 (first row first column)
  lcd.print(distance); //print the distance on lcd
  lcd.print(" cm from ground"); //prints text on lcd

//speed of imopact
  distance = distance;
  float impact_speed = sqrt(2*9.81*distance); //defines the speed of impact by calculating it using the diatnce to the gorund (v^2=u^2+2as)
  lcd.setCursor(0,1); //set wrtign point on 2nd row
  Serial.print("Impact speed would be: "); //print text on terminal
  Serial.println(impact_speed); //print the impact distance on terminal
  lcd.print("Impact: "); //prints text on lcd
  lcd.print(impact_speed); //prints the impact distance on lcd
  lcd.print(" cm/s"); //prints units of speed on lcd

  delay(1000); //wait 1 second befreo running this whole function again
}