#include "filter.h"
#include "ltc1867.h"
#include "calc.h"
#include "device.h"
#include "debug.h"
#include "cli.h"
#include "user_misc.h"

static uint8_t filter_pool[32 * 1024] __attribute__((section(".bss_ccm"))) __attribute__((aligned(4)));
struct data_filter_s data_filter[4] __attribute__((section(".bss_ccm")));

const uint16_t median_length[4] = {1024, 128, 128, 128};
const uint16_t window_length[4] = {256, 64, 64, 64};

void filter_init(void)
{
    uint32_t length;

    /* get data_filter length init parameters */
    for (uint8_t i = 0; i < 4; i++) {
        data_filter[i].median_len = bitmap_main->filter_median[i];
        data_filter[i].window_len = bitmap_main->filter_window[i];
    }
    /* check whether pool is enough */
    length = 0;
    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == GAS_ID_INVALID) {
            continue;
        } else {
            data_filter[i].median_buff = (struct list_s *)(filter_pool + length);
            length += (data_filter[i].median_len * sizeof(struct list_s));
            data_filter[i].window_buff = (uint32_t *)(filter_pool + length);
            length += (data_filter[i].window_len * sizeof(uint32_t));
        }
    }
    /* use default length para when para is too big */
    if (length > sizeof(filter_pool)) {
        length = 0;
        for (uint8_t i = 0; i < 4; i++) {
            data_filter[i].median_buff = (struct list_s *)(filter_pool + length);
            data_filter[i].median_len = median_length[i];
            length += (data_filter[i].median_len * sizeof(struct list_s));
            data_filter[i].window_buff = (uint32_t *)(filter_pool + length);
            data_filter[i].window_len = window_length[i];
            length += (data_filter[i].window_len * sizeof(uint32_t));
        }
    }

    for (uint8_t i = 0; i < 4; i++) {
        uint16_t len;

        /* media filter init */
        data_filter[i].median_idx = 0;
        len = data_filter[i].median_len;
        /* last node is for head node */
        for (uint16_t j = 1; j < len - 1; j++) {
            data_filter[i].median_buff[j].prev = &(data_filter[i].median_buff[j - 1]);
            data_filter[i].median_buff[j].next = &(data_filter[i].median_buff[j + 1]);
            data_filter[i].median_buff[j].data = 0;
        }
        /* first node */
        data_filter[i].median_buff[0].prev = &(data_filter[i].head);
        data_filter[i].median_buff[0].next = &(data_filter[i].median_buff[1]);
        data_filter[i].median_buff[0].data = 0;
        /* last node */
        data_filter[i].median_buff[len - 1].prev = &(data_filter[i].median_buff[len - 2]);
        data_filter[i].median_buff[len - 1].next = &(data_filter[i].head);
        data_filter[i].median_buff[len - 1].data = 0;
        /* head node */
        data_filter[i].head.prev = &(data_filter[i].median_buff[len - 1]);
        data_filter[i].head.next = &(data_filter[i].median_buff[0]);
        data_filter[i].head.data = 0; /* head.data is for store input data */

        /* window filter init */
        len = data_filter[i].window_len;
        for (uint16_t j = 0; j < len; j++) {
            data_filter[i].window_buff[j] = 0;
        }
        data_filter[i].window_sum = 0;
        data_filter[i].window_idx = 0;
    }
    flag.filter_climb = data_filter[0].median_len + data_filter[0].window_len;
}

void filter_quadratic_process(void)
{
    static float peak[4] = {0.0f, 0.0f, 0.0f, 0.0f};
    static float rative[4] = {0.0f, 0.0f, 0.0f, 0.0f};
    struct quadratic_coefficient_s coeff;
    float result_fit;
    uint32_t position;

    position = (data_ltc1867.sample_total - CALC_POINTS) / 2;
    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == GAS_ID_INVALID) {
            continue;
        }
        calc_quadratic_fit(data_ltc1867.p_calc[i] + position, &coeff, CALC_POINTS);
        result_fit = coeff.c - coeff.b * coeff.b / (4.0f * coeff.a);
        if (result_fit > 32768.0f && result_fit < 65536.0f) {
            peak[i] = result_fit;
        } else if (result_fit < 32768.0f && result_fit > 0.0f) {
            rative[i] = result_fit;
        } else {
            return;
        }
        data_filter[i].raw_f = peak[i] - rative[i];
        debug_write_data(DEBUG_CH_QUADRATIC + i, data_filter[i].raw_f);
        if (data_filter[i].raw_f < 0.0f) {
            data_filter[i].raw_u = 0;
        } else if (data_filter[i].raw_f >= 65535.0f) {
            data_filter[i].raw_u = 655360;
        } else {
            data_filter[i].raw_u = (uint32_t)(data_filter[i].raw_f * 10.0f);
        }
        if (cli_data_comb.force_available & (1 << (i + CLI_FORCE_FILTER_INPUT1))) {
            data_filter[i].raw_u = cli_data_comb.force_data[i + CLI_FORCE_FILTER_INPUT1];
        }
    }
}

void filter_median_process(void)
{
    struct list_s *list, *iter;
    uint32_t data;
    uint16_t idx, len;

    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == GAS_ID_INVALID) {
            continue;
        }
        idx = data_filter[i].median_idx;
        list = &(data_filter[i].median_buff[idx]);
        /* delete old node */
        list->prev->next = list->next; /* connect prev to next */
        list->next->prev = list->prev; /* connect next to prev */
        /* add new node */
        data = data_filter[i].raw_u;
        data_filter[i].median_buff[idx].data = data;
        len = data_filter[i].median_len + 1;
        data_filter[i].head.data = 0xFFFFFFFF;
        iter = data_filter[i].head.next;
        for (uint16_t j = 0; j < len; j++) {
            if (data > iter->data) {
                iter = iter->next;
            } else {
                /* insert new node */
                iter->prev->next = list;
                list->next = iter;
                list->prev = iter->prev;
                iter->prev = list;
                break;
            }
        }
        /* search median node */
        iter = &(data_filter[i].head);
        for (uint16_t j = 0; j < len / 2; j++) {
            iter = iter->next;
        }
        /* calc median data */
        data_filter[i].median_output = (iter->data + iter->next->data) / 2;
        debug_write_data(DEBUG_CH_MEDIAN + i, (float)(data_filter[i].median_output));
        /* iterate median_idx */
        data_filter[i].median_idx++;
        if (data_filter[i].median_idx >= data_filter[i].median_len) {
            data_filter[i].median_idx = 0;
        }
    }
}

void filter_window_process(void)
{
    uint32_t sum, val;
    uint16_t idx;

    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == GAS_ID_INVALID) {
            continue;
        }
        idx = data_filter[i].window_idx;
        sum = data_filter[i].window_sum;
        val = data_filter[i].median_output;
        sum = sum - data_filter[i].window_buff[idx];
        data_filter[i].window_buff[idx] = val;
        sum = sum + val;
        data_filter[i].window_sum = sum;
        data_filter[i].window_output = sum / data_filter[i].window_len;
        debug_write_data(DEBUG_CH_WINDOW + i, (float)(data_filter[i].window_output));
        data_filter[i].window_idx++;
        if (data_filter[i].window_idx >= data_filter[i].window_len) {
            data_filter[i].window_idx = 0;
        }
    }
}

static int flag_for_sig_init_error(uint8_t ch, uint32_t sig)
{
    const struct 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 sig_fixed;
    uint8_t idx_t = 0; /* temperature index */

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

    /* step2: get temperature index */
    for (idx_t = 0; idx_t < p->temperature_points; idx_t++) {
        if (temp < 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 */
    }

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

    /* step4: calc sig0 value by temperature */
    k = ((float)(p->sig[0][idx_t]) - (float)(p->sig[0][idx_t - 1])) / temp_quantum;
    sig_fixed = k * temp + p->sig[0][idx_t - 1];

    /* laset sig > sig_fixed * 80% */
    if (sig < sig_fixed * 0.8f) {
        return 1;
    } else {
        return 0;
    }
}

static void flag_for_sig_init_error_process(void)
{
    uint32_t flag_fault = 0xFFFFFFFF;

    for (uint8_t ch = 0; ch < 4; ch++) {
        if (data_gas_info.id[ch] == GAS_ID_INVALID) {
            flag_fault |= (1 << ch);
            continue;
        }
        if (flag_for_sig_init_error(ch, data_filter[ch].window_output)) {
            flag_fault |= (1 << ch);
        } else {
            flag_fault &= ~(1 << ch);
        }
    }
    flag.gas_fault = flag_fault;
}

void filter_output_process(void)
{
    if (flag.filter_climb > 0) {
        flag.filter_climb--;
        if (system_tick_cnt > 25 * 1000) {
            while (flag.filter_climb) {
                filter_median_process();
                filter_window_process();
                flag.filter_climb--;
            }
        }
        if (flag.filter_climb == 0) {
            flag_for_sig_init_error_process();
        }
        return;
    }
    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == GAS_ID_INVALID) {
            device_data.gas_sig[i] = 0;
        } else {
            device_data.gas_sig[i] = data_filter[i].window_output;
        }
    }
}

void filter_loop(void)
{
    if (data_ltc1867.calc_flag == flag.filter) {
        return;
    }
    flag.filter = data_ltc1867.calc_flag;
    filter_quadratic_process();
    filter_median_process();
    filter_window_process();
    filter_output_process();
}
