         _size = T;
       }
     }
-  }
-
-  int average() 
-  {
-    memcpy(&_sorted, &_v, sizeof(_v)); 
-
-    qsort(&_sorted, T, sizeof(int), compare_int);
-
-    int s = 0;
-
-    for (int i = 2; i < T - 2; ++i) 
+
+    update_average();
+  }
+
+  inline int* values() 
+  {
+    return _v;  
+  } 
+
+  void update_average() 
+  {
+    float v = 0;
+    for (int i = 0; i < T; ++i) 
     {
-      s += _sorted[i];
-    }
-
-    return s / (T - 2 - 2);
+      v += _v[i];
+    }
+
+    _average = (int)(v / T);
+  }
+
+  inline int average() 
+  {
+    return _average;
   }
 };
 
-Queue<12> measurements;
-int lastDistance = 0;
+Queue<40> measurements;
+float lastDistance = 0;
 
 int lastLedState = 0;
 unsigned long lastCheck = 0;

public:
  int _size;
  int _v[T];
  int _sorted[T];

public:
  Queue() 
  {
    _size = 0;
    
    memset(&_v, 0, sizeof(_v));
  }

  int size() 
  {
    return _size;  
  }

  void invalidate()
  {
    _size = 0;
  }

  bool valid() 
  {
    return size() == T;
  }

void configure(String& message) {
  sscanf(message.c_str() + 5, "%d %d %d", &settings.floorLevel, &settings.safeDepth, &settings.warnDepth);

#if DEBUG_ENABLED       
  Serial.print("Configure floor level ");
  Serial.print(settings.floorLevel);
  Serial.print(" and safe depth ");
