#include "app_config.h"
#include "interface.h"
#include "app_frm_monitor.h"
#include "app_frm_signal.h"
#include "app_frm_timer.h"
#include "app_param_manage.h"

#include "app_power.h"

#include "app_brake.h"
#include "app_differential_drive.h"

// 使用内联函数
static inline uint8_t setBrakeOn(void)  { return 1; }
static inline uint8_t setBrakeOff(void) { return 0; }

BrakeSystem brake_data;

// 判断是否需要刹车
static uint8_t shouldApplyBrake()
{
    return (brake_data.emergency_stop_switch || 
            brake_data.remote_emergency_stop ||
            (brake_data.mode_signal == 0 && brake_data.remote_fault) ||
            (brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021  修改不计算以太网故障
//            (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
}

// 判断是否需要释放刹车
static uint8_t shouldReleaseBrake()
{
    return (!brake_data.emergency_stop_switch &&
            !brake_data.remote_emergency_stop &&
            ((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
             (brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021  修改不计算以太网故障
//             (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
}



// 输出处理函数
static void brakeOutput(void *signal_id)
{
    (void)signal_id;
    // 根据电机状态，填充发送数据结构，发送信号 
    switch (brake_data.brake_motor_state)
    {
    case 1: // 电机前进状态
        un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
        un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
        un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
        un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
        printf("Brake: Motor forward\n");
        break;

    case 2: // 电机后退状态
        un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
        un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); 
        un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
        un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();        
        printf("Brake: Motor reverse\n");
        break;

    default:
        un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
        un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
        un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
        un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();        
        printf("Brake: Motor off\n");
        break;
    }  
    
    publishMessage(&un_h_bridge_output, 1);
    publishMessage(&un_inf_can_kgf_output1, 1);     
}



// 修改刹车定时器处理函数
static void brakeTimerProcess(void *signal_id)
{
    (void)signal_id;
    
//#ifdef OIL_BRAKE        
    switch (brake_data.state)
    {
    case BRAKE_STATE_IDLE:
        if (shouldApplyBrake())
        {
            brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
            if( 0 == brake_data.brake_direction)
            {
               brake_data.brake_motor_state = 1;               
            }
            else
            {
               brake_data.brake_motor_state = 2;                    
            }            
            brakeOutput(NULL);
            timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
        }
        break;

    case BRAKE_STATE_BRAKE_ON:
        if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
        {
            brake_data.state = BRAKE_STATE_RELEASING_BRAKE;

            if( 0 == brake_data.brake_direction)
            {
               brake_data.brake_motor_state = 2;               
            }
            else
            {
               brake_data.brake_motor_state = 1;                    
            }         
            brakeOutput(NULL);
            timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
        }
        break;

    case BRAKE_STATE_APPLYING_BRAKE:
        if (!brake_data.brake_apply_timer.active)
        {
            brake_data.state = BRAKE_STATE_BRAKE_ON;
            brake_data.brake_motor_state = 0;
            brakeOutput(NULL);
            brake_data.brake_position = 1; // 刹车位置：1表示刹车
        }
        break;

    case BRAKE_STATE_RELEASING_BRAKE:
        if (!brake_data.brake_release_timer.active)
        {
            brake_data.state = BRAKE_STATE_IDLE;
            brake_data.brake_motor_state = 0;
            brakeOutput(NULL);
            brake_data.brake_position = 0; // 刹车位置：0表示未刹车
        }
        break;

    default:
        printf("ERROR: Unknown state\n");
        brake_data.state = BRAKE_STATE_IDLE;
        break;
    }
    // 如果刹车位置有变化，存入EEPROM
    if (brake_data.brake_position != brake_data.old_brake_position)
    {
        setParam("brk_pos", (float)brake_data.brake_position);
        brake_data.old_brake_position = brake_data.brake_position;
        
        printf("writeE2 brake_position = %d\n",brake_data.brake_position);
        
    }
    timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
}


// 处理所有输入信号的函数
static void brakeInput(void *signal_id)
{
//    BrakeSystem old_data = brake_data; // 定义并初始化old_data

    // 填充数据
    if (signal_id == &un_sw_sample)
    {
        brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
    }
    else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
    {
        brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
        brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
    }
    else if (signal_id == &can_fault_info)
    {
        brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
        brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
    }
    else if (signal_id == &ethernet_fault_Info)
    {
        brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
    }

}

void paramUpdate(void *signal_id)
{
   brake_data.brake_direction = (uint8_t)getParam("brk_rev");
   timerStart(&brake_data.brake_param_timer, 1000, 1);
}



// 修改APP模块的初始化函数
void brakeAppInit(void)
{
    // 初始化
    memset(&brake_data, 0, sizeof(BrakeSystem));
    brake_data.state = BRAKE_STATE_IDLE;
    // 初始化时恢复刹车位置
    brake_data.brake_position = (uint8_t)getParam("brk_pos");
    brake_data.old_brake_position = brake_data.brake_position;
    // 根据刹车位置恢复刹车状态
    if (brake_data.brake_position == 1)
    {
        brake_data.state = BRAKE_STATE_BRAKE_ON;
    }
    else
    {
        brake_data.state = BRAKE_STATE_IDLE;
    }

    // 初始化定时器
    timerInit(&brake_data.brake_timer);
    timerInit(&brake_data.brake_apply_timer);
    timerInit(&brake_data.brake_release_timer);
    timerInit(&brake_data.brake_param_timer);     

    // 订阅输入信号,处理刹车逻辑
    subscribe(&un_sw_sample, brakeInput);
    subscribe(&un_remote_control_input, brakeInput);
    subscribe(&can_fault_info, brakeInput);
    subscribe(&ethernet_fault_Info, brakeInput);

    
    // 订阅定时器信号,用于状态机的定时处理
    subscribe(&brake_data.brake_timer, brakeTimerProcess);
    subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
    subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
    subscribe(&brake_data.brake_param_timer, paramUpdate);

    // 启动定时器，定期调用 brakeTimerProcess
    timerStart(&brake_data.brake_timer, 500, 1);
    timerStart(&brake_data.brake_param_timer, 1000, 1);
    
    printf("app_brake: initial OK \n");
}