const int TRIG_PIN = 2;
const int ECHO_PIN = 4;
const int range = 30;

void sort(unsigned long results[]){
  int is_sorted = 0;
  int j = 0;
  while(is_sorted == 0){
    is_sorted = 1;
    for(int i = 0; i < range - j - 1; i++){
      if(results[i] > results[i + 1]){
      unsigned long temp = results[i];
      results[i] = results[i + 1];
      results[i + 1] = temp;
      is_sorted = 0;
      }
    }
    j++;
  }
}

unsigned long trimmed_mean(unsigned long results[]){
  int cutoff_range;
cutoff_range = range * 0.25;
  unsigned long trimmed_mean = 0;
  int sum = 0;
  for(int i = cutoff_range; i < range - cutoff_range; i++){
     sum += results[i];
  }
  trimmed_mean = sum / (range - (2 * cutoff_range));
  return trimmed_mean;
}

void measure(unsigned long results[]){
  for(int i = 0; i < range; i++){
    unsigned long duration, distanceCm;
      digitalWrite(TRIG_PIN, LOW);
      delayMicroseconds(2);
      digitalWrite(TRIG_PIN, HIGH);
      delayMicroseconds(10);
      digitalWrite(TRIG_PIN, LOW);
      duration = pulseIn(ECHO_PIN,HIGH);
      distanceCm = duration / 29.1 / 2 ;
      results[i] = distanceCm;
      delay(2);
     /* Serial.print("\nIteration number ");
      Serial.print(i);
      Serial.print("\nValue ");
      Serial.print(distanceCm); */
    }
}

void setup() {
 // initialize serial communication:
    Serial.begin(9600);
    pinMode(TRIG_PIN,OUTPUT);
    pinMode(ECHO_PIN,INPUT);
}

void loop()
{
    
    unsigned long results[range];
    measure(results);
    sort(results);
    unsigned long truncated_mean;
    truncated_mean = trimmed_mean(results);

     Serial.print("\nMean ");
    Serial.print(truncated_mean);
    /
    if(truncated_mean == 0){
      for(int i = 0; i < range; i++){
      Serial.print("\nElement number ");
      Serial.print(i);
      Serial.print(" is equal to ");
      Serial.print(results[i]);
    }
    Serial.print("\n\n\n");
    delay(3000);
    } */
}