#include "sgp40.h"


/****************************************************************/
bool SGP40::Initialize(){
   bool success(true);

   success &= ReadChipID();

   VocAlgorithm_init(&vocAlgorithmParameters);

   if(success) success = WriteSettings();
   
   m_initialized = success;
   return m_initialized;
}


/****************************************************************/
bool SGP40::ReadChipID(){
   uint8_t id = 0;

   //ReadRegister(ID_ADDR, &id, 1);

   switch(id){
      default:
         m_chip_model = ChipModel_UNKNOWN;
         return false;
   }

   return true;
}


/****************************************************************/
bool SGP40::WriteSettings(){
    uint8_t data[3];
    uint8_t ord(0);

    i2c->beginTransmission(SGP40_ADDRESS);
    i2c->write(highByte(REG_MEASURE_TEST));
    i2c->write(lowByte(REG_MEASURE_TEST));
    i2c->endTransmission();

    delay(320);

    i2c->requestFrom(static_cast<uint8_t>(SGP40_ADDRESS), uint8_t(3));

    while(i2c->available()){
        data[ord++] = i2c->read();
    }
    if(ord != 3) return false;
    uint16_t results = (data[0] << 8) | data[1];

    return (results == test_pass);
}


/****************************************************************/
bool SGP40::begin(){
   bool success = Initialize();
   success &= m_initialized;
   return success;
}

/****************************************************************/
bool SGP40::reset(){
   WriteRegister(RESET_ADDR, 0x02);
   delay(10); //max. startup time according to datasheet
   return(begin());
}

/****************************************************************/
bool SGP40::ReadData(uint8_t data[SENSOR_DATA_LENGTH], float RH, float T){
    bool success;
    uint8_t ord(0);

    RH = (RH>100?100: (RH<0?0:RH));
    T =  (T>130?130: (T<-45?-45:T));

    uint16_t RH_ticks = (uint16_t)(RH * 65535 / 100); // Convert RH from %RH to ticks
    uint16_t T_ticks = (uint16_t)((T + 45) * 65535 / 175); // Convert T from DegC to ticks
    
    i2c->beginTransmission(SGP40_ADDRESS);
    i2c->write(highByte(REG_MEASURE_RAW));
    i2c->write(lowByte(REG_MEASURE_RAW));

    i2c->write(highByte(RH_ticks));
    i2c->write(lowByte(RH_ticks));
    i2c->write(CRC8(RH_ticks));

    i2c->write(highByte(T_ticks));
    i2c->write(lowByte(T_ticks));
    i2c->write(CRC8(T_ticks));

    i2c->endTransmission();

    delay(30);

    i2c->requestFrom(SGP40_ADDRESS, SENSOR_DATA_LENGTH);

    while(i2c->available()){
        data[ord++] = i2c->read();
    }

    return success;
}


/****************************************************************/
int32_t SGP40::voc(float RH, float T){
  if(!m_initialized) return 0;
  uint8_t data[SENSOR_DATA_LENGTH];
  ReadData(data, RH, T);
  uint16_t results = (data[0]<<8) | data[1];
  int32_t voci = 0;
  VocAlgorithm_process(&vocAlgorithmParameters, results, &voci);

  return voci;
    
}
void SGP40::read(int32_t& voc, float RH, float T){
   if(!m_initialized) return;
  uint8_t data[SENSOR_DATA_LENGTH];
  ReadData(data, RH, T);
  uint16_t results = (data[0]<<8) | data[1];
  VocAlgorithm_process(&vocAlgorithmParameters, results, &voc);
    return;
}

/****************************************************************/
SGP40::ChipModel SGP40::chipModel(){
   return m_chip_model;
}


/****************************************************************/
bool SGP40::WriteRegister(uint16_t addr, uint8_t data){
  i2c->beginTransmission(SGP40_ADDRESS);
  i2c->write(highByte(addr));
  i2c->write(lowByte(addr));
  i2c->write(data);
  i2c->endTransmission();

  return true;
}


/****************************************************************/
bool SGP40::ReadRegister(uint8_t addr,uint8_t data[],uint8_t length){
  uint8_t ord(0);

  i2c->beginTransmission(SGP40_ADDRESS);
  i2c->write(addr);
  i2c->endTransmission();

  i2c->requestFrom(static_cast<uint8_t>(SGP40_ADDRESS), length);

  while(i2c->available()){
    data[ord++] = i2c->read();
  }

  return ord == length;
}


uint8_t SGP40::CRC8(uint16_t data){
  uint8_t crc = 0xFF;
  crc ^= (data >> 8);
  for (uint8_t i = 0 ; i < 8 ; i++){
    if ((crc & 0x80) != 0) crc = (uint8_t)((crc << 1) ^ 0x31);
    else                   crc <<= 1;
  }
  crc ^= (uint8_t)data;
  for (uint8_t i = 0 ; i < 8 ; i++){
    if ((crc & 0x80) != 0) crc = (uint8_t)((crc << 1) ^ 0x31);
    else                   crc <<= 1;
  }
  return crc; 
}