#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Keypad Setup
const byte ROW_NUM = 4;
const byte COL_NUM = 4;
char keys[ROW_NUM][COL_NUM] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte pin_rows[ROW_NUM] = {9, 8, 7, 6};
byte pin_column[COL_NUM] = {5, 4, 3, 2};
Keypad keypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COL_NUM);

// Ultrasonic Sensor Setup
#define TRIG_PIN 12
#define ECHO_PIN 11
long duration;
float distance, lastDistance, speed, maxSpeed = 0;
float tripDistance = 0.0;
bool paused = false, showAvgSpeed = false, isCmPerSec = true;
unsigned long lastTime, startTime;

void setup() {
  lcd.begin(16, 2);
  lcd.print("Speedometer");
  delay(1000);
  lcd.clear();

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  lastTime = millis();
  startTime = millis();
}

void loop() {
  if (!paused && millis() - lastTime >= 1000) {  // Update every second
    lastTime = millis();
    
    distance = measureDistance();
    speed = (distance - lastDistance) / 1000;  // Speed in m/s
    if (isCmPerSec) speed *= 100;  // Convert to cm/s if needed
    
    tripDistance += abs(distance - lastDistance) / 100;  // Convert cm to m
    maxSpeed = max(maxSpeed, speed);  // Track maximum speed
    lastDistance = distance;
    
    displaySpeedAndDistance();
  }

  char key = keypad.getKey();
  if (key) handleKeyPress(key);
}

float measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);
  return (duration * 0.0344) / 2;  // Distance in cm
}

void handleKeyPress(char key) {
  lcd.clear();
  switch (key) {
    case '1': displayCurrentSpeed(); break;
    case '2': displayTripDistance(); break;
    case '3': resetReadings(); break;
    case '4': toggleUnits(); break;
    case '5': toggleAverageSpeed(); break;
    case '6': displayMaxSpeed(); break;
    case '7': resetMaxSpeed(); break;
    case '8': togglePauseResume(); break;
    case '9': displayElapsedTime(); break;
    case 'A': resetTripDistance(); break;
    case 'B': setMaxDistanceAlert(); break;
    case 'C': setTargetSpeed(); break;
    case 'D': lcd.clear(); break;
  }
}

void displaySpeedAndDistance() {
  lcd.setCursor(0, 0);
  if (showAvgSpeed) {
    float avgSpeed = tripDistance / ((millis() - startTime) / 1000.0);
    lcd.print("Avg Speed:");
    lcd.print(avgSpeed, 1);
    lcd.print(isCmPerSec ? "cm/s" : "m/s");
  } else {
    lcd.print("Speed:");
    lcd.print(speed, 1);
    lcd.print(isCmPerSec ? "cm/s" : "m/s");
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Trip:");
  lcd.print(tripDistance, 2);
  lcd.print(" m");
}

// Key Functions
void displayCurrentSpeed() {
  lcd.print("Current Speed:");
  lcd.setCursor(0, 1);
  lcd.print(speed, 1);
  lcd.print(isCmPerSec ? " cm/s" : " m/s");
  delay(2000);
}

void displayTripDistance() {
  lcd.print("Trip Distance:");
  lcd.setCursor(0, 1);
  lcd.print(tripDistance, 2);
  lcd.print(" m");
  delay(2000);
}

void resetReadings() {
  speed = 0;
  tripDistance = 0;
  maxSpeed = 0;
  lcd.print("All Readings Reset");
  delay(1000);
}

void toggleUnits() {
  isCmPerSec = !isCmPerSec;
  lcd.print(isCmPerSec ? "Units: cm/s" : "Units: m/s");
  delay(1000);
}

void toggleAverageSpeed() {
  showAvgSpeed = !showAvgSpeed;
  lcd.print(showAvgSpeed ? "Avg Speed Mode" : "Real-Time Mode");
  delay(1000);
}

void displayMaxSpeed() {
  lcd.print("Max Speed:");
  lcd.setCursor(0, 1);
  lcd.print(maxSpeed, 1);
  lcd.print(isCmPerSec ? " cm/s" : " m/s");
  delay(2000);
}

void resetMaxSpeed() {
  maxSpeed = 0;
  lcd.print("Max Speed Reset");
  delay(1000);
}

void togglePauseResume() {
  paused = !paused;
  lcd.print(paused ? "Paused" : "Resumed");
  delay(1000);
}

void displayElapsedTime() {
  unsigned long elapsedTime = (millis() - startTime) / 1000;
  lcd.print("Elapsed Time:");
  lcd.setCursor(0, 1);
  lcd.print(elapsedTime);
  lcd.print(" sec");
  delay(2000);
}

void resetTripDistance() {
  tripDistance = 0.0;
  lcd.print("Trip Reset");
  delay(1000);
}

void setMaxDistanceAlert() {
  lcd.print("Set Dist Alert");
  // Implement the functionality as desired, such as setting an alert distance
  delay(1000);
}

void setTargetSpeed() {
  lcd.print("Set Target Speed");
  // Implement the functionality as desired, such as warning if speed exceeds target
  delay(1000);
}