const int TRIG_PIN = 4; // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN = 5; // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int LED_PIN  = 3; // Arduino pin connected to LED's pin
const int BUZZER = 6;
const int DISTANCE_THRESHOLD = 200; // centimeters

// variables will change:
float duration_us, distance_cm;
float filterArray[20];
float distance;


void setup() {
  Serial.begin (9600);       // initialize serial port
  pinMode(TRIG_PIN, OUTPUT); // set arduino pin to output mode
  pinMode(ECHO_PIN, INPUT);  // set arduino pin to input mode
  pinMode(LED_PIN, OUTPUT);  // set arduino pin to output mode

}

void loop() {
  for (int sample = 0; sample < 20; sample++){
    filterArray[sample] = ultrasonicMeasure();
    delay(30); // avoid ultrasonic interfering 
  }
  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;
  
  // print the value to Serial Monitor
  Serial.print("distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  
  
}

float ultrasonicMeasure(){
   // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN, HIGH);
  // calculate the distance
  distance_cm = 0.017 * duration_us;

  if(distance_cm > DISTANCE_THRESHOLD){
    digitalWrite(LED_PIN, HIGH); // turn on LED
    tone(6,400,100);
  }
  else{
    digitalWrite(LED_PIN, LOW);  // turn off LED
    noTone(6);
  }

  // print the value to Serial Monitor
  Serial.print("distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  
}