#include "main.h"
#include "M3508.h"
#include "can.h"
#include "gpio.h"
#include <string.h> 
#include <stdlib.h> 
#include <math.h>  // 用于isnan、isinf函数
#include "PID.h"
#include "Remote.h"
#include "CToC.h"


// 四个电机的控制器
Motor_Controller_t motor_controller[4] = {0};
// 存储四个电机的控制命令
int16_t motor_commands[4] = {0};
	
motor_measure_t motor_chassis[4] = {0};

static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];						
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
	 uint32_t send_mail_box;
	 chassis_tx_message.StdId = 0x200;
	 chassis_tx_message.IDE = CAN_ID_STD;
	 chassis_tx_message.RTR = CAN_RTR_DATA;
	 chassis_tx_message.DLC = 0x08;
	 chassis_can_send_data[0] = motor1 >> 8;
	 chassis_can_send_data[1] = motor1;
	 chassis_can_send_data[2] = motor2 >> 8;
	 chassis_can_send_data[3] = motor2;
	 chassis_can_send_data[4] = motor3 >> 8;
	 chassis_can_send_data[5] = motor3;
	 chassis_can_send_data[6] = motor4 >> 8;
	 chassis_can_send_data[7] = motor4;
	 HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, 
	chassis_can_send_data, &send_mail_box);

}


/**
 * @brief 将RPM转换为rad/s
 */
float RPM_To_RadPerSec(int16_t rpm)
{
    return (float)rpm * 2.0f * 3.1415926535f / 60.0f;
}

/**
 * @brief 将rad/s转换为RPM
 */
int16_t RadPerSec_To_RPM(float rad_per_sec)
{
    return (int16_t)(rad_per_sec * 60.0f / (2.0f * 3.1415926535f));
}

/**
 * @brief 设置单个电机的目标速度
 * @param motor_id: 电机ID (0-3)
 * @param target_speed_rad_s: 目标速度 (rad/s)
 */
void Set_Motor_Target_Speed(uint8_t motor_id, float target_speed_rad_s)
{
    if (motor_id < 4) {
        motor_controller[motor_id].target_speed = target_speed_rad_s;
    }
}

/**
 * @brief 电机速度控制任务（带数据验证）
 */
void Motor_Speed_Control_Task(float dt)
{
    for (int i = 0; i < 4; i++) {
        // 验证电机反馈数据
        if (isnan(motor_controller[i].measure.speed_rpm) || 
            motor_controller[i].measure.speed_rpm > 10000 || 
            motor_controller[i].measure.speed_rpm < -10000) {
            // 数据异常，使用默认值
            motor_controller[i].measure.speed_rpm = 0;
        }
        
        // 将电机反馈的RPM转换为rad/s
        float actual_speed_rads = RPM_To_RadPerSec(motor_controller[i].measure.speed_rpm);
        
        // 验证转换结果
        if (isnan(actual_speed_rads) || isinf(actual_speed_rads)) {
            actual_speed_rads = 0.0f;
        }
        
        // 计算速度误差
        float speed_error = motor_controller[i].target_speed - actual_speed_rads;
        
        // 验证误差值
        if (isnan(speed_error) || isinf(speed_error)) {
            speed_error = 0.0f;
        }
        
        // PID计算
        float pid_output = PID_Calculate(&motor_controller[i].speed_pid, speed_error, dt);
        
        // 验证PID输出
        if (isnan(pid_output) || isinf(pid_output)) {
            pid_output = 0.0f;
            // 重置这个电机的PID状态
            PID_PositionClean(&motor_controller[i].speed_pid);
        }
        
        // 存储控制命令
        motor_commands[i] = (int16_t)pid_output;
        
        
    }
}


/**
 * @brief 麦克纳姆轮速度解算函数
 * @param vx: X方向速度 (m/s) - 正数前进，负数后退
 * @param vy: Y方向速度 (m/s) - 正数右移，负数左移  
 * @param vz: 旋转角速度 (rad/s) - 正数逆时针，负数顺时针
 */
void Drive_Motor(float vx, float vy, float vz)
{
    float A = WHEELBASE_WIDTH + WHEELBASE_LENGTH;
    
    // 标准麦克纳姆轮运动学逆解算公式
    // 轮子编号：0-右上, 1-左上, 2-左下, 3-右下
     
    // 标准麦克纳姆轮运动学逆解算公式
    float wheel1_speed = ( vx +vy + vz * A  ) / WHEEL_RADIUS;  // 右上轮
    float wheel2_speed = ( vx - vy + vz * A  ) / WHEEL_RADIUS;  // 左上轮
    float wheel3_speed = (-vx -vy + vz * A  ) / WHEEL_RADIUS;  // 左下轮
    float wheel4_speed = (-vx +vy +vz * A  ) / WHEEL_RADIUS;  // 右下轮

    Set_Motor_Target_Speed(0, wheel1_speed);  // 右上轮
    Set_Motor_Target_Speed(1, wheel2_speed);  // 左上轮
    Set_Motor_Target_Speed(2, wheel3_speed);  // 左下轮
    Set_Motor_Target_Speed(3, wheel4_speed);  // 右下轮
}


float Map_Remote_to_Speed(int16_t input, float max_speed)
{
    if (input > 660) input = 660;
    if (input < -660) input = -660;
    
    // 死区内直接返回0
    if (abs(input) < 50) {
        return 0.0f;
    }
    
    // 不在死区内，正常计算速度
    return (input / 660.0f) * max_speed;
}
/**
 * @brief 处理遥控器数据并设置底盘速度
 */
void Process_Remote_Control(void)
{
    // 映射遥控器值到速度 (m/s)
    float vx = Map_Remote_to_Speed(Remote_RxData.Remote_L_UD,2.0f);   // 前后方向，最大2m/s
    float vy = Map_Remote_to_Speed(Remote_RxData.Remote_L_RL, 2.0f);  // 左右方向，最大2m/s  
    float vz = Map_Remote_to_Speed(Remote_RxData.Remote_R_RL, 3.0f);   // 旋转方向，最大3rad/s
    
    // 调用运动学解算
    Drive_Motor(vx, vy, vz);
}