/**
 * @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"
#include "mode_diagnosis.h"

mode_params_u mode_params __attribute__((section("NOINIT"), zero_init)); // 模式参数
mode_t mode;

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

/**
 * @brief  工作模式参数保存,回调处理
 * @return {*}
 * @note
 */
static void mode_params_save_cb(void)
{
    if (mode_params_storage != NULL)
    {
        storage_write_all(mode_params_storage, (uint8_t *)&mode_params);
    }
}

/**
 * @brief 气压统计
 * @return {*}
 * @note 调用周期：> 100ms,小于改值S口会读到0值
 */
void mode_pressure_statistics(void)
{
    if (mode_diagnosis_get()->run == FALSE)
    {
        static uint8_t wait_tick = 0; // 定时器周期是100ms，因为功耗关闭传感器到恢复数据采集需要一定时间
        float32 pressure = 0.0f;
        if (++wait_tick < 3) // 300ms内不处理
        {
            return;
        }

        if (udevice.press_sensor_enable != TRUE)
        {
            return;
        }
        static uint8_t id = PRESSURE_PARAM_MAX;
        static float32 pressure_cross_point_last = 0.0f;

        switch (id)
        {
        case PRESSURE_PARAM_S:
            pressure = trim_pressure_point(DIN_SUPPLY_PRESSURE, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_S), udevice.press_unit), udevice.press_unit);
            // rt_data.pressure_s = lpf_update(&mode_get()->pressure_s_window, pressure);
            rt_data.pressure_s = pressure;
            if (pressure != 0.0f)
            {
                id = PRESSURE_PARAM_A;
                BP_S_POWER_OFF();
                BP_A_POWER_ON();
            }
            else
            {
                id = PRESSURE_PARAM_S;
                BP_S_POWER_ON();
            }
            rt_data.pressure_s_kpa = pressure_unit2kpa(rt_data.pressure_s, udevice.press_unit);
            break;
        case PRESSURE_PARAM_A:
            pressure = trim_pressure_point(DIN_PRESSURE_PORT_A, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_A), udevice.press_unit), udevice.press_unit);
            // rt_data.pressure_a = lpf_update(&mode_get()->pressure_a_window, pressure);
            rt_data.pressure_a = pressure;
            if (rt_data.pressure_s != 0) // S口压力不为0的情况下才读取A口和B口的压力
            {
                id = PRESSURE_PARAM_B;
                BP_A_POWER_OFF();
                BP_B_POWER_ON();
            }
            else
            {
                rt_data.pressure_a = 0;
                id = PRESSURE_PARAM_S;
                BP_A_POWER_OFF();
                BP_S_POWER_ON();
            }
            rt_data.pressure_a_kpa = pressure_unit2kpa(rt_data.pressure_a, udevice.press_unit);
            break;
        case PRESSURE_PARAM_B:
            pressure = trim_pressure_point(DIN_PRESSURE_PORT_B, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_B), udevice.press_unit), udevice.press_unit);
            // rt_data.pressure_b = lpf_update(&mode_get()->pressure_b_window, pressure);
            rt_data.pressure_b = pressure;
            id = PRESSURE_PARAM_S;
            BP_B_POWER_OFF();
            BP_S_POWER_ON();
            if (rt_data.pressure_s != 0) // S口压力不为0的情况下才读取A口和B口的压力
            {
                pressure_cross_point_last = ((rt_data.pressure_a + rt_data.pressure_b) * 0.5f / rt_data.pressure_s) * 100;
                if (pressure_cross_point_last <= 40)
                {
                    rt_data.pressure_cross_point = 0;
                    break;
                }
                else if (pressure_cross_point_last >= 80)
                {
                    rt_data.pressure_cross_point = 0;
                    break;
                }
                rt_data.pressure_cross_point = lpf_update(&mode_get()->pressure_cross_point_lpf, pressure_cross_point_last);
            }
            else
            {
                rt_data.pressure_b = 0;
            }

            rt_data.pressure_b_kpa = pressure_unit2kpa(rt_data.pressure_b, udevice.press_unit);
            break;
        case PRESSURE_PARAM_MAX:
            id = PRESSURE_PARAM_S;
            BP_S_POWER_ON();
            break;
        default:
            break;
        }
        wait_tick = 0;
    }
    else
    {
        if (BP_S_IS_POWER_ON() == FALSE)
        {
            BP_S_POWER_ON();
        }

        if (BP_A_IS_POWER_ON() == FALSE)
        {
            BP_A_POWER_ON();
        }

        if (BP_B_IS_POWER_ON() == FALSE)
        {
            BP_B_POWER_ON();
        }

        rt_data.pressure_s = trim_pressure_point(DIN_SUPPLY_PRESSURE, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_S), udevice.press_unit), udevice.press_unit);
        if (rt_data.pressure_s != 0.0f)
        {
            rt_data.pressure_s_kpa = pressure_unit2kpa(rt_data.pressure_s, udevice.press_unit);
            rt_data.pressure_a = trim_pressure_point(DIN_PRESSURE_PORT_A, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_A), udevice.press_unit), udevice.press_unit);
            rt_data.pressure_a_kpa = pressure_unit2kpa(rt_data.pressure_a, udevice.press_unit);
            rt_data.pressure_b = trim_pressure_point(DIN_PRESSURE_PORT_B, pressure_kpa2unit(get_pressure(PRESSURE_PARAM_B), udevice.press_unit), udevice.press_unit);
            rt_data.pressure_b_kpa = pressure_unit2kpa(rt_data.pressure_b, udevice.press_unit);
        }
    }
}

void mode_statistics(void)
{
    // 在控制模式下统计稳定次数(最好加入目标值变化)
    if (mode_get()->interface_req.mode_is_control() == TRUE)
    {
        if (mode_get()->interface_req.mode_control_idle() == TRUE )
        {
            if (mode_get()->is_stable == FALSE)
            {
                rt_save.travel_count++;
            }
            mode_get()->is_stable = TRUE;
        }else{
            mode_get()->is_stable = FALSE;
        }
    }

    rt_data.ip_current = ip2current();
}

/**
 * @brief 行程统计(100ms以上运行一次)
 * @return {*}
 * @note
 */
void mode_travel_statistics(void)
{
    float32 current = 0.0f;   // 输入电流
    float32 deviation = 0.0f; // 系统偏差
    float32 target_travel = 0.0f;
    // 输入电流
    current = get_current(FILTER_AVERAGE);
    rt_data.loop_current = kalman_update(&mode_get()->loop_current_km, current);

    // 判断蓝牙是否需要关闭
    if (wireless_can_work(rt_data.loop_current) == FALSE)
    {
        if (TRUE == BLE_IS_ENABLE())
        {
            hart_ble_dinit(); // 关闭蓝牙
            rt_data.flag.bits.ble_on = FALSE;
        }
    }

    // 获取目标行程
    if (udevice.inst_mode == INST_IS)
    {
        // 模拟模式：通过4~20mA计算目标
        if (*mode_get()->ctrl.mode == ANALOG_CTRL_MODE)
        {
            target_travel = i2psb(rt_data.loop_current);
            rt_data.travel_set_pt = kalman_update(&mode_get()->target_travel_km, target_travel);
            rt_save.travel_set_pt = rt_data.travel_set_pt;
        }
        // 数字模式：通过HART指令设定目标
        else if (*mode_get()->ctrl.mode == DIGITAL_CTRL_MODE)
        {
            rt_save.travel_set_pt = rt_data.travel_set_pt;
        }
        // 测试模式：通过上位机下发目标
        else if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
        {
            rt_data.travel_set_pt = rt_data.test_target;
        }
    }
    else
    {
        if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
        {
            rt_data.travel_set_pt = rt_data.test_target;
        }
        else
        {
            // 手动加载模式
            if (udevice.manual_loader == ALLOW_SERVO_CTRL)
            {
                target_travel = i2psb(rt_data.loop_current);
                rt_data.travel_set_pt = kalman_update(&mode_get()->target_travel_km, target_travel);
                rt_save.travel_set_pt = rt_data.travel_set_pt;
            }
            else
            {
                rt_data.travel_set_pt = 0;
                rt_data.target_travel = 0;
            }
        }
    }
    // 行程限制
    rt_data.target_travel = travel_set_point_deal(rt_data.travel_set_pt);

    // 目标行程处理
    if (rt_data.flag.bits.app_init_over == TRUE)
    {
        // 小信号切除
        rt_data.target_travel = small_signal_deal(rt_data.target_travel);
        // 分程处理
        if (udevice.inst_mode == INST_IS)
        {
            rt_data.target_travel = part_travel_deal(rt_data.target_travel);
        }
        rt_data.target_travel = floorf(rt_data.target_travel * 100.f) / 100.f;
    }

    rt_data.pid_target = get_pid_travel(rt_data.target_travel);

    // 实际行程处理
    if (rt_data.actual_travel > udevice.cutoff_limit_lo && rt_data.actual_travel < udevice.cutoff_limit_hi)
    {
        deviation = udevice.deviation;
    }

    rt_data.servo_feedback = rt_data.actual_travel + deviation;

    rt_data.crossover_out_u = get_actual_travel_adc();

    // 显示内容
    rt_data.show_target = get_show_travel(rt_data.travel_set_pt);
    if (ABS(rt_data.travel_set_pt - rt_data.show_target) > MA_TO_PERCENT_01)
    {
        lpf_window_reset(&mode_get()->show_target_window);
    }
    rt_data.show_target = lpf_window_update(&mode_get()->show_target_window, rt_data.show_target);

    rt_data.show_actual = get_show_travel(rt_data.servo_feedback);
    rt_data.show_actual = get_show_actual_travel(rt_data.show_target, rt_data.show_actual, 0.1f);
    rt_data.show_loop = rt_data.loop_current;
}

// 行程统计
static void mode_stroke_statistics(void)
{
    static float32 last_travel = 0.0f;
    static float32 last_target = 0.0f;
    float32 err = ABS(rt_data.pid_actual - last_travel);
    float32 err_target = ABS(rt_data.pid_target - last_target);     // 目标位置的差值
    float32 err_move = ABS(rt_data.pid_actual - last_target);       // 实际位置和上次目标位置的差值
    float32 err_act = ABS(rt_data.pid_actual - rt_data.pid_target); // 实际位置和目标位置的差值
    if (mode_get()->interface_req.mode_is_adjusting() == FALSE)     // 不在整定中
    {
        // 累计行程
        if (err >= udevice.travel_sum_dead)
        {
            rt_save.travel_accum += err;
            last_travel = rt_data.pid_actual;
        }

        // 设定的目标位置-当前目标位置>=死区
        if (err_target >= udevice.act_sum_dead)
        {
            // 移动的距离大于等于死区，实际距离目标位置小于死区
            if ((err_move >= udevice.act_sum_dead) && (err_act < udevice.act_sum_dead))
            {
                last_target = rt_data.pid_target;
                rt_save.cycle_count++;
            }
        }
        else
        {
            // 实际位置和目标位置差值大于死区，移动距离大于死区
            if ((err_act > udevice.act_sum_dead) && (err > udevice.act_sum_dead))
            {
                rt_save.cycle_count++;
            }
        }
    }
}

/*********************************************************************************************************************/
/**
 * @brief 工作模式控制管理
 * @param {uint8_t} work_mode
 * @return {*}
 * @note
 */
void mode_master_detection(void)
{
    switch (*mode_get()->ctrl.mode)
    {
    case DIGITAL_CTRL_MODE:
    case ANALOG_CTRL_MODE:
    {
        if (udevice.inst_mode == INST_IS || udevice.manual_loader == ALLOW_SERVO_CTRL)
        {
            if (mode_get()->ctrl.state != TRUE)
            {
                // 当前控制算法模块从禁止到允许
                mode_get()->ctrl.state = TRUE;
                mode_get()->interface_req.mode_process_start();
                pdctrl_run(); // 输出软使能
            }
        }
        else
        {
            mode_get()->ctrl.state = FALSE;
        }
        break;
    }
    case TEST_CTRL_MODE:
        mode_get()->ctrl.state = TRUE;
        break;
    case STOP_MODE: // 停机
        mode_get()->ctrl.state = FALSE;
        // 关闭LCD
        // 关闭诊断
        // 由各自模块处理
        break;
    case WAIT_MODE: // 待机
        mode_get()->ctrl.state = FALSE;
        break;
    default:
        break;
    }
}

/**
 * @brief 采集传感器数据转换接口
 * @return {*}
 * @note
 */
void mode_ctrl_gather(void)
{
    if (mode_ctrl_process_allow_run() == FALSE)
    {
        return;
    }

    if (mode_get()->interface_req.mode_is_adjusting() == FALSE)
    {
        mode_travel_statistics();
        mode_stroke_statistics();
    }

    mode_pressure_statistics(); // 气压轮询读取
}
/**
 * @brief 控制输出模式函数
 *
 * 该函数负责处理并输出实际行程数据。
 */
void mode_ctrl_output(void)
{
    if (*mode_get()->ctrl.mode == DIGITAL_CTRL_MODE ||
        *mode_get()->ctrl.mode == ANALOG_CTRL_MODE)
    {
        actual_travel_deal(rt_data.actual_travel); // 实际行程处理，对外输出
    }
    else if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
    {
        if (calib_param[CALIBPARA_VIP].is_calibration == IS_CALIBRATION)
        {
            pwm_output_position(rt_data.show_target);
        }
        else
        {
            pwm_duty2current(rt_data.loop_current);
        }
    }
    else
    {
        actual_travel_deal(0);
    }
}

BOOL mode_ctrl_process_allow_run(void)
{
    if (is_run_max_time() == TRUE)
    {

        return FALSE;
    }

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

    /**
     * @brief Checks the control mode and loop current to determine if the algorithm should run.
     *
     * This function checks if the current control mode is either ANALOG_CTRL_MODE or TEST_CTRL_MODE.
     * If the control mode matches one of these modes, it further checks if the loop current is below
     * the threshold defined by ALGORITHM_RUN_CURRENT. If the loop current is below the threshold,
     * the function returns without performing any further actions.
     *
     * @note This function assumes that mode_get() returns a pointer to a structure containing the control mode.
     * @note The function also assumes that rt_data.loop_current is a variable representing the current loop current.
     */
    if (*mode_get()->ctrl.mode == DIGITAL_CTRL_MODE || *mode_get()->ctrl.mode == TEST_CTRL_MODE)
    {
        if (rt_data.loop_current < ALGORITHM_RUN_CURRENT)
        {
            return FALSE;
        }
    }

    return TRUE;
}
/**
 * @brief 控制模式
 * @return {*}
 * @note 该模块在TIM7中执行
 */
void mode_ctrl_process(void)
{
    if (mode_ctrl_process_allow_run() == FALSE)
    {
        return;
    }
    mode_travel_statistics();
    mode_statistics();

    // 当电流达到4mA时才进行控制
    switch (udevice.dev_algorithm_mode)
    {
    case MODE_CONSTANT_CONTROL_ALGORITHM:
        mode_dac_process();
        break;
    case MODE_SPEED_CONTROL_ALGORITHM:
        mode_pwmp_process();
        break;
    case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
        mode_pwmp_hd_process();
        break;
    case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
        mode_control_process();
        break;
    default:
        DBG_ASSERT(FALSE __DBG_LINE);
        break;
    }
    mode_diagnosis_inspection();
}

/**
 * @brief 切换测试模式
 * @param {BOOL} state
 * @return {*}
 * @note 测试模式
 */
void mode_enter_test(test_item_e item)
{
    mode_get()->test_item = item;
}

/**
 * @brief 工作模式初始化
 * @return {*}
 * @note
 */
void mode_init(void)
{
    rt_data.time_cycle.mode = TIM_CYCLE(MODE_TIM);
    osel_memset((uint8_t *)&mode, 0, sizeof(mode_t));
    mode_get()->is_stable = FALSE;
    mode_get()->positioner_model = udevice.dev_model;
    mode_get()->ctrl.mode = (uint8_t *)&udevice.control_mode;
    mode_autoload_change(rt_data.time_cycle.mode * 10);

    // 反初始化，释放内存
    mode_dac_dinit();
    mode_pwmp_dinit();
    mode_pwmp_hd_dinit();
    mode_control_dinit();
    pdctrl_init(PDCTRL_DAC);
    switch (udevice.dev_algorithm_mode)
    {
    case MODE_CONSTANT_CONTROL_ALGORITHM:
        mode_dac_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.dac,
                      mode_params_save_cb);
        break;
    case MODE_SPEED_CONTROL_ALGORITHM:
        mode_pwmp_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.pwmp,
                       mode_params_save_cb);
        break;
    case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
        mode_pwmp_hd_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.pwmp_hd,
                          mode_params_save_cb);
        break;
    case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
        mode_control_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.control,
                          mode_params_save_cb);
        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_control != NULL __DBG_LINE);
    DBG_ASSERT(mode_get()->interface_req.mode_is_adjusting != NULL __DBG_LINE);

    // 滤波初始化
    kalman_init(&mode_get()->loop_current_km, 0.05f);
    kalman_init(&mode_get()->target_travel_km, 0.1f);
    mode_get()->pressure_cross_point_lpf.alpha = 0.1;
    lpf_window_init(&mode_get()->show_target_window, 10);
    lpf_init(&mode_get()->pressure_cross_point_lpf);
    lpf_init(&mode_get()->pressure_s_window);
    lpf_init(&mode_get()->pressure_a_window);
    lpf_init(&mode_get()->pressure_b_window);
}

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