#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
#define TRIG_PIN 9
#define ECHO_PIN 10

float filterArray[20];
float distance;

void setup(){
  pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
lcd.init();
lcd.backlight();
}

void loop(){
  lcd.clear();
  lcd.print("wait...");
  for (int sample = 0; sample < 20; sample++){
    filterArray[sample] = ultrasonicMeasure();
    delay(50);
  }

  for (int i = 0; i < 19; i++){
    for (int j = i + 1; j < 20; j++){
      if (filterArray[i] > filterArray[j]) {
        float swap = filterArray[i];
        filterArray[i] = filterArray[j];
        filterArray[j] = swap;
      }
    }
  }

  double sum = 0;
  for (int sample = 5; sample < 15; sample++){
    sum += filterArray[sample];
  }

  distance = sum / 10;
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Avg. Distance");
  lcd.setCursor(5, 1);
  lcd.print(distance);
  lcd.print(" cm");
  delay(2000);
}

float ultrasonicMeasure(){
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  float duration_us = pulseIn(ECHO_PIN, HIGH);
  float distance_cm = 0.017 * duration_us;
  return distance_cm;
}