#include"bc_co2_module_arduino.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 _BC_LP8_RX_ERROR_STATUS0 (3 + 0xa7 - 0x80)
#define _BC_LP8_RX_ERROR_STATUS1 (3 + 0xa6 - 0x80)
// #define RX_CONC (3 + 0x9a - 0x80) //no pressure correction or noise filtering
// #define RX_CONC (3 + 0x9c - 0x80) //pressure correction no noise filtering
// #define RX_CONC (3 + 0xa8 - 0x80) //no pressure correction, noise filtered
#define RX_CONC (3 + 0xaa - 0x80) //pressure correction and noise filtering

static struct lp8_t sensor;
unsigned long delay_started = 0;
bool delay_running = false;
static bool diag;

bool init_co2_module(long baudrate) {
    if(baudrate > 0) {
        diag = true;
    }
    //init comms
    Serial1.begin(9600, SERIAL_8N2);    //init Serial comm to lp8 sensor, 8data bits 2 stop bits, no parity
    Wire.begin();   //init arduino i2c
    
    reset_module();

    return(true);
}

bool reset_module() {
    sensor.first_measurement_done = false;  //no state writeback
    sensor.calibration_run = false;
    sensor.error=LP8_ERROR_CLEAR;
    sensor.pressure = 10124;
    sensor.valid = false;

    //set appropriate port directions
    tca_set_direction(0x00);
    // tca_set_direction(0x08);
    
    //set proper pins as outputs
    tca_set_port(0xee);
    //charge the cap
    tca_set_port(0xe8);
    //start non blocking delay to charge the cap
    delay_started = millis();
    delay_running = true;

    sensor.state = LP8_STATE_INITIALIZE;
    return(true);
}

static void measure(){
    static char i = 0;
    size_t length;
    uint8_t rxbuf;

    start:
        switch (sensor.state) {
            case LP8_STATE_ERROR:
            {   
                if(diag) {
                    Serial.println(sensor.state);
                    Serial.print("error_state: ");
                    Serial.println(sensor.error);
                }
                reset_module();
                goto start;
            }
            case LP8_STATE_READY:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }

                i = 0;
                tca_set_direction(0x00);
                tca_set_port(0xee);

                sensor.state = LP8_STATE_INITIALIZE;
                goto start;
            }
            case LP8_STATE_INITIALIZE:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }
                //wait 15s to ensure cap is sufficiently charged
                if(delay_running && ((millis() - delay_started) >= 15000)) {
                    delay_running = false;
                    sensor.state = LP8_STATE_PRECHARGE;    
                }
                else if(!delay_running) {
                    sensor.state = LP8_STATE_PRECHARGE;
                }
                else {
                    return;
                }

                goto start;
            }
            case LP8_STATE_PRECHARGE:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }
                tca_set_port(0xe8);
                sensor.state = LP8_STATE_CHARGE;

                goto start;
            }
            case LP8_STATE_CHARGE:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }
                tca_set_port(0xe0);
                sensor.state = LP8_STATE_BOOT;

                goto start;
            }
            case LP8_STATE_BOOT:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }
                while(get_ready_state() != 0) {
                    delay(10);
                    i++;
                    if(i >= 21) {  //210ms wait for ready pin
                        if(diag) {
                            Serial.println("boot_tout!");
                        }
                        sensor.error = LP8_ERROR_BOOT_TIMEOUT;
                        sensor.state = LP8_STATE_ERROR;
                        goto start;
                    }
                }
                i = 0;

                if(sensor.first_measurement_done == false){
                    sensor.tx_buffer[0] = MODBUS_ADDR;
                    sensor.tx_buffer[1] = MODBUS_WRITE;
                    sensor.tx_buffer[2] = 0x00;
                    sensor.tx_buffer[3] = 0x80;
                    sensor.tx_buffer[4] = 0x01;
                    sensor.tx_buffer[5] = INITIAL_MEASUREMENT;
                    sensor.tx_buffer[6] = 0x28;
                    sensor.tx_buffer[7] = 0x7e;
                    if(diag) {
                        Serial.println("initial_measure");
                    }
                    length = 8;
                }
                else {
                    uint16_t crc16;

                    sensor.tx_buffer[0] = MODBUS_ADDR;
                    sensor.tx_buffer[1] = MODBUS_WRITE;
                    sensor.tx_buffer[2] = 0x00;
                    sensor.tx_buffer[3] = 0x80;
                    sensor.tx_buffer[4] = 0x1a;

                    if(sensor.calibration_run == true){
                        sensor.tx_buffer[5] = sensor.calibration;
                        if(diag) {
                           Serial.println("calibration_data");
                        }
                    }
                    else {
                        sensor.tx_buffer[5] = SEQUENTIAL_MEASUREMENT;
                        if(diag) {
                            Serial.println("seq_measure");
                        }
                    }

                    // memcpy(sensor.tx_buffer[6], sensor.sensor_state, 23);
                    while(i < 24){
                        sensor.tx_buffer[6 + i] = sensor.sensor_state[i];
                        i++;
                    }
                    if(diag) {
                        Serial.println("memcpyied");
                    }

                    sensor.tx_buffer[29] = sensor.pressure >> 8;
                    sensor.tx_buffer[30] = sensor.pressure;

                    crc16 = calculate_crc16(sensor.tx_buffer, 31);
                    
                    if(diag) {
                        Serial.println("calc_crc");
                    }

                    sensor.tx_buffer[31] = crc16;
                    sensor.tx_buffer[32] = crc16 >> 8;
                    
                    if(diag) {
                        Serial.println("buffer_made");
                    }

                    i = 0;
                    length = 33;
                }

                Serial1.write(sensor.tx_buffer, length);
                delay(100);  //need time to process transmission

                sensor.state = LP8_STATE_BOOT_READ;
                goto start;
            }
            case LP8_STATE_BOOT_READ:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }

                while(1) {
                    rxbuf = Serial1.read();
                    sensor.rx_buffer[i] = rxbuf;
                    i++;
                    if(rxbuf == 0xff){
                        break;
                    }
                }
                if(sensor.rx_buffer[0] != MODBUS_ADDR) {
                    sensor.error = LP8_ERROR_BOOT_READ_DEVICE_ADDRESS;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if(sensor.rx_buffer[1] != sensor.tx_buffer[1]) {
                    sensor.error = LP8_ERROR_BOOT_READ_COMMAND;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if(calculate_crc16(sensor.rx_buffer, 4) != 0){
                    sensor.error = LP8_ERROR_BOOT_READ_CRC;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                i = 0;

                sensor.state = LP8_STATE_MEASURE;
                goto start;               
            }
            case LP8_STATE_MEASURE:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }

                while(get_ready_state() != 1){
                    delay(10);  //blocking, there is nothing else to do while measuring anyway
                    i++;
                    if(i >= 25){  //timeout after 250ms
                        if(diag) {
                            Serial.println("read_tout!");
                        }
                        sensor.error = LP8_ERROR_MEASURE_SIGNAL_RDY_TIMEOUT;
                        sensor.state = LP8_STATE_ERROR;
                        goto start;
                    }
                }
                i = 0;

                sensor.tx_buffer[0] = MODBUS_ADDR;
                sensor.tx_buffer[1] = MODBUS_READ;
                sensor.tx_buffer[2] = 0x00;
                sensor.tx_buffer[3] = 0x80;
                sensor.tx_buffer[4] = 0x2c;
                uint16_t crc16 = calculate_crc16(sensor.tx_buffer, 5);
                sensor.tx_buffer[5] = crc16;
                sensor.tx_buffer[6] = crc16 >> 8;

                length = 7;

                Serial1.write(sensor.tx_buffer, length);
                delay(100);  //need time to process transmission

                sensor.state = LP8_STATE_MEASURE_READ;
            }
            case LP8_STATE_MEASURE_READ:
            {
                if(diag) {
                    Serial.println(sensor.state);
                }
                while(1) {
                    rxbuf = Serial1.read();
                    sensor.rx_buffer[i] = rxbuf;
                    i++;
                    if(i > 49){
                        break;
                    }
                }
                if(diag) {
                    Serial.println("read_resp");
                }
                i = 0;

                tca_set_port(0xee); //turn off vbb

                if(sensor.rx_buffer[0] != MODBUS_ADDR){
                    sensor.error = LP8_ERROR_MEASURE_READ_DEVICE_ADDRESS;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if(sensor.rx_buffer[1] != sensor.tx_buffer[1]){
                    sensor.error = LP8_ERROR_MEASURE_READ_COMMAND;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if(calculate_crc16(sensor.rx_buffer, 49) != 0){
                    if(diag) {
                        Serial.println("bad_recv_crc");
                    }
                    sensor.error = LP8_ERROR_MEASURE_READ_CRC;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if(diag) {
                    Serial.print("error0: ");
                    Serial.println(sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS0]);
                    Serial.print("error1: ");
                    Serial.println(sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS1]);
                }

                
                if (sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS0] == 32) {
                    sensor.error = LP8_ERROR_MEASURE_READ_OOR;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if (sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS1] == 4) {
                    sensor.error = LP8_ERROR_MEASURE_READ_STATUS1_ADC;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if (sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS1] == 2) {
                    sensor.error = LP8_ERROR_MEASURE_READ_STATUS1_VCAP2;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }
                if (sensor.rx_buffer[_BC_LP8_RX_ERROR_STATUS1] == 1) {
                    sensor.error = LP8_ERROR_MEASURE_READ_STATUS1_VCAP1;
                    sensor.state = LP8_STATE_ERROR;
                    goto start;
                }

                // memcpy(sensor.sensor_state, sensor.rx_buffer[4], 23);
                if(diag) {
                    Serial.println("save_state");
                }
                while(i < 24) {
                    sensor.sensor_state[i] = sensor.rx_buffer[4 + i];
                    i++;
                }

                sensor.first_measurement_done = true;

                sensor.concentration = (int16_t) sensor.rx_buffer[RX_CONC] << 8;
                sensor.concentration |= (int16_t) sensor.rx_buffer[RX_CONC + 1];

                sensor.valid = (sensor.concentration >= 0) && (sensor.concentration < 10000);
                sensor.error = LP8_ERROR_CLEAR;
                // tca_set_port(0xee);
                sensor.state = LP8_STATE_READY;
                return;

            }
            default:
                return;
        }

}

int16_t get_co2_concentration(uint16_t pressure) {
    
    sensor.pressure = pressure;
    measure();

    if(sensor.valid){
        return(sensor.concentration);
    }
    else {
        return(-1);
    }
}

bool tca_set_port(uint8_t port) {

    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_CONFIG);
    Wire.write(port);
    Wire.endTransmission();

    return(true);
}

bool tca_set_direction(uint8_t dir) {
    Wire.beginTransmission(TCA_ADDR);
    Wire.write(TCA_REG_OUTPUT_PORT);
    Wire.write(dir);
    Wire.endTransmission();

    return(true);
}

uint8_t get_ready_state() {
    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;
}