#include "app_config.h"
#include "app_dependence.h"
#include "interface.h"

#include "app_turn.h"
#include "app_differential_drive.h"
#include "app_param_manage.h"
#include <math.h>
#include <stdio.h>

TurnData    turn_data;
Timer       turn_timer2;                // 定时器结构体


// 浮点数转uint32_t（按小端序存储）
void floatToUint32(float num, uint32_t *result) 
{
    // 将浮点数的内存数据直接拷贝到uint32_t变量
    memcpy(result, &num, sizeof(num));
}








static void turnOutput(void *signal_id)
{
   (void)signal_id;
   uint32_t float_temp = 0;
   
   switch(turn_data.direction)
   {
       case 0:
         //左电机
         floatToUint32(turn_data.left_zero_position,&float_temp);
         un_sdo_output1.bit_data.data = float_temp;
         floatToUint32(turn_data.left_speed*2,&float_temp);
         un_sdo_output2.bit_data.data = float_temp;         
         //右电机
         floatToUint32(turn_data.right_zero_position,&float_temp);
         un_sdo_output5.bit_data.data = float_temp;
         floatToUint32(turn_data.right_speed*2,&float_temp); 
         un_sdo_output6.bit_data.data = float_temp;         
       break;
       case 1://左转
         //左电机
         floatToUint32(turn_data.inner_rad + turn_data.left_zero_position,&float_temp);
         un_sdo_output1.bit_data.data = float_temp;         
         floatToUint32(turn_data.left_speed,&float_temp);
         un_sdo_output2.bit_data.data = float_temp;         
         //右电机
         floatToUint32(turn_data.outer_rad + turn_data.right_zero_position,&float_temp);
         un_sdo_output5.bit_data.data = float_temp;         
         floatToUint32(turn_data.right_speed,&float_temp);
         un_sdo_output6.bit_data.data = float_temp;          
       break;     
       case 2://右转
         //左电机
         floatToUint32(turn_data.outer_rad + turn_data.left_zero_position,&float_temp);
         un_sdo_output1.bit_data.data = float_temp;             
         floatToUint32(turn_data.left_speed,&float_temp);
         un_sdo_output2.bit_data.data = float_temp;          
         //右电机
         floatToUint32(turn_data.inner_rad + turn_data.right_zero_position,&float_temp);
         un_sdo_output5.bit_data.data = float_temp;            
         floatToUint32(turn_data.right_speed,&float_temp);
         un_sdo_output6.bit_data.data = float_temp;         
       break;default:;     
   }
   
   un_sdo_output1.bit_data.index = LOC_REF_INDEX;
   un_sdo_output1.bit_data.object_index = 0x0000;   
//   publishMessage(&un_sdo_output1, 1);//设定左电机位置
   
   un_sdo_output2.bit_data.index = LIMIT_SPEED_INDEX;
   un_sdo_output2.bit_data.object_index = 0x0000;   
//   publishMessage(&un_sdo_output2, 1);//设定右电机位置  
   
   un_sdo_output5.bit_data.index = LOC_REF_INDEX;
   un_sdo_output5.bit_data.object_index = 0x0000;   
//   publishMessage(&un_sdo_output5, 1);//设定左电机位置
   
   un_sdo_output6.bit_data.index = LIMIT_SPEED_INDEX;
   un_sdo_output6.bit_data.object_index = 0x0000;   
//   publishMessage(&un_sdo_output6, 1);//设定右电机位置        

}

//// 预充完成 需要先发一个失能，再使能
//void turnChargeFinish(uint8_t *stt, uint8_t *cnt)
//{
//    switch(stt[0])//先发送切换模式以及电机失能，后面直接使能
//    {
//        case 0:
//          if(cnt[0] >= 20)//发送50次
//          {
//              cnt[0] = 0; 
//              stt[0] = 1;               
//          }
//          else
//          {
//              un_sdo_output5.bit_data.cmd = 0x2F;
//              un_sdo_output5.bit_data.object_index = 0x6060;
//              un_sdo_output5.bit_data.sub_index = 0;
//              un_sdo_output5.bit_data.data = 0x01;//设定位置模式
//              publishMessage(&un_sdo_output5, 1);
//
//              un_sdo_output4.bit_data.cmd = 0x2B;
//              un_sdo_output4.bit_data.object_index = 0x6040;
//              un_sdo_output4.bit_data.sub_index = 0;
//              un_sdo_output4.bit_data.data = 0x06;//电机失能
//              publishMessage(&un_sdo_output4, 1);
//              cnt[0] ++; 
//              stt[0] = 0;
//              printf("position! \n");
//          }
//       break;
//       
//       case 1:
//
//          cnt[0] = 0; 
//          stt[0] = 1; 
//          printf("disable! \n");          
//       break;default:;
//    }
//}

/**
 * 计算阿克曼转向几何中的外轮转向角（弧度制）
 * @param L 轴距（前后轮距离，单位：米）
 * @param W 轮距（左右轮距离，单位：米）
 * @param inner_rad 内轮转向角（弧度）
 * @param[out] curvature 返回路径曲率（单位：1/米）
 * @return 外轮转向角（弧度）
 */
float calculate_outer_angle(float L, float W, float inner_rad, float* curvature)
{
    // 保存原始符号
    int sign = (inner_rad < 0) ? -1 : 1;

    // 使用绝对值进行计算
    float inner_temp = fabsf(inner_rad);

    // 修正转弯半径计算（关键修改点）
    float R_inner = L / tanf(inner_temp);      // 内轮转弯半径
    float R_outer = R_inner + W;               // 外轮转弯半径 = 内轮半径 + 轮距
    
    float R = R_inner + W / 2;  // 整车转弯半径

    // 计算曲率（注意符号）
    *curvature = sign / R;    

    // 计算外轮转向角：atan(L/R_outer)
    float delta_o = atanf(L / R_outer);

    // 恢复原始符号
    return sign * delta_o;
}





// 转向函数
static void turnProcess(void *signal_id)
{
    static uint8_t turn_process_state = 0;
    static uint8_t turn_process_cnt = 0;
    
    (void)signal_id;    
    if((turn_data.desired_curvature) > 0.01)//右转
    {
       turn_data.direction = 1;       
    }
    else if((turn_data.desired_curvature) < -0.01)//左转
    {
       turn_data.direction = 2;         
    }
    else//回中
    {
       turn_data.direction = 0;
    }
    
    turn_data.inner_rad = turn_data.desired_curvature*turn_data.left_max_position/2.0;//计算角度，内轮角度 -2-2的曲率对应  转换为限定角度
    turn_data.outer_rad = calculate_outer_angle(turn_data.wheel_base, turn_data.track_width, turn_data.inner_rad, &turn_data.curvature);//计算外轮角度以及整车曲率
    
    
    
    
    
//    //解决上电未接can用上位机调试电机，抱闸未打开的问题。
//    if(power_data.current_state == POWER_WORKING)//抱闸控制  
//    {
//       un_inf_can_kgf_output1.bit_data.KGF01 = 1;//打开抱闸 
//    }
//    else
//    {
//       un_inf_can_kgf_output1.bit_data.KGF01 = 0;//抱闸
//    }

    if ((turn_data.current_state == POWER_WORKING) && (1 == turn_data.motor_sleep))//work且电机上电才运行
    {      
        switch(turn_process_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
        {
            case 0:
              if(turn_process_cnt >= 20)//发送50次
              {
                  turn_process_cnt = 0; 
                  turn_process_state = 1;               
              }
              else
              {
                 turn_process_cnt ++;
                  
                  un_sdo_output4.bit_data.index = RUM_MODE;
                  un_sdo_output4.bit_data.object_index = 0x0;                  
                  un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//设定位置模式
                  publishMessage(&un_sdo_output4, 1);
                  
                  un_sdo_output8.bit_data.index = RUM_MODE;
                  un_sdo_output8.bit_data.object_index = 0x0;                  
                  un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//设定位置模式 
                  publishMessage(&un_sdo_output8, 1);                  
              }
              break;
           
            case 1:
              if(turn_process_cnt >= 10)//发送50次
              {
                  turn_process_cnt = 0; 
                  turn_process_state = 2;               
              }
              else
              {
                  turn_process_cnt ++;
                  
                  un_sdo_output3.bit_data.index = 0x0;
                  un_sdo_output3.bit_data.object_index = 0x0;                  
                  un_sdo_output3.bit_data.data = 0x0;//
                  publishMessage(&un_sdo_output3, 1);
                   
                  un_sdo_output7.bit_data.index = 0x0;
                  un_sdo_output7.bit_data.object_index = 0x0;                  
                  un_sdo_output7.bit_data.data = 0x0;//
                  publishMessage(&un_sdo_output7, 1);                 
              }
            break;        
           
           case 2:
              turnOutput(signal_id); 
              turn_process_cnt = 0; 
              turn_process_state = 2;          
           break;default:;
        }             
    }
    else//每次掉电都得重新发送使能
    {
       turn_process_state = 0;
       turn_process_cnt = 0;
       turn_data.motor_sleep = 0;
    }
}

// 处理所有输入信号的函数
static void turnInput(void *signal_id)
{
    (void)signal_id;      

    if (signal_id == &diff_data)
    {
        turn_data.mode = diff_data.mode; 
        turn_data.desired_curvature = diff_data.desired_curvature;        
    }
    else if(signal_id == &power_data)//电机上电
    {
        turn_data.current_state = power_data.current_state; 
    }
    else{}
    turn_data.motor_sleep = 1;     
    turnProcess(signal_id);//处理映射
}


void turnParametersInit(void *signal_id)
{
    (void)signal_id;  // 标记变量为已使用，避免编译器警告
    //读取参数
    turn_data.right_max_position = (float)getParam("R_maxP"); // 右转最大位置，rad
    turn_data.right_zero_position = (float)getParam("R_zeroP"); // 右转最大位置，rad
    turn_data.right_speed = (float)getParam("R_speed"); // 右转最大位置，rad
    
    turn_data.left_max_position = (float)getParam("L_maxP"); // 右转最大位置，rad
    turn_data.left_zero_position = (float)getParam("L_zeroP"); // 右转最大位置，rad
    turn_data.left_speed = (float)getParam("L_speed"); // 右转最大位置，rad 
    
    
    turn_data.wheel_base = (float)getParam("whl_bas");  //轴距
    turn_data.track_width = (float)getParam("tra_wid"); //轮距       
    
    timerStart(&turn_data.turn_timer,1000,1);//1s调用一次 
}

void motorInit(void *signal_id)
{
    if(turn_data.current_state == POWER_WORKING)
    {
        un_sdo_output4.bit_data.index = RUM_MODE;
        un_sdo_output4.bit_data.object_index = 0x0;                  
        un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//设定位置模式
        publishMessage(&un_sdo_output4, 1);

        un_sdo_output8.bit_data.index = RUM_MODE;
        un_sdo_output8.bit_data.object_index = 0x0;                  
        un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//设定位置模式 
        publishMessage(&un_sdo_output8, 1);     

        un_sdo_output3.bit_data.index = 0x0;
        un_sdo_output3.bit_data.object_index = 0x0;                  
        un_sdo_output3.bit_data.data = 0x0;//
        publishMessage(&un_sdo_output3, 1);

        un_sdo_output7.bit_data.index = 0x0;
        un_sdo_output7.bit_data.object_index = 0x0;                  
        un_sdo_output7.bit_data.data = 0x0;//
        publishMessage(&un_sdo_output7, 1);   
    }
//    timerStart(&turn_timer2,1000,1);//1s调用一次   
}





// 修改APP模块的初始化函数
void turnAppInit(void)
{
    // 初始化    
    memset(&turn_data, 0, sizeof(TurnData));  
    timerInit(&turn_data.turn_timer);
    timerInit(&turn_timer2);    
    
    
    //读取参数
    turn_data.right_max_position = (float)getParam("R_maxP"); // 右转最大位置，rad
//    turn_data.right_min_position = (float)getParam("R_minP"); // 右转最大位置，rad    
    turn_data.right_zero_position = (float)getParam("R_zeroP"); // 右转最大位置，rad
    turn_data.right_speed = (float)getParam("R_speed"); // 右转最大位置，rad
    
    turn_data.left_max_position = (float)getParam("L_maxP"); // 右转最大位置，rad
//    turn_data.left_min_position = (float)getParam("L_minP"); // 右转最大位置，rad    
    turn_data.left_zero_position = (float)getParam("L_zeroP"); // 右转最大位置，rad
    turn_data.left_speed = (float)getParam("L_speed"); // 右转最大位置，rad    

    // 订阅输入信号,处理刹车逻辑
    subscribe(&diff_data, turnInput);  
    subscribe(&power_data,  turnInput);     

    subscribe(&turn_data.turn_timer, turnParametersInit); 
    
    subscribe(&turn_timer2, motorInit);
   
    
    timerStart(&turn_data.turn_timer,1000,1);//1s调用一次 
    timerStart(&turn_timer2,1000,1);//1s调用一次   
    
    printf("turnApp: initial OK \n");
}