#ifdef SENSOR_VL53L1X
//Connect SCL to D1, SDA to D2, GND and 3v3
#include "sensor_vl53l1x.h"

Sensor_VL53L1X::Sensor_VL53L1X()
{
  vl53l1x = new VL53L1X();
}

void Sensor_VL53L1X::init() //Things to be done during setup()
{    
  Serial.println("initializing vl53l1x");
  vl53l1x->setTimeout(500);
  if (!vl53l1x->init()) {
    Serial.println("No vl53l1x found!");
  }else{
    init_ok=true;
    vl53l1x->setDistanceMode(VL53L1X::Long);
    vl53l1x->setMeasurementTimingBudget(50000);
    vl53l1x->startContinuous(1000); //This period should be at least as long as the timing budget.
  }
}

//Also called during setup()
void Sensor_VL53L1X::setSettings(float minchange, unsigned long senddelaymax, unsigned long readdelay)
{
    data.minchange=minchange;
    data.senddelaymax=senddelaymax;
    data.readdelay=readdelay;
}


//Called during setup
void Sensor_VL53L1X::advertise(HomieNode& p_sensorNode)
{
    sensorNode = &p_sensorNode;
    sensorNode->advertise("tofstatus");
    sensorNode->advertise("tofrange");
    sensorNode->advertise("tofpeaksignal");
    sensorNode->advertise("tofambient");
}

void Sensor_VL53L1X::sensorloop()
{
  if (init_ok) {
    sensordata &d=data;
    
    bool _changed=false;
    if (millis() >= (d.lastreadtime+d.readdelay)) {
      if (millis() >= (lastread_vl53l1x+d.readdelay)) { //avoid reading sensor twice in a short time
        //tcs.getRawData(&value_tcs_r, &value_tcs_g, &value_tcs_b, &value_tcs_c);
        vl53l1x->read();
        lastread_vl53l1x=millis();
      }
      
      d.value=vl53l1x->ranging_data.range_mm;
      
      /* for debugging
      Serial.print("range: ");
      Serial.print(vl53l1x.ranging_data.range_mm);
      Serial.print("\tstatus: ");
      Serial.print(VL53L1X::rangeStatusToString(vl53l1x.ranging_data.range_status));
      Serial.print("\tstatus=");
      Serial.print(vl53l1x.ranging_data.range_status);
      Serial.print("\tpeak signal: ");
      Serial.print(vl53l1x.ranging_data.peak_signal_count_rate_MCPS);
      Serial.print("\tambient: ");
      Serial.print(vl53l1x.ranging_data.ambient_count_rate_MCPS);
      Serial.println();
      */
     //&& VL53L1X::rangeStatusToString(vl53l1x->ranging_data.range_status)=="range valid"
      if (abs((int)d.lastsentvalue-d.value)>=d.minchange ){  //int abs
        //if (vl53l1x->ranging_data.range_status==0) { // "0 is range valid"
          _changed=true;
        //}
      }
      /*if (lastsentvalue_vl53l1x_status!=vl53l1x->ranging_data.range_status) { //sensor status changed
        _changed=true;
      }*/
      d.lastreadtime=millis();
    }

    if (_changed || millis() >= (d.lastsent+d.senddelaymax)) {
      Serial.print("Sending VL53L1X range. reason=");
      if (_changed) Serial.println("change"); else Serial.println("time");

      Homie.getLogger() << "range vl53l1x " << ": " << d.value << endl;
      
      sensorNode->setProperty("tofstatus").send(VL53L1X::rangeStatusToString(vl53l1x->ranging_data.range_status));
      sensorNode->setProperty("tofrange").send(String(d.value));
      sensorNode->setProperty("tofpeaksignal").send(String(vl53l1x->ranging_data.peak_signal_count_rate_MCPS));
      sensorNode->setProperty("tofambient").send(String(vl53l1x->ranging_data.ambient_count_rate_MCPS));
      
      d.lastsentvalue=d.value;
      lastsentvalue_vl53l1x_status=vl53l1x->ranging_data.range_status;
      
      d.lastsent=millis();
    }
  }
}

#endif