#include<Wire.h>

#define TCA_ADDR 0x38
#define TCA_REG_INPUT_PORT 0x00
#define TCA_REG_OUTPUT_PORT 0x01
#define TCA_POLARITY_INVERSION 0x02
#define TCA_REG_CONFIG 0x03
#define TCA_RDY_PIN 7
#define MODBUS_ADDR 0xfe
#define MODBUS_WRITE 0x41
#define MODBUS_READ 0x44
#define INITIAL_MEASUREMENT 0x10
#define SEQUENTIAL_MEASUREMENT 0x20
#define RX_CONC (3 + 0x9a - 0x80)

void setup() {
    Serial.begin(115200);
    Serial.println("ebin co2 sensor");
    Serial1.begin(9600, SERIAL_8N2);
    Wire.begin();

    byte wirestat;
    
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_OUTPUT_PORT);
    Wire.write(0x00);
    wirestat = Wire.endTransmission();

    Serial.print("Wireop1: ");
    Serial.println(wirestat);
    
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_CONFIG);
    Wire.write(0xee);
    wirestat = Wire.endTransmission();

    Serial.print("Wireop2: ");
    Serial.println(wirestat);

    Serial.println("init done.");
}

void loop() {
    float ppm;
    Serial.println("Measuring CO2");
    ppm = measureCo2();
    Serial.print("result:");
    Serial.println(ppm);
    delay(10000);
    delay(10000);
    
}

int measureCo2(){
    uint8_t tx_packet[33];
    uint8_t rx_packet[49];
    uint8_t rxbuf;
    int16_t result;
    static char i = 0;
    static char j = 0;
    byte wirestat;
    
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_CONFIG);
    Wire.write(0xe8);
    wirestat = Wire.endTransmission();

    Serial.print("State1: ");
    Serial.println(wirestat);
    
    delay(200);
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_CONFIG);
    Wire.write(0xe0);
//    Wire.write(0x80);
    wirestat = Wire.endTransmission();

    Serial.print("State2 : ");
    Serial.println(wirestat);

    Serial.println(getRdyStatus(), HEX);
    
    delay(500);

    Serial.println(getRdyStatus(), HEX);
    
    tx_packet[0] = MODBUS_ADDR;
    tx_packet[1] = MODBUS_WRITE;
    tx_packet[2] = 0x00;
    tx_packet[3] = 0x80;
    tx_packet[4] = 0x01;
    tx_packet[5] = INITIAL_MEASUREMENT;
    tx_packet[6] = 0x28;
    tx_packet[7] = 0x7e;
    Serial1.write(tx_packet, 8);

    delay(100);
    
    while(1){
            rxbuf = Serial1.read();
            rx_packet[j] = rxbuf;
            j++;
            if(j > 49){
                break;
            }
        }
        j = 0;
        Serial.print("resp:");
        while(j<49){
            Serial.print(rx_packet[j], HEX);
            Serial.print(" ");
            j++;
        };
    Serial.println();
    j = 0;
    i = 0;
    Serial.println(getRdyStatus(), HEX);
    delay(500);
    
    while(getRdyStatus() != 1){
        delay(10);
        Serial.print(",");
    }    
        tx_packet[0] = MODBUS_ADDR;
        tx_packet[1] = MODBUS_READ;
        tx_packet[2] = 0x00;
        tx_packet[3] = 0x80;
        tx_packet[4] = 0x2c;
        uint16_t crc16 = calculate_crc16(tx_packet, 5);
        tx_packet[5] = crc16;
        tx_packet[6] = crc16 >> 8;

        Serial1.write(tx_packet, 7);
        i = 0;
        delay(100);
        //while(Serial1.available() > 0){
        while(1){
            rxbuf = Serial1.read();
            rx_packet[j] = rxbuf;
            j++;
            if(j > 49){
                break;
            }
        }
        j = 0;
        Serial.print("recvd:");
        while(j<49){
            Serial.print(rx_packet[j], HEX);
            Serial.print(" ");
            j++;
        };
        Serial.println();
        j = 0;
      

    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_CONFIG);
    Wire.write(0xee);
    wirestat = Wire.endTransmission();
    
    result = rx_packet[RX_CONC] << 8;
    result |= rx_packet[RX_CONC + 1];
    
    return(result);
        

}

uint8_t getRdyStatus(){
    byte state;
    byte rdyval;
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_INPUT_PORT);
    Wire.endTransmission();
    Wire.requestFrom(TCA_ADDR, 1);
    state = Wire.read();
    rdyval = ((state >> (uint8_t) TCA_RDY_PIN) & 0x01);
    return(rdyval);
}

static uint16_t calculate_crc16(uint8_t *buffer, uint8_t length) {
    uint16_t crc16;
    
    for (crc16 = 0xffff; length != 0; length--, buffer++) {
        crc16 ^= *buffer;
        for (int i = 0; i < 8; i++) {
            if ((crc16 & 1) != 0) {
                crc16 >>= 1;
                crc16 ^= 0xa001;
            }
            else {
                crc16 >>= 1;
            }
        }
    }
    return crc16;
}