// ESP32 - 4 Ultrasonic Sensors - 3 LEDs

// Front Sensor
#define TRIG_F 5
#define ECHO_F 18

// Back Sensor
#define TRIG_B 17
#define ECHO_B 16

// Left Sensor
#define TRIG_L 4
#define ECHO_L 0

// Right Sensor
#define TRIG_R 2
#define ECHO_R 15

// LEDs
#define GREEN_LED 12
#define YELLOW_LED 13
#define RED_LED 14

long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // timeout 30ms
  long distance = duration * 0.034 / 2;

  return distance;
}

void setup() {
  Serial.begin(9600);

  pinMode(TRIG_F, OUTPUT);
  pinMode(ECHO_F, INPUT);

  pinMode(TRIG_B, OUTPUT);
  pinMode(ECHO_B, INPUT);

  pinMode(TRIG_L, OUTPUT);
  pinMode(ECHO_L, INPUT);

  pinMode(TRIG_R, OUTPUT);
  pinMode(ECHO_R, INPUT);

  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  Serial.println("ESP32 4-Ultrasonic Sensor System Started");
}

void loop() {
  long dFront = getDistance(TRIG_F, ECHO_F);
  long dBack  = getDistance(TRIG_B, ECHO_B);
  long dLeft  = getDistance(TRIG_L, ECHO_L);
  long dRight = getDistance(TRIG_R, ECHO_R);

  Serial.print("Front: "); Serial.print(dFront); Serial.print(" cm | ");
  Serial.print("Back: ");  Serial.print(dBack);  Serial.print(" cm | ");
  Serial.print("Left: ");  Serial.print(dLeft);  Serial.print(" cm | ");
  Serial.print("Right: "); Serial.print(dRight); Serial.println(" cm");

  // Find minimum distance
  long minDist = min(min(dFront, dBack), min(dLeft, dRight));

  if (minDist > 50) {
    digitalWrite(GREEN_LED, HIGH);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(RED_LED, LOW);
  }
  else if (minDist > 20 && minDist <= 50) {
    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, HIGH);
    digitalWrite(RED_LED, LOW);
  }
  else {
    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(RED_LED, HIGH);
  }

  delay(500);
}