#define TRIG_PIN 9      // Ultrasonic sensor Trigger pin
#define ECHO_PIN 10     // Ultrasonic sensor Echo pin
#define GREEN_LED 3     // Green LED pin (Go)
#define RED_LED 4       // Red LED pin (Stop)

long duration;
int distance;

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  Serial.begin(9600);  // Initialize serial communication
}

void loop() {
  distance = getDistance();  // Get the distance from the ultrasonic sensor
  
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Stop-and-Go logic without blinking
  if (distance < 20) {
    // Obstacle detected: Stop (Red LED ON, Green LED OFF)
    digitalWrite(RED_LED, HIGH);    // Steady ON
    digitalWrite(GREEN_LED, LOW);  // OFF
    Serial.println("Status: STOP (Obstacle detected)");
  } else {
    // Path clear: Go (Green LED ON, Red LED OFF)
    digitalWrite(RED_LED, LOW);    // OFF
    digitalWrite(GREEN_LED, HIGH); // Steady ON
    Serial.println("Status: GO (Path clear)");
  }

  delay(200);  // Short delay to stabilize readings
}

// Function to calculate distance using the ultrasonic sensor
int getDistance() {
  // Send a 10us pulse to the TRIG pin
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read the echo pin, and calculate distance in cm
  duration = pulseIn(ECHO_PIN, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}