#include "app_config.h"
#include "app_dependence.h"
#include "interface.h"
#include "app_turntable.h"
#include "app_pid.h"
#include "app_param_manage.h"

#include "app_frm_monitor.h"
#include "app_frm_signal.h"
#include "app_frm_timer.h"

#include "drive_rs04.h"
#include "sdrv_vic.h"
#include <math.h>

PID_t   turnable_speed_pid;
PID_t   turnable_position_pid;

TurnableData  turnable_data = {0};


/**
 * @brief 将笛卡尔坐标 (x,y,z) 转换为球坐标 (r,θ,φ)
 * 
 * @param x         X轴坐标值（单位：米）
 * @param y         Y轴坐标值（单位：米）
 * @param z         Z轴坐标值（单位：米）
 * @param[out] out  输出球坐标结构体指针
 * @return bool     转换是否成功：
 *                  - true:  转换成功
 *                  - false: 输入无效（包含NaN/INF或out为NULL）
 * 
 * @note 特殊输入处理：
 *       1. 如果输入包含 NaN 或无穷大，返回 false
 *       2. 当 r < 1e-10 时视为原点，设置 out=(0, 0, 0)
 *       3. 在Z轴附近 (|z/r| ≈ 1) 时自动截断到 [-1,1] 保证数值稳定性
 * 
 * @warning 使用要求：
 *          - 必须检查返回值，不能直接使用out内容
 *          - out指针必须指向有效内存
 * 
 * @example 正确用法：
 *   SphericalCoordinate sph;
 *   if (cartesianToSpherical(1.0f, 0.0f, 0.0f, &sph)) {
 *       // 使用sph...
 *   }
 */
uint8_t cartesianToSpherical(float x, float y, float z, SphericalCoordinate* out) 
{
    // 参数有效性检查（防御性编程）
    if (!out || !isfinite(x) || !isfinite(y) || !isfinite(z)) {
        return 0;
    }

    // 计算径向距离 r = √(x² + y² + z²)
    out->r = sqrtf(x * x + y * y + z * z);
    
    // 原点判定（使用容差避免浮点误差）
    if (out->r < 1e-10f) {
        out->theta = 0.0f;
        out->phi = 0.0f;
        return 1;
    }
    
    // 极角 θ = acos(z/r) 的数值稳定性处理
    float z_over_r = z / out->r;
    if (z_over_r > 1.0f)  z_over_r = 1.0f;   // 处理上溢出
    if (z_over_r < -1.0f) z_over_r = -1.0f;  // 处理下溢出
    out->theta = acosf(z_over_r);
    
    // 方位角 φ = atan2(y, x)
    out->phi = atan2f(y, x);
    
    return 1;
}




// 计算CRC8校验（多项式 x^8 + 1，简单的累加和）
uint8_t encoder_calculate_crc(const uint8_t* data, uint8_t length) 
{
    uint8_t crc = 0x00;
    for (int i = 0; i < length; i++) {
        crc += data[i];
    }
    return crc;
}















static void setTurnableMotorOutput()
{ 
    
    turnable_data.out_pitch_motor_ampere = constrain(turnable_data.out_pitch_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX);
    turnable_data.out_left_motor_ampere  = constrain(turnable_data.out_left_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX);    
    
    
    setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, LIMIT_SPEED_INDEX,turnable_data.out_pitch_motor_ampere);
    setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output4, LOC_REF_INDEX,turnable_data.desired_pitch_position);    

    setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, SPD_REF,turnable_data.out_left_motor_ampere);    
      
    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100);
    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100);     
    
    publishMessage(&un_sdo_output1, 1);
    publishMessage(&un_sdo_output2, 1);  
    publishMessage(&un_sdo_output4, 1);      
}





// 转台
static void turnableProcess(void *signal_id)
{
//    if((turnable_data.current_state == POWER_WORKING))//高压上电才运行
//    {
      switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
      {
          case 0:
            timerStart(&turnable_data.turnable_timer, 1000, 1); // 启动定时器，1s
            turnable_data.turnable_state = 1;
          break;
            
          case 1:
            if (!turnable_data.turnable_timer.active)// 1s定时
            { 
               turnable_data.turnable_state = 2;     
            }
            else
            {
               turnable_data.turnable_state = 1;                
            }
          break;
          
          case 2://模式设置              
            if(turnable_data.turnable_cnt >= 5)//发送5次
            {
                turnable_data.turnable_cnt = 0; 
                turnable_data.turnable_state = 3;               
            }
            else
            {
                turnable_data.turnable_cnt ++; 
                turnable_data.turnable_state = 2;  
                
                setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP);
                setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, VELOCITY_MODE); 
                
                setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output5, LIMIT_CUR,5);   //设置最大电流为5A              
                
                publishMessage(&un_sdo_output1, 1);
                publishMessage(&un_sdo_output2, 1); 
                publishMessage(&un_sdo_output5, 1);                 
                
            }
            break;
            
//------------------------------------------------------------------------------          
          case 3:
            if(turnable_data.turnable_cnt >= 5)//发送5次
            {
                turnable_data.turnable_cnt = 0; 
                turnable_data.turnable_state = 4;            
            }
            else
            {
                turnable_data.turnable_cnt ++;
                turnable_data.turnable_state = 3;
                
                motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1);                  
                motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2);
                
                publishMessage(&un_sdo_output1, 1);
                publishMessage(&un_sdo_output2, 1);                 
            }
          break;        
         
         case 4:
            turnable_data.turnable_cnt = 0; 
            turnable_data.turnable_state = 4;
            
            setTurnableMotorOutput();//输出函数
         break;
         
         default:break;
      }        

}




void turnableParametersInit(void *signal_id)
{
    (void)signal_id;  // 标记变量为已使用，避免编译器警告
    
    if(0 == un_right_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
    {
       motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output3);
       publishMessage(&un_sdo_output3, 1);         
    }
    
    if(0 == un_pitch_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
    {
       motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output3);
       publishMessage(&un_sdo_output3, 1);         
    }


    turnable_data.desired_horizontal_speed = getParam("turn_sp");
    turnable_data.desired_pitch_speed = getParam("pit_sp");    
    
    turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值
    turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值

    printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere);
    printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere);    
    printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere); 
//    printf( "desired  speed %f\n",turnable_data.desired_speed);   
    printf( "speed %f\n",turnable_data.speed);     
    printf( "turnable state %d\n",turnable_data.turnable_state);      

    timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 
}




static void dataRequest(void *signal_id)
{
    str_magnetic_encoder.magnetic_data = ENCODER_HEADER;
  
    publishMessage(&str_magnetic_encoder, 1);    
    timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次   
}








// 差速输入处理函数
static void turnableInput(void *signal_id)
{
    if(signal_id == &power_data)//电机上电
    {
        turnable_data.current_state = power_data.current_state; 
    }
    else if(signal_id == &un_computer_turnable_Input)
    {
        turnable_data.position_x = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) );
        turnable_data.position_y = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_y) );        
        turnable_data.position_z = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_z) );              
    }
    else if(signal_id == &un_encoder_data_input)
    {
        if( un_encoder_data_input.arr[sizeof(un_encoder_data_input)-1] == encoder_calculate_crc(&un_encoder_data_input.arr[0], sizeof(un_encoder_data_input)-1) )//CRC校验
        {
           turnable_data.horizontal_position = (float)(un_encoder_data_input.bit_data.abs_value)*2.0f*PI/ENCODER_MAX_COUNTS;//将数据转换为实际角度             
        }
    }
    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 =
      
        float x_axis_temp = (float)(un_remote_control_input.bit_data.x_axis) - REMOTE_ZERO;
        if( ( x_axis_temp > 50 ) || ( x_axis_temp < -50 ) )
        {
            turnable_data.out_left_motor_ampere = 0.02*(x_axis_temp);//计算电流
            turnable_data.out_right_motor_ampere =  turnable_data.out_left_motor_ampere;            
        }
        else
        {
            turnable_data.out_left_motor_ampere = 0;//计算电流
            turnable_data.out_right_motor_ampere =  turnable_data.out_left_motor_ampere;          
        }

        x_axis_temp = (float)(un_remote_control_input.bit_data.y_axis) - REMOTE_ZERO;
        if(x_axis_temp > 50) //根据Y轴数据来定义
        {
           turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp);    
           turnable_data.desired_pitch_position = turnable_data.max_pitch_postion;
        }
        else if(x_axis_temp < -50)
        {
           turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp);
           turnable_data.desired_pitch_position = turnable_data.min_pitch_postion;            
        }
        else
        {
           turnable_data.out_pitch_motor_ampere = 0;            
        }
    }    
    else if(signal_id == &un_pitch_intput)
    {
        turnable_data.pitch_position = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_angle),-RS02_ANGULAR_VELOCITY_MAX,RS02_ANGULAR_VELOCITY_MAX,MOTOR_ANGLE_DEADZONE );          
    }    
    else{}

    turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),-RS02_ANGULAR_VELOCITY_MAX,RS02_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE );       
    turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f;    
    
    
    if ( (power_data.current_state == POWER_STANDBY) || (power_data.current_state == POWER_SLEEP) )//这几种状态可以转转台
    {
        turnable_data.turnable_state = 0;//清空状态。保证每次上电都初始化  
    }   
    else
    {
        turnableProcess(signal_id);//处理映射        
    }    
     
//    timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 
}


void turnableInit()
{
    // 初始化速度 PID 控制器
    initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);

//    // 设置速度 PID 控制器的参数
//    setPidParameters(&turnable_speed_pid,
//                    getParam("spd_kp"),
//                    getParam("spd_ki"),
//                    getParam("spd_kd"),
//                    getParam("spd_il"),
//                    getParam("spd_ol")
//    );
  
    //目标参数初始化
    str_magnetic_encoder.ip[0] = 192;
    str_magnetic_encoder.ip[1] = 168;    
    str_magnetic_encoder.ip[2] = 17;    
    str_magnetic_encoder.ip[3] = 33;
    str_magnetic_encoder.port = 2011; 
    
    subscribe(&un_remote_control_input, turnableInput);      
    subscribe(&un_computer_turnable_Input, turnableInput);
    subscribe(&un_pitch_intput, turnableInput);
    
    timerInit(&turnable_data.turnable_timer);
    timerInit(&turnable_data.turnable_timer1);    
    
//    timerInit(&turnable_data.turnable_timer2); 
//    subscribe(&turnable_data.turnable_timer2, turnableInput);
//    
//    timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 
    
    subscribe(&turnable_data.turnable_timer1, turnableParametersInit);    
    timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次     
    
    timerInit(&turnable_data.turnable_timer3); 
    subscribe(&turnable_data.turnable_timer3, dataRequest);
    timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次     
    
    subscribe(&un_encoder_data_input, turnableInput);    
    
    
    turnable_data.turnable_state = 0;
    un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
    un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
    un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
       
    
    un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
    un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
    un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
    
    printf( "turnable: initial OK %d\n",getCurrentTime());
}