#include <Servo.h>

// front
const int trigPin1 = 9; 
const int echoPin1 = 10; 

// right
const int trigPin2 = 7;  
const int echoPin2 = 8;  

// left
const int trigPin3 = 5;  
const int echoPin3 = 6;  

// object of class
Servo servoLeft;   
Servo servoRight;  

// Variables distances to store from sensir
long distance1, distance2, distance3;


const int buzzerPin = 13; 


const int obstacleThreshold = 50; 
const int midRangeThreshold = 200; 

void setup() {
  Serial.begin(9600);         
  pinMode(trigPin1, OUTPUT);  // front
  pinMode(echoPin1, INPUT);   

  pinMode(trigPin2, OUTPUT);  // right
  pinMode(echoPin2, INPUT);   

  pinMode(trigPin3, OUTPUT);  // left
  pinMode(echoPin3, INPUT);   

  pinMode(buzzerPin, OUTPUT); 
  
  // Attach servos to respective pins
  servoLeft.attach(11);  
  servoRight.attach(12);  

  // initial servo pos
  servoLeft.write(90);  // L 90 degrees (center position)
  servoRight.write(90); // R 90 degrees (center position)
}
void buttonSos() {

}
void loop() {
  
  int angleLeft = 0;
  while (angleLeft <= 180) {
    // Move the left sensor servo
    servoLeft.write(angleLeft);
    delay(10);              

    // 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++;  
  }

  
  int angleRight = 0;
  while (angleRight <= 180) {
    // Move the right sensor servo
    servoRight.write(angleRight);
    delay(10);               

    // 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++;  
  }

  // Small delay before the next loop
  delay(250);
}

// 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; 
  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); 
  } 
  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);
  }
}

// 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
}