#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() {
  int obstacleDirection = 0; // 0: No obstacle, -1: Left, 1: Right
  
  // 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) {
      obstacleDirection = angle < 90 ? -1 : 1;
    }
  }

  // Determine the direction to move based on the obstacle
  if (obstacleDirection == -1) {
    Serial.println("Obstacle detected on the left. Move right to avoid it.");
    digitalWrite(buzzerPinLeft, HIGH); // Activate left buzzer
    digitalWrite(buzzerPinRight, LOW); // Deactivate right buzzer
  } else if (obstacleDirection == 1) {
    Serial.println("Obstacle detected on the right. Move left to avoid it.");
    digitalWrite(buzzerPinLeft, LOW); // Deactivate left buzzer
    digitalWrite(buzzerPinRight, HIGH); // Activate right buzzer
  } else {
    Serial.println("No obstacle detected.");
    digitalWrite(buzzerPinLeft, LOW); // Deactivate left buzzer
    digitalWrite(buzzerPinRight, LOW); // Deactivate right buzzer
  }

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