Garbage

/**
 * @file garbage.ino
 * @author Luis Vega (lvega1@uncc.edu)
 * @brief Garage lid with ToF sensors for open lid and send notification
 *        if gargabe is full.
 * @version 0.1
 * @date 2022-10-09
 *
 * @copyright Copyright (c) 2022
 *
 */

#pragma region Garbage project {
  #include <Wire.h>
  #include "SparkFun_VL53L5CX.h"
  #include "Adafruit_VL6180X.h"

  #pragma region NOTIFICATION_FUNCTIONALITY {
    time32_t lastTime = 0; // seconds
    double avg_fill_dist = 0.0;
    SparkFun_VL53L5CX myImager;
    VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

    int imageResolution = 0; //Used to pretty print output
    int imageWidth = 0; //Used to pretty print output

    void WH_Hndlr(const char *event, const char *data);
    void VL53L5CX_ERROR_CB(SF_VL53L5CX_ERROR_TYPE errorCode, uint32_t errorValue);
  #pragma endregion } NOTIFICATION_FUNCTIONALITY
  #pragma region LID_FUNCTIONALITY {
    int  range = 0;
    int status = 0;
    double lux = 0.0;
    Adafruit_VL6180X vl = Adafruit_VL6180X();

    bool manual = false;
    int ds_min = 300; // 290
    int ds_max = 1150; // 1980
    int val;    // variable to read the value from the analog pin
    Servo ds;
  #pragma endregion } LID_FUNCTIONALITY

  void setup() {
    Wire.begin(); //This resets to 100kHz I2C
    Wire.setClock(400000); //Sensor has max I2C freq of 400kHz

    #pragma region NOTIFICATION_FUNCTIONALITY {
      #pragma region SparkFun_VL53L5CX {
        delay(1000);

        myImager.setErrorCallback(VL53L5CX_ERROR_CB);
        while(!myImager.begin()) {
          Particle.publish("ToF-Test", "Failed to find VL53L5CX sensor. Testing address 0x29...", PRIVATE);
          delay(100);
          if(myImager.begin(0x29)) break;
          // Particle.publish("ToF-Test", "failed", PRIVATE);
          delay(5000);
        }


        // while(!myImager.setAddress(0x44));
        // if(myImager.setAddress(0x44) == false) {
        //   Particle.publish("ToF-Test", "Device failed to set new address. Please try again. Freezing...", PRIVATE);
        // };

        delay(500);

        myImager.setResolution(8 * 8); //Enable all 64 pads
        imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8
        imageWidth = sqrt(imageResolution); //Calculate printing width

        myImager.startRanging();
      #pragma endregion } SparkFun_VL53L5CX

      Particle.variable("avg_fill_dist", avg_fill_dist);

      // Subscribe to the integration response event
      Particle.subscribe("hook-response/garbage_full", WH_Hndlr, MY_DEVICES);
    #pragma endregion } NOTIFICATION_FUNCTIONALITY
    #pragma region LID_FUNCTIONALITY {
      #pragma region Adafruit_VL6180X {
        // Serial.println("Adafruit VL6180x test!");
        Particle.publish("ToF-Test", "Adafruit VL6180x test!", PRIVATE);
        // Particle.publish("ToF-Test", PRIVATE);
        while(!vl.begin()) {
          // Serial.println("Failed to find sensor");
          Particle.publish("ToF-Test", "Failed to find VL6180X sensor", PRIVATE);
          delay(1000);
          // while (true) i2cScanner();
        }
        // Serial.println("Sensor found!");
        Particle.publish("ToF-Test", "Sensor found!", PRIVATE);

        pinMode(A1, OUTPUT);
        analogWriteResolution(A1, 12);

        Particle.variable("Lux", lux);
        Particle.variable("range", range);
        Particle.variable("status", status);
      #pragma endregion } Adafruit_VL6180X
      #pragma region SERVO_FUNCTIONS {
        manual = false;

        Particle.function("setValue", setValue);
        Particle.function("setCloseValue", setCloseValue);
        Particle.function("setOpenValue", setOpenValue);
      #pragma endregion } SERVO_FUNCTIONS
    #pragma endregion } LID_FUNCTIONALITY
  }
  void loop() {
    #pragma region NOTIFICATION_FUNCTIONALITY {
      // Poll sensor for new data
      if (myImager.isDataReady() == true)
      {
        if (myImager.getRangingData(&measurementData)) //Read distance data into array
        {
          //The ST library returns the data transposed from zone mapping shown in datasheet
          //Pretty-print data with increasing y, decreasing x to reflect reality
          avg_fill_dist = 0.0;
          for (int y = 0 ; y <= imageWidth * (imageWidth - 1) ; y += imageWidth)
          {
            for (int x = imageWidth - 1 ; x >= 0 ; x--)
            {
              avg_fill_dist += measurementData.distance_mm[x + y];
              // Serial.print("\t");
              // Serial.print(measurementData.distance_mm[x + y]);
              // Serial.print(measurementData.distance_mm[x + y]);
            }
            // Serial.println();
          }
          avg_fill_dist /= 64.0;

          if(avg_fill_dist < 100 ) {
            if(Time.now() - lastTime > 3600) {
              Particle.publish("garbage_full", String(avg_fill_dist), PRIVATE);
              lastTime = Time.now();
            }
          }
          // Serial.println();
        }
      }
    #pragma endregion } NOTIFICATION_FUNCTIONALITY
    #pragma region LID_FUNCTIONALITY {
      lux = vl.readLux(VL6180X_ALS_GAIN_5);

      // Serial.print("Lux: "); Serial.println(lux);
      // Particle.publish("ToF-Test", "Lux: " +  String(lux), PRIVATE);

      range = vl.readRange();
      status = vl.readRangeStatus();

      if(!manual) {
        if (status == VL6180X_ERROR_NONE) {
          // val = map(range, 5, 200, ds_min, ds_max);     // scale it for use with the servo (value between 0 and 180)
          // analogWrite(A1, val, 180);
          if(range < 100) open();

          // Serial.print("Range: "); Serial.println(range);
          // Particle.publish("ToF-Test", "Range: " + String(range), PRIVATE);
        } else {
          analogWrite(A1, ds_max, 180);
        }

        // // Some error occurred, print it out!

        if  ((status >= VL6180X_ERROR_SYSERR_1) && (status <= VL6180X_ERROR_SYSERR_5)) {
          // Serial.println("System error");
          Particle.publish("ToF-Test", "System error", PRIVATE);
        }
        else if (status == VL6180X_ERROR_ECEFAIL) {
          // Serial.println("ECE failure");
          // Particle.publish("ToF-Test", String(range), PRIVATE);
        }
        else if (status == VL6180X_ERROR_NOCONVERGE) {
          // Serial.println("No convergence");
          // Particle.publish("ToF-Test", "ECE failure", PRIVATE);
        }
        else if (status == VL6180X_ERROR_RANGEIGNORE) {
          // Serial.println("Ignoring range");
          Particle.publish("ToF-Test", "Ignoring range", PRIVATE);
        }
        else if (status == VL6180X_ERROR_SNR) {
          // Serial.println("Signal/Noise error");
          Particle.publish("ToF-Test", "Signal/Noise error", PRIVATE);
        }
        else if (status == VL6180X_ERROR_RAWUFLOW) {
          // Serial.println("Raw reading underflow");
          Particle.publish("ToF-Test", "Raw reading underflow", PRIVATE);
        }
        else if (status == VL6180X_ERROR_RAWOFLOW) {
          // Serial.println("Raw reading overflow");
          Particle.publish("ToF-Test", "Raw reading overflow", PRIVATE);
        }
        else if (status == VL6180X_ERROR_RANGEUFLOW) {
          // Serial.println("Range reading underflow");
          Particle.publish("ToF-Test", "Range reading underflow", PRIVATE);
        }
        else if (status == VL6180X_ERROR_RANGEOFLOW) {
          // Serial.println("Range reading overflow");
          Particle.publish("ToF-Test", "Range reading overflow", PRIVATE);
        }
      } else {
        // val = map(range, 0, 200, 0, 1023);     // scale it for use with the servo (value between 0 and 180)
        analogWrite(A1, val, 180);
      }
    #pragma endregion } LID_FUNCTIONALITY

    delay(50);
  }

  #pragma region SERVO_FUNCTIONS {
    int setOpenValue(String value) {
      int temp = value.toInt();

      if(temp >= 290 && temp < ds_max) {
        ds_min = value.toInt();
      } else {
        return -1;
      }

      return 0;
    }
    int setCloseValue(String value) {
      int temp = value.toInt();

      if(temp > ds_min && temp < 1980) {
        ds_max = value.toInt();
      } else {
        return -1;
      }

      return 0;
    }
    int setValue(String value) {
      int temp = value.toInt();

      if(temp >= 0) {
        manual = true;
        val = temp;
      } else manual = false;

      return 0;
    }
    void open() {
      int temp;

      for(int i = 0; i < 1000; i++) {
        temp = map(i, 1000, 0, ds_min, ds_max);     // scale it for use with the servo (value between 0 and 180)
        analogWrite(A1, temp, 180);
        delay(3);
      }
      do
        range = vl.readRange();
      while(range < 100);
      delay(10000);
      for(int i = 999; i >=0; i--) {
        temp = map(i, 1000, 0, ds_min, ds_max);     // scale it for use with the servo (value between 0 and 180)
        analogWrite(A1, temp, 180);
        delay(3);
      }
    }
  #pragma endregion } SERVO_FUNCTIONS
  #pragma region NOTIFICATION_FUNCTIONALITY {
    void VL53L5CX_ERROR_CB(SF_VL53L5CX_ERROR_TYPE errorCode, uint32_t errorValue) {
      switch(errorCode) {
        case SF_VL53L5CX_ERROR_TYPE::NO_ERROR:
          Particle.publish("ToF-Test", "VL53L5CX no error found", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::I2C_INITIALIZATION_ERROR:
          Particle.publish("ToF-Test", "VL53L5CX error: I2C initialization", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::I2C_NOT_RESPONDING:
          Particle.publish("ToF-Test", "VL53L5CX error: I2C not responding", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::DEVICE_INITIALIZATION_ERROR:
          Particle.publish("ToF-Test", "VL53L5CX error: Device initialization", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_DEVICE:
          Particle.publish("ToF-Test",
            "VL53L5CX error: Invalid device (device: " + String(myImager.deviceId) +
            ", revision: " + String(myImager.revisionId) + ")", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_FREQUENCY_SETTING:
          Particle.publish("ToF-Test", "VL53L5CX error: Invalid frequency setting", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_RANGING_MODE:
          Particle.publish("ToF-Test", "VL53L5CX error: Invalid ranging mode", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_CHANGE_I2C_ADDRESS:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot change I2C address", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_START_RANGING:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot start ranging", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_STOP_RANGING:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot stop ranging", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_DATA_READY:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get data ready", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_RESOLUTION:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get resolution", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_RANGING_DATA:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get ranging data", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_RESOLUTION:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot set resolution", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_POWER_MODE:
          Particle.publish("ToF-Test", "VL53L5CX error: cannot set power mode", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_POWER_MODE:
          Particle.publish("ToF-Test", "VL53L5CX error: cannot get power mode", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::DEVICE_NOT_ALIVE:
          Particle.publish("ToF-Test", "VL53L5CX error: Device not alive", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_INTEGRATION_TIME:
          Particle.publish("ToF-Test", "VL53L5CX error: Invalid integration time", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_INTEGRATION_TIME:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot set integration time", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_INTEGRATION_TIME:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get integration time", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_SHARPENER_VALUE:
          Particle.publish("ToF-Test", "VL53L5CX error: Invalid sharpener value", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_SHARPENER_VALUE:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot set sharpener value", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_SHARPENER_VALUE:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get sharpener value", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_TARGET_ORDER:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot set target order", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_TARGET_ORDER:
          Particle.publish("ToF-Test", "VL53L5CX error: Cannot get target order", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::INVALID_TARGET_ORDER:
          Particle.publish("ToF-Test", "VL53L5CX error: Invalid target order", PRIVATE);
          break;
        case SF_VL53L5CX_ERROR_TYPE::UNKNOWN_ERROR:
          Particle.publish("ToF-Test", "VL53L5CX error: Unknown error", PRIVATE);
          break;
        default:
          Particle.publish("ToF-Test", "undefined VL53L5CX error code ", PRIVATE);
      }
    }
    void WH_Hndlr(const char *event, const char *data) {
      // Handle the integration response
    }
  #pragma endregion } NOTIFICATION_FUNCTIONALITY

  void i2cScanner() {
    byte error, address;
    int nDevices;

    Particle.publish("ToF-Test", "Scanning... ", PRIVATE);

    nDevices = 0;
    for(address = 1; address < 127; address++ )
    {
      // The i2c_scanner uses the return value of
      // the Write.endTransmisstion to see if
      // a device did acknowledge to the address.
      Wire.beginTransmission(address);
      error = Wire.endTransmission();

      if (error == 0)
      {
        // Serial.print("I2C device found at address 0x");
        Particle.publish("ToF-Test", "I2C device found at address " + String(address), PRIVATE);
        nDevices++;
      }
      else if (error==4)
      {
        // Serial.print("Unknown error at address 0x");
        Particle.publish("ToF-Test", "Unknown error at address 0x" + String(address), PRIVATE);
      }
    }
    if (nDevices == 0)
      // Serial.println("No I2C devices found\n");
      Particle.publish("ToF-Test", "No I2C devices found.", PRIVATE);
    else
      // Serial.println("done\n");
      Particle.publish("ToF-Test", "done.", PRIVATE);

    delay(5000);           // wait 5 seconds for next scan
  }
#pragma endregion } Garbage project

#pragma region Webhook Test {
  // void myHandler(const char *event, const char *data);
  // void setup() {
  //   // Subscribe to the integration response event
  //   Particle.subscribe("hook-response/garbage_full", myHandler, MY_DEVICES);
  // }


  // void loop() {
  //   // Get some data
  //   String data = String(10);
  //   // Trigger the integration
  //   Particle.publish("garbage_full", data, PRIVATE);
  //   // Wait 60 seconds
  //   delay(60000);
  // }

  // void myHandler(const char *event, const char *data) {
  //   // Handle the integration response
  // }
#pragma endregion } Webhook Test
#pragma region I2C scanner test {
  /**
   * @brief scans for I2C sensors and prints out their address.
   *
   */

  // #include <Wire.h>

  // void setup()
  // {
  //   Wire.begin();

  //   // while (!Serial);             // Leonardo: wait for serial monitor
  //   Particle.publish("ToF-Test", "I2C Scanner ", PRIVATE);
  // }


  // void loop()
  // {
  //   byte error, address;
  //   int nDevices;

  //   Particle.publish("ToF-Test", "Scanning... ", PRIVATE);

  //   nDevices = 0;
  //   for(address = 1; address < 127; address++ )
  //   {
  //     // The i2c_scanner uses the return value of
  //     // the Write.endTransmisstion to see if
  //     // a device did acknowledge to the address.
  //     Wire.beginTransmission(address);
  //     error = Wire.endTransmission();

  //     if (error == 0)
  //     {
  //       // Serial.print("I2C device found at address 0x");
  //       Particle.publish("ToF-Test", "I2C device found at address 0x" + String(address), PRIVATE);
  //       nDevices++;
  //     }
  //     else if (error==4)
  //     {
  //       // Serial.print("Unknown error at address 0x");
  //       Particle.publish("ToF-Test", "Unknown error at address 0x" + String(address), PRIVATE);
  //     }
  //   }
  //   if (nDevices == 0)
  //     // Serial.println("No I2C devices found\n");
  //     Particle.publish("ToF-Test", "No I2C devices found.", PRIVATE);
  //   else
  //     // Serial.println("done\n");
  //     Particle.publish("ToF-Test", "done.", PRIVATE);

  //   delay(5000);           // wait 5 seconds for next scan
  // }
#pragma endregion } I2C scanner test
#pragma region I2C Change Address test {

//   #include <Wire.h>

//   #include <SparkFun_VL53L5CX.h>

//   SparkFun_VL53L5CX myImager;
//   VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

//   int imageResolution = 0; //Used to pretty print output
//   int imageWidth = 0; //Used to pretty print output

//   void VL53L5CX_ERROR_CB(SF_VL53L5CX_ERROR_TYPE errorCode, uint32_t errorValue);

//   void setup() {
//     // Serial.begin(115200);
//     delay(1000);
//     // Serial.println("SparkFun VL53L5CX Imager Example");

//     Wire.begin(); //This resets I2C bus to 100kHz
//     Wire.setClock(1000000); //Sensor has max I2C freq of 1MHz

//     // Serial.println(F("Initializing sensor board. This can take up to 10s. Please wait."));
//     myImager.setErrorCallback(VL53L5CX_ERROR_CB);
//     while(!myImager.begin()) {
//       // Particle.publish("ToF-Test", "Sensor not found - running scanner", PRIVATE);
//       Particle.publish("ToF-Test", "Testing address 0x29...", PRIVATE);
//       delay(100);
//       if(myImager.begin(0x29)) break;
//       Particle.publish("ToF-Test", "failed", PRIVATE);
//       // while(true) delay(5000);
//       delay(5000);
//     }

//     // while (Serial.available()) //Clear incoming key presses
//     //   Serial.read();

//     // Serial.println(F("Press any key to set device address to 0x44"));
//     // int deviceAddress = 0x44; //Valid: 0x08 <= address <= 0x77

//     // while (Serial.available() == false) //Clear incoming key presses
//       ; //Do nothing

//     // while(!myImager.setAddress(deviceAddress));
//     // if(myImager.setAddress(deviceAddress) == false) {
//     //   Particle.publish("ToF-Test", "Device failed to set new address. Please try again. Freezing...", PRIVATE);
//     // };

//     delay(500);

//     // int newAddress = myImager.getAddress();

//     // Serial.print(F("New address is: 0x"));
//     // Serial.println(newAddress, HEX);

//     // Serial.println(F("See advanced I2C example to start sensor with new address."));

//     myImager.setResolution(8 * 8); //Enable all 64 pads
//     imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8
//     imageWidth = sqrt(imageResolution); //Calculate printing width

//     myImager.startRanging();
//   }
//   void loop() {
//     i2cScanner();
//   }

//   void VL53L5CX_ERROR_CB(SF_VL53L5CX_ERROR_TYPE errorCode, uint32_t errorValue) {
//     switch(errorCode) {
//       case SF_VL53L5CX_ERROR_TYPE::NO_ERROR:
//         Particle.publish("ToF-Test", "VL53L5CX no error found", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::I2C_INITIALIZATION_ERROR:
//         Particle.publish("ToF-Test", "VL53L5CX error: I2C initialization", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::I2C_NOT_RESPONDING:
//         Particle.publish("ToF-Test", "VL53L5CX error: I2C not responding", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::DEVICE_INITIALIZATION_ERROR:
//         Particle.publish("ToF-Test", "VL53L5CX error: Device initialization", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_DEVICE:
//         Particle.publish("ToF-Test",
//           "VL53L5CX error: Invalid device (device: " + String(myImager.deviceId) +
//           ", revision: " + String(myImager.revisionId) + ")", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_FREQUENCY_SETTING:
//         Particle.publish("ToF-Test", "VL53L5CX error: Invalid frequency setting", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_RANGING_MODE:
//         Particle.publish("ToF-Test", "VL53L5CX error: Invalid ranging mode", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_CHANGE_I2C_ADDRESS:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot change I2C address", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_START_RANGING:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot start ranging", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_STOP_RANGING:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot stop ranging", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_DATA_READY:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get data ready", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_RESOLUTION:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get resolution", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_RANGING_DATA:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get ranging data", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_RESOLUTION:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot set resolution", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_POWER_MODE:
//         Particle.publish("ToF-Test", "VL53L5CX error: cannot set power mode", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_POWER_MODE:
//         Particle.publish("ToF-Test", "VL53L5CX error: cannot get power mode", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::DEVICE_NOT_ALIVE:
//         Particle.publish("ToF-Test", "VL53L5CX error: Device not alive", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_INTEGRATION_TIME:
//         Particle.publish("ToF-Test", "VL53L5CX error: Invalid integration time", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_INTEGRATION_TIME:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot set integration time", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_INTEGRATION_TIME:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get integration time", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_SHARPENER_VALUE:
//         Particle.publish("ToF-Test", "VL53L5CX error: Invalid sharpener value", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_SHARPENER_VALUE:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot set sharpener value", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_SHARPENER_VALUE:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get sharpener value", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_SET_TARGET_ORDER:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot set target order", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::CANNOT_GET_TARGET_ORDER:
//         Particle.publish("ToF-Test", "VL53L5CX error: Cannot get target order", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::INVALID_TARGET_ORDER:
//         Particle.publish("ToF-Test", "VL53L5CX error: Invalid target order", PRIVATE);
//         break;
//       case SF_VL53L5CX_ERROR_TYPE::UNKNOWN_ERROR:
//         Particle.publish("ToF-Test", "VL53L5CX error: Unknown error", PRIVATE);
//         break;
//       default:
//         Particle.publish("ToF-Test", "undefined VL53L5CX error code ", PRIVATE);
//     }
//   }
//   void i2cScanner() {
//     byte error, address;
//     int nDevices;

//     Particle.publish("ToF-Test", "Scanning... ", PRIVATE);

//     nDevices = 0;
//     for(address = 1; address < 127; address++ )
//     {
//       // The i2c_scanner uses the return value of
//       // the Write.endTransmisstion to see if
//       // a device did acknowledge to the address.
//       Wire.beginTransmission(address);
//       error = Wire.endTransmission();

//       if (error == 0)
//       {
//         // Serial.print("I2C device found at address 0x");
//         Particle.publish("ToF-Test", "I2C device found at address " + String(address), PRIVATE);
//         nDevices++;
//       }
//       else if (error==4)
//       {
//         // Serial.print("Unknown error at address 0x");
//         Particle.publish("ToF-Test", "Unknown error at address 0x" + String(address), PRIVATE);
//       }
//     }
//     if (nDevices == 0)
//       // Serial.println("No I2C devices found\n");
//       Particle.publish("ToF-Test", "No I2C devices found.", PRIVATE);
//     else
//       // Serial.println("done\n");
//       Particle.publish("ToF-Test", "done.", PRIVATE);

//     delay(5000);           // wait 5 seconds for next scan
//   }
#pragma endregion } I2C Change Address test
#pragma region Servor test {
  /**
   * @brief Test servor to make sure is connected properly
   *
   */
  // int val = 0;    // variable to read the value from the analog pin
  // void setup() {
  //   pinMode(A1, OUTPUT);
  //   analogWriteResolution(A1, 10);
  // }

  // void loop() {
  //   val %= 1024;
  //   Particle.publish("Servo-Test", String(val), PRIVATE);
  //   analogWrite(A1, val++, 180);
  //   delay(100);
  // }
#pragma endregion } Servor test