#ifdef SENSOR_MHZ19B
#include "sensor_mhz19b.h"



Sensor_MHZ19B::Sensor_MHZ19B(int prx,int ptx)
{
  pin_rx=prx;
  pin_tx=ptx;

  mhz19 = new MHZ19();
  mhz19_swSerial = new SoftwareSerial();

  /*
  * MHZ19 Library: https://platformio.org/lib/show/1620/SevSegSPI
  * Software Serial Library: https://platformio.org/lib/show/168/EspSoftwareSerial

  */
  // SW Serial
  //SW Serial RX: to mhz19 tx (green cable)
  //SW Serial TX: to mhz19 rx (blue cable)
  //co2 sensor needs 5v. Maybe better to Connect USB 5V directly (not through wemos d1 onboard diode which gives only 4.7V! at '5V' output)

  //if ABC is disabled (see in setup function) sensor should be calibrated manually. leave outdoors (=400ppm) with no direct sunlight for >20min, then connect HD pin to GND for at least 7 seconds.
  /* Pinout  (view from top, connector at the bottom)
  * Vin, GND, NC, PWM
  *  |    |    |    |
  * /-----------------\
  * |                 |
  * |                 |
  * |                 |
  * |                 |
  * \-----------------/
  *    |  |  |  |  |
  *   Vo Rx Tx NC HD
  * 
  *       [Connector]
  */

  //value will be in [ppm]
}

void Sensor_MHZ19B::init() //Things to be done during setup()
{    
  Serial.println("initializing MHZ19B");
  mhz19_swSerial->begin(BAUD_RATE_MHZ19, SWSERIAL_8N1, pin_rx, pin_tx, false, 256);
  mhz19->setSerial(mhz19_swSerial);
  
  uint8_t mhz19abctries=10;
  while(!mhz19->disableABC() && mhz19abctries>0) { //disable automatic baseline correction (abc does calibration every 24h -> needs to have 400ppm co2 level sometime during that time)
    delay(500); //wait some time for mhz to be initialized
    Serial.print("disableABC Failed! try="); Serial.println(mhz19abctries);
    mhz19abctries--;
  }
  if (mhz19abctries>0) {
    Serial.println("mhz19 abc disabled successfully");
    init_ok=true;
  }
}

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




//Called during setup
void Sensor_MHZ19B::advertise(HomieNode& p_sensorNode)
{
    sensorNode = &p_sensorNode;
    sensorNode->advertise("co2");
    #ifdef MHZ19CALIBRATIONTOPIC      
      sensorNode->advertise("mhz19calibration").settable(mhz19calibrationHandler);
    #endif
}

void Sensor_MHZ19B::sensorloop()
{
  if (init_ok) {
    sensordata &d=data;
    
    bool _changed=false;
    if (millis() >= (d.lastreadtime+d.readdelay)) {
      mhz19_ready=mhz19->isReady();
      //d.value=mhz19->readValue(); //[ppm]
      d.value=mhz19_readValue_reimplemented(mhz19_swSerial, mhz19); //[ppm]  reimplemented function to fix no response issue
      Homie.getLogger() << "read co2 " << ": " << d.value << " status=" << mhz19_ready << endl;
      if (fabs(d.lastsentvalue-d.value)>=d.minchange){
        _changed=true;
      }
      d.lastreadtime=millis();
    }

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

      Homie.getLogger() << "co2 " << ": " << d.value << endl;
      if (mhz19_ready){ //send no co2 values if not warmed up. can take several miniutes
        sensorNode->setProperty("co2").send(String(d.value));
      }else{
        Homie.getLogger() << "co2 not ready. didnt sent" << endl;
      }
      
      d.lastsentvalue=d.value;
      
      d.lastsent=millis();
    }
  }
}


byte Sensor_MHZ19B::mhz19_getCheckSum(byte* packet) {
  byte checksum = 0;
  for(uint8_t i = 1; i < 8; i++) {
    checksum += packet[i];
  }
  checksum = 0xff - checksum;
  checksum += 1;
  return checksum;
}

int Sensor_MHZ19B::mhz19_readValue_reimplemented(Stream *_streamRef, MHZ19 *_mhz19Ref) { //same function as in mhz19 library from klevytskyi, but with delay between cmd send and response check
  byte CMD_READ[9] = {0xFF, 0x01, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79}; // Read command
  unsigned int co2 = -1;
  unsigned char response[9];
  _streamRef->write(CMD_READ, 9);

  unsigned long _startwait=millis();
  while (millis()-_startwait<100) { //wait for mhz19 to send response
    //wait
  }

  if (_streamRef->available()) {
    _streamRef->readBytes(response, 9);

    byte crc = mhz19_getCheckSum(response);

    if (response[0] == 0xFF && response[1] == CMD_READ[2] && response[8] == crc) {
      unsigned int responseHigh = (unsigned int) response[2];
      unsigned int responseLow = (unsigned int) response[3];
      unsigned int ppm = (256*responseHigh) + responseLow;
      co2 = ppm;
    }
  }

  return co2;
}

void Sensor_MHZ19B::calibrateZero() {
  mhz19->calibrateZero();
}

#endif