#include "sgp40.h"
#include <Arduino.h>

#define SENSOR_DATA_LENGTH 3

/****************************************************************/
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);
}

/****************************************************************/
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;
}