#include <Servo.h>
 
const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;
const int buzzerPin = A2;
const int switchPin = 6;
const int whiteLEDPin = A0;
const int redLEDPin = 13;
const int greenLEDPin = 12;
const int blueLEDPin = 11;
 
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
 
bool systemOn = false;
 
void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(switchPin, INPUT_PULLUP);
  pinMode(whiteLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);
  pinMode(blueLEDPin, OUTPUT);
 
  servo1.attach(7);
  servo2.attach(8);
  servo3.attach(9);
  servo4.attach(10);
 
  digitalWrite(whiteLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(blueLEDPin, LOW);
}
 
void loop() {
  if (digitalRead(switchPin) == LOW) {
    systemOn = !systemOn;
    delay(500); // debounce delay
  }
 
  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;
 
    // Checking tolerance
    if ((abs(distance1 - 400) <= 20 && abs(distance2 - 400) <= 20) ||
        (abs(distance1 - 350) <= 20 && abs(distance2 - 350) <= 20) ||
        (abs(distance1 - 350) <= 20 && abs(distance2 - 300) <= 20) ||
        (abs(distance1 - 300) <= 20 && abs(distance2 - 300) <= 20)) {
      // Object detected within tolerance
      buzzTwice();
      moveServos(distance1, distance2);
    } else {
      // Object detected outside of tolerance
      buzzOnce();
      digitalWrite(whiteLEDPin, HIGH);
      digitalWrite(redLEDPin, HIGH);
      digitalWrite(greenLEDPin, HIGH);
      digitalWrite(blueLEDPin, HIGH);
      delay(3000); // Buzz for 3 seconds
      digitalWrite(whiteLEDPin, LOW);
      digitalWrite(redLEDPin, LOW);
      digitalWrite(greenLEDPin, LOW);
      digitalWrite(blueLEDPin, LOW);
    }
  }
}
 
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);
}

void moveServos(long distance1, long distance2) {
  if (abs(distance1 - 400) <= 20 && abs(distance2 - 400) <= 20) {
    for (int angle = 0; angle <= 180; angle++) {
      servo1.write(angle);
      delay(15);
    }
    digitalWrite(whiteLEDPin, HIGH);
    delay(1000);
    digitalWrite(whiteLEDPin, LOW);
  } else if (abs(distance1 - 350) <= 20 && abs(distance2 - 350) <= 20) {
    for (int angle = 0; angle <= 180; angle++) {
      servo2.write(angle);
      delay(15);
    }
    digitalWrite(redLEDPin, HIGH);
    delay(1000);
    digitalWrite(redLEDPin, LOW);
  } else if (abs(distance1 - 350) <= 20 && abs(distance2 - 300) <= 20) {
    for (int angle = 0; angle <= 180; angle++) {
      servo3.write(angle);
      delay(15);
    }
    digitalWrite(greenLEDPin, HIGH);
    delay(1000);
    digitalWrite(greenLEDPin, LOW);
  } else if (abs(distance1 - 300) <= 20 && abs(distance2 - 300) <= 20) {
    for (int angle = 0; angle <= 180; angle++) {
      servo4.write(angle);
      delay(15);
    }
    digitalWrite(blueLEDPin, HIGH);
    delay(1000);
    digitalWrite(blueLEDPin, LOW);
  }
}