/**
 * @file mode.c
 * @author xxx
 * @date 2023-09-18 10:00:52
 * @brief 此文件用于实现不同工作模式的功能
 * @copyright Copyright (c) 2023 by xxx, All Rights Reserved.
 */

#include <math.h>
#include "app.h"
#include "main.h"
#include "mode.h"

mode_params_t mode_params; // 模式参数
static mode_t _mode;

static uint8_t deal_dev_work_mode(void)
{
    uint8_t mode = udevice.dev_work_mode;

    if (udevice.dev_work_mode == FORBIDEN_MODE || udevice.dev_work_mode == TEST_MODE)
    {
        return mode;
    }
    else
    {

        return mode;
    }
}

/**
 * @brief  改变自动重装载寄存器的值
 * @param {uint16_t} autoload
 * @return {*}
 * @note
 */
static void mode_autoload_change(uint16_t autoload)
{
    LL_TIM_SetAutoReload(TIM7, autoload);
}

/**
 * @brief  工作模式参数保存,回调处理
 * @return {*}
 * @note
 */
static void mode_params_save_cb(void)
{
    fal_execution_kv_write(KEY_CALIBPARA_PARAM, (uint8_t *)&calib_param, (CALIBPARA_NUM * sizeof(calib_param_t)));
    fal_execution_kv_write(KEY_MODE_PARAM, (uint8_t *)&mode_params, sizeof(mode_params_t));
    fal_execution_kv_read(KEY_MODE_PARAM, (uint8_t *)&mode_params, sizeof(mode_params_t));
}

/**
 * @brief 测试模式
 * @return {*}
 * @note
 */
void mode_test_process(void)
{
    switch (mode_get()->test_project)
    {
    case MODE_DIAGNOSIS: // EPM特性诊断
        mode_get()->ctrl.state = FALSE;
        flow_event = FLOW_EVENT_DIAGNOSIS;
        break;
    default:
        break;
    }
}

/**
 * @brief 行程统计(100ms以上运行一次)
 * @return {*}
 * @note
 */
void mode_travel_statistics(void)
{
    static float32 loop_current_last = 0.0f; // 上一次的电流
    static uint8_t change_count = 0;
    float32 loop = 0.0;
    // 输入电流
    loop = get_current();
    rt_data.loop_current = get_current_deal(loop);
    // 过滤掉不稳定的电流值
    if (loop_current_last != rt_data.loop_current)
    {
        if (change_count++ >= 2)
        {
            loop_current_last = rt_data.loop_current;
            change_count = 0;
        }
        else
        {
            return;
        }
    }
    else
    {
        change_count = 0;
        lpf_window_reset(mode_get()->show_loop_lpf);
        lpf_window_reset(mode_get()->show_actual_lpf);
    }

    // 获取目标行程
    if (mode_get()->ctrl.mode == ON_LINE_MODE) // 在线模式
    {
        rt_data.target_travel = i2psb(rt_data.loop_current);
        rt_data.target_travel = (uint16_t)(rt_data.target_travel * 10) * 0.1f;
    }
    else if (mode_get()->ctrl.mode == OFF_LINE_MODE) // 离线模式
    {
        rt_data.target_travel = udevice.travel_setpoint;
    }
    else if (mode_get()->ctrl.mode == TEST_MODE) // 测试模式
    {
    }

    // 目标行程处理
    rt_data.target_travel = target_travel_deal(rt_data.target_travel);
    pid_target = get_pid_travel(rt_data.target_travel);
    show_target = get_show_travel(rt_data.target_travel);

    // 实际行程处理
    rt_data.actual_travel = actual_travel_deal(rt_data.actual_travel);
    show_actual = get_show_travel(rt_data.actual_travel);
    // show_actual = lpf_window_update(mode_get()->show_actual_lpf, show_actual);
    show_actual = kalman_update(&mode_get()->show_actual_kalman, show_actual);
    show_loop = lpf_window_update(mode_get()->show_loop_lpf, rt_data.loop_current);
}

/**
 * @brief 气压统计
 * @return {*}
 * @note
 */
static void pressure_statistics(void)
{
    if (udevice.press_sensor_enable != TRUE)
    {
        return;
    }
    static uint8_t id = ADC_INDEX_CLOSEALL;
    switch (id)
    {
    case ADC_INDEX_PRESSSOURCE:
        rt_data.pressure_s = get_pressure(ADC_INDEX_PRESSSOURCE);
        id = ADC_INDEX_PRESSOUTA;
        BP_S_POWER_OFF();
        BP_A_POWER_ON();
        break;
    case ADC_INDEX_PRESSOUTA:
        rt_data.pressure_a = get_pressure(ADC_INDEX_PRESSOUTA);
        id = ADC_INDEX_PRESSOUTB;
        BP_A_POWER_OFF();
        BP_B_POWER_ON();
        break;
    case ADC_INDEX_PRESSOUTB:
        rt_data.pressure_b = get_pressure(ADC_INDEX_PRESSOUTB);
        id = ADC_INDEX_PRESSSOURCE;
        BP_B_POWER_OFF();
        BP_S_POWER_ON();
        break;
    case ADC_INDEX_CLOSEALL:
        id = ADC_INDEX_PRESSSOURCE;
        BP_S_POWER_ON();
        break;
    default:
        break;
    }
}

/**
 * @brief 算法控制模块
 * @return {*}
 * @note 该模块在TIM7中执行
 */
void mode_process(void)
{
    pressure_statistics(); // 气压轮询读取

    if (mode_get()->ctrl.state == FALSE) // 不进行算法控制
    {
        return;
    }

    // travel_statistics因为在flow中被更新，如果UI更新时会导致延时，所以在这里更新
    // 系统的tick是10ms一次,200ms更新一次数据
    if (FALSE == mode_get()->interface_req.mode_is_adjusting())
    {
        if (sys_get_tick() - mode_get()->alog_control_ticks >= 20) // 200ms
        {
            mode_get()->alog_control_ticks = sys_get_tick();
            mode_travel_statistics();
        }
    }
    else
    {
        if (sys_get_tick() - mode_get()->alog_control_ticks >= 100) // 1000ms
        {
            mode_get()->alog_control_ticks = sys_get_tick();
            mode_travel_statistics();
        }
    }

    // 当电流达到4mA时才进行控制
    if (rt_data.loop_current >= LOOP_CURRENT_MIN)
    {
        if (udevice.dev_algorithm_mode == MODE_CONSTANT_CONTROL_ALGORITHM)
        {
        }
        else if (udevice.dev_algorithm_mode == MODE_SPEED_CONTROL_ALGORITHM)
        {
        }
        else if (udevice.dev_algorithm_mode == MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM)
        {
            mode_pwmp_hd_process();
        }
        else if (udevice.dev_algorithm_mode == MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM)
        {
        }
        else
        {
            DBG_ASSERT(FALSE __DBG_LINE);
        }
    }
    else
    {
        pdctrl_out(0);
    }
}

/**
 * @brief 模式处理
 * @param {uint8_t} work_mode
 * @return {*}
 * @note
 */
void mode_detection(void)
{
    mode_get()->ctrl.mode = deal_dev_work_mode();

    switch (mode_get()->ctrl.mode)
    {
    case ON_LINE_MODE:
    case OFF_LINE_MODE:
    case WAIT_MODE:
        if (mode_get()->ctrl.state != TRUE)
        {
            mode_get()->ctrl.state = TRUE;
            pdctrl_run(); // 输出软使能
        }
        break;
    case FORBIDEN_MODE:
        if (mode_get()->ctrl.state != FALSE)
        {
            mode_get()->ctrl.state = FALSE;
            pdctrl_stop(); // 输出软禁止
        }
        break;
    case TEST_MODE:
        mode_get()->ctrl.state = FALSE;
        mode_test_process();
        break;
    default:
        break;
    }
}

/**
 * @brief 工作模式初始化
 * @return {*}
 * @note
 */
void mode_init(void)
{
    osel_memset((uint8_t *)&_mode, 0, sizeof(mode_t));

    mode_get()->positioner_model = udevice.dev_model;
    mode_get()->alog_control_ticks = sys_get_tick();
    mode_autoload_change(mode_default_autoload);

    // 反初始化，释放内存

    mode_pwmp_hd_dinit();

    switch (udevice.dev_algorithm_mode)
    {
    case MODE_CONSTANT_CONTROL_ALGORITHM:
        pdctrl_init(PDCTRL_DAC);
        break;
    case MODE_SPEED_CONTROL_ALGORITHM:
        pdctrl_init(PDCTRL_PWMP);
        break;
    case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
        pdctrl_init(PDCTRL_PWMP);
        //pdctrl_init(PDCTRL_DAC);
        mode_pwmp_hd_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.mode_pwmp_hd_params,
                          mode_params_save_cb);
        break;
    case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
        pdctrl_init(PDCTRL_PWM);
        break;
    default:
        DBG_ASSERT(FALSE __DBG_LINE);
        break;
    }

    DBG_ASSERT(mode_get()->interface_req.mode_process_start != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_process_stop != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_adjust_start != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_adjust_stop != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_get_adjust_data != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_adjust_result != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_adjust_step_count != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_adjust_step_current != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_control_idle != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_is_adjusting != NULL __DBG_LINE);

    mode_get()->show_loop_lpf = lpf_window_init(10);
    mode_get()->show_actual_lpf = lpf_window_init(10);

    kalman_init(&mode_get()->loop_kalman);
    kalman_init(&mode_get()->show_actual_kalman);
    mode_get()->loop_kalman.filter_limit = 0.3f;
    mode_get()->show_actual_kalman.filter_limit = 0.3f;
}

/**
 * @brief 工作模式获取
 * @return {*}
 * @note
 */
mode_t *mode_get(void)
{
    return &_mode;
}