#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Pin assignments
const int firstIRSensorPin = 2;
const int secondIRSensorPin = 3;
const int servoPin = 9;   // Change to the desired servo control pin
const int greenLEDPin = 10;  // Change to the desired pin for the green LED
const int redLEDPin = 11;    // Change to the desired pin for the red LED

// Distance between the sensors in centimeters
const float distanceBetweenSensors = 10.0;

// Speed calculation variables
unsigned long startTime;
unsigned long endTime;

// Display setup
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Change the address if needed

// Servo setup
Servo myservo;

// LED states
bool greenLEDState = true;  // Initially, green LED is on
bool redLEDState = false;

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

  // Initialize the LCD display
  lcd.begin(16, 2);

  // Set up sensor pins
  pinMode(firstIRSensorPin, INPUT);
  pinMode(secondIRSensorPin, INPUT);

  // Attach the servo to its pin
  myservo.attach(servoPin);

  // Set up LED pins
  pinMode(greenLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);

  // Initially, turn on the green LED and turn off the red LED
  digitalWrite(greenLEDPin, HIGH);
  digitalWrite(redLEDPin, LOW);
  lcd.clear();
}

void loop() {
  // Step 1: Waiting for an object to pass by the first IR sensor
  while (digitalRead(firstIRSensorPin) == HIGH) {
    // Wait until the first sensor is triggered
  }

  // Step 2: Object detected by the first sensor, now wait for the second sensor
  while (digitalRead(secondIRSensorPin) == LOW) {
    // Wait until the second sensor is triggered
  }

  // Step 3: Object passed both sensors, calculate and display speed
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Calculating speed");

  startTime = millis(); // Record the time when the object passes the second sensor

  while (digitalRead(secondIRSensorPin) == HIGH) {
    // Wait until the object moves past the second sensor
  }

  endTime = millis(); // Record the time when the object passes the second sensor

  // Calculate speed in km/h
  float timeInSeconds = (endTime - startTime) / 1000.0;  // Convert time to seconds
  float speed = (distanceBetweenSensors / timeInSeconds) * 3.6;  // Convert speed to km/h

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Speed: ");
  lcd.print(speed, 2);  // Display speed with 2 decimal places
  lcd.print(" km/h");

  // Step 4: Check if speed is more than 10 km/h and actuate the servo
  if (speed > 10.0) {
    lcd.setCursor(0, 1);
    lcd.print("High Speed!");

    // Turn off the green LED and turn on the red LED
    digitalWrite(greenLEDPin, LOW);
    digitalWrite(redLEDPin, HIGH);

    // Move the servo to a specific angle (adjust as needed)
    myservo.write(0);  // 0 degrees is just an example, adjust according to your servo's requirements
    delay(5000);  // Keep the servo in the actuated position for 5 seconds (adjust as needed)

    // Return the servo to its initial position
    myservo.write(90);  // 90 degrees is just an example, adjust according to your servo's requirements

    // Turn off the red LED and turn on the green LED
    digitalWrite(greenLEDPin, HIGH);
    digitalWrite(redLEDPin, LOW);
  }

  // Step 5: Wait for 1 second before restarting the process
  delay(1000);

  // Turn off the display at the end of the program
  lcd.clear();
  lcd.noBacklight();
  lcd.noDisplay();
  delay(1000);  // Delay to make sure the display turns off before the program ends

  // Reinitialize the display for the next iteration
  lcd.backlight();
  lcd.display();
}