const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;
const int switchPin = 6;
const int buzzerPin = 7;
const int orangeLEDPin = 8;
const int yellowLEDPin = 9;
const int blueLEDPin = 10;
const int greenLEDPin = 11;
const int redLEDPin = 12;

bool systemOn = false;

void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(switchPin, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);
  pinMode(orangeLEDPin, OUTPUT);
  pinMode(yellowLEDPin, OUTPUT);
  pinMode(blueLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);

  digitalWrite(orangeLEDPin, LOW);
  digitalWrite(yellowLEDPin, LOW);
  digitalWrite(blueLEDPin, LOW);
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);
}

void loop() {
  if (digitalRead(switchPin) == LOW) {
    systemOn = !systemOn;
    delay(500);
  }

  if (systemOn) {
    long duration1, distance1, duration2, distance2;
    digitalWrite(trigPin1, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin1, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin1, LOW);
    duration1 = pulseIn(echoPin1, HIGH);
    distance1 = duration1 * 0.034 / 2;

    digitalWrite(trigPin2, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin2, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin2, LOW);
    duration2 = pulseIn(echoPin2, HIGH);
    distance2 = duration2 * 0.034 / 2;

    if ((abs(distance1 - 4000) <= 20 && abs(distance2 - 4000) <= 20) ||
        (abs(distance1 - 3500) <= 20 && abs(distance2 - 4000) <= 20) ||
        (abs(distance1 - 3000) <= 20 && abs(distance2 - 4000) <= 20) ||
        (abs(distance1 - 2500) <= 20 && abs(distance2 - 4000) <= 20)) {
      buzzTwice();
      lightLEDs(distance1, distance2);
    } else {
      buzzOnce();
    }
  }
}

void buzzTwice() {
  for (int i = 0; i < 2; i++) {
    digitalWrite(buzzerPin, HIGH);
    delay(500);
    digitalWrite(buzzerPin, LOW);
    delay(500);
  }
}

void buzzOnce() {
  digitalWrite(buzzerPin, HIGH);
  delay(1000);
  digitalWrite(buzzerPin, LOW);
  digitalWrite(orangeLEDPin, HIGH);
  delay(1000);
  digitalWrite(orangeLEDPin, LOW);
}

void lightLEDs(long distance1, long distance2) {
  if (abs(distance1 - 4000) <= 20 && abs(distance2 - 4000) <= 20) {
    digitalWrite(redLEDPin, HIGH);
    delay(1000);
    digitalWrite(redLEDPin, LOW);
    delay(500);
    digitalWrite(redLEDPin, HIGH);
    delay(1000);
    digitalWrite(redLEDPin, LOW);
  } else if (abs(distance1 - 3500) <= 20 && abs(distance2 - 4000) <= 20) {
    digitalWrite(greenLEDPin, HIGH);
    delay(1000);
    digitalWrite(greenLEDPin, LOW);
    delay(500);
    digitalWrite(greenLEDPin, HIGH);
    delay(1000);
    digitalWrite(greenLEDPin, LOW);
  } else if (abs(distance1 - 3000) <= 20 && abs(distance2 - 4000) <= 20) {
    digitalWrite(blueLEDPin, HIGH);
    delay(1000);
    digitalWrite(blueLEDPin, LOW);
    delay(500);
    digitalWrite(blueLEDPin, HIGH);
    delay(1000);
    digitalWrite(blueLEDPin, LOW);
  }else if (abs(distance1 - 2500) <= 20 && abs(distance2 - 4000) <= 20) {
    digitalWrite(yellowLEDPin, HIGH);
    delay(1000);
    digitalWrite(yellowLEDPin, LOW);
    delay(500);
    digitalWrite(yellowLEDPin, HIGH);
    delay(1000);
    digitalWrite(yellowLEDPin, LOW);
  }
}