#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;
Timer diff_dir_timer;


// 定义全局变量
DiffData diff_data;

PID_t speed_pid;
PID_t yaw_rate_pid;


PID_t Acc_front_speed_pid;
PID_t Dec_front_speed_pid;



// 设置电机输出
void setMotorOutput(float *out_speed, uint16_t max_torque, uint16_t feed_power, uint16_t discharge_power)
{   
    float abs_speed[4] = {0.0,0.0,0.0,0.0};
//--------------------------------------------------------------    
    if(out_speed[2] > 0)
    {
       un_motor_output1.bit_data.gear = 1;
    }
    else if(out_speed[2] < 0)
    {
       un_motor_output1.bit_data.gear = 2;          
    }
    else{}
    
    if(out_speed[0] > 0)
    {
       un_motor_output3.bit_data.gear = 1;
    }
    else if(out_speed[0] < 0)
    {
       un_motor_output3.bit_data.gear = 2;          
    }
    else{}    
    
    if(out_speed[1] > 0)
    {
       un_motor_output2.bit_data.gear = 1;
    }
    else if(out_speed[1] < 0)
    {
       un_motor_output2.bit_data.gear = 2;          
    }
    else{}       
    
    
    if(out_speed[3] > 0)
    {
       un_motor_output4.bit_data.gear = 1;
    }
    else if(out_speed[3] < 0)
    {
       un_motor_output4.bit_data.gear = 2;          
    }
    else{}       

    
//    if(diff_data.out_motor_dir != 0)//如果为0那就是空挡，那就保留之前的挡位否则以输入的速度为挡位
//    {
//        un_motor_output1.bit_data.gear = diff_data.out_motor_dir;
//        un_motor_output2.bit_data.gear = diff_data.out_motor_dir;        
//        un_motor_output3.bit_data.gear = diff_data.out_motor_dir;        
//        un_motor_output4.bit_data.gear = diff_data.out_motor_dir;       
//    }
//    
    
//    un_motor_output1.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退
//    un_motor_output3.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退    
                                                             
//    un_motor_output2.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2;
//    un_motor_output4.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2;

    
    for(uint8_t i = 0; i<4; i++)
    {
//       if(out_speed[i] < 0) //给负扭矩就发0，刹车
//       {
//          out_speed[i] = 0;
//       }
       abs_speed[i] = fabs(out_speed[i]);//要扩大10倍  扭矩比例是10
    }
  
    un_motor_output1.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000//限定值为256
    un_motor_output1.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000
    un_motor_output3.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000//限定值为256
    un_motor_output3.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000
    
    un_motor_output2.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000    
    un_motor_output2.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000
    un_motor_output4.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[3]) ); // 20240921 增加偏移量 30000    
    un_motor_output4.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[3]) ); // 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.motor_direction = 1;//电机2和3是反相的 
//    un_motor_output2.bit_data.motor_direction = 1;
    un_motor_output1.bit_data.motor_direction = 1;//电机2和3是反相的 
    un_motor_output2.bit_data.motor_direction = 0;    
    un_motor_output3.bit_data.motor_direction = 1;
    un_motor_output4.bit_data.motor_direction = 0;     
    
    
    // can挡位一直有效
    un_motor_output1.bit_data.can_gear = 1;
    un_motor_output2.bit_data.can_gear = 1;
    un_motor_output3.bit_data.can_gear = 1;
    un_motor_output4.bit_data.can_gear = 1;
    
    un_motor_output1.bit_data.can_break = 1;
    un_motor_output2.bit_data.can_break = 1;
    un_motor_output3.bit_data.can_break = 1;
    un_motor_output4.bit_data.can_break = 1;    
}

// 限制值在最小值和最大值之间
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);
    // 计算当前速度
    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 根据轮速差动态调整电机扭矩（带非负限制）
 * @param speed_left 左轮速度（单位：rpm或自定义）
 * @param speed_right 右轮速度（单位：rpm或自定义）
 * @param torque_left 左轮扭矩指针（单位：Nm或自定义）
 * @param torque_right 右轮扭矩指针（单位：Nm或自定义）
 * @param threshold 触发调整的速差阈值（单位同轮速）
 * @param k 扭矩调整系数（无量纲，建议0<k<1）
 * @note 函数会直接修改传入的扭矩值，并确保扭矩不小于0
 */
void adjust_torque_by_speed_diff(float speed_left, float speed_right, 
                               float* torque_left, float* torque_right,
                               float threshold, float k) 
{
    // 计算轮速差绝对值
    float speed_diff = fabsf(speed_left - speed_right);
    
    if (speed_diff > threshold) {
        // 计算需要减少的扭矩量（速差超出阈值部分×系数）
        float torque_reduction = (speed_diff - threshold) * k;
        
        if (speed_left > speed_right) {
            // 左轮过快时减少左扭矩，并限制最小值为0
            *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f);
        } else {
            // 右轮过快时减少右扭矩，并限制最小值为0
            *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f);
        }
    }
}

// 计算左右电机速度
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 max_torque = (float)getParam("maxTorq");
    
    linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
    yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);    
    
    float left_speed_mps = linear_velocity_x + yaw_rate;
    float 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{}
    
    // 返回计算结果
    motor_speed[0] = left_speed_mps;
    motor_speed[2] = left_speed_mps; 

    motor_speed[1] = right_speed_mps;
    motor_speed[3] = right_speed_mps; 
    
//    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],50, 0.2);     
//    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],50, 0.2);  
    
#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;
    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) < 50)//速度慢所以设置位10转
    {
        left_motor_rpm = 0;
    }
    if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转
    {
        right_motor_rpm = 0;
    }
    
    // 返回计算结果
    *left_motor_speed = left_motor_rpm;
    *right_motor_speed = right_motor_rpm;
#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);

    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);
    }
    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时，设定为原地转向
    if (fabsf(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;
    }

    
    if( ( (diff_data.desired_speed > 0) && (diff_data.speed > 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed < 0) )  )//同相
    {
        if( fabs(diff_data.desired_speed) > fabs(diff_data.speed) )//期望大于当前速度，表示加速，所以用大PID
        {
           setPidParameters(&speed_pid,
                             Acc_front_speed_pid.kp,
                             Acc_front_speed_pid.ki,
                             Acc_front_speed_pid.kd,
                             Acc_front_speed_pid.integral_limit,
                             Acc_front_speed_pid.output_limit
           );           
        }
        else
        {
           setPidParameters(&speed_pid,
                             Dec_front_speed_pid.kp,
                             Dec_front_speed_pid.ki,
                             Dec_front_speed_pid.kd,
                             Dec_front_speed_pid.integral_limit,
                             Dec_front_speed_pid.output_limit
           );             
            
        }
    }//反相就先小PID减速
    else
    {
       setPidParameters(&speed_pid,
                         Dec_front_speed_pid.kp,
                         Dec_front_speed_pid.ki,
                         Dec_front_speed_pid.kd,
                         Dec_front_speed_pid.integral_limit,
                         Dec_front_speed_pid.output_limit
       );             
    }
    
    
    
//    float positive_speed = fabs(diff_data.desired_speed);//取绝对值  20250701 扭矩控制不需要考虑正负
//    
//    if(diff_data.desired_speed < 0)//判断方向 小于0就反转，大于0电机正转
//    {
//       diff_data.out_motor_dir = 2;
//    }
//    else if(diff_data.desired_speed > 0)
//    {
//       diff_data.out_motor_dir = 1;      
//    }
//    else
//    {
//       diff_data.out_motor_dir = 0;  
//    }
    
    // 使用 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();
    
//    if( ( (output_speed > 0) && (diff_data.speed < 0) ) || ( (output_speed < 0) && (diff_data.speed > 0) ) )//方向相反不给反方向扭矩，直接清0，给反方向扭矩转速有问题
//    {
//        if( ( (diff_data.desired_speed > 0) && (diff_data.speed < 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed > 0) ) )//期望和当前速度相反的话，那直接运行反方向扭矩，解决下坡不能减速的问题。
//        {
//          
//        }
//        else
//        {
//            output_speed = 0;       
//            resetPidIntegral(&speed_pid);              
//        }
//    }
//    
//    if( (0 == output_speed) && (diff_data.desired_speed != 0) )//速度超过解决发0直接减速的问题
//    { 
//        output_speed = 1; 
//    }
    
    
    
    // 限制输出速度在当前速度和最大加速度计算出来的速度之间 
    // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);

//    printf("output_speed: %f, output_yaw: %f\n", output_speed, output_yaw_rate);
    
//    if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && (diff_data.left_motor_speed < 50.0f) && (diff_data.right_motor_speed > -50.0f))//手柄回中，速度小的时候清0 
//    {
//        resetPidIntegral(&speed_pid);
//        resetPidIntegral(&yaw_rate_pid);        
//        output_speed = 0;
//        output_yaw_rate = 0;
//    }          
//    
    
    if( (0 == diff_data.desired_speed) && ( fabs(diff_data.left_motor_speed) < 10 ) && ( fabs(diff_data.right_motor_speed) < 10) )//手柄回中，速度小的时候清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_motor_speed[0]);
    
//    diff_data.out_motor_speed[0] = calculatePidOutput(&Left_front_speed_pid, diff_data.out_left_motor_speed, diff_data.left_front_motor_speed, 0.0f, dt);    
//    diff_data.out_motor_speed[1] = calculatePidOutput(&right_front_speed_pid, diff_data.out_right_motor_speed, diff_data.right_front_motor_speed, 0.0f, dt);      
//    diff_data.out_motor_speed[2] = calculatePidOutput(&Left_Rear_speed_pid, diff_data.out_left_motor_speed, diff_data.left_rear_motor_speed, 0.0f, dt);    
//    diff_data.out_motor_speed[3] = calculatePidOutput(&right_Rear_speed_pid, diff_data.out_right_motor_speed, diff_data.right_rear_motor_speed, 0.0f, dt);      
    
//    diff_data.out_motor_speed[0] = diff_data.out_left_motor_speed;
//    diff_data.out_motor_speed[2] = diff_data.out_left_motor_speed; 
//    
//    diff_data.out_motor_speed[1] = diff_data.out_right_motor_speed;
//    diff_data.out_motor_speed[3] = diff_data.out_right_motor_speed;     
    
    // 设置电机输出 
    setMotorOutput(&diff_data.out_motor_speed[0],  
                   diff_data.max_Torq,
                   (uint16_t)getParam("feedPwr"), 
                   (uint16_t)getParam("dispPwr"));
    // 发布左右电机期望转速,电源在工作状态才能发送
    if (power_data.current_state == POWER_WORKING)
    {
        publishMessage(&un_motor_output1, 1);
        publishMessage(&un_motor_output2, 1);                
    }
    publishMessage(&diff_data, 1);    
}


/**
 * @brief 电机状态处理函数
 * @param ctx 电机状态上下文指针，包含当前状态、转速和输出扭矩数据
 * @param original_dir 电机原始方向
 * @note 该函数实现基于转速和扭矩方向的状态转换逻辑：
 *       - 初始态：转速为0时根据扭矩方向切换前进/后退状态
 *       - 前进/后退态：转速归零时返回初始态
 *       - 默认处理：异常状态自动复位到初始态
 * @warning 需确保ctx->out_motor_speed[0]已正确初始化（正数表前进，负数表后退）
 */
void handleMotorState(DiffData *ctx,uint8_t original_dir)
{
    switch (ctx->state)
    {
        case STATE_INIT://初始
        {
            if (ctx->speed == 0.0f)
            {
                if (ctx->out_motor_speed[0] > 0.0f)  // 修改为数组索引
                {
                    ctx->state = STATE_FORWARD;   // 扭矩为正 → 前进
                    ctx->motor_dir = FORWARD;
                }
                else if (ctx->out_motor_speed[0] < 0.0f)  // 修改为数组索引
                {
                    ctx->state = STATE_BACKWARD;  // 扭矩为负 → 后退
                    ctx->motor_dir = BACKWARD;                    
                }
                else
                {
                    ctx->state = STATE_INIT;                 
                }
            }
            else//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向
            {
               if(1 == original_dir) 
               {
                   ctx->motor_dir = FORWARD; 
                   ctx->state = STATE_FORWARD;   // 保持前进  
               }
               else
               {
                   ctx->motor_dir = BACKWARD;
                   ctx->state = STATE_BACKWARD;   // 保持后退                    
               }  
            }
              
            break;
        }
        case STATE_FORWARD://前进
        {
            if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) )
            {
                ctx->state = STATE_INIT;
            }
            else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] < 0.0f) )//直接给反相扭矩减速
            {
                ctx->motor_dir = BACKWARD;
                ctx->state = STATE_BACKWARD;   // 保持后退
            }
            else
            {
                ctx->motor_dir = FORWARD; 
                ctx->state = STATE_FORWARD;   // 保持前进                
            }
            break;
        }
        
//        case DEC_FORWARD://前进减速
//        {
//            if(ctx->speed > 0)//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向
//            {
//                 ctx->motor_dir = BACKWARD;
//                 ctx->state = STATE_BACKWARD;   // 保持后退                    
//            }
//            else
//            {
//                ctx->state = STATE_INIT;              
//            }
//            break;
//        }        
//        
        case STATE_BACKWARD://后退
        {
            if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) )
            {
                ctx->state = STATE_INIT;
            }
            else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] > 0.0f) )//直接给反相扭矩减速
            {
                ctx->motor_dir = FORWARD; 
                ctx->state = STATE_FORWARD;   // 保持前进  
            }
            else
            {
                ctx->motor_dir = BACKWARD;
                ctx->state = STATE_BACKWARD;   // 保持后退            
            }            
            break;
        }
        
        // 异常处理
        default:
        {
            ctx->state = STATE_INIT;
            break;
        }
    }
}


/**
 * @brief 速度方向校准函数（需每100ms调用一次）
 */
void calibrateSpeedDirection(DiffData *ctx) 
{
    switch (ctx->dir_state) 
    {
        //-------------------------------------------
        // 初始状态：检测速度与扭矩方向是否冲突
        //-------------------------------------------
        case STATE_CALIB_INIT:
        {
            if (ctx->speed * ctx->out_motor_speed[0] < 0) 
            {
                // 方向冲突，保存当前值并进入判断状态
                ctx->speed_last = ctx->speed;
                ctx->torq_last = ctx->out_motor_speed[0];
                ctx->counter = 0;
                ctx->dir_state = STATE_CALIB_JUDGE;
            }
            break;
        }

        //-------------------------------------------
        // 判断状态：持续300ms验证方向异常
        //-------------------------------------------
        case STATE_CALIB_JUDGE: 
        {
            bool is_same_dir = (ctx->speed * ctx->speed_last > 0) && 
                              (ctx->out_motor_speed[0] * ctx->torq_last > 0);
            bool is_value_grow = (fabs(ctx->speed) > fabs(ctx->speed_last)) && 
                                (fabs(ctx->out_motor_speed[0]) > fabs(ctx->torq_last));

            if (is_same_dir && is_value_grow) 
            {
                ctx->counter ++;
                ctx->dir_state = STATE_CALIB_JUDGE; //保持当前状态              
                if (ctx->counter >= 3)  
                {
                    // 300ms验证通过，强制反向以及切换状态
                    if(BACKWARD == ctx->motor_dir)
                    {
                        ctx->motor_dir = FORWARD;
                        ctx->state = STATE_FORWARD;                     
                    }
                    else
                    {
                        ctx->motor_dir = BACKWARD;
                        ctx->state = STATE_BACKWARD;                         
                    }
                    ctx->dir_state = STATE_CALIB_INIT;
                }
                ctx->speed_last = ctx->speed;//判断完之后，继续保存
                ctx->torq_last = ctx->out_motor_speed[0];                
            } 
            else 
            {  
                // 验证失败回初始态
                ctx->dir_state = STATE_CALIB_INIT;
            }
            break;
        }
    }
}

void diff100ms(void *signal_id)
{
//    calibrateSpeedDirection(&diff_data);
    timerStart(&diff_dir_timer,100,1);//100ms调用一次 
}


// 差速输入处理函数
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.01, 2, 2, 1, 0.5);
            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, 10, 1, 5);
        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) )// 处理第一个电机速度信号（左侧电机）
    { 
        float speed_temp = fabs ( (float) ( (int16_t)(un_motor_input1.bit_data.speed) ) );
        
        if(1 == diff_data.motor_dir)//1为负数  20250620 修改为判断电机转动方向， 挡位不可靠
        {
           diff_data.left_rear_motor_speed = -speed_temp;
        }
        else 
        {
           diff_data.left_rear_motor_speed = speed_temp;
        }        
        
        
        if(fabs(diff_data.left_rear_motor_speed) < SWITCH_SPEED)
        {
           diff_data.left_rear_motor_speed = 0;
        }

        
        speed_temp = fabs ( (float) ( (int16_t)(un_motor_input3.bit_data.speed) ) );   
        if(1 == diff_data.motor_dir)//1为负数
        {
           diff_data.left_front_motor_speed = -speed_temp;
        }
        else
        {
           diff_data.left_front_motor_speed = speed_temp;           
        }
        
        if(fabs(diff_data.left_front_motor_speed) < SWITCH_SPEED)//死区
        {
           diff_data.left_front_motor_speed = 0;
        }
        
        if(fabs(diff_data.left_front_motor_speed) < fabs(diff_data.left_rear_motor_speed) )//取最小值,避免打滑，拉高转速
        {
             diff_data.left_motor_speed = diff_data.left_front_motor_speed;
        }
        else 
        {
             diff_data.left_motor_speed = diff_data.left_rear_motor_speed;          
        }
        
//        printf("motor1: %f, motor3: %f\n", diff_temp[0], diff_temp[1]);           
//        diff_data.left_motor_speed = 0;// 左侧两个电机所以取平均值        

    }
    else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右侧电机）
    {
      
        float speed_temp = fabs ( (float) ( (int16_t)( un_motor_input2.bit_data.speed) ) ); 
        if(1 ==  diff_data.motor_dir)//1为负数
        {
           diff_data.right_front_motor_speed = - speed_temp;
        }
        else
        {
           diff_data.right_front_motor_speed = speed_temp;
        }  
//        
        if(fabs(diff_data.right_front_motor_speed) < SWITCH_SPEED)//死区
        {
           diff_data.right_front_motor_speed = 0;
        }        
        
        
        speed_temp = fabs( (float) ( (int16_t)(un_motor_input4.bit_data.speed) ) );  
        
        if(1 == diff_data.motor_dir)//1为负数
        {
           diff_data.right_rear_motor_speed = - speed_temp;
        }
        else 
        {
           diff_data.right_rear_motor_speed = speed_temp;
        }

        if(fabs(diff_data.right_rear_motor_speed) < SWITCH_SPEED)//死区
        {
           diff_data.right_rear_motor_speed = 0;
        }  

        if( fabs(diff_data.right_front_motor_speed) < fabs(diff_data.right_rear_motor_speed) )//取最小值,避免打滑，拉高转速
        {
             diff_data.right_motor_speed = diff_data.right_front_motor_speed;
        }
        else
        {
             diff_data.right_motor_speed = diff_data.right_rear_motor_speed;          
        }
        
//        diff_data.right_motor_speed = 0;// 左侧两个电机所以取平均值        
        
    }
    else{}

//     printf("rightspeed: %f, leftspeed: %f\n", diff_data.right_motor_speed, diff_data.left_motor_speed); 
    // 急停开关
    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 = 0;//停车 就为0
//    }
//    else
//    {

//    }
    
    diff_data.speed_inter = (uint16_t)(int16_t)speed_pid.integral;
    diffProcess(&diff_data);//计算左右电机期望转速
    
    
    printf("dir_state: %d\n", diff_data.state);  
    handleMotorState(&diff_data,un_motor_input3.bit_data.rotation_dir);//方向判断函数 20250630增加
    
    publishMessage(&diff_data, 1);
}



// 预充完成处理函数
void preChargeFinish(void *signal_id)
{
    (void)signal_id;

    for(uint8_t i = 0; i<4; i++)
    {
       diff_data.out_motor_speed[i] = 0;
    }    
    
    setMotorOutput(&diff_data.out_motor_speed[0],(uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
    // 档位
    un_motor_output1.bit_data.gear = 0; // 0表示空挡
    un_motor_output2.bit_data.gear = 0;
    publishMessage(&un_motor_output1, 1);
    publishMessage(&un_motor_output2, 1);
}


void diffParametersInit(void *signal_id)
{
    (void)signal_id;  // 标记变量为已使用，避免编译器警告
//    setPidParameters(&speed_pid,
//                      getParam("spd_kp"),
//                      getParam("spd_ki"),
//                      getParam("spd_kd"),
//                      getParam("spd_il"),
//                      getParam("spd_ol")
//     );

    // 设置曲率 PID 控制器的参数
     setPidParameters(&yaw_rate_pid,
                      getParam("crv_kp"),
                      getParam("crv_ki"),
                      getParam("crv_kd"),
                      getParam("crv_il"),
                      getParam("crv_ol")
     );
     
    // 设置曲率 PID 控制器的参数
     setPidParameters(&Dec_front_speed_pid,
                      getParam("mot_kp"),
                      getParam("mot_ki"),
                      getParam("mot_kd"),
                      getParam("mot_il"),
                      getParam("mot_ol")
     );     
     
    // 设置曲率 PID 控制器的参数
     setPidParameters(&Acc_front_speed_pid,
                      getParam("spd_kp"),
                      getParam("spd_ki"),
                      getParam("spd_kd"),
                      getParam("spd_il"),
                      getParam("spd_ol")
     );        
     
    diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
    printf("PID_P: %f, PID_I: %f\n", speed_pid.kp, speed_pid.ki);
    
    printf("left_front: %f, right_front: %f, left_rear: %f, right_rear: %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("left_front_tou: %f, right_front_tou: %f, left_rear_tou: %f, right_rear_tou: %f\n", diff_data.out_motor_speed[0], diff_data.out_motor_speed[2],diff_data.out_motor_speed[2], diff_data.out_motor_speed[3]);    
        
    
    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(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置曲率 PID 控制器的参数
     setPidParameters(&Dec_front_speed_pid,
                      getParam("mot_kp"),
                      getParam("mot_ki"),
                      getParam("mot_kd"),
                      getParam("mot_il"),
                      getParam("mot_ol")
     );     
     
    // 初始化加速 PID 控制器
    initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
    // 设置曲率 PID 控制器的参数
     setPidParameters(&Acc_front_speed_pid,
                      getParam("spd_kp"),
                      getParam("spd_ki"),
                      getParam("spd_kd"),
                      getParam("spd_il"),
                      getParam("spd_ol")
     );       
     

    subscribe(&diff_app_timer, diffParametersInit);
    timerStart(&diff_app_timer,1000,1);//1s调用一次 
    
    
    subscribe(&diff_dir_timer, diff100ms);
    timerStart(&diff_dir_timer,100,1);//100ms调用一次     
    

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