// Pin definitions
const int trigPin = 2;       // Trig pin for ultrasonic sensor
const int echoPin = 3;       // Echo pin for ultrasonic sensor

const int ledRed = 16;       // Red LED pin
const int ledYellow = 17;    // Yellow LED pin
const int ledGreen = 18;     // Green LED pin
const int ledExtra = 19;     // Extra LED pin

const int buttonPin = 15;    // Button pin (GPIO 15)

void setup() {
  // Begin serial communication
  Serial.begin(9600);

  // Set up LED pins as outputs
  pinMode(ledRed, OUTPUT);
  pinMode(ledYellow, OUTPUT);
  pinMode(ledGreen, OUTPUT);
  pinMode(ledExtra, OUTPUT);

  // Set up ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Button as input with internal pull-up resistor enabled
  pinMode(buttonPin, INPUT_PULLUP);  // Use internal pull-up resistor
}

long readDistanceCM() {
  // Send a pulse to the Trig pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the duration from Echo pin
  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout after 30ms
  long distance = duration * 0.034 / 2;  // Calculate distance in cm
  return distance;
}

void loop() {
  // Check if the button is pressed
  if (digitalRead(buttonPin) == LOW) {  // Button is pressed when LOW due to pull-up resistor
    long distance = readDistanceCM();
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    // Reset LEDs
    digitalWrite(ledRed, LOW);
    digitalWrite(ledYellow, LOW);
    digitalWrite(ledGreen, LOW);
    digitalWrite(ledExtra, LOW);

    // Light up LEDs based on distance
    if (distance < 10) {
      digitalWrite(ledRed, HIGH);  // Red LED for distances under 10 cm
    } else if (distance < 20) {
      digitalWrite(ledYellow, HIGH);  // Yellow LED for distances under 20 cm
    } else if (distance < 30) {
      digitalWrite(ledGreen, HIGH);  // Green LED for distances under 30 cm
    } else {
      digitalWrite(ledExtra, HIGH);  // Extra LED for distances over 30 cm
    }

    delay(500); // Small delay to avoid spamming
  } else {
    // If the button is not pressed, turn off all LEDs
    digitalWrite(ledRed, LOW);
    digitalWrite(ledYellow, LOW);
    digitalWrite(ledGreen, LOW);
    digitalWrite(ledExtra, LOW);
  }
}