 #include "SmartThings.h"
 
 #define DEBUG_ENABLED      1
+// #define DEBUG_RAW_ENABLED  1
 
 #define PIN_THING_RX       3
 #define PIN_THING_TX       2

void loop() {
  unsigned long time = millis();

  if (time - lastCheck > 2500) {
    lastLedState = !lastLedState;
    
    digitalWrite(PIN_WORKING_LED, lastLedState ? HIGH : LOW);

    int cm = sonar.ping_cm();
    
    if (cm > 0) {
      if (abs(lastDistance - cm) > 1) {
        lastDistance = cm;

#ifdef DEBUG_ENABLED       
        Serial.print("Ping: ");
        Serial.print(cm);
        Serial.println("cm");
#endif        

        if (settings.floorLevel > 0 && settings.safeDepth > 0) {
          int depth = settings.floorLevel - cm;

          if (depth <= settings.warnDepth) {
            smartthing.shieldSetLED(0, 4, 0);
          } else {
            if (depth <= settings.safeDepth) {
              smartthing.shieldSetLED(6, 4, 0); 
            } else {
              smartthing.shieldSetLED(8, 0, 0);               
            }
          }
        }

        snprintf(message, sizeof(message), "level:%d", cm);
        smartthing.send(message);
      }
    }
    

  0
};

int compare_int(const void *a, const void *b) 
{
  return *(const int *)a - *(const int *)b;
}

template <int T> class Queue
{
public:
  int _size;
  int _v[T];
  int _sorted[T];

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

  int size() 
  {
    return _size;  
  }

  bool valid() 
  {
    return size() == T;
  }
  
  void push(int v) 
  {
    if (v > 0) 
    {
      for (int i = 1; i < T; ++i)     
      {
        _v[i - 1] = _v[i];
      }
  
      _v[T - 1] = v;

      ++_size;
      if (_size > T) 
      {
        _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) 
    {
      s += _sorted[i];
    }

    return s / (T - 2 - 2);
  }
};

Queue<12> measurements;
int lastDistance = 0;

int lastLedState = 0;
unsigned long lastCheck = 0;
unsigned long lastUpdate = 0;

char message[MESSAGE_BUF];

