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

struct data_cali_s data_cali_flash __attribute__((section(".bss_ccm")));
struct data_cali_s data_cali_ram __attribute__((section(".bss_ccm")));

uint8_t cali_get_zero_stage(uint8_t stage)
{
    return (stage >> 0) & 0xF;
}

uint8_t cali_get_span_stage(uint8_t stage)
{
    return (stage >> 4) & 0xF;
}

void cali_init(void)
{

}

void cali_update_from_flash(void)
{
    uint32_t *addr;
    uint32_t *data_ram;
    uint32_t *data_flash;

    addr = (uint32_t *)cali_flash_get_info();
    data_ram = (uint32_t *)(&data_cali_ram);
    data_flash = (uint32_t *)(&data_cali_flash);
    for (uint32_t i = 0; i < CALI_FLASH_CALI_SIZE; i += 4) {
        *data_ram = *addr;
        *data_flash = *addr;
        data_ram++;
        data_flash++;
        addr++;
    }
    for (uint8_t ch = 0; ch < 4; ch++) {
        uint8_t stage;
        uint8_t stage_zero, stage_span;

        if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
            if (ch != 0) {
                data_cali_ram.bitmap[ch - 1] = ~0;
                data_cali_ram.stage[ch - 1] = CALI_STAGE_EMPTY;
                data_cali_ram.ref_zero[ch - 1] = 0;
                data_cali_ram.sig_zero[ch - 1] = 0;
                data_cali_ram.ref_span[ch - 1] = 0;
                data_cali_ram.sig_span[ch - 1] = 0;
                data_cali_ram.concentration[ch - 1] = 0;
                data_cali_ram.temperature_zero[ch - 1] = 0;
                data_cali_ram.temperature_span[ch - 1] = 0;
            }
            continue;
        }
        stage = data_cali_ram.stage[ch - 1];
        stage_zero = cali_get_zero_stage(stage);
        stage_span = cali_get_span_stage(stage);
        if ((stage_zero == CALI_STAGE_ACTIVED) && (stage_span == CALI_STAGE_ACTIVED)) {
            data_cali_ram.bitmap[ch - 1] = ~3;
            cali_zero_parse_from_flash(ch);
            cali_zero_update_table(ch);
            cali_span_parse_from_flash(ch);
            cali_span_update_table(ch);
        } else if (stage_zero == CALI_STAGE_ACTIVED) {
            data_cali_ram.bitmap[ch - 1] = ~1;
            cali_zero_parse_from_flash(ch);
            cali_zero_update_table(ch);
        } else {
            data_cali_ram.bitmap[ch - 1] = ~0;
        }
    }
}

void cali_zero_update_table(uint8_t ch)
{
    const struct lookup_table_s * const p = &(*bitmap_gas)[ch].lookup_table;
    float diff = data_cali_ram.zero_b0_diff[ch - 1];

    for (uint8_t i = 0; i < data_lookup_table[ch].temperature_points; i++) {
        data_lookup_table[ch].b0[i] = (float)(p->b0_value[i]) + diff;
    }
}

void cali_span_update_table(uint8_t ch)
{
    const struct lookup_table_s * const p = &(*bitmap_gas)[ch].lookup_table;
    float scale = data_cali_ram.span_b0b_scale[ch - 1];

    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)(p->delta_b_value[i][j]) * scale;
        }
    }
}

/* this function is used when init concentration calc data, update from flash after power on */
// void cali_update_concentration_table(uint8_t ch)
// {
//     uint8_t stage, stage_zero, stage_span;

//     if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
//         return;
//     }
//     stage = data_cali_flash.stage[ch - 1];
//     stage_zero = cali_get_zero_stage(stage);
//     stage_span = cali_get_span_stage(stage);
//     if (stage_zero == CALI_STAGE_ACTIVED) {
//         cali_zero_update_table(ch);
//     }
//     if (stage_span == CALI_STAGE_ACTIVED) {
//         cali_span_update_table(ch);
//     }
// }

float cali_conc_calc_b0_static(uint8_t ch, uint32_t temperature)
{
    const struct lookup_table_s * const p = &(*bitmap_gas)[ch].lookup_table;
    float k; /* delta_b = y1 + (y2 - y1) / (x2 - x1) * (temp - x1) */
    uint8_t idx_t = 0; /* temperature index */
    float b0;

    /* step1: get temperature index */
    for (idx_t = 0; idx_t < p->temperature_points; idx_t++) {
        if (temperature < p->detector_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 */
    }
    /* step2: calc const variable of temperature */
    k = (float)(temperature - p->detector_temperature[idx_t - 1]) / \
        (float)(p->detector_temperature[idx_t] - p->detector_temperature[idx_t - 1]);
    /* step3: calc b0 value by temperature */
    b0 = (float)p->b0_value[idx_t - 1] + \
         k * \
        ((float)(p->b0_value[idx_t]) - \
         (float)(p->b0_value[idx_t - 1]));

    return b0;
}

float cali_conc_calc_b0_dynamic(uint8_t ch, uint32_t temperature)
{
    struct data_lookup_table_s *p; /* just for reduce text char in code */
    float temp; /* use struct data_lookup_table_s, so temperature type is float */
    float k; /* delta_b = y1 + (y2 - y1) / (x2 - x1) * (temp - x1) */
    uint8_t idx_t = 0; /* temperature index */
    float b0;

    /* step1: initialization variable */
    p = &data_lookup_table[ch];
    temp = (float)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 */
    k = (temp - p->temperature[idx_t - 1]) / \
        (p->temperature[idx_t] - p->temperature[idx_t - 1]);
    /* step5: calc b0 value by temperature */
    b0 = p->b0[idx_t - 1] + k * (p->b0[idx_t] - p->b0[idx_t - 1]);

    return b0;
}

uint8_t cali_zero_record(uint8_t ch)
{
    int32_t abs_concentration;
    uint32_t concentration;

    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return 0xFF;
    }
    /* assert: sig and ref count are not 0 */
    if ((device_data.gas_sig[data_gas_info.ref[ch]] == 0) || (device_data.gas_sig[ch] == 0)) {
        return 0x01;
    }
    /* assert: concentration can not exceed drift limit */
    if (device_data.gas_concentration[ch] < 0) {
        abs_concentration = 0 - device_data.gas_concentration[ch];
    } else {
        abs_concentration = device_data.gas_concentration[ch];
    }
    concentration = (uint32_t)abs_concentration;
    if (concentration > (*bitmap_gas)[ch].head.drift_limit) {
        return 0x02;
    }
    /* update struct data by zero record */
    data_cali_ram.temperature_zero[ch - 1] = (uint16_t)device_data.detector_temperature;
    data_cali_ram.ref_zero[ch - 1] = device_data.gas_sig[data_gas_info.ref[ch]];
    data_cali_ram.sig_zero[ch - 1] = device_data.gas_sig[ch];
    data_cali_ram.stage[ch - 1] = CALI_STAGE_RECORDED;
    /* record difference of cali */
    data_cali_ram.zero_b0_diff[ch - 1] = \
    (float)data_cali_ram.sig_zero[ch - 1] / (float)data_cali_ram.ref_zero[ch - 1] * 1000000.0f - \
    cali_conc_calc_b0_static(ch, data_cali_ram.temperature_zero[ch - 1]);

    return 0;
}

/* +-------------+-------+--------+-----------+--------+-------+
   |     b0-b    |  ...  |   t0   |temperature|   t1   |  ...  |
   +-------------+-------+--------+-----------+--------+-------+
   |     ...     |       |        |           |        |       |
   +-------------+-------+--------+-----------+--------+-------+
   |     c0      |       |[t0][c0]|> b0b_c0  <|[t1][c0]|       |
   +-------------+-------+--------+-----\/----+--------+-------+
   |concentration|       |        |   b0b_c   |        |       |
   +-------------+-------+--------+-----/\----+--------+-------+
   |     c1      |       |[t0][c1]|> b0b_c1  <|[t1][c1]|       |
   +-------------+-------+--------+-----------+--------+-------+
   |     ...     |       |        |           |        |       |
   +-------------+-------+--------+-----------+--------+-------+ */
float cali_conc_calc_b0b(uint8_t ch, uint32_t temperature, uint32_t concentration)
{
    const struct lookup_table_s * const p = &(*bitmap_gas)[ch].lookup_table;
    float k; /* delta_b = y1 + (y2 - y1) / (x2 - x1) * (temp - x1) */
    uint8_t idx_t = 0; /* temperature index */
    uint8_t idx_c = 0; /* concentration index */
    float b0b_c0, b0b_c1, b0b_t;

    /* step1: get temperature index */
    for (idx_t = 0; idx_t < p->temperature_points; idx_t++) {
        if (temperature < p->detector_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 */
    }
    /* step2: get concentration index */
    for (idx_c = 0; idx_c < p->concentration_points; idx_c++) {
        if (concentration < p->gas_concentration[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 */
    }
    /* step3: calc const variable of temperature */
    k = (float)(temperature - p->detector_temperature[idx_t - 1]) / \
        (float)(p->detector_temperature[idx_t] - p->detector_temperature[idx_t - 1]);
    /* step4: calc b0b value by temperature */
    b0b_c0 = (float)(p->delta_b_value[idx_c - 1][idx_t - 1]) + \
             k * \
             ((float)(p->delta_b_value[idx_c - 1][idx_t]) - \
             (float)(p->delta_b_value[idx_c - 1][idx_t - 1]));
    b0b_c1 = (float)(p->delta_b_value[idx_c][idx_t - 1]) + \
             k * \
             ((float)(p->delta_b_value[idx_c][idx_t]) - \
             (float)(p->delta_b_value[idx_c][idx_t - 1]));

    /* step5: calc b0b value by concentration */
    b0b_t = b0b_c0 + \
    (float)(concentration - p->gas_concentration[idx_c - 1]) / \
    (float)(p->gas_concentration[idx_c] - p->gas_concentration[idx_c - 1]) * \
    (b0b_c1 - b0b_c0);

    return b0b_t;
}

uint8_t cali_span_record(uint8_t ch, uint32_t conc)
{
    int32_t temp_zero, temp_span, temp_diff;
    float b0b_table, b0b;

    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return 0xFF;
    }
    /* assert: sig and ref count are not 0 */
    if ((device_data.gas_sig[data_gas_info.ref[ch]] == 0) || (device_data.gas_sig[ch] == 0)) {
        return 0x01;
    }
    /* assert: minimun calibration value <= span concentration <= range1 */
    if (conc < (*bitmap_gas)[ch].head.min_cali_value || \
        conc > (*bitmap_gas)[ch].head.range1) {
        return 0x02;
    }
    /* assert: zero calibration data is available */
    if (cali_get_zero_stage(data_cali_flash.stage[ch - 1]) != CALI_STAGE_ACTIVED) {
        return 0x03;
    }
    /* assert: calibration temperature of abs(span  - zero) < 2 degree */
    temp_zero = (int32_t)data_cali_flash.temperature_zero[ch - 1];
    temp_span = (int32_t)device_data.detector_temperature;
    temp_diff = temp_span - temp_zero;
    if (temp_diff > 200 || temp_diff < -200) {
        return 0x03;
    }
    /* assert: 0.5 <= b0b / b0b(in lookup table) <= 5 */
    b0b = (float)device_data.gas_sig[ch] / (float)device_data.gas_sig[data_gas_info.ref[ch]] * 1000000.0f;
    b0b = cali_conc_calc_b0_dynamic(ch, (uint32_t)temp_span) - b0b;
    b0b_table = cali_conc_calc_b0b(ch, (uint32_t)temp_span, conc);
    b0b = b0b / b0b_table;
    if (b0b < 0.5f || b0b > 5.0f) {
        return 0x04;
    }
    /* update struct data by span record */
    data_cali_ram.temperature_span[ch - 1] = (uint16_t)device_data.detector_temperature;
    data_cali_ram.ref_span[ch - 1] = device_data.gas_sig[data_gas_info.ref[ch]];
    data_cali_ram.sig_span[ch - 1] = device_data.gas_sig[ch];
    data_cali_ram.concentration[ch - 1] = conc;
    data_cali_ram.stage[ch - 1] = CALI_STAGE_RECORDED << 4;
    /* record scale of cali */
    data_cali_ram.span_b0b_scale[ch - 1] = b0b;

    return 0;
}

uint8_t cali_zero_activate(uint8_t ch)
{
    int32_t temp_record, temp_active, temp_diff;

    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return 0xFF;
    }
    /* assert: zero data has been recorded */
    if (cali_get_zero_stage(data_cali_ram.stage[ch - 1]) != CALI_STAGE_RECORDED) {
        return 0x01;
    }
    /* assert: active temperature and record temperature must be within 2 degree */
    temp_record = (int32_t)data_cali_ram.temperature_zero[ch - 1];
    temp_active = (int32_t)device_data.detector_temperature;
    temp_diff = temp_active - temp_record;
    if (temp_diff > 200 || temp_diff < -200) {
        return 0x02;
    }
    if (data_cali_flash.word_remain > 0) {
        return 0x03;
    }
    /* update concentration calc data */
    cali_zero_update_table(ch);
    /* clear span data */
    data_cali_ram.span_b0b_scale[ch - 1] = 1.0f;
    cali_span_update_table(ch);
    data_cali_ram.bitmap[ch - 1] = ~1;
    /* copy data from data_cali_ram to data_cali_flash */
    data_cali_flash.ref_zero[ch - 1] = data_cali_ram.ref_zero[ch - 1];
    data_cali_flash.sig_zero[ch - 1] = data_cali_ram.sig_zero[ch - 1];
    data_cali_flash.temperature_zero[ch - 1] = data_cali_ram.temperature_zero[ch - 1];
    data_cali_flash.stage[ch - 1] = CALI_STAGE_ACTIVED; /* bit[3:0] for zero, bit[7:4] for span */
    if (flag_cali_by_button == 0) {
        data_cali_flash.word_remain = CALI_FLASH_WORD_CNT;
    }

    return 0;
}

uint8_t cali_span_activate(uint8_t ch)
{
    int32_t temp_record, temp_active, temp_diff;

    if ((data_gas_info.id[ch] == GAS_ID_INVALID) || (data_gas_info.id[ch] == GAS_ID_REF)) {
        return 0xFF;
    }
    /* assert: span data has been recorded */
    if (cali_get_span_stage(data_cali_ram.stage[ch - 1]) != CALI_STAGE_RECORDED) {
        return 0x01;
    }
    /* assert: active temperature and record temperature must be within 2 degree */
    temp_record = (int32_t)data_cali_ram.temperature_span[ch - 1];
    temp_active = (int32_t)device_data.detector_temperature;
    temp_diff = temp_active - temp_record;
    if (temp_diff > 200 || temp_diff < -200) {
        return 0x02;
    }
    if (data_cali_flash.word_remain > 0) {
        return 0x03;
    }
    /* update concentration calc data */
    cali_span_update_table(ch);
    data_cali_ram.bitmap[ch - 1] = ~3;
    /* copy data from data_cali_ram to data_cali_flash */
    data_cali_flash.ref_span[ch - 1] = data_cali_ram.ref_span[ch - 1];
    data_cali_flash.sig_span[ch - 1] = data_cali_ram.sig_span[ch - 1];
    data_cali_flash.concentration[ch - 1] = data_cali_ram.concentration[ch - 1];
    data_cali_flash.temperature_span[ch - 1] = data_cali_ram.temperature_span[ch - 1];
    data_cali_flash.stage[ch - 1] = CALI_STAGE_ACTIVED | (CALI_STAGE_ACTIVED << 4); /* bit[3:0] for zero, bit[7:4] for span */
    data_cali_flash.word_remain = CALI_FLASH_WORD_CNT;

    return 0;
}

void cali_restore(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 */
    concentration_lookup_table_copy(ch);
    /* set data_cali_ram data */
    data_cali_ram.bitmap[ch - 1] = ~0;
    data_cali_ram.stage[ch - 1] = CALI_STAGE_EMPTY;
    data_cali_ram.ref_zero[ch - 1] = 0;
    data_cali_ram.sig_zero[ch - 1] = 0;
    data_cali_ram.ref_span[ch - 1] = 0;
    data_cali_ram.sig_span[ch - 1] = 0;
    data_cali_ram.concentration[ch - 1] = 0;
    data_cali_ram.temperature_zero[ch - 1] = 0;
    data_cali_ram.temperature_span[ch - 1] = 0;
    data_cali_ram.zero_b0_diff[ch - 1] = 0.0f;
    data_cali_ram.span_b0b_scale[ch - 1] = 1.0f;
    /* set data_cali_flash data */
    data_cali_flash.bitmap[ch - 1] = ~0;
    data_cali_flash.stage[ch - 1] = CALI_STAGE_EMPTY;
    data_cali_flash.ref_zero[ch - 1] = 0;
    data_cali_flash.sig_zero[ch - 1] = 0;
    data_cali_flash.ref_span[ch - 1] = 0;
    data_cali_flash.sig_span[ch - 1] = 0;
    data_cali_flash.concentration[ch - 1] = 0;
    data_cali_flash.temperature_zero[ch - 1] = 0;
    data_cali_flash.temperature_span[ch - 1] = 0;
    if (flag_cali_by_button == 0) {
        data_cali_flash.word_remain = CALI_FLASH_WORD_CNT;
    }
}

void cali_zero_parse_from_flash(uint8_t ch)
{
    data_cali_ram.zero_b0_diff[ch - 1] = \
    (float)data_cali_ram.sig_zero[ch - 1] / (float)data_cali_ram.ref_zero[ch - 1] * 1000000.0f - \
    cali_conc_calc_b0_static(ch, data_cali_ram.temperature_zero[ch - 1]);
}

void cali_span_parse_from_flash(uint8_t ch)
{
    float b0b, b0b_table;

    b0b = (float)data_cali_ram.sig_span[ch - 1] / (float)data_cali_ram.ref_span[ch - 1] * 1000000.0f;
    b0b = cali_conc_calc_b0_dynamic(ch, (uint32_t)data_cali_ram.temperature_span[ch - 1]) - b0b;
    b0b_table = cali_conc_calc_b0b(ch, (uint32_t)data_cali_ram.temperature_span[ch - 1], data_cali_ram.concentration[ch - 1]);
    data_cali_ram.span_b0b_scale[ch - 1] = b0b / b0b_table;
}
