#include <LiquidCrystal.h>

// Ultrasonic sensor pins
const int trigPin = A5;  // Trigger pin of the ultrasonic sensor
const int echoPin = 13;  // Echo pin of the ultrasonic sensor

// Variables for ultrasonic sensor
long duration;  // Time taken for the ultrasonic pulse to return
float distance;  // Distance measured by the ultrasonic sensor
float previousDistance = 0;  // Distance measured in the previous loop
unsigned long previousTime = 0;  // Time recorded in the previous loop
float speed = 0;  // Calculated speed of the vehicle

// IR sensor and LED pin definitions
const int ir1Pin = A1;         // IR Sensor 1
const int ir2Pin = A2;         // IR Sensor 2
const int ir3Pin = A3;         // IR Sensor 3
const int ir4Pin = A4;         // IR Sensor 4
const int redLed1Pin = 10;     // Red LED 1 for IR1
const int redLed2Pin = A0;     // Red LED 2 for IR2
const int yellowLed1Pin = 3;   // Yellow LED 1 for IR3
const int yellowLed2Pin = 12;   // Yellow LED 2 for IR4
const int greenLed1Pin = 7;    // Green LED 1 for IR1 and IR2
const int greenLed2Pin = 6;    // Green LED 2 for IR3 and IR4


// Additional LED pin definition (assumed based on setup function)


// Initialize the LCD (assumes a 16x2 display)
// LCD Pin Connections: RS - 11, E - 9, D4 - 4, D5 - 5, D6 - 6, D7 - 7
LiquidCrystal lcd(11, 9, 4, 5, 6, 7);

void setup() {
  // Initialize serial communication for debugging
    Serial.begin(9600);
     
       // Set pin modes for ultrasonic sensor
         pinMode(trigPin, OUTPUT);
           pinMode(echoPin, INPUT);

               
                 


                       // Set pin modes for IR sensors and LEDs
                         pinMode(ir1Pin, INPUT);
                           pinMode(ir2Pin, INPUT);
                             pinMode(ir3Pin, INPUT);
                               pinMode(ir4Pin, INPUT);
                                 pinMode(yellowLed1Pin, OUTPUT);
                                   pinMode(yellowLed2Pin, OUTPUT);
                                     pinMode(redLed1Pin, OUTPUT);
                                       pinMode(redLed2Pin, OUTPUT);
                                         pinMode(greenLed1Pin, OUTPUT);
                                           pinMode(greenLed2Pin, OUTPUT);

                                             // Initialize the LCD
                                               lcd.begin(16, 2);  // Specify the LCD size (16 columns and 2 rows)

                                                 // Optional: Print initial message or clear the screen
                                                   lcd.clear();
                                                     lcd.print("Initializing...");

                                                       // Short delay for initialization message
                                                         delay(1000);
                                                         }

                                                         void loop() {
                                                           // Measure distance with the ultrasonic sensor
                                                             distance = measureDistance();

                                                               // Calculate speed
                                                                 unsigned long currentTime = millis();
                                                                   if (previousTime > 0) { // Avoid division by zero on the first loop
                                                                       speed = calculateSpeed(previousDistance, distance, previousTime, currentTime);
                                                                         }

                                                                           // Update previous distance and time
                                                                             previousDistance = distance;
                                                                               previousTime = currentTime;

                                                                                 // Print speed on the LCD
                                                                                   lcd.clear();  // Clear the previous display
                                                                                     lcd.setCursor(0, 0);  // First row
                                                                                       lcd.print("Speed: ");
                                                                                         lcd.print(speed, 2); // Display speed in m/s with 2 decimal places
                                                                                           lcd.print(" m/s");

                                                                                             // Read IR sensor values
                                                                                               int ir1State = digitalRead(ir1Pin);
                                                                                                 int ir2State = digitalRead(ir2Pin);
                                                                                                   int ir3State = digitalRead(ir3Pin);
                                                                                                     int ir4State = digitalRead(ir4Pin);

                                                                                                       // Debugging output
                                                                                                         Serial.print("IR1: ");
                                                                                                           Serial.print(ir1State);
                                                                                                             Serial.print(" IR2: ");
                                                                                                               Serial.print(ir2State);
                                                                                                                 Serial.print(" IR3: ");
                                                                                                                   Serial.print(ir3State);
                                                                                                                     Serial.print(" IR4: ");
                                                                                                                       Serial.println(ir4State);

                                                                                                                         // Control LEDs based on IR sensor states
                                                                                                                           // Control Yellow LED 1 based on IR1 sensor
                                                                                                                              if (ir1State == HIGH) {
                                                                                                                                  digitalWrite(redLed1Pin, HIGH);  // Turn on red LED 1 for IR1
                                                                                                                                      Serial.println("Red light 1: IR1 detected object");
                                                                                                                                        } else {
                                                                                                                                            digitalWrite(redLed1Pin, LOW);   // Turn off red LED 1 for IR1
                                                                                                                                              }

                                                                                                                                                if (ir2State == HIGH) {
                                                                                                                                                    digitalWrite(redLed2Pin, HIGH);  // Turn on red LED 2 for IR2
                                                                                                                                                        Serial.println("Red light 2: IR2 detected object");
                                                                                                                                                          } else {
                                                                                                                                                              digitalWrite(redLed2Pin, LOW);   // Turn off red LED 2 for IR2
                                                                                                                                                                }

                                                                                                                                                                  // Yellow LEDs
                                                                                                                                                                    if (ir3State == HIGH) {
                                                                                                                                                                        digitalWrite(yellowLed1Pin, HIGH);  // Turn on yellow LED 1 for IR3
                                                                                                                                                                            Serial.println("Yellow light 1: IR3 detected object");
                                                                                                                                                                              } else {
                                                                                                                                                                                  digitalWrite(yellowLed1Pin, LOW);   // Turn off yellow LED 1 for IR3
                                                                                                                                                                                    }

                                                                                                                                                                                      if (ir4State == HIGH) {
                                                                                                                                                                                          digitalWrite(yellowLed2Pin, HIGH);  // Turn on yellow LED 2 for IR4
                                                                                                                                                                                              Serial.println("Yellow light 2: IR4 detected object");
                                                                                                                                                                                                } else {
                                                                                                                                                                                                    digitalWrite(yellowLed2Pin, LOW);   // Turn off yellow LED 2 for IR4
                                                                                                                                                                                                      }

                                                                                                                                                                                                        // Green LEDs
                                                                                                                                                                                                          if (ir1State == LOW && ir2State == LOW) {
                                                                                                                                                                                                              digitalWrite(greenLed1Pin, HIGH);  // Turn on green LED 1 for IR1 and IR2
                                                                                                                                                                                                                } else {
                                                                                                                                                                                                                    digitalWrite(greenLed1Pin, LOW);   // Turn off green LED 1 for IR1 and IR2
                                                                                                                                                                                                                      }
                                                                                                                                                                                                                        
                                                                                                                                                                                                                          if (ir3State == LOW && ir4State == LOW) {
                                                                                                                                                                                                                              digitalWrite(greenLed2Pin, HIGH);  // Turn on green LED 2 for IR3 and IR4
                                                                                                                                                                                                                                } else {
                                                                                                                                                                                                                                    digitalWrite(greenLed2Pin, LOW);   // Turn off green LED 2 for IR3 and IR4
                                                                                                                                                                                                                                      }


                                                                                                                                                                                                                                        delay(100); // Small delay to stabilize readings
                                                                                                                                                                                                                                        }

                                                                                                                                                                                                                                        // Function to measure distance using the ultrasonic sensor
                                                                                                                                                                                                                                        float measureDistance() {
                                                                                                                                                                                                                                          digitalWrite(trigPin, LOW);  // Ensure the trigger pin is low
                                                                                                                                                                                                                                            delayMicroseconds(2);
                                                                                                                                                                                                                                              digitalWrite(trigPin, HIGH);  // Send a 10us high pulse to trigger
                                                                                                                                                                                                                                                delayMicroseconds(10);
                                                                                                                                                                                                                                                  digitalWrite(trigPin, LOW);
                                                                                                                                                                                                                                                    duration = pulseIn(echoPin, HIGH);  // Read the echo pin for the duration of the pulse
                                                                                                                                                                                                                                                      // Calculate the distance (duration in microseconds, speed of sound is 343 m/s)
                                                                                                                                                                                                                                                        float dist = (duration * 0.0343) / 2; // Distance in cm
                                                                                                                                                                                                                                                          return dist;
                                                                                                                                                                                                                                                          }

                                                                                                                                                                                                                                                          // Function to calculate speed
                                                                                                                                                                                                                                                          float calculateSpeed(float previousDistance, float currentDistance, unsigned long previousTime, unsigned long currentTime) {
                                                                                                                                                                                                                                                            float distanceDifference = currentDistance - previousDistance; // Difference in cm
                                                                                                                                                                                                                                                              float timeDifference = (currentTime - previousTime) / 1000.0; // Difference in seconds
                                                                                                                                                                                                                                                                float speed = distanceDifference / timeDifference; // Speed in cm/s
                                                                                                                                                                                                                                                                  return speed / 100.0; // Convert speed to m/s
                                                                                                                                                                                                                                                                  }