const byte triggerPin = 9, echoPin = 10;
const unsigned long timeoutUs = 25000;
unsigned long measuredAt = 0;
float samples[5];
byte nextSample = 0, sampleCount = 0;

float median() {
  float sorted[5];
  for (byte i = 0; i < sampleCount; ++i) sorted[i] = samples[i];
  for (byte i = 1; i < sampleCount; ++i) {
    float value = sorted[i];
    int j = i - 1;
    while (j >= 0 && sorted[j] > value) {
      sorted[j + 1] = sorted[j]; --j;
    }
    sorted[j + 1] = value;
  }
  return sorted[sampleCount / 2];
}

void setup() {
  pinMode(triggerPin, OUTPUT); digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() {
  const unsigned long now = millis();
  if (now - measuredAt < 100) return;
  measuredAt = now;
  digitalWrite(triggerPin, LOW); delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH); delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  const unsigned long duration = pulseIn(echoPin, HIGH, timeoutUs);
  const float cm = duration * 0.0343f / 2.0f;
  if (duration == 0 || cm < 2.0f || cm > 400.0f) {
    sampleCount = 0; nextSample = 0;
    Serial.println("No valid echo");
    return;
  }
  samples[nextSample] = cm;
  nextSample = (nextSample + 1) % 5;
  if (sampleCount < 5) ++sampleCount;
  Serial.print("cm="); Serial.print(cm, 1);
  Serial.print(" median_cm="); Serial.println(median(), 1);
}