#include <Wire.h>
#include <MPU6050.h>
#include <Keypad.h>
#include <string.h>
#include "NMEA.h"
#include <LiquidCrystal.h>

// Setting up the LCS
LiquidCrystal lcd(24, 26, 34, 36, 40, 42);

// Initialise LED indicator for locking system
const int loginLED = 13;

// Set up the buzzer
int buzzer = 23;


// Defining the layout of the keypad
const byte ROWS = 4;
const byte COLS = 4;

// Defining the Symbols on the Keypad
char layout[4][4] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'},
};

// Specifying the pin numbers of connection layout for rows and columns of keypad
byte row_pins[4] = {10, 9, 8, 7};
byte col_pins[4] = {6, 5, 4, 3};

// Map the keypad configuration to pin numbers in connection
Keypad lock = Keypad(makeKeymap(layout), row_pins, col_pins, ROWS, COLS);

// Defining a Password Structure
typedef struct Password {
  char pswd[7]; // Password is a 6-digit combination
  int length;
} passkey;

// Create 2 password struct objects one for storing password and one fo checking
passkey stored_pswd;
passkey entered_pswd;
int mode = 0; // Mode indicates weather program in is password reading or writing mode
int login_count; // grows by one every time a login is successful

// Initialise the MPU6050 Sensor
MPU6050 mpu;
int16_t ax, ay, az; // Set up acceleration variables
int16_t gx, gy, gz; // Set up gyroscope variables

// Crash detection thresholds
float crashThreshold = 2.0;         // Acceleration threshold 
float rotationThreshold = 200.0;    // Rotation speed threshold (degrees/second)
bool crashDetected = false;

// Setting up Ultra sonic sensor for speed detection

// Ultrasonic sensor pins
const int trigPin = 15;
const int echoPin = 14;

// Variables for speed calculation
float distance = 0;
float speed = 0;
unsigned long duration;
float timeInterval = 0;  // Time interval in seconds
unsigned long previousTime = 0;  // Store the last loop time
const float speedThreshold = 52;  // Speed threshold in cm/s

// Set up for GPS-6M Sensor
#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))

union {
  char bytes[4];
  float valor;
} velocidadeGPS;

float latitude;
float longitude;

NMEA gps(GPRMC);  // Creates a GPS data connection with sentence type GPRMC


// Define the pin numbers for the LED and LDR (Light Dependent Resistor)
#define led 12
#define light A0

// Threshold in lux to determine when to turn on the LED
const int luxThreshold = 5000;


void setup() {
  Serial.begin(115200);
  Serial1.begin(9600);  // Serial1 is connected to the custom chip
  Wire.begin();

  // Initialize the MPU6050
  mpu.initialize();

  // Initialise Buzzer
  pinMode(buzzer, OUTPUT);

  // Initialize ultrasonic sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialisation for the Keypad locking system
  stored_pswd.pswd[6] = '\0';
  stored_pswd.length = 0;
  entered_pswd.pswd[6] = '\0';
  entered_pswd.length = 0;
  login_count = stored_pswd.pswd[5] == '\0' ? 1 : 0;
  pinMode(loginLED, OUTPUT);

  // Set the pin modes for the LED and LDR
  pinMode(light, INPUT);
  pinMode(led, OUTPUT);

  // Set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
}

void loop() {
  monitorKeypad();
  monitorHeadLights();
  if(crashDetected || (login_count==0 && speed > 10)) {
    getGPSPosition();
  }else {
    getSpeed();
    detectCrash();
  }
}

void convertCoordinatesToCartesian(float latitude, float longitude) {

  float latRadius = latitude  * (PI) / 180;  // Convert from Degrees to Radians
  float lonRadius = longitude * (PI) / 180;

  int earthRadius = 6371; // Radius in km

  float posX = earthRadius * cos(latRadius) * cos(lonRadius);
  float posY = earthRadius * cos(latRadius) * sin(lonRadius);

  Serial.print("        X: ");  Serial.println(posX);
  Serial.print("        Y: ");  Serial.println(posY);
}


void getGPSPosition() {
  while (Serial1.available()) {             // Waits for serial port data

    char serialData = Serial1.read();       // Receives data from GPS serial port
    // Serial.print(serialData);


    if (gps.decode(serialData)) {           // Checks if the GPS sentence is valid
      if (gps.gprmc_status() == 'A') {      // Checks if GPS status is 'A'

        velocidadeGPS.valor = gps.gprmc_speed(KMPH);        // Receives GPS speed in km/h

      } else {
        velocidadeGPS.valor = 0;
      }

      latitude  = gps.gprmc_latitude();
      longitude = gps.gprmc_longitude();

      // Add line break
      // Serial.println();
      // Serial.println();

      // // Show Latitude
      // Serial.print(" Latitude: ");
      // Serial.println(latitude, 8);

      // // Show Longitude
      // Serial.print("Longitude: ");
      // Serial.println(longitude, 8);

      // // Show Speed ​​in km/h
      // Serial.print("    Speed: ");
      // Serial.print(velocidadeGPS.valor);
      // Serial.println(" Km/h");

      Serial.print("GPS Tracking Activated! Location of Cycle:  ");
      Serial.print("https://maps.google.com/?q=");
      Serial.print(latitude, 8);
      Serial.print(",");
      Serial.println(longitude, 8);

      // Converts Geographic Coordinates to Cartesian Plane
      // convertCoordinatesToCartesian(latitude, longitude);
    }
  }
}

void detectCrash() {
  // Get accelerometer readings (acceleration)
  mpu.getAcceleration(&ax, &ay, &az);

  // Convert raw readings to G-force
  float accX = ax / 16384.0;
  float accY = ay / 16384.0;
  float accZ = az / 16384.0;

  // Calculate total acceleration magnitude
  float totalAcc = sqrt(accX * accX + accY * accY + accZ * accZ);

  // Get gyroscope readings (rotation)
  mpu.getRotation(&gx, &gy, &gz);

  // Convert raw gyroscope readings to degrees/second
  float rotX = gx / 131.0;
  float rotY = gy / 131.0;
  float rotZ = gz / 131.0;

  // Check for crash based on either acceleration or rotation
  if ((totalAcc > crashThreshold || abs(rotX) > rotationThreshold || abs(rotY) > rotationThreshold || abs(rotZ) > rotationThreshold) && !crashDetected) {
    Serial.println("Crash Detected!!!");
    crashDetected = true;
    lcd.setCursor(0, 0);
    lcd.print("CRASH DETECTED!!!");
  }

  delay(10);
}

void getSpeed() {
  unsigned long currentTime = millis();  // Get current time in milliseconds
  timeInterval = (currentTime - previousTime) / 1000.0;  // Convert to seconds
  previousTime = currentTime;

  // mpu.getAcceleration(&ax, &ay, &az);
  // Convert raw readings to G-force
  float accX = ax / 16384.0;
  float accY = ay / 16384.0;
  float accZ = az / 16384.0;

  // Calculate acceleration in m/s²
 float acceleration = sqrt(pow(accX, 2) + pow(accY, 2) + pow(accZ, 2));

 // Measure distance using ultrasonic sensor
  digitalWrite(trigPin, LOW);  // Clear the trigPin
  delayMicroseconds(2);
  
  digitalWrite(trigPin, HIGH);  // Set the trigPin high for 10us
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Read the echoPin and calculate the distance
  duration = pulseIn(echoPin, HIGH);  // Get the echo time in microseconds
  distance = (duration * 0.034 / 2);  // Convert time to distance (in cm)

  // Calculate speed using the formula: speed = acceleration * distance * timeInterval
  speed = acceleration * distance * timeInterval;

  // Print the results
  // Serial.print("Simulated Ax: "); Serial.print(ax);
  // Serial.print(" | Acceleration (m/s²): "); Serial.print(acceleration);
  // Serial.print(" | Distance (cm): "); Serial.print(distance);
  // Serial.print(" | Speed (m/s): "); Serial.println(speed);
  if(!crashDetected) {
    // Display speed
    lcd.setCursor(0, 0);
    lcd.print("Speed: ");
    lcd.print(speed, 1);
    lcd.print(" cm/s   ");  // Extra spaces to clear residual text
  }

  if(speed >= speedThreshold) {
    tone(buzzer, 1000);
  } else {
    noTone(buzzer);
  }

  delay(10);
}

void read_password(char digit) {
  if(entered_pswd.length < 6 ) {
    entered_pswd.pswd[entered_pswd.length] =  digit;
    entered_pswd.length += 1;
  } 
  if(entered_pswd.length > 5) {
    if(strcmp(stored_pswd.pswd, entered_pswd.pswd) == 0) {
      Serial.println("Access Granted");
      for(int i=0; i < 6; i++) {
        entered_pswd.pswd[i] = '\0';
      }
      entered_pswd.length = 0;
      login_count++;
    } else {
      Serial.println("Access Denied");
      for(int i=0; i < 6; i++) {
        entered_pswd.pswd[i] = '\0';
      }
      entered_pswd.length = 0;
    }
  }
}

void write_password(char digit) {
  if(stored_pswd.length < 6 ) {
    stored_pswd.pswd[stored_pswd.length] =  digit;
    stored_pswd.length += 1;
  } 
  if(stored_pswd.length > 5) {
    login_count = 0;
    Serial.print("Password Updated Successfully.\nNew Password is: ");
    Serial.println(stored_pswd.pswd);
    mode = 0;
  }
}

void monitorKeypad() {
  char key_pressed = lock.getKey();

  if(key_pressed == '#' && login_count > 0) {
    mode = 1;
    Serial.println("Change Password");
    stored_pswd.length = 0;
    entered_pswd.length = 0;
  }

  if(key_pressed == '*') {
    mode = 0;
    if(stored_pswd.pswd[5] != '\0')
      login_count = 0;
      Serial.println("Log out Successful!");
    stored_pswd.length = 0;
    entered_pswd.length = 0;
  }

  if(mode == 0 && key_pressed != '\0' && key_pressed != '*' && key_pressed != '#') {
    read_password(key_pressed);
  }

  if(mode == 1 && key_pressed != '\0' && key_pressed != '*' && key_pressed != '#') {
    write_password(key_pressed);
  }

  if(login_count > 0) {
    digitalWrite(loginLED, HIGH);;
  } else {
    digitalWrite(loginLED, LOW);
  }

  delay(10);
}

void monitorHeadLights() {
  // Read the analog value from the LDR (0 to 1023)
  int LDR_status = analogRead(light);

  // Convert the analog reading to an approximate lux value
  // Adjust the conversion formula as necessary
  int lux = map(LDR_status, 0, 1023, 10000, 0); // Assuming a range: 0 = dark, 1023 = very bright

  // Check if the lux value is below the threshold
  if (lux < luxThreshold) {
    // If it is dark, turn on the LED
    digitalWrite(led, HIGH);
  } else {
    // If it is bright, turn off the LED
    digitalWrite(led, LOW);
  }
  // Delay to avoid rapid changes in LED state
  delay(10);
}