#include "device.h"
#include "user_misc.h"
// #include "cali.h"
#include "concentration.h"

struct data_lookup_table_s data_lookup_table[4] __attribute__((section(".bss_ccm")));

/* copy data from bitmap in flash to calc area */
void concentration_lookup_table_copy(uint8_t ch)
{
    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return;
    }
    /* move data from const bitmap to data area */
    data_lookup_table[ch].temperature_points = (*bitmap_gas)[ch].lookup_table.temperature_points;
    data_lookup_table[ch].concentration_points = (*bitmap_gas)[ch].lookup_table.concentration_points;
    for (uint8_t i = 0; i < data_lookup_table[ch].temperature_points; i++) {
        data_lookup_table[ch].temperature[i] = (float)((*bitmap_gas)[ch].lookup_table.detector_temperature[i]);
        data_lookup_table[ch].b0[i] = (float)((*bitmap_gas)[ch].lookup_table.b0_value[i]);
    }
    for (uint8_t i = 0; i < data_lookup_table[ch].concentration_points; i++) {
        data_lookup_table[ch].concentration[i] = (float)((*bitmap_gas)[ch].lookup_table.gas_concentration[i]);
    }
    for (uint8_t i = 0; i < data_lookup_table[ch].concentration_points; i++) {
        for (uint8_t j = 0; j < data_lookup_table[ch].temperature_points; j++) {
            data_lookup_table[ch].delta_b[i][j] = (float)((*bitmap_gas)[ch].lookup_table.delta_b_value[i][j]);
        }
    }
}

void concentration_init(void)
{
    for (uint8_t ch = 0; ch < 4; ch++) {
        /* move data from const bitmap to data area */
        concentration_lookup_table_copy(ch);
        /* calc value if cali is active */
        // cali_update_concentration_table(ch);
    }
}

int32_t concentration_neg_drift_compensation(uint8_t ch, int32_t reading)
{
    static uint8_t drift_update_enable[4] = {1, 1, 1, 1};
    static uint8_t neg_reading_num[4] = {0, 0, 0, 0};
    static int32_t neg_reading_sum[4] = {0, 0, 0, 0};
    static int32_t zero_neg_drift[4] = {0, 0, 0, 0};
    static int32_t reading_fixed[4] = {0, 0, 0, 0};
    static uint8_t neg_fixed_num[4] = {0, 0, 0, 0};

    reading_fixed[ch] = reading;
    
    if(drift_update_enable[ch]) {
        if (reading < 0) {
            neg_reading_sum[ch] += reading;
            neg_reading_num[ch] ++;
            reading_fixed[ch] = 0;
            if(neg_reading_num[ch] >= 10) {
                zero_neg_drift[ch] = neg_reading_sum[ch] / neg_reading_num[ch];
                drift_update_enable[ch] = 0;
                neg_reading_num[ch] = 0;
                neg_reading_sum[ch] = 0;
            }
        } else {
            neg_reading_num[ch] = 0;
            neg_reading_sum[ch] = 0;
        }
    }
    if (zero_neg_drift[ch]) {
        reading_fixed[ch] = reading - zero_neg_drift[ch];
        if (reading_fixed[ch] < 0) {
            neg_fixed_num[ch]++;
            if (neg_fixed_num[ch] >= 10) {
                drift_update_enable[ch] = 1;
                neg_fixed_num[ch] = 0;
            }
        } else {
            neg_fixed_num[ch] = 0;
        }
    }

    return ((reading_fixed[ch] > 0) ? reading_fixed[ch] : 0);
}

int32_t concentration_calc(uint8_t ch)
{
    struct data_lookup_table_s *p; /* just for reduce text char in code */
    float temp, temp_quantum;
    float k; /* delta_b = y1 + (y2 - y1) / (x2 - x1) * (temp - x1) */
    float cnt; /* cnt = sig / ref * 10^6 */
    uint8_t idx_t = 0; /* temperature index */
    uint8_t idx_c = 0; /* concentration index */

    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return 0;
    }
    /* step1: initialization variable  */
    p = &data_lookup_table[ch];
    temp = (float)(device_data.detector_temperature);

    /* step2: get temperature index */
    for (idx_t = 0; idx_t < p->temperature_points; idx_t++) {
        if (temp < p->temperature[idx_t]) {
            break;
        }
    }
    if (idx_t == 0) {
        idx_t = 1; /* temperature is less than lowest temperature */
    } else if (idx_t >= p->temperature_points) {
        idx_t = p->temperature_points - 1; /* temperature is more than highest temperature */
    } else {
        /* calc with linear relation */
    }

    /* step3: calc const variable of temperature */
    temp = temp - p->temperature[idx_t - 1];
    temp_quantum = p->temperature[idx_t] - p->temperature[idx_t - 1];

    /* step4: calc b0 value by temperature */
    k = (p->b0[idx_t] - p->b0[idx_t - 1]) / temp_quantum;
    p->calc_b0 = k * temp + p->b0[idx_t - 1];

    /* step5: calc delta_b table value by temperature when different concentration */
    p->calc_b[0] = 0.0f;
    for (idx_c = 1; idx_c < p->concentration_points; idx_c++) {
        k = (p->delta_b[idx_c][idx_t] - p->delta_b[idx_c][idx_t - 1]) / temp_quantum;
        p->calc_b[idx_c] = k * temp + p->delta_b[idx_c][idx_t - 1];
    }

    /* step6: calc delta_cnt */
    cnt = (float)device_data.gas_sig[ch] / (float)device_data.gas_sig[data_gas_info.ref[ch]] * 1000000.0f;
    cnt = p->calc_b0 - cnt;

    /* step7: get concentration index */
    for (idx_c = 0; idx_c < p->concentration_points; idx_c++) {
        if (cnt < p->calc_b[idx_c]) {
            break;
        }
    }
    if (idx_c == 0) {
        idx_c = 1; /* cnt is less than lowest delta_b */
    } else if (idx_c >= p->concentration_points) {
        idx_c = p->concentration_points - 1; /* cnt is more than highest delta_b */
    } else {
        /* calc with linear relation */
    }

    /* step8: calc concentration by cnt */
    k = (p->concentration[idx_c] - p->concentration[idx_c - 1]) / (p->calc_b[idx_c] - p->calc_b[idx_c - 1]);
    temp = k * (cnt - p->calc_b[idx_c - 1]) + p->concentration[idx_c - 1];

    return (int32_t)temp;
}

void concentration_loop(void)
{
    if (flag.filter == flag.concentration) {
        return;
    }
    flag.concentration = flag.filter;
    if ((data_gas_info.comb == GAS_COMB_1_1) || (data_gas_info.comb == GAS_COMB_2_1)) {
        for (uint8_t i = 0; i < 4; i++) {
            if ((data_gas_info.id[i] != GAS_ID_INVALID) && (data_gas_info.id[i] != GAS_ID_REF)) {
                device_data.gas_concentration[i] = concentration_calc(i);
                device_data.gas_neg_concentration[i] = concentration_neg_drift_compensation(i, device_data.gas_concentration[i]);
            } else {
                device_data.gas_concentration[i] = 0;
                device_data.gas_neg_concentration[i] = 0;
            }
        }
    } else if (data_gas_info.comb == GAS_COMB_2_2) {
        for (uint8_t i = 0; i < 4; i++) {
            if ((data_gas_info.id[i] != GAS_ID_INVALID) && (data_gas_info.id[i] != GAS_ID_REF)) {
                device_data.gas_concentration[i] = concentration_calc(i);
                device_data.gas_neg_concentration[i] = concentration_neg_drift_compensation(i, device_data.gas_concentration[i]);
            } else {
                device_data.gas_concentration[i] = 0;
                device_data.gas_neg_concentration[i] = 0;
            }
        }
    } else if (data_gas_info.comb == GAS_COMB_3_1) {
        int32_t sum = 0;
        int32_t conc;
        uint8_t ref = 0;
        for (uint8_t i = 0; i < 4; i++) {
            if ((data_gas_info.id[i] != GAS_ID_INVALID) && (data_gas_info.id[i] != GAS_ID_REF)) {
                conc = concentration_calc(i);
                device_data.gas_concentration[i] = conc;
                device_data.gas_neg_concentration[i] = concentration_neg_drift_compensation(i, device_data.gas_concentration[i]);
                sum += conc;
            } else {
                ref = i;
            }
        }
        device_data.gas_concentration[ref] = sum / 3;
        device_data.gas_neg_concentration[ref] = concentration_neg_drift_compensation(ref, device_data.gas_concentration[ref]);
    } else {
        for (uint8_t i = 0; i < 4; i++) {
            device_data.gas_concentration[i] = 0;
            device_data.gas_neg_concentration[i] = 0;
        }
    }
}
