#include <ESP32Servo.h>

// Pin definitions
const int trigPin = 5;
const int echoPin = 18;
const int redLEDPin = 16;
const int greenLEDPin = 25;
const int blueLEDPin = 17;
const int buzzerPin = 4;
const int servopin = 14;


// Variables
float duration, distance;
Servo myservo;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(servopin);
  pinMode(redLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);
  pinMode(blueLEDPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  myservo.write(90);

  // Start with green LED on and red LED off
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);
  digitalWrite(blueLEDPin, LOW);
  digitalWrite(buzzerPin, LOW);
}

void loop() {
  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Set the trigPin on HIGH state for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);

  // Calculate the distance
  distance = duration * 0.034 / 2;

  // Print the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);

  // Check if the distance is less than a threshold (e.g., 50 cm)
  if (distance < 200) {
    
    digitalWrite(greenLEDPin, HIGH);
    digitalWrite(redLEDPin, HIGH);
    digitalWrite(blueLEDPin, HIGH);
    myservo.write(180);

    // Play a simple melody
    tone(buzzerPin, 1000); // Play tone at 1000 Hz
    delay(200);
    tone(buzzerPin, 1500); // Play tone at 1500 Hz
    delay(200);
    tone(buzzerPin, 2000); // Play tone at 2000 Hz
    delay(200);
    noTone(buzzerPin); // Stop the tone
  } else if (distance < 250) {
  
    digitalWrite(greenLEDPin, HIGH);
    digitalWrite(redLEDPin, HIGH);
    digitalWrite(blueLEDPin, LOW);
    noTone(buzzerPin); // Ensure the buzzer is off
    myservo.write(90);

  } else if (distance < 300) {

    digitalWrite(redLEDPin, HIGH);
    digitalWrite(greenLEDPin, LOW);
    digitalWrite(blueLEDPin, LOW);
    myservo.write(90);
    noTone(buzzerPin);
  }


  // Wait for a short period before the next loop
  delay(500);
}

//MUHAMMAD ASYWAD BIN A MAJID
//10DDT22F2023
//PRACTICAL TEST