#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_pid.h"

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

Timer diff_app_timer;

// 定义全局变量
DiffData diff_data;

PID_t speed_pid;
PID_t yaw_rate_pid;

PID_t left_feed_pid;
PID_t right_feed_pid;


/**
 * @brief 根据挡位和输入转矩计算输出转矩值
 * @param gear 挡位状态（STATE_FORWARD/STATE_BACKWARD/其他）
 * @param input_torque 输入转矩值
 * @return 处理后的转矩值（已包含偏移量和系数）
 */
float calculateTorqueOutput(uint8_t gear, float input_torque) 
{
    const float OFFSET = 300.0f;    // 偏移量常量
    const float SCALE_FACTOR = 100.0f;  // 缩放系数
    const float DEFAULT_VALUE = 30000.0f; // 默认输出值

    float output_torque;
    
    if (gear == STATE_FORWARD) 
    {
        output_torque = (input_torque + OFFSET) * SCALE_FACTOR;
    } 
    else if (gear == STATE_BACKWARD) 
    {
        output_torque = (-input_torque + OFFSET) * SCALE_FACTOR;
    }
    else 
    {
        output_torque = DEFAULT_VALUE;
    }
    
    return output_torque;
}

/**
 * @brief 车辆状态控制状态机
 * @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护
 * @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD)
 * @param speed 当前车速（单位：km/h），0表示静止状态
 * @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向
 */
void handleVehicleState(MotorState *ctx, float speed, float torque) 
{
    switch (*ctx) 
    {
        // 初始状态：根据扭矩方向初始化
        case STATE_INIT:
        {
            if (torque >= 0.0f) 
            {
                *ctx = STATE_FORWARD;  // 正扭矩进前进档
            }
            else 
            {
                *ctx = STATE_BACKWARD; // 负扭矩进倒档
            }
            break;
        }

        // 前进状态：零速且反向扭矩超阈值切倒档
        case STATE_FORWARD:
        {
            if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) ) 
            {
                *ctx = STATE_BACKWARD;  // 满足条件切换
            }
            else 
            {
                *ctx = STATE_FORWARD;   // 否则保持
            }
            break;
        }

        // 倒车状态：零速且正向扭矩超阈值切前进
        case STATE_BACKWARD:
        {
            if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) ) 
            {
                *ctx = STATE_FORWARD;   // 满足条件切换
            }
            else 
            {
                *ctx = STATE_BACKWARD;  // 否则保持
            }
            break;
        }
        
        default:;  // 异常处理
    }
}


// 设置电机输出
void setMotorOutput(float *out_torq, float max_torque, uint16_t left_feed_power,uint16_t right_feed_power, uint16_t discharge_power)
{
  
    float abs_left_front_speed  = 0;
    float abs_right_front_speed = 0;
    float abs_left_rear_speed   = 0; 
    float abs_right_rear_speed  = 0;
    
    // 档位
    un_motor_output1.bit_data.gear = (out_torq[0] >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
    un_motor_output2.bit_data.gear = (out_torq[1] >= 0) ? 1 : 2;
    un_motor_output3.bit_data.gear = (out_torq[2] >= 0) ? 2 : 1; //20250717 2,3电机反相
    un_motor_output4.bit_data.gear = (out_torq[3] >= 0) ? 2 : 1;

    abs_left_front_speed  = fabsf(out_torq[0]); //根据挡位增加转矩方向
    abs_right_front_speed = fabsf(out_torq[1]);     
    abs_left_rear_speed   = fabsf(out_torq[2]);     
    abs_right_rear_speed  = fabsf(out_torq[3]);     

 
    // 设置左右电机期望转速
    un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_front_speed) + 30000); // 20240921 增加偏移量 30000
    un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_front_speed) + 30000); // 20240921 增加偏移量 30000
    un_motor_output3.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_rear_speed) + 30000); // 20240921 增加偏移量 30000
    un_motor_output4.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_rear_speed) + 30000); // 20240921 增加偏移量 30000


    // 设置模式为扭矩模式
    un_motor_output1.bit_data.mode = MOTOR_MODE; 
    un_motor_output2.bit_data.mode = MOTOR_MODE; 
    un_motor_output3.bit_data.mode = MOTOR_MODE; 
    un_motor_output4.bit_data.mode = MOTOR_MODE;     

    // 设置扭矩
    un_motor_output1.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
    un_motor_output2.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
    un_motor_output3.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
    un_motor_output4.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量    
    
    // 设置馈电功率
    un_motor_output1.bit_data.feed_power = left_feed_power;
    un_motor_output2.bit_data.feed_power = right_feed_power;
    un_motor_output3.bit_data.feed_power = left_feed_power;
    un_motor_output4.bit_data.feed_power = right_feed_power;    

    // 设置放电功率
    un_motor_output1.bit_data.discharge_power = discharge_power;
    un_motor_output2.bit_data.discharge_power = discharge_power; 
    un_motor_output3.bit_data.discharge_power = discharge_power;
    un_motor_output4.bit_data.discharge_power = discharge_power;     
}


// 限制值在最小值和最大值之间
float constrain(float value, float min_val, float max_val)
{
    if (value < min_val)
    {
        return min_val;
    }
    else if (value > max_val)
    {
        return max_val;
    }
    else
    {
        return value;
    }
}

// 计算当前速度、角速度
uint8_t calculateCurrentSpeedYawRate(void)
{
    // 获取轮子周长
    float wheel_circumference = (float)getParam("whl_dia") * M_PI;
    // 获取减速比
    float gear_ratio = (float)getParam("gRatio");
    if (fabsf(gear_ratio) < EPSILON)
    {
        return 0; // 避免除以0的情况
    }
    // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
    
    
    float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
    float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
    
//    float left_speed_mps = 0;
//    float right_speed_mps = 0;    
    
//    float left_speed_mps  = 0;
//    float right_speed_mps = 0;
    
    // 计算当前速度
    diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f;
    // 计算速度差
    float speed_diff = left_speed_mps - right_speed_mps;
    // 计算角速度
    float wheel_base = (float)getParam("whl_bas");
    if (fabsf(wheel_base) < EPSILON)
    {
        return 0; // 避免除以0的情况
    }
    diff_data.yaw_rate = speed_diff / wheel_base;

    return 0;
}


// 计算加速度
float calculateAcceleration(float speed, float previous_speed, float dt)
{
    if (fabs(dt) < EPSILON)
    {
        return 0; // 避免除以0的情况
    }
    float acceleration = (speed - previous_speed) / dt;
    return acceleration;
}

// 计算减速度
float calculateDeceleration(float speed, float previous_speed, float dt)
{
    if (fabs(dt) < EPSILON)
    {
        return 0; // 避免除以0的情况
    }
    float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度
    return deceleration;
}

// 计算最大速度
float calculateMaxSpeed()
{
    // 获取最大电机转速 (RPM)
    float max_rpm = (float)getParam("max_rpm");
    // 获取轮子周长
    float wheel_circumference = (float)getParam("whl_dia") * M_PI;
    // 获取减速比
    float gear_ratio = (float)getParam("gRatio");
    if (fabsf(gear_ratio) < EPSILON)
    {
        return 0; // 避免除以0的情况
    }
    // 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
    float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio);
    return max_speed;
}

// 计算最大加速度
float calculateMaxAcceleration(void)
{
    // 获取车辆参数
    float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩
    float vehicle_mass = (float)getParam("VehMass"); // 车辆质量
    float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径
    float gear_ratio = (float)getParam("gRatio"); // 减速比
    if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON )
    {
        return 0; // 避免除以0的情况
    }
    // 减速比计算扭矩
    float effective_torque = max_motor_torque * gear_ratio;
    // 计算最大加速度
    float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass;
    return max_acceleration;
}

// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
void calculateCurrentState(float dt)
{
    static float previous_speed = 0.0f;
    // 更新当前速度和当前角速度
    calculateCurrentSpeedYawRate();
    // 更新加速度、减速度等，根据需要计算
    diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt);
    diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt);
    diff_data.max_speed = calculateMaxSpeed();
    previous_speed = diff_data.speed;
}
/**
 * @brief 判断减速状态（最简逻辑）  如果同向或者有一个为0，或者都为0，那么判断绝对值大小，如果期望绝对值小于当前绝对值，那就为减速
 * @param target_speed  期望速度（带方向）
 * @param current_speed 当前速度（带方向）
 * @return 1:减速, 0:加速或保持
 */
uint8_t is_Decelerating(float target_speed, float current_speed, float des_yaw_rate)
{
    // 特殊处理双零状态,双零表示刹车
    if ( (target_speed == 0.0f && current_speed == 0.0f)  )//如果又减速的话也刹车  //|| (0 != des_yaw_rate)
    {
        return 2; // 驻车
    }  
    // 核心逻辑：方向相反 或 (同向/含零且期望绝对值 < 当前绝对值)
    return (signbit(target_speed) != signbit(current_speed)) || 
           (fabs(target_speed) < fabs(current_speed));
}


// 计算左右电机速度
void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed)
{
    // 防止速度过低导致不必要的计算
    if (fabs(max_speed) < EPSILON)
    {
        motor_speed[0] = 0.0f;
        motor_speed[1] = 0.0f;        
        motor_speed[2] = 0.0f;        
        motor_speed[3] = 0.0f;        
        return;
    }

#if THROTTLE_PID_MODE
  
    float left_speed_mps  = 0.0f;  
    float right_speed_mps = 0.0f;
    
    float max_torque = diff_data.max_Torq;

    linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
    yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
    
    left_speed_mps = linear_velocity_x + yaw_rate;
    right_speed_mps = linear_velocity_x - yaw_rate;

//扭矩分配		
    if(max_torque < left_speed_mps)
    {
        right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
        left_speed_mps = max_torque;
    }
    else if(-max_torque > left_speed_mps)
    {
        right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
        left_speed_mps = -max_torque;
    } 
    else if(max_torque < right_speed_mps)
    {
        left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
        right_speed_mps = max_torque;
    }
    else if(-max_torque > right_speed_mps)
    {
        left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
        right_speed_mps = -max_torque;
    }
    else{}
    
//    printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);   
//    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);     
//    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);      
//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);  
    
    motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
    motor_speed[2] = left_speed_mps; 

    motor_speed[1] = right_speed_mps;
    motor_speed[3] = right_speed_mps;  

    handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed,   motor_speed[0]); //通过扭矩以及速度来判断挡位
    handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed,  motor_speed[1]); 
    handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed,    motor_speed[2]); 
    handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed,   motor_speed[3]); 

//    distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
//    distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);  
    
#else    
    
    // 限制线速度和偏航率
    linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed);
    float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f);
    yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate);

    // 计算旋转速度
    float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;

    // 计算车辆左右线速度 (m/s)
    float left_speed_mps = linear_velocity_x + rotational_velocity;  //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
    float right_speed_mps = linear_velocity_x - rotational_velocity;

    // 计算轮子周长
    float wheel_circumference = (float)getParam("whl_dia") * M_PI;

    // 将车辆左右线速度转换为轮子转速 (RPM)
    float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference;
    float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference;

    // 获取减速比
    float gear_ratio = (float)getParam("gRatio");

    // 将轮子转速转换为电机转速，考虑减速比
    float left_motor_rpm = left_wheel_rpm * gear_ratio;
    float right_motor_rpm = right_wheel_rpm * gear_ratio;

    // 限制电机的最大和最小转速
    float max_motor_rpm = (float)getParam("max_rpm");
    left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm);
    right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
    
    // 当电机转速小于50转时，设置为0
//    if (fabsf(left_motor_rpm) < 20)//速度慢所以设置位10转
//    {
//        left_motor_rpm = 0;
//    }
//    if (fabsf(right_motor_rpm) < 20)//速度慢所以设置位10转
//    {
//        right_motor_rpm = 0;
//    }
    // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
 //   left_motor_rpm = -left_motor_rpm;
    // 返回计算结果
    motor_speed[0] = left_motor_rpm;//加速状态，没有负扭矩，要么前进加速要么后退加速
    motor_speed[2] = left_motor_rpm; 

    motor_speed[1] = right_motor_rpm;
    motor_speed[3] = right_motor_rpm;
 
    diff_data.left_motor_state = is_Decelerating(left_motor_rpm, diff_data.left_motor_speed, diff_data.desired_yaw_rate);
    diff_data.right_motor_state = is_Decelerating(right_motor_rpm, diff_data.right_motor_speed, diff_data.desired_yaw_rate);
    
//    printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);      
    
    //馈电PID计算
    static float previous_time11 = 0.0f;

    float time1 = (float)getCurrentTime();
    float dt = (time1 - previous_time11) / PERIOD_TICK; 
    previous_time11 = time1;    
    
    float left_feed_pwoer = calculatePidOutput(&left_feed_pid, left_motor_rpm, diff_data.left_motor_speed, 0.0f, dt);//左右馈电PID
    float right_feed_pwoer = calculatePidOutput(&right_feed_pid, right_motor_rpm, diff_data.right_motor_speed, 0.0f, dt);    
    
    if(1 == diff_data.left_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
    {
       diff_data.left_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
    }
    else if(2 == diff_data.left_motor_state)
    {
       diff_data.left_motor_feed_power = diff_data.max_feed_power;
    }
    else
    {
       diff_data.left_motor_feed_power = 0.0f;      
    }
    
    if(1 == diff_data.right_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
    {
       diff_data.right_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
    }
    else if(2 == diff_data.left_motor_state)
    {
       diff_data.right_motor_feed_power = diff_data.max_feed_power;
    }    
    else
    {
       diff_data.right_motor_feed_power = 0.0f;      
    }   
    
    //限制最大馈电功率
    if(diff_data.left_motor_feed_power > diff_data.max_feed_power)
    {
       diff_data.left_motor_feed_power = diff_data.max_feed_power;
    }
    
    if(diff_data.right_motor_feed_power > diff_data.max_feed_power)
    {
       diff_data.right_motor_feed_power = diff_data.max_feed_power;
    }    

#endif    
}

// 映射遥控器速度，分为死区、低速区和高速区。
float mapRemoteControlSpeed(
    float input_speed,
    float deadzone_limit,
    float input_max,
    float output_max,
    float input_slow,
    float output_slow
)
{
    float output_speed = 0.0f;
    // 获取输入速度的绝对值
    float abs_input = fabsf(input_speed);
//diff_data.desired_speed, 0.1, 2, 10, 1, 5

//  diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);

    if (abs_input < deadzone_limit + EPSILON)
    {
        output_speed = 0.0f;// 死区
    }
    else if (abs_input < input_slow + EPSILON)// 低速区
    {
        output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit);//
    }
    else if (abs_input <= input_max + EPSILON)// 高速区
    {
        output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow);// 0.2 + （3 - 0.5）*  （15-0.2） / （17 - 0.5）   
    }
    else // 超出范围
    {
        output_speed = output_max;
    }
    
    // 根据原始输入速度的符号恢复方向
    if (input_speed < 0)
    {
        output_speed = -output_speed;
    }
    return output_speed;
}



// 差速处理函数
static void diffProcess(void *signal_id)
{
    (void)signal_id;
    static float previous_time1 = 0.0f;

    float time1 = (float)getCurrentTime();
    float dt = (time1 - previous_time1) / PERIOD_TICK; 
    previous_time1 = time1;

    // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
    calculateCurrentState(dt);

    // 当速度小于1时，设定为原地转向  20250321 修改为考虑负号
    if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) )
    {
        diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f;     
    }
    else if( (diff_data.desired_speed < 0) && (diff_data.desired_speed >= -1.0f) )
    {
         diff_data.desired_yaw_rate = diff_data.desired_curvature * -1.0f;
    }
    else
    {
        diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
    }
    
//    printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
    // 使用 PID 控制器计算输出速度和曲率
    float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt);
    float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt);
    
    // 计算最大加速度，用函数计算
    float max_acceleration = calculateMaxAcceleration();
    // 限制输出速度在当前速度和最大加速度计算出来的速度之间
    // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);

    // if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 
    // {
    //     resetPidIntegral(&speed_pid);
    //     resetPidIntegral(&yaw_rate_pid);        
    //     output_speed = 0;
    //     output_yaw_rate = 0;
    // }   
    
    // 使用差速车辆动力学模型计算左右电机的期望速度
    computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
   

    // 设置电机输出
    setMotorOutput(&diff_data.out_torq[0], 
                   diff_data.max_Torq,//
                   diff_data.left_motor_feed_power, 
                   diff_data.right_motor_feed_power,                    
                   (uint16_t)getParam("dispPwr"));
    // 发布左右电机期望转速,电源在工作状态才能发送
    if (power_data.current_state == POWER_WORKING)
    {
        publishMessage(&un_motor_output1, 1);
        publishMessage(&un_motor_output2, 1);    
        publishMessage(&un_motor_output3, 1);
        publishMessage(&un_motor_output4, 1); 
    }
    
    
    un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10);
    un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10);    
    
    un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
    un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
    
//    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_speed_diff);
//    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);    
    
    publishMessage(&diff_data, 1);     
    
}
// 差速输入处理函数
static void diffInput(void *signal_id)
{ 
    if (signal_id == &un_sw_sample)
    {
        diff_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) )// 遥控器断线，不更新数据
    {
        diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b;
        diff_data.mode = un_remote_control_input.bit_data.switch_c == 1 ? MODE_AUTO : MODE_MANUAL;

        if (diff_data.mode == MODE_MANUAL)
        {
            diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
            diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
            // 单位转换
            diff_data.desired_speed = diff_data.desired_speed * 0.01f;
            diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
            // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
            diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);         
            diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
            
            diff_data.desired_curvature = -diff_data.desired_curvature;
            
            if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题   
            {
                diff_data.desired_curvature = diff_data.desired_curvature;               
            }
            else
            {
                diff_data.desired_curvature = -diff_data.desired_curvature;                 
            }           
        }
    }
    else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) )
    {
        diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed));
        diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature));
        // 单位转换
        diff_data.desired_speed = diff_data.desired_speed * 0.01f;
        diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
        // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
        diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.2, 2, 10, 1, 5);//20250320  修改死区为0.2解决停不住的问题
        diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
    }
    else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) )
    {
        diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed));
        diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature));
        // 单位转换
        diff_data.desired_speed = diff_data.desired_speed * 0.01f;
        diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;//  20241016 增加转弯反相
        // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
        diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5);
        diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
    } 
    else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机）
    {
        diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量
        diff_data.left_rear_motor_speed  = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
        diff_data.left_rear_motor_speed  = - diff_data.left_rear_motor_speed;//20250708 增加反相   
        
        if( fabs(diff_data.left_front_motor_speed) < 20)//速度死区
        {
            diff_data.left_front_motor_speed = 0;
        }
        
        if( fabs(diff_data.left_rear_motor_speed) < 20)//速度死区
        {
            diff_data.left_rear_motor_speed = 0;
        }        
        
//        if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
//        {
//           motor_speed_temp = diff_data.left_front_motor_speed;
//        }
//        else
//        {
//           motor_speed_temp = diff_data.left_rear_motor_speed;           
//        }
//        diff_data.left_motor_speed = motor_speed_temp;
        
        diff_data.left_motor_speed = (diff_data.left_front_motor_speed + diff_data.left_rear_motor_speed)/2.0f;        
    }
    else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
    {
        diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000));  // 20250502 1号控制器增加反相 
        
        diff_data.right_rear_motor_speed  = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));   
        diff_data.right_rear_motor_speed  = - diff_data.right_rear_motor_speed;//20250708 增加反相 

        if( fabs(diff_data.right_front_motor_speed) < 20)//速度死区
        {
            diff_data.right_front_motor_speed = 0;
        }
        
        if( fabs(diff_data.right_rear_motor_speed) < 20)//速度死区
        {
            diff_data.right_rear_motor_speed = 0;
        }     
        
        
//        if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
//        {
//           motor_speed_temp = diff_data.right_rear_motor_speed;
//        }
//        else
//        {
//           motor_speed_temp = diff_data.right_front_motor_speed;           
//        }

        diff_data.right_motor_speed = (diff_data.right_rear_motor_speed + diff_data.right_front_motor_speed)/2.0f; 
    }
    
    // 急停开关
    diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close());

    // 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关
    if (diff_data.emergency_stop_state == 1)
    {
        diff_data.desired_speed = 0.0;
        diff_data.desired_curvature = 0.0;
    }
    // 遥控器断线，而且是在手动模式，期望值清0
    if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) )
    {
        diff_data.desired_speed = 0.0;
        diff_data.desired_curvature = 0.0;
    }
    
    if (diff_data.emergency_stop_state == 1)//刹车  20241017 增加的扭矩限制
    {
        diff_data.max_Torq = 5;//20240403修改。刹车就是5N
    }
    else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩
    {
        diff_data.max_Torq = 5;//停车 就为0  20250425 修改为5，解决手柄回中，震荡问题
    }
    else
    {
        diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
    }
    if((power_data.current_state == POWER_WORKING))//电机上电才运行
    {
        diffProcess(&diff_data);//计算左右电机期望转速
    }
    else
    {
        resetPidIntegral(&speed_pid);
        resetPidIntegral(&yaw_rate_pid);      
    }

}



// 预充完成处理函数
void preChargeFinish(void *signal_id)
{
    (void)signal_id;
    
    float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};

    setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), 0,0, (uint16_t)getParam("dispPwr"));
    // 档位
    un_motor_output1.bit_data.gear = 0; // 0表示空挡
    un_motor_output2.bit_data.gear = 0;
    un_motor_output3.bit_data.gear = 0; // 0表示空挡
    un_motor_output4.bit_data.gear = 0;    
    
    publishMessage(&un_motor_output1, 1);
    publishMessage(&un_motor_output2, 1);
    publishMessage(&un_motor_output3, 1);
    publishMessage(&un_motor_output4, 1);    
}


void diffParametersInit(void *signal_id)
{
    (void)signal_id;  // 标记变量为已使用，避免编译器警告
    
    if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID
    {
        setPidParameters(&speed_pid,
                          getParam("Ospd_kp"),
                          getParam("Ospd_ki"),
                          getParam("Ospd_kd"),
                          getParam("Ospd_il"),
                          getParam("Ospd_ol")
         );

        setPidParameters(&yaw_rate_pid,
                          getParam("Ocrv_kp"),
                          getParam("Ocrv_ki"),
                          getParam("Ocrv_kd"),
                          getParam("Ocrv_il"),
                          getParam("Ocrv_ol")
         );         
    }
    else//手动模式
    {
        setPidParameters(&speed_pid,
                          getParam("spd_kp"),
                          getParam("spd_ki"),
                          getParam("spd_kd"),
                          getParam("spd_il"),
                          getParam("spd_ol")
         );

        setPidParameters(&yaw_rate_pid,
                          getParam("crv_kp"),
                          getParam("crv_ki"),
                          getParam("crv_kd"),
                          getParam("crv_il"),
                          getParam("crv_ol")
         );       
    }
    
    // 设置曲率 PID 控制器的参数
     setPidParameters(&left_feed_pid,
                      getParam("mot_kp"),
                      getParam("mot_ki"),
                      getParam("mot_kd"),
                      getParam("mot_il"),
                      getParam("mot_ol")
     );     
     
    // 设置曲率 PID 控制器的参数
     setPidParameters(&right_feed_pid,
                      left_feed_pid.kp,
                      left_feed_pid.ki,
                      left_feed_pid.kd,
                      left_feed_pid.integral_limit,
                      left_feed_pid.output_limit    
     );   
     
//    diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩   
//    diff_data.max_Torq = (float)getParam("maxTorq"); 
     
     diff_data.max_feed_power = (uint16_t)getParam("feedPwr");
    
    // if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
    // {
    //    diff_data.diff_dead_zone = 2;
    // }
    // else
    // {
    //    diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩       
    // }   
    
    printf("left_speed: %f, des_speed: %f,left_feed: %d\n", diff_data.left_motor_speed, diff_data.out_torq[0],diff_data.left_motor_feed_power);    //left_motor_rpm, diff_data.left_motor_speed
    
    printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
    printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);   
//    printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);     
    
    printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\n",diff_data.left_front_motor_speed,diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed,diff_data.right_rear_motor_speed);
    
//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);     
    printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);    
    
    float deffspeed  = (float)((int16_t)(un_remote_control_input.bit_data.speed));
    float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
    // 单位转换
    deffspeed = deffspeed * 0.01f;
    deffcurvature = deffcurvature * 0.0001f;   
    
    printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);    
    
    printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);       
     
    timerStart(&diff_app_timer,1000,1);//1s调用一次 
}

// 差速初始化函数
void diffAppInit(void)
{
    // 初始化 diff_data
    memset(&diff_data, 0, sizeof(DiffData)); 

    // 订阅相关信号
    subscribe(&un_sw_sample, diffInput);            // 急停开关、高压开关
    subscribe(&un_motor_input1, diffInput);
    subscribe(&un_motor_input2, diffInput);
    
    
    subscribe(&un_auto_computer_input, diffInput);
    subscribe(&un_manual_computer_input, diffInput);
    subscribe(&un_remote_control_input, diffInput);   
    subscribe(&power_data.pre_charge_finish, preChargeFinish);

    // 初始化速度 PID 控制器
    initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置速度 PID 控制器的参数
     setPidParameters(&speed_pid,
                      getParam("spd_kp"),
                      getParam("spd_ki"),
                      getParam("spd_kd"),
                      getParam("spd_il"),
                      getParam("spd_ol")
     );

    // 初始化曲率 PID 控制器
    initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置曲率 PID 控制器的参数
     setPidParameters(&yaw_rate_pid,
                      getParam("crv_kp"),
                      getParam("crv_ki"),
                      getParam("crv_kd"),
                      getParam("crv_il"),
                      getParam("crv_ol")
     );

    // 初始化减速 PID 控制器
    initializePid(&left_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置 PID 控制器的参数
     setPidParameters(&left_feed_pid,
                      getParam("mot_kp"),
                      getParam("mot_ki"),
                      getParam("mot_kd"),
                      getParam("mot_il"),
                      getParam("mot_ol")
     );     
     
    // 初始化加速 PID 控制器
    initializePid(&right_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置 PID 控制器的参数
     setPidParameters(&right_feed_pid,
                      left_feed_pid.kp,
                      left_feed_pid.ki,
                      left_feed_pid.kd,
                      left_feed_pid.integral_limit,
                      left_feed_pid.output_limit
     );            
     
     
    subscribe(&diff_app_timer, diffParametersInit);
    timerStart(&diff_app_timer,1000,1);//1s调用一次 

    printf("diffControl: diffAppInit OK \n");
}