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

PID_t   turnable_speed_pid;
PID_t   turnable_position_pid;

TurnableData  turnable_data = {0};





/**
 * @brief 带死区的原始数据到物理量转换函数（简单版）
 * @param raw_value 原始16位无符号整数值 [0, 65535]
 * @param min 物理量最小值（如 -10.0）
 * @param max 物理量最大值（如 +10.0）
 * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
 * @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
 */
static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) 
{
    // 1. 计算实际物理量值
    float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);

    // 2. 判断是否在死区内（绝对值 ≤ deadzone）
    if (fabs(physical_value) <= deadzone) 
    {
        return 0.0f;  // 死区内返回0
    } 
    else 
    {
        return physical_value;  // 死区外返回实际值
    }
}



/**
 * @brief 将浮点数转换为uint32_t（按小端序存储）
 * @param num 输入的浮点数
 * @return 转换后的uint32_t值（直接内存拷贝结果）
 * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
 */
uint32_t floatToUint32(float num) 
{
    uint32_t result;
    // 将浮点数的内存数据直接拷贝到uint32_t变量
    memcpy(&result, &num, sizeof(num));
    return result;
}


/**
 * @brief 电机失能函数（停止电机运行）
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @return 0: 成功, -1: 参数无效
 * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
 */
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式3：电机失能 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 清零数据域 */
    unsdodata->tx_can_data.bit_data.data = 0;
    unsdodata->tx_can_data.bit_data.index = 0;
    unsdodata->tx_can_data.bit_data.object_index = 0;    
    
    return 0;
}

/**
 * @brief 电机使能函数（启动电机运行）
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @return 0: 成功, -1: 参数无效
 * @note 通信模式4：电机使能
 */
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式4：电机使能 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 清零数据域 */
    unsdodata->tx_can_data.bit_data.data = 0;
    unsdodata->tx_can_data.bit_data.index = 0;
    unsdodata->tx_can_data.bit_data.object_index = 0;  
    
    return 0;
}

/**
 * @brief 设置电机运行模式
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @param mode 要设置的模式值 (具体含义需参考电机协议文档)
 * @return 0: 成功, -1: 参数无效
 * @note RUM_MODE应为预定义的宏，表示运行模式索引
 */
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 设置数据域 */
    unsdodata->tx_can_data.bit_data.index = RUM_MODE;      /* 运行模式索引 */
    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
    unsdodata->tx_can_data.bit_data.data = mode;           /* 模式值 */
    
    return 0;
}

/**
 * @brief 写入电机参数
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @param index 要写入的参数索引 (具体含义需参考电机协议文档)
 * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
 * @return 0: 成功, -1: 参数无效
 * @note 使用floatToUint32函数转换浮点参数
 */
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 设置数据域 */
    unsdodata->tx_can_data.bit_data.index = index;         /* 参数索引 */
    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
    unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
    
    return 0;
}


/**
 * @brief 动态斜率限制（支持变时间间隔）
 * @param last_command   上一次的电流指令值
 * @param target_current 本次目标电流指令
 * @param delta_time     距离上一次调用的时间间隔 (s)
 * @return              限制后的安全电流指令
 */
float dynamic_current_limit(float *last_command, float target_current, float delta_time) 
{
    // 计算期望的变化量
    float desired_change = target_current - *last_command;

    // 计算两种限制
    float step_limit = MAX_STEP;
    float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制

    // 选择更严格的限制
    float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;

    // 应用限制并返回新指令
    float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
    *last_command = *last_command + actual_change;//更新过去值

    return *last_command + actual_change;
}

static void setTurnableMotorOutput()
{ 
    static float previous_time2 = 0.0f;

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

    turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt);
    turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt);     
    turnable_data.out_left_motor_ampere_limit  = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt);
    
    
    
   
    setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, IQ_REF_INDEX,turnable_data.out_pitch_motor_ampere_limit);
    setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit);  
    setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID,  &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit); 
    
    
    un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10);
    un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_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)(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_output3, 1);    
}





// 转台
static void turnableProcess(void *signal_id)
{
  
    static float previous_time1 = 0.0f;

    float time1 = (float)getCurrentTime();
    float dt = (time1 - previous_time1) / PERIOD_TICK; 
    previous_time1 = time1;
//    if((turnable_data.current_state == POWER_WORKING))//高压上电才运行
//    {
        switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
        {
            case 0:
              timerStart(&turnable_data.turnable_timer, 1000, 0); // 启动定时器，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, CURRENT_MODE);                  
                  setMotorMode(MASTER_CANID, TURN_MOTOR_CANID,  &un_sdo_output3, CURRENT_MODE);
                  
                  publishMessage(&un_sdo_output1, 1);
                  publishMessage(&un_sdo_output2, 1);
                  publishMessage(&un_sdo_output3, 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);
                  motorEnable(MASTER_CANID, TURN_MOTOR_CANID,  &un_sdo_output3); 
                  
                  publishMessage(&un_sdo_output1, 1);
                  publishMessage(&un_sdo_output2, 1);
                  publishMessage(&un_sdo_output3, 1);                   
              }
            break;        
           
           case 4:
              turnable_data.out_left_motor_ampere = calculatePidOutput(&turnable_speed_pid, turnable_data.desired_speed, turnable_data.speed, 0.0f, dt);
              
              turnable_data.turnable_cnt = 0; 
              turnable_data.turnable_state = 4; 
              
              setTurnableMotorOutput();//输出函数
           break;
           
           default:break;
        }        
//    }
//    else
//    {
//        turnable_data.turnable_cnt ++;
//        turnable_data.turnable_state = 0;    
//    }
}




void turnableParametersInit(void *signal_id)
{
    (void)signal_id;  // 标记变量为已使用，避免编译器警告
    
    setPidParameters(&turnable_speed_pid,
                    getParam("spd_kp"),
                    getParam("spd_ki"),
                    getParam("spd_kd"),
                    getParam("spd_il"),
                    getParam("spd_ol")
    );  
    
    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 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.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) );

    }
    else{}    

    turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE );
    turnable_data.left_motor_speed  = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE );        
    turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f;    
    
    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")
    );
  
    subscribe(&un_computer_turnable_Input, 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调用一次     
    
    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_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
    un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
    un_turn_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());
}