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

  0
};

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

char message[MESSAGE_BUF];


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

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

    measurements.push(sonar.ping());    

    if (measurements.valid()) 
    {
      int cm = round((float)measurements.average() / US_ROUNDTRIP_CM);
      
#ifdef DEBUG_RAW_ENABLED
      Serial.print("Raw: ");
      Serial.print((float)measurements.average() / US_ROUNDTRIP_CM);
      Serial.print("  ::  ");
      for (int i = 0; i < measurements.size(); ++i) 
      {
        Serial.print(measurements._sorted[i]);
        Serial.print(" ");
      }
      Serial.println("");
#endif  

      if (time - lastUpdate > 2500) 
      {
        if (lastDistance != cm) {
          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);
  
          lastUpdate = time;
        }        
      }
    }
    
