#include <Wire.h>              // For I2C commnication
#include <MPU6050.h>           // For the MPU6050 sensoor
#include <LiquidCrystal_I2C.h> // For the LCD

// HC-SR04 pins
const int trigPin = 9;
const int echoPin = 10;

// Initialise Gyro
MPU6050 mpu;

// Initialises the LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);  // 16 columns and 2 rows

long previousDistance = 0;
long currentDistance = 0;
float velocity = 0;
float NegativeVelocity = 0;  //Vairable to store the negative velocity
unsigned long previousMillis = 0;  // Store the time of the last second

float pitch = 0;  // Pitch angle in degres
float yaw = 0;    // Yaw angle in degrees

// Timing variabls for serial print and LCD update
unsigned long lastSerialUpdate = 0;
const long serialUpdateInterval = 2000;  // 2 seconds for seriel update interval
unsigned long lastLCDUpdate = 0;
const long lcdUpdateInterval = 20;  // 20ms for LCD update intervall

void setup() {
  // Start serial communication for debugging
  Serial.begin(9600);

  // Initialise the gyros
  Wire.begin();
  mpu.initialize();

  // Initialise the proxmity sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialise the LCD
  lcd.begin(16, 2);  // Set the LCD dimenions
  lcd.setBacklight(1);  // Turn on backlight
  lcd.clear();          // Clear the screen at the start
  lcd.setCursor(0, 0);  // Set cursor to top-left corner
}

void loop() {
  // Get the current time
  unsigned long currentMillis = millis(); // millis is how many ms since the script started
  float deltaTime = (currentMillis - previousMillis) / 1000.0;  // Time in seconds

  // Get the gyro data from the MPU6050 (angular velocity in degrees per second)
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // Convert gyro readings to degrees per second (default sensitivity scale factor)
  float gyroXdeg = gyroX / 131.0;
  float gyroZdeg = gyroZ / 131.0;

  // Integrate the gyroscope data to calculate pitch and yaw
  pitch += gyroXdeg * deltaTime;  // Integrating to get pitch
  yaw += gyroZdeg * deltaTime;    //Integrating to get yaw

  // Wrap pitch and yaw to the range 0-360 degrees
  pitch = fmod(pitch, 360.0);  // Wrap pitch to 0-360 degrees
  if (pitch < 0) {
    pitch += 360.0;  // Ensure positive value if negative
  }

  yaw = fmod(yaw, 360.0);    // Wrap yaw to 0-360 degrees
  if (yaw < 0) {
    yaw += 360.0;  // Ensure positive value if negative
  }

  // Measure distance using the proximity sensor (uncorrected)
  long rawDistance = measureDistance();  // This gives the uncorrected distance (raw reading)

  // NOTE TO SELF - DON'T FORGET TO CORRECT DISTANCE LATER

  // Apply pitch and yaw correction to the measured distance
  currentDistance = adjustDistanceForPitchAndYaw(rawDistance, pitch, yaw);

  // Calculate velocity if enough time has passed
  if (deltaTime > 0) {
    // Calculate the velocity based on the change in corrected distance
    float deltaDistance = (currentDistance - previousDistance) / 100.0;  // Convert cm to metres

    // Calculate the velocity (in metres per second)
    velocity = deltaDistance / deltaTime;

    // Only consider negative velocity changes (when the distance decreases)
    if (velocity < 0) {
      // Update the negative velocity if the current velocity is lower (more negative)
      if (velocity < NegativeVelocity) {
        NegativeVelocity = velocity;
      }
    }

    //Reset the negative velocity after 1 second
    if (currentMillis - previousMillis >= 1000) {
      NegativeVelocity = 0;  // Reset the negative velocity
      previousMillis = currentMillis;  // Reset the time for the next second
    }

    // Store the current corrected distance for the next loop
    previousDistance = currentDistance;
  }

  // update the LCD every 20ms
  if (currentMillis - lastLCDUpdate >= lcdUpdateInterval) {
    lastLCDUpdate = currentMillis;  // Update the last LCD update time

    // Update the pitch and yaw display on the LCD (first row)
    lcd.setCursor(0, 0);  // Move cursor to the first row
    lcd.print("P:");
    lcd.print(pitch, 1);  // Print pitch angle with one decimal place
    lcd.print(" Y:");
    lcd.print(yaw, 1);  // Print yaw angle with one decimal place

    // Display the negative velocity on the second row
    lcd.setCursor(0, 1);  // Move cursor to the second row
    lcd.print("Vel: ");
    lcd.print(abs(NegativeVelocity), 2);  // Print the absolute value of the negative velocity
    lcd.print(" m/s");

    // Makes sure the first row has no leftover characters
    lcd.print("    "); 
  }

  // Update serial output every 2 seconds
  if (currentMillis - lastSerialUpdate >= serialUpdateInterval) {
    lastSerialUpdate = currentMillis;  // Update the last serial update time

    // Check if the +ve value of the corrected distance is greater than the raw distance AND current distance is positive
    if (abs(currentDistance) > abs(rawDistance) || currentDistance < 0) { //If not, error is printed
      Serial.println("Error! The floor is out of range of the proximity detector. Please vary the Pitch/Yaw.");
    }

    //print to the serial monitor for debugging
    Serial.print("Proximity Sensor Distance: ");
    Serial.print(rawDistance);  // Print the uncorrected distance (raw reading)
    Serial.println(" cm");

    Serial.print("MPU6050 Data - Pitch: ");
    Serial.print(pitch, 1);  // Print pitch angle with one decimal place
    Serial.print(" Yaw: ");
    Serial.print(yaw, 1);  // Print yaw angle with one decimal place
    Serial.println();

    Serial.print("Corrected Distance to Floor (cm): ");
    Serial.println(currentDistance);  // Print the corrected distance considering pitch and yaw

    Serial.print("Calculated Speed (m/s): ");
    Serial.println(abs(velocity), 2);  // Print absolute velocity in m/s
  }

  delay(10);  // Wait 10ms before the next loop iteration
}

// Function to measure distance using the HC-SR04 sensor
long measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in cm
  long distance = duration * 0.0343 / 2;
  delay(50); // Add a 50ms delay before the next measurement
  return distance;
}

// Function to adjust distance for pitch and yaw (compensate for sensor tilt)
long adjustDistanceForPitchAndYaw(long measuredDistance, float pitch, float yaw) {
  // Convert pitch and yaw to radians
  float pitchRadians = pitch * PI / 180.0;
  float yawRadians = yaw * PI / 180.0;

  // Adjust the distance based on the pitch and yaw angles
  float adjustedDistance = measuredDistance * cos(pitchRadians) * cos(yawRadians);
  return (long)adjustedDistance;
}