#define LED_BUILTIN 16
#define LED_RED 33
#define LED_YELLOW 25
#define LED_GREEN 26
#define LED_WHITE 27
#define LED_HCSR04_TRIGGER 32
#define LED_HCSR04_ECHO 35

#include <Arduino.h>

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_WHITE, OUTPUT);
  pinMode(LED_HCSR04_TRIGGER, OUTPUT);
  pinMode(LED_HCSR04_ECHO, INPUT);
  Serial.begin(115200);
}

long measureDistance() {
  digitalWrite(LED_HCSR04_TRIGGER, LOW);
  delayMicroseconds(2);
  digitalWrite(LED_HCSR04_TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(LED_HCSR04_TRIGGER, LOW);

  long duration = pulseIn(LED_HCSR04_ECHO, HIGH);
  long distance = duration * 0.034 / 2; // Convert time to distance (in cm)
  return distance;
}

void loop() {
  long distance = measureDistance();
  Serial.print("Distance: ");
  Serial.println(distance);

  // Reset all LEDs
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_WHITE, LOW);

  // Light up LEDs based on distance
  if (distance < 10) {
    digitalWrite(LED_RED, HIGH); // Very close
  } else if (distance < 20) {
    digitalWrite(LED_YELLOW, HIGH); // Moderately close
  } else if (distance < 30) {
    digitalWrite(LED_GREEN, HIGH); // Far
  } else {
    digitalWrite(LED_WHITE, HIGH); // Very far
  }

  delay(500); // Delay for readability
}