/*
 * @Date: 2025-07-18 09:24:34
 * @Author: mypx
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-09-26 08:58:26
 * @FilePath: data_process.c
 * @Description: 
 * Copyright (c) 2025 by mypx, All Rights Reserved.
 */
#include "data_process.h"
#include "data_sampling.h"
#include "et_log.h"
#include "pm_common.h"
#include "pm_meas.h"
#include "servo.h"
#include <rtthread.h>
#include <stdio.h>
#include <string.h>

#if (ENABLE_IIR_FILTER == 1)
#include "et_iir_filter.h"
et_iir_filter_t iir_filter;

/* 这些系数使用巴特沃斯设计，形式为：
   H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) 
   注意：a_coeffs[0] = 1.0 (通常为1) */
float a_coeff[] = {1.000000000f, -0.855625000f, 0.212125000f}; // 分母系数 (a0=1, a1, a2)
float b_coeff[] = {0.089125000f, 0.178250000f, 0.089125000f};  // 分子系数 (b0, b1, b2)
                                                               // 对于1个二阶阶段，order=2，state_size=3
#define FILTER_STAGES 1                                        // 二阶节数量 (每个二阶节贡献2阶)
#define FILTER_ORDER  (2 * FILTER_STAGES)                      // 实际滤波器阶数
#define STATE_SIZE    (FILTER_ORDER + 1)                       // 状态缓冲区大小
// 状态缓冲区大小说明：
// - 对于N阶滤波器，需要N+1个状态元素存储历史输入和输出
// - 例如：2阶滤波器需要3个元素存储x[n], x[n-1], x[n-2]
float           in_state[STATE_SIZE]  = {0};
float           out_state[STATE_SIZE] = {0};
et_iir_filter_t filter;
et_iir_config_t config;
#endif

static struct rt_thread data_process_thread;
ALIGN(RT_ALIGN_SIZE)
static rt_uint8_t data_process_stack[DATA_PROCESS_STACK_SIZE];
float             adc_tmp_buffer[DATA_PROCESS_UNIT_COUNT]    = {0};
float             adc_filter_buffer[DATA_PROCESS_UNIT_COUNT] = {0};

#define ANALYSIS_SIZE DATA_PROCESS_UNIT_COUNT

static bool cali_flag = false;
static bool rdy_flag  = false;

pm_meas_t pm_meas;

static double trend_buffer[TREND_BUFFER_SIZE] = {0.0};

void set_pm_stage(int argc, char **argv)
{
    if (argc != 2)
    {
        rt_kprintf("Usage: my_cmd <int_value>\n");
        return;
    }

    int stage     = atoi(argv[1]);
    pm_meas.state = stage;
}
MSH_CMD_EXPORT_ALIAS(set_pm_stage, sst, set state);
uint32_t data_count = 0;

// 只考虑校准和测量状态，其他状态由采集线程  或者主线程管理
void data_process_thread_entry(void *parameter)
{
    (void)parameter;
    rt_err_t  result      = RT_EOK;
    int       find_status = 0;
    uint16_t *raw_buf;
    uint16_t *used_buf;

    while (1)
    {
        result = rt_mq_recv(&adc_mq, &raw_buf, sizeof(adc_raw_buffer), RT_WAITING_FOREVER);
        if (result == RT_EOK)
        {
            data_sampling_stop();
            if (get_find_zero_state(&pm_meas) == ZF_FAST_CLOSE_STAGE && fsm_state == false && pm_meas.p50 > 0.1f)
                sv630p_speed_mode_stop(&sv630p_handle);
#if (ENABLE_IIR_FILTER == 1)
            if (et_iir_filter_process(&iir_filter, (const void *)raw_buf, (void *)adc_filter_buffer) == IIR_OK)
            {
                used_buf = adc_filter_buffer;
            }
#else
            used_buf = raw_buf;
#endif
#if (SAMPLING_RAW_DATA == 1)
            for (int i = 0; i < DATA_PROCESS_UNIT_COUNT; i++)
            {
                ET_DBG("r:%d, f:%d", raw_buf[i], (uint16_t)adc_filter_buffer[i]);
            }
#endif

#if (SAMPLING_RAW_DATA == 0)
            pm_error_led_on(); // for timing
            find_status = find_zero_process(&pm_meas, (const uint16_t *)used_buf, DATA_PROCESS_UNIT_COUNT);
            pm_error_led_off();
#endif
            if (find_status == 0)
            {
                data_sampling_start();
                if (get_find_zero_state(&pm_meas) == ZF_FAST_CLOSE_STAGE && fsm_state == false && pm_meas.p50 > 0.1f)
                    sv630p_speed_mode_resume(&sv630p_handle);
            }
            else
            {
                ET_ERR("Something wrong");
            }
        }
    }
}

int data_process_init(void)
{
    rt_err_t result;

    // 初始化 pm_meas 结构体
    memset(&pm_meas, 0, sizeof(pm_meas_t));
    pm_meas.target_dir = MEASURE_DEF_DIR;

    //etk_ringbuffer_init(&pm_meas.rb, trend_buffer, TREND_BUFFER_SIZE, ETK_RB_DOUBLE);
    trend_handle_init(&pm_meas.trend_handle);
    etk_slope_init(&pm_meas.slope_handle, trend_buffer, TREND_BUFFER_SIZE, ETK_RB_DOUBLE);
    // 确保事件初始化时名称正确
    result = rt_event_init(&pm_event, "pm_evt", RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        rt_kprintf("Event init failed: 0x%04X\n", result);
        return -1;
    }
    //valley_seek_init(&pm_meas.valley_seek);
    working_set_state(PM_MEAS_TEST_Mode);
    if (et_zero_cross_create(&pm_meas.zc, 100) != 0)
    {
        ET_ERR("zc create failed\n");
        return -1;
    }

    int bin_center = et_goertzel_calc_bin(50, sampling_rate, ANALYSIS_SIZE);
    if (bin_center < 0)
    {
        ET_ERR("bin calc failed: %d\n", bin_center);
        return -1;
    }
    if (et_goertzel_init(&pm_meas.cfg, ANALYSIS_SIZE, bin_center, DC_OFFSET_ADC_VALUE) < 0)
    {
        ET_ERR("goertzel init failed\n");
        return -1;
    }

#if (ENABLE_IIR_FILTER == 1)
    config.direct_form_ii_f32.a_coeffs  = a_coeff;
    config.direct_form_ii_f32.b_coeffs  = b_coeff;
    config.direct_form_ii_f32.in_state  = in_state;
    config.direct_form_ii_f32.out_state = out_state;
    et_iir_scale_t scale                = IIR_SCALE_F32_IDENTITY;

    ET_IIR_Result iir_ret = et_iir_filter_init(&iir_filter, "band-pass Filter", ARM_IIR_LATTICE_F32,
                                               FILTER_STAGES, // 二阶节阶段数 (注意：不是滤波器总阶数)
                                               1,             // 块大小为1
                                               &scale, &config, IIR_SCALING_DISABLED);

    if (iir_ret != IIR_OK)
    {
        ET_ERR("IIR filter init failed: %d\n", iir_ret);
        return -1;
    }
#endif

    etk_position_pid_init(&pm_meas.pid,
                         1.8f,                     // kp: 提高比例增益，加快响应速度
                         1.8f,                     // ki: 增加积分增益，加快接近目标时的收敛速度
                         0.25f,                    // kd: 进一步降低微分作用，减少阻尼
                         PID_TARGET_ZERO_POS_VAL,  // 目标值设置为0.6
                         2.5f,                     // 积分限幅，允许更多积分累积加快收敛
                         -FZ_FAST_CLOSE_STAGE_RPM, // 最小输出(限制最大反转速度)
                         FZ_FAST_CLOSE_STAGE_RPM, -1);
    result = rt_thread_init(&data_process_thread,       // 线程控制块指针
                            "d-process",                // 线程名称
                            data_process_thread_entry,  // 入口函数
                            RT_NULL,                    // 入口参数
                            data_process_stack,         // 栈起始地址
                            sizeof(data_process_stack), // 栈大小
                            DATA_PROCESS_PRIORITY,      // 优先级（0-255）
                            10);                        // 时间片

    if (result == RT_EOK)
    {
        rt_kprintf("d-process thread initialized successfully\n");
        // 启动线程
        rt_thread_startup(&data_process_thread);
    }
    else
    {
        rt_kprintf("Thread init failed: 0x%04X\n", result);
    }
    return 0;
}