#include <WiFi.h>
#include <ESP32Servo.h>
#include <NewPing.h>

// WiFi credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// Servo motors
Servo motorFL; // Front-left motor
Servo motorFR; // Front-right motor
Servo motorBL; // Back-left motor
Servo motorBR; // Back-right motor

// Ultrasonic sensor pins
#define TRIG_FRONT 5
#define ECHO_FRONT 18
#define TRIG_BACK 19
#define ECHO_BACK 21

// LED pins
#define WHITE_LED_FRONT 14
#define RED_LED_BACK 27

// Buzzer pin
#define BUZZER_PIN 12

// Ultrasonic sensor parameters
#define MAX_DISTANCE 400 // Max distance for ultrasonic sensors (in cm)
NewPing frontSensor(TRIG_FRONT, ECHO_FRONT, MAX_DISTANCE);
NewPing backSensor(TRIG_BACK, ECHO_BACK, MAX_DISTANCE);

void setup() {
  Serial.begin(9600);
  Serial.print("Connecting to WiFi");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");

  // Attach servos to their pins
  motorFL.attach(32);  // Front-left motor
  motorFR.attach(33);  // Front-right motor
  motorBL.attach(25);  // Back-left motor
  motorBR.attach(26);  // Back-right motor

  // Set LED pins as output
  pinMode(WHITE_LED_FRONT, OUTPUT);
  pinMode(RED_LED_BACK, OUTPUT);

  // Set buzzer pin as output
  pinMode(BUZZER_PIN, OUTPUT);
}

void loop() {
  // Read distance from front sensor
  int distanceFront = frontSensor.ping_cm();
  if (distanceFront > 200 || distanceFront == 0) {
    digitalWrite(WHITE_LED_FRONT, HIGH); // Turn on white LED
    noTone(BUZZER_PIN); // Stop buzzer
  } else {
    digitalWrite(WHITE_LED_FRONT, LOW); // Turn off white LED
    tone(BUZZER_PIN, 1000); // Play sound (1kHz) when front sensor is below 200cm
  }

  if (distanceFront > 0 && distanceFront <= 200) {
    digitalWrite(RED_LED_BACK, HIGH); // Turn on red LED
  } else {
    digitalWrite(RED_LED_BACK, LOW); // Turn off red LED
  }

  // Read distance from back sensor
  int distanceBack = backSensor.ping_cm();
  if (distanceBack > 0 && distanceBack <= 200) {
    digitalWrite(RED_LED_BACK, HIGH); // Turn on red LED
    tone(BUZZER_PIN, 1000); // Play sound (1kHz) when back sensor is below 200cm

    // Move the motors in opposite direction when back sensor detects distance below 200cm
    motorFR.write(0);   // Reverse for right motor
    motorFL.write(180); // Forward for left motor to go in opposite direction

    motorBR.write(0);   // Reverse for back-right motor
    motorBL.write(180); // Forward for back-left motor to go in opposite direction
  } else {
    digitalWrite(RED_LED_BACK, LOW); // Turn off red LED
    noTone(BUZZER_PIN); // Stop buzzer

    // If no object detected in back, move motors forward as default
    motorFR.write(180); // Forward for right motor
    motorFL.write(0);   // Reverse for left motor to go in opposite direction

    motorBR.write(180); // Forward for back-right motor
    motorBL.write(0);   // Reverse for back-left motor to go in opposite direction
  }

  delay(100); // Small delay to stabilize readings
}