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

// --- I2C LCD ---
#define I2C_SDA_PIN 21
#define I2C_SCL_PIN 22
LiquidCrystal_I2C lcd(0x27, 16, 2); // Verify your LCD I2C address (0x27 or 0x3F)

// --- ULTRASONIC SENSOR PINS (S1 & S2 for Speed) ---
const int TRIG_S1 = 5;  const int ECHO_S1 = 18; // Sensor 1
const int TRIG_S2 = 19; const int ECHO_S2 = 23; // Sensor 2

// --- CONSTANTS ---
const float SENSOR_DISTANCE_CM = 15.0; // Fixed separation distance (cm)
const int DETECTION_THRESHOLD_CM = 50; // Max distance for object detection

// Time in milliseconds after which timing is reset if S2 is missed
const unsigned long SPEED_TIMEOUT_MS = 500; 

// --- VARIABLES ---
float carSpeed = 0.0;
unsigned long startTime = 0; // Stores time in milliseconds
bool isTimingStarted = false;

// ----------------------------------------------------------------------------
// 🏃 Function: Get Distance (cm)
// ----------------------------------------------------------------------------
float getDistance(int trigPin, int echoPin) {
  // Clear trigger, send pulse, clear trigger
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure time for echo to return (timeout 30000 us)
  long duration = pulseIn(echoPin, HIGH, 30000);
  if (duration == 0) return -1; 

  // Calculate distance in cm (Speed of sound * duration / 2)
  return duration * 0.034 / 2;
}

// ----------------------------------------------------------------------------
// ⏱️ Function: Measure Speed
// ----------------------------------------------------------------------------
void measureSpeed() {
  float distS1 = getDistance(TRIG_S1, ECHO_S1);
  delay(5); // Prevent sonic crosstalk

  // 1. START TIMING: Object hits S1
  if (distS1 > 0 && distS1 < DETECTION_THRESHOLD_CM && !isTimingStarted) {
    startTime = millis(); // START time recorded
    isTimingStarted = true;
  }

  if (isTimingStarted) {
    float distS2 = getDistance(TRIG_S2, ECHO_S2);
    
    // 2. STOP TIMING: Object hits S2
    if (distS2 > 0 && distS2 < DETECTION_THRESHOLD_CM) {
      unsigned long travelTime_ms = millis() - startTime;
      
      // Calculate speed only if time is measurable (>= 1ms)
      if (travelTime_ms >= 1) { 
        // Speed (cm/s) = Distance (cm) / Time (s)
        carSpeed = SENSOR_DISTANCE_CM / (travelTime_ms / 1000.0); 

        // --- Serial Output ---
        Serial.print("Speed = ");
        Serial.print(carSpeed, 2);
        Serial.println(" cm/s");
      }
      
      // Reset immediately after calculation
      isTimingStarted = false;
      startTime = 0;
    }

    // 3. TIMEOUT: Reset if the object missed S2
    if (millis() - startTime > SPEED_TIMEOUT_MS) {
      isTimingStarted = false;
      startTime = 0;
    }
  }
}

// ----------------------------------------------------------------------------
// 🖥️ Function: Update LCD Display
// ----------------------------------------------------------------------------
void updateDisplay() {
  lcd.setCursor(0, 0);
  lcd.print("Speed:          "); 
  lcd.setCursor(7, 0);
  lcd.print(carSpeed, 1);
  lcd.print(" cm/s");

  lcd.setCursor(0, 1);
  lcd.print("Status:         "); // Clear line 2
  lcd.setCursor(0, 1);
  lcd.print(isTimingStarted ? "Timing..." : "Ready");
}

// ----------------------------------------------------------------------------
// ⚙️ Setup
// ----------------------------------------------------------------------------
void setup() {
  Serial.begin(115200);

  // --- LCD Initialization ---
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Ultrasonic Speed");
  lcd.setCursor(0, 1);
  lcd.print("Initialized...");
  delay(1500);

  // --- Sensor Pin Initialization ---
  pinMode(TRIG_S1, OUTPUT); pinMode(ECHO_S1, INPUT);
  pinMode(TRIG_S2, OUTPUT); pinMode(ECHO_S2, INPUT);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("System Ready");
  delay(800);
  lcd.clear();
}

// ----------------------------------------------------------------------------
// 🔁 Loop
// ----------------------------------------------------------------------------
void loop() {
  measureSpeed();
  updateDisplay();
  
  delay(10); 
}