#include "chassis_control.h"
#include "CAN_receive.h"
#include "remote_control.h"
#include <math.h>
#include <string.h>

#define PI 3.1415926535f

static pid_t pid_motor[4];        // 四个底盘电机的PID控制器
static int16_t target_speed[4];  // 四个轮子的目标转速(rpm)
static Chassis_Speed chassis_speed;          // 底盘目标速度(vx,vy,vw)
static ChassisMode chassis_mode = CHASSIS_NORMAL; // 底盘当前模式

static void omni_calc(Chassis_Speed *speed, int16_t *out_speed);
static void absolute_cal(Chassis_Speed *absolute_speed, float angle);

/**
 * @brief 初始化底盘电机的PID
 */
void chassis_control_init(void)
{
    for (int i = 0; i < 4; i++)
    {
        pid_init(&pid_motor[i], 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);
    }
    memset(target_speed, 0, sizeof(target_speed));
    memset(&chassis_speed, 0, sizeof(chassis_speed));
}

/**
 * @brief 设置底盘目标速度（vx, vy, vw）
 */
void chassis_set_target_speed(const Chassis_Speed *speed)
{
    if (speed == NULL) return;
    chassis_speed.vx = speed->vx;
    chassis_speed.vy = speed->vy;
    chassis_speed.vw = speed->vw;
}

/**
 * @brief 设置底盘模式
 */
void chassis_set_mode(ChassisMode mode)
{
    chassis_mode = mode;
}

/**
 * @brief 底盘控制主循环：全向轮运动学解算 + PID + CAN发送
 */
void chassis_control_task(void)
{
    // 遥控器掉线检测：超过100ms无信号，停止CAN发送，电机随惯性停下
    if (!is_rc_online())
    {
        for (int i = 0; i < 4; i++)
        {
            pid_motor[i].integral = 0;     // 清空PID积分，防止重新上线时积分饱和
            pid_motor[i].last_error = 0;   // 清空上次误差
        }
        memset(target_speed, 0, sizeof(target_speed));  // 目标转速清零
        return;  // 不发送CAN信号，电机随惯性停下
    }
    const motor_measure_t *m[4];
    m[0] = get_chassis_motor_measure_point(0);
    m[1] = get_chassis_motor_measure_point(1);
    m[2] = get_chassis_motor_measure_point(2);
    m[3] = get_chassis_motor_measure_point(3);

    absolute_cal(&chassis_speed, 0);

    int16_t cur[4];
    for (int i = 0; i < 4; i++)
    {
        cur[i] = (int16_t)pid_calc(&pid_motor[i], (float)target_speed[i], (float)m[i]->speed_rpm);
    }

    CAN_cmd_chassis(cur[0], cur[1], cur[2], cur[3]);
}

/**
 * @brief 全向轮运动学解算：将 vx,vy,vw 转换为四个轮子的转速
 *        轮子编号：0=左, 1=前, 2=右, 3=后
 */
static void omni_calc(Chassis_Speed *speed, int16_t *out_speed)
{
    float wheel_rpm_ratio;  // 速度(m/s)到转速(rpm)的转换系数
    float L;                // 底盘中心到轮子的距离 = LENGTH_A + LENGTH_B
    int16_t wheel_rpm[4];

    // 60 / 轮周长 * 减速比 = 转速转换系数 (m/s → rpm)
    // 例如：vx=1m/s → 轮子60/0.152/19=7500rpm(电机端)
    wheel_rpm_ratio = 60.0f / WHEEL_PERIMETER * CHASSIS_DECELE_RATIO;
    L = LENGTH_A + LENGTH_B;

    // 轮子编号：基于用户观察修正
    // M1(0x201)=左前, M2(0x202)=左后, M3(0x203)=右前, M4(0x204)=右后
    // 物理层vy符号：+, -, -, + 是X型麦轮的标准平移公式（右侧电机反向安装，代码层vy: +, -, +, -）
    // 四个轮子对角配对：左前+右后同向，右前+左后同向，两组反向
    wheel_rpm[0] = (int16_t)(( speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左前轮
    wheel_rpm[1] = (int16_t)(( speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左后轮
    wheel_rpm[2] = (int16_t)((-speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右前轮
    wheel_rpm[3] = (int16_t)((-speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右后轮

    memcpy(out_speed, wheel_rpm, 4 * sizeof(int16_t));
}

/**
 * @brief 坐标系变换：将遥控器坐标系的速度转换到底盘坐标系
 * @param absolute_speed 遥控器输入的目标速度
 * @param angle 云台相对于底盘的角度（度）
 */
static void absolute_cal(Chassis_Speed *absolute_speed, float angle)
{
    float angle_hd;          // 角度转弧度
    Chassis_Speed temp_speed; // 旋转后的临时速度

    angle_hd = angle * PI / 180.0f;  // 角度转弧度

    // 坐标系旋转矩阵：将遥控器坐标系速度旋转到当前底盘坐标系
    temp_speed.vw = absolute_speed->vw;
    temp_speed.vx = absolute_speed->vx * cosf(angle_hd) - absolute_speed->vy * sinf(angle_hd);
    temp_speed.vy = absolute_speed->vx * sinf(angle_hd) + absolute_speed->vy * cosf(angle_hd);

    omni_calc(&temp_speed, target_speed);  // 全向轮解算，结果存入target_speed
}