const int trigPin = 11;
const int echoPin = 12;

const int RED_LED    = 3;
const int YELLOW_LED = 5;
const int GREEN_LED  = 6;

const float NEAR_TH = 10.0;   // inches
const float MID_TH  = 20.0;   // inches

unsigned long lastSampleMs = 0;
const unsigned long samplePeriodMs = 100; // 10 Hz

float filtered = 30.0;  // start "far"

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

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

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

void loop() {
  unsigned long now = millis();

  if (now - lastSampleMs >= samplePeriodMs) {
    lastSampleMs = now;

    float d = getDistanceInches();

    // ignore bad reads; otherwise smooth a little
    if (d > 0 && d < 200) {
      filtered = 0.8 * filtered + 0.2 * d;
    }

    Serial.print("in: ");
    Serial.println(filtered);

    // one LED on based on distance
    digitalWrite(RED_LED, LOW);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(GREEN_LED, LOW);

    if (filtered <= NEAR_TH) {
      digitalWrite(RED_LED, HIGH);
    } else if (filtered < MID_TH) {
      digitalWrite(YELLOW_LED, HIGH);
    } else {
      digitalWrite(GREEN_LED, HIGH);
    }
  }
}

float getDistanceInches() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  unsigned long echoTime = pulseIn(echoPin, HIGH, 25000); // timeout
  if (echoTime == 0) return -1;

  return echoTime / 148.0; // inches
}