#include <Servo.h>

// Define pins for the first sensor (Front)
const int trigPin1 = 9;  // Trigger pin for Front Sensor
const int echoPin1 = 10; // Echo pin for Front Sensor

// Define pins for the second sensor (Right)
const int trigPin2 = 7;  // Trigger pin for Right Sensor
const int echoPin2 = 8;  // Echo pin for Right Sensor

// Define pins for the third sensor (Left)
const int trigPin3 = 5;  // Trigger pin for Left Sensor
const int echoPin3 = 6;  // Echo pin for Left Sensor

// Define Servo pins
Servo servoLeft;   // Create servo object for Left sensor
Servo servoRight;  // Create servo object for Right sensor

// Variables to store the distances
long distance1, distance2, distance3;

// Buzzer pin
const int buzzerPin = 13; // Pin connected to the buzzer

// Thresholds for obstacle detection
const int obstacleThreshold = 50; // in cm (critical alert)
const int midRangeThreshold = 200; // in cm (moderate alert)

void setup() {
  Serial.begin(9600);         // Initialize Serial Monitor
  pinMode(trigPin1, OUTPUT);  // Set Front Sensor Trigger pin as output
  pinMode(echoPin1, INPUT);   // Set Front Sensor Echo pin as input
  pinMode(trigPin2, OUTPUT);  // Set Right Sensor Trigger pin as output
  pinMode(echoPin2, INPUT);   // Set Right Sensor Echo pin as input
  pinMode(trigPin3, OUTPUT);  // Set Left Sensor Trigger pin as output
  pinMode(echoPin3, INPUT);   // Set Left Sensor Echo pin as input
  pinMode(buzzerPin, OUTPUT); // Set buzzer pin as output
  
  // Attach servos to respective pins
  servoLeft.attach(11);   // Left sensor servo connected to pin 11
  servoRight.attach(12);  // Right sensor servo connected to pin 12

  // Set the initial positions of the servos (optional)
  servoLeft.write(90);  // Set left servo to 90 degrees (center position)
  servoRight.write(90); // Set right servo to 90 degrees (center position)
}

void loop() {
  // Sweep left servo from 0 to 180 degrees while checking for obstacles
  int angleLeft = 0;
  while (angleLeft <= 180) {
    // Move the left sensor servo
    servoLeft.write(angleLeft);
    delay(10);               // Delay for servo to move

    // Read distances from all sensors once
    distance1 = readDistance(trigPin1, echoPin1); // Front Sensor
    distance2 = readDistance(trigPin2, echoPin2); // Right Sensor
    distance3 = readDistance(trigPin3, echoPin3); // Left Sensor

    // Check for obstacles and provide alert with different beep patterns
    checkObstacle(distance1, "Front");
    checkObstacle(distance2, "Right");
    checkObstacle(distance3, "Left");

    angleLeft++;  // Increment angle for the next position
  }

  // Sweep right servo from 0 to 180 degrees while checking for obstacles
  int angleRight = 0;
  while (angleRight <= 180) {
    // Move the right sensor servo
    servoRight.write(angleRight);
    delay(10);               // Delay for servo to move

    // Read distances from all sensors once
    distance1 = readDistance(trigPin1, echoPin1); // Front Sensor
    distance2 = readDistance(trigPin2, echoPin2); // Right Sensor
    distance3 = readDistance(trigPin3, echoPin3); // Left Sensor

    // Check for obstacles and provide alert with different beep patterns
    checkObstacle(distance1, "Front");
    checkObstacle(distance2, "Right");
    checkObstacle(distance3, "Left");

    angleRight++;  // Increment angle for the next position
  }

  // Small delay before the next loop
  delay(500); // Adjust the delay as needed
}

// Function to read distance using Ultrasonic Sensor
long readDistance(int trigPin, int echoPin) {
  long duration;
  long distance;

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

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // Calculate distance in cm
  return distance;
}

// Function to check obstacle and trigger the appropriate buzzer alert
void checkObstacle(long distance, String sensorName) {
  if (distance <= obstacleThreshold) {
    // Critical obstacle detected
    Serial.print(sensorName);
    Serial.print(" Sensor: ");
    Serial.print(distance);
    Serial.println(" cm - Critical obstacle detected!");
    buzzerAlert(1500); // High-pitched urgent beep for critical obstacles
    delay(300); // Wait for 300ms before the next reading
  } 
  else if (distance <= midRangeThreshold) {
    // Obstacle detected in moderate range
    Serial.print(sensorName);
    Serial.print(" Sensor: ");
    Serial.print(distance);
    Serial.println(" cm - Obstacle detected!");

    // Gradually increase beep frequency as distance decreases
    int frequency = map(distance, obstacleThreshold, midRangeThreshold, 500, 2000);
    buzzerAlert(frequency); // Beep with a frequency based on distance
    delay(300); // Short delay to avoid continuous beeping
  }
}

// Function to alert with buzzer, with frequency control
void buzzerAlert(int frequency) {
  tone(buzzerPin, frequency);  // Generate a tone with the specified frequency
  delay(300);                  // Keep the buzzer on for 300ms (adjustable)
  noTone(buzzerPin);           // Stop the buzzer
}