/*libries for the MPU6050*/
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

/*set up for the lcd*/
#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 10, 9, 8, 7); /*the pins that the lcd will use*/

/*naming pins 2 and 3 to make code easier to understand */
#define PIN_TRIGGER 3
#define PIN_INPUT 2

void setup() {
  /*setting up the distance monitor*/
  Serial.begin(9600); /*setting the baud rate to be used (the rate at which the device outputs data)*/
  pinMode(PIN_TRIGGER, OUTPUT); /*setting the trigger pin to cause the HC-SR04 to ouput a signal*/
  pinMode(PIN_INPUT, INPUT); /*setting the input pin to display when the HC-SR04 detects a echo*/

  /*insuring the MPU can initiolise*/
  mpu.begin();

  mpu.setGyroRange(MPU6050_RANGE_250_DEG); /*setting the range of the gyro, i believe the default is 250 any way*/
  delay(100);

  /*setting up the lcd*/
  lcd.setCursor(1,1); /*sets the start point for text to be outputted on the lcd*/
}

void loop() {
  /*hc-sr04*/
  /*taking a mesurement*/
  digitalWrite(PIN_TRIGGER, HIGH); /*writing a high voltage (5v) to the trigger pin, triggering an output*/
  delayMicroseconds(10);
  digitalWrite(PIN_TRIGGER, LOW); /*writing a low voltage (0v) to the trigger pin, to stop it outputting*/

  /*reading the result of the mesurement*/
  int duration_of_pulse = pulseIn(PIN_INPUT, HIGH);

  /*converting the duration to distance using convertions on the HC-SR04 data sheet*/
  int length_in_cm = duration_of_pulse / 58;
  Serial.println(length_in_cm);
  /*the length in cm can be abit off, as the real value increases the discrepancy between the real
  valuse and the calculated value getts bigger, but the discrepancy ranges from 1 to 3 cm so has a
  negligable inpact on the calculated speed of inpact*/

  /*mpu6050*/
  /*getting the readings from the gyro*/
  sensors_event_t a, gyro, temp;
  mpu.getEvent(&a, &gyro, &temp);

  /*getting the x and y angles form the gyro*/
  /*only dealing with cases with in 90degree rande because its 
  unrealistic that the person would be tilting more than that */
  double anglex = M_PI/2 - gyro.gyro.x;
  double anglez = M_PI/2 - gyro.gyro.z;

  /*outputting the x and y angles to the screen for debugging */
  Serial.print("x = ");
  Serial.println(anglex);
  Serial.print("z = ");
  Serial.println(anglez);

  float distance_to_ground;

  if (anglex <= anglez) { /*testing which angle is bigger, as if the person
  is leaning more in the x than in the y, the y will have no effect on the 
  distance to the ground so only one of the angles needs to be considered*/
    Serial.println("x is bigger");
    distance_to_ground = sin(anglex) * length_in_cm; /*using built in sin function to find dicgance to ground*/
  } else{
    Serial.println("y is bigger");
    distance_to_ground = sin(anglez) * length_in_cm; /*using built in sin function to find dicgance to ground*/
  }

  Serial.print("real distance to ground = "); /*displaying the real distance */
  Serial.println(distance_to_ground);

  delay(10);

  /*calculationg the speed*/
  float speed = sqrt((2*9.8*distance_to_ground) / 100);

  /*LCD*/
  lcd.clear(); /*clearing the lcd so any previous text is removed */

  lcd.println("speed of inpact: \n");
  lcd.print(speed);
  lcd.print("m/s");




  delay(1000);


}