#include "filter.h"
#include "ltc1867.h"
#include "device.h"
#include "user_misc.h"

static uint32_t sum_gas1[FILTER_SUM_LEN_GAS1] __attribute__((section(".bss_ccm")));
static uint32_t sum_gas2[FILTER_SUM_LEN_GAS2] __attribute__((section(".bss_ccm")));
static uint32_t sum_gas3[FILTER_SUM_LEN_GAS3] __attribute__((section(".bss_ccm")));
static uint32_t sum_gas4[FILTER_SUM_LEN_GAS4] __attribute__((section(".bss_ccm")));
static struct list_s median_gas1[FILTER_MEDIAN_LEN_GAS1] __attribute__((section(".bss_ccm")));
static struct list_s median_gas2[FILTER_MEDIAN_LEN_GAS2] __attribute__((section(".bss_ccm")));
static struct list_s median_gas3[FILTER_MEDIAN_LEN_GAS3] __attribute__((section(".bss_ccm")));
static struct list_s median_gas4[FILTER_MEDIAN_LEN_GAS4] __attribute__((section(".bss_ccm")));
static uint32_t window_gas1[FILTER_WINDOW_LEN_GAS1] __attribute__((section(".bss_ccm")));
static uint32_t window_gas2[FILTER_WINDOW_LEN_GAS2] __attribute__((section(".bss_ccm")));
static uint32_t window_gas3[FILTER_WINDOW_LEN_GAS3] __attribute__((section(".bss_ccm")));
static uint32_t window_gas4[FILTER_WINDOW_LEN_GAS4] __attribute__((section(".bss_ccm")));

struct data_filter_s data_filter[4] __attribute__((section(".bss_ccm")));

void filter_init(void)
{
    /* data_filter parameter init */
    data_filter[0].sum_len = FILTER_SUM_LEN_GAS1;
    data_filter[0].sum_buff = sum_gas1;
    data_filter[0].median_len = FILTER_MEDIAN_LEN_GAS1;
    data_filter[0].median_buff = median_gas1;
    data_filter[0].window_len = FILTER_WINDOW_LEN_GAS1;
    data_filter[0].window_buff = window_gas1;
    data_filter[1].sum_len = FILTER_SUM_LEN_GAS2;
    data_filter[1].sum_buff = sum_gas2;
    data_filter[1].median_len = FILTER_MEDIAN_LEN_GAS2;
    data_filter[1].median_buff = median_gas2;
    data_filter[1].window_len = FILTER_WINDOW_LEN_GAS2;
    data_filter[1].window_buff = window_gas2;
    data_filter[2].sum_len = FILTER_SUM_LEN_GAS3;
    data_filter[2].sum_buff = sum_gas3;
    data_filter[2].median_len = FILTER_MEDIAN_LEN_GAS3;
    data_filter[2].median_buff = median_gas3;
    data_filter[2].window_len = FILTER_WINDOW_LEN_GAS3;
    data_filter[2].window_buff = window_gas3;
    data_filter[3].sum_len = FILTER_SUM_LEN_GAS4;
    data_filter[3].sum_buff = sum_gas4;
    data_filter[3].median_len = FILTER_MEDIAN_LEN_GAS4;
    data_filter[3].median_buff = median_gas4;
    data_filter[3].window_len = FILTER_WINDOW_LEN_GAS4;
    data_filter[3].window_buff = window_gas4;

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

        /* sum filter init */
        len = data_filter[i].sum_len;
        for (uint16_t j = 0; j < len; j++) {
            data_filter[i].sum_buff[j] = 0;
        }
        data_filter[i].sum_sum = 0;
        data_filter[i].sum_idx = 0;

        /* 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 = FILTER_WINDOW_LEN_GAS1 + FILTER_MEDIAN_LEN_GAS1;
}

void filter_input_data(uint8_t channel, uint32_t data)
{
    data_filter[channel].input = data;
}

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

    for (uint8_t i = 0; i < 4; i++) {
        idx = data_filter[i].sum_idx;
        sum = data_filter[i].sum_sum;
        val = data_filter[i].input;
        sum = sum - data_filter[i].sum_buff[idx];
        data_filter[i].sum_buff[idx] = val;
        sum = sum + val;
        data_filter[i].sum_sum = sum;
        data_filter[i].sum_idx++;
        if (data_filter[i].sum_idx >= data_filter[i].sum_len) {
            data_filter[i].sum_idx = 0;
        }
    }
}

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++) {
        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].sum_sum;
        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;
        /* 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++) {
        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;
        data_filter[i].window_idx++;
        if (data_filter[i].window_idx >= data_filter[i].window_len) {
            data_filter[i].window_idx = 0;
        }
    }
}

void filter_output_process(void)
{
    if (flag.filter_climb > 0) {
        flag.filter_climb--;
        return;
    }
    for (uint8_t i = 0; i < 4; i++) {
        if (data_gas_info.id[i] == 0xFFFFFFFF) {
            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_sum_process();
    filter_median_process();
    filter_window_process();
    filter_output_process();
}
