const int trigPin1 = 6;
const int echoPin1 = 5;
const int trigPin2 = 4;
const int echoPin2 = 3;
const int switchPin = 7;
const int buzzerPin = 8;
const int whiteLEDPin = 9;
const int yellowLEDPin = 10;
const int blueLEDPin = 11;
const int greenLEDPin = 12;
const int redLEDPin = 13;//pi definitions for sensors, leds, buzzer, and switch
const int DIST_THRESHOLD = 20;
const int BUZZ_DURATION = 500;
const int LED_BLINK_DURATION = 1000;//thresholds for distance measurements
bool systemOn = false;   //variable to store system status
void setup() {  // Set pins as input or output
  Serial.begin(9600);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(switchPin, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);
  pinMode(whiteLEDPin, OUTPUT);
  pinMode(yellowLEDPin, OUTPUT);
  pinMode(blueLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);
      //setup funtion innitialising pins
  turnOffAllLEDs();
        //turn off all leds
}
void loop() {
  while (digitalRead(switchPin) == LOW) {     //check if switch is pressed
    Serial.println(digitalRead(switchPin));
    systemOn = !systemOn; // Toggle system status
    delay(100); // Debounce delay
  }
  if (systemOn) {  // If the system is on, perform distance measurement and control LEDs and buzzer
    measureAndControl(); // Measure distances and control buzzer and LEDs
  }
}
void measureAndControl() {// Function to measure distances and control buzzer and LEDs
  long distance1, distance2;
  distance1 = measureDistance(trigPin1, echoPin1);
  distance2 = measureDistance(trigPin2, echoPin2);
  Serial.println(" ");
  Serial.print(distance1);
  Serial.print(" ");
  Serial.println(distance2);
  if ((abs(distance1 - 250) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD)) {
    buzzTwice();
    Serial.println("1");
    lightLEDs(distance1, distance2);
  } else  if ((abs(distance1 - 300) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD)) {
    buzzTwice();
    Serial.println("2");
    lightLEDs(distance1, distance2);
  } else  if ((abs(distance1 - 350) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD)) {
    buzzTwice();
    Serial.println("3");
    lightLEDs(distance1, distance2);
  } else  if ((abs(distance1 - 400) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD)) {
    buzzTwice();
    Serial.println("4");
    lightLEDs(distance1, distance2);
  } else {
    buzzOnce();
    Serial.println("0");
  }
}
int measureDistance(int trigPin, int echoPin) {// Function to measure distance for a sensor
  int distance = 0;
  int duration = 0;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
  return distance;
}
void buzzTwice() {// Function to buzz the buzzer twice
  for (int i = 0; i < 2; i++) {
    digitalWrite(buzzerPin, HIGH);
    delay(BUZZ_DURATION);
    digitalWrite(buzzerPin, LOW);
    delay(BUZZ_DURATION);
  }
}
void buzzOnce() {// Function to buzz the buzzer once and blink the pink LED
  digitalWrite(buzzerPin, HIGH);
  delay(BUZZ_DURATION);
  digitalWrite(buzzerPin, LOW);
  digitalWrite(whiteLEDPin, HIGH);
  delay(LED_BLINK_DURATION);
  digitalWrite(whiteLEDPin, LOW);
}
void lightLEDs(long distance1, long distance2) {// Function to control LEDs based on distance measurements
  Serial.println("Deciding LED");
  if (abs(distance1 - 250) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD) {
    blinkLED(redLEDPin);
  } else if (abs(distance1 - 300) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD) {
    blinkLED(greenLEDPin);
  } else if (abs(distance1 - 350) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD) {
    blinkLED(blueLEDPin);
  } else if (abs(distance1 - 400) <= DIST_THRESHOLD && abs(distance2 - 400) <= DIST_THRESHOLD) {
    blinkLED(yellowLEDPin);
  } else {
    blinkLED(whiteLEDPin);
  }
}
void blinkLED(int pin) {// Function to blink an LED
  digitalWrite(pin, HIGH);
  delay(LED_BLINK_DURATION);
  digitalWrite(pin, LOW);
  delay(LED_BLINK_DURATION / 2);
}
void turnOffAllLEDs() {// Function to turn off all LEDs
  digitalWrite(whiteLEDPin, LOW);
  digitalWrite(yellowLEDPin, LOW);
  digitalWrite(blueLEDPin, LOW);
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);
}