#include <Servo.h>

const int trigPin = 2;
const int echoPin = 3;
const int buzzerPinLeft = 8;
const int buzzerPinRight = 9;
const int servoPin = 10;
const int minDistance = 0;
const int maxDistance = 200;

long duration;
int distance;
Servo myServo;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPinLeft, OUTPUT);
  pinMode(buzzerPinRight, OUTPUT);
  myServo.attach(servoPin);
}

void loop() {
  // Sweep the servo motor from 0 to 180 degrees
  for (int angle = 0; angle <= 180; angle += 10) {
    myServo.write(angle);
    delay(500); // Allow time for the servo to move and the sensor to stabilize

    // Trigger the ultrasonic sensor
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    duration = pulseIn(echoPin, HIGH);
    distance = duration * 0.034 / 2;

    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print(" - Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    if (distance < maxDistance && distance > minDistance) {
      int intensity = map(distance, minDistance, maxDistance, 50, 255);
      if (angle < 90) { // Left side
        for (int i = 0; i < 5; i++) {
          digitalWrite(buzzerPinLeft, HIGH);
          delay(intensity);
          digitalWrite(buzzerPinLeft, LOW);
          delay(intensity);
        }
      } else { // Right side
        for (int i = 0; i < 5; i++) {
          digitalWrite(buzzerPinRight, HIGH);
          delay(intensity);
          digitalWrite(buzzerPinRight, LOW);
          delay(intensity);
        }
      }
    } else {
      digitalWrite(buzzerPinLeft, LOW);
      digitalWrite(buzzerPinRight, LOW);
    }
  }

  // Sweep the servo motor back from 180 to 0 degrees
  for (int angle = 180; angle >= 0; angle -= 10) {
    myServo.write(angle);
    delay(500);

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    duration = pulseIn(echoPin, HIGH);
    distance = duration * 0.034 / 2;

    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print(" - Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    if (distance < maxDistance && distance > minDistance) {
      int intensity = map(distance, minDistance, maxDistance, 50, 255);
      if (angle < 90) {
        for (int i = 0; i < 5; i++) {
          digitalWrite(buzzerPinLeft, HIGH);
          delay(intensity);
          digitalWrite(buzzerPinLeft, LOW);
          delay(intensity);
        }
      } else {
        for (int i = 0; i < 5; i++) {
          digitalWrite(buzzerPinRight, HIGH);
          delay(intensity);
          digitalWrite(buzzerPinRight, LOW);
          delay(intensity);
        }
      }
    } else {
      digitalWrite(buzzerPinLeft, LOW);
      digitalWrite(buzzerPinRight, LOW);
    }
  }

  delay(1000); // Add a delay before the next scan
}