#include <stdio.h> //basic assigned library 
#include <math.h> //library for mathematical functions 
#include <Adafruit_MPU6050.h> //library for the device itself 
#include <Adafruit_Sensor.h> //standard library for sensors 
#include <Wire.h> //enables the 12C integrated circuit 
#include <LiquidCrystal.h> //Library for the control of the LCD
LiquidCrystal lcd (12,11,10,9,8,7); //RS,E,D4,D5,D6,D7 pin definitions(in that order)
Adafruit_MPU6050 mpu; //declares the object for the mpu
#define PIN_TRIG 3 //assigns the trig pin to digital 3 on the arduino uno
#define PIN_ECHO 4 //assigns the echo pin to digital 4 on the arduino uno
#define PIN_SDA A4 //assigns the SDA pin of the gyro to the A4 pin
#define PIN_SCL A5 //assigns the SCL pin of the gyro to the A5 pin 
float anglex = 0; // sets a float for the angle of x 
float anglez = 0; //sets a float for the angle of z
unsigned long previousMillis = 0; //variable to track timing 
float acceleration = 9.8; //sets a constant value for acceleration
float speed = 0; // sets a float value for the speed allowing us to do calculations
float distance = 0; //sets a float for the distance to allow us to do calculations 
float deltatime = 0; //sets a float for the time change 
float real_distance = 0;
void setup() //function to setup the components 
{
  Serial.begin(115200); //sets the rate for data transmission 
  pinMode(PIN_TRIG, OUTPUT); //outputs the data from the TRIG pin of the distance sensor 
  pinMode(PIN_ECHO, INPUT); //outputs the data from the ECHO pin of the distance sensor
  mpu.setGyroRange(MPU6050_RANGE_250_DEG); //allows measurement up to 250 per second 
  mpu.begin(); //begins the mpu measuring 
  lcd.begin(16,2); //sets up the area of the LCD that can be used 
  lcd.print("Initialising..."); //prints a message to show that setup has worked 
  delay(2000); //2 second delay until setup finishes 
  lcd.clear(); //clears the LCD of any messages 
}

void loop () //main function of the arduino
{
  unsigned long currentMillis = millis(); //sets up a variable for the current timeflo elapsed
  deltatime = (currentMillis-previousMillis) / 1000;
  digitalWrite(PIN_TRIG, HIGH); //voltage is set to 5V, these functions work like a true or false 
  delayMicroseconds(10); //10 microsecond delay between the pulse sent and read 
  digitalWrite(PIN_TRIG, LOW); //voltage is lower than 5V 
  int duration = pulseIn(PIN_ECHO, HIGH); //sets an integer for reading the pulse on the pin
  Serial.print("\nDistance reading from sensor in CM: "); //conducts a serial print of the distance 
  Serial.println(duration / 58); //prints the value of the distance 
  sensors_event_t a, g, temp; //tells the sensor to populate these fields
  mpu.getEvent(&a, &g, &temp); //fetches the data of the acceleration, gyroscope and temperature 
  anglex += g.gyro.x * deltatime; //calculates the angle of x
  previousMillis = currentMillis; //resets the variable for the previous time to equal to the current time, to count the next time 
  Serial.print("Angle x: "); //prints an area for x 
  Serial.print(anglex); //prints the x angle 
  anglez += g.gyro.z * deltatime; //calculates the z angle 
  previousMillis = currentMillis;
  Serial.print("\nAngle z: "); //prints the area for z angle display
  Serial.print(anglez); //prints the angle of z 
  float total_angle = sqrt(pow(anglex, 2) +pow(anglez,2)); //calculates the total tilt angle of the person using the magnitude formula 
  Serial.print ("\nTotal angle: "); //serial print space for the total angle 
  Serial.println(total_angle); //serial print of the total angle 
  distance = (duration/58) * cos(total_angle); //calculates the distance 
  real_distance = distance/100; //converts the distance to metres 
  if (distance > 0) //if function for any given distance to provide a calculation
  {
    Serial.print("\nDistance: "); //sets up print area for distance 
    Serial.println(real_distance); //prints the distance 
    speed = sqrt(2 * acceleration * real_distance); //calculation of the speed 
    Serial.print("\nSpeed: "); //serial print of the speed to verify the speed output of the LCD
    Serial.println(speed); //prints the actual value of the sped
    lcd.setCursor(0, 0); //Sets the cursor position on the LCD screen to the first line and first row
    for (int i = 0; i < 16; i++) lcd.print(' '); //clears the pervious characters of the previous line of the LCD
    lcd.setCursor(0, 0); //Sets the cursor position on the LCD screen to the first line and first row
    lcd.print("Speed: "); //prints an area for the speed to go to on the LCD screen
    lcd.print(speed); //outputs the value of the LCD 
  }
  delay(1000);// provides a 1 second delay so there is not a large amount of data per second 
}