/* =============================================================================
 * chassis.c - 底盘模块:麦轮逆解 + 四轮转速 PID + 跟随云台(双 INS 世界系)
 * 电机槽位(与 legacy 实际发送一致,已修正 legacy 后轮反馈交叉的 bug):
 *   0x201 右前  0x202 左前  0x203 左后  0x204 右后
 * 逆解(legacy Mecanum_PowerMoveControl 实际生效版本):
 *   w_wheel = (vx ± vy ± w*(rx+ry)/100) / R * 18143.66
 * 跟随(2026-09 二次重写):改回 Yaw GM6020(0x205) 编码器多圈相对角——
 *   编码器测的是"云台相对底盘"的机械角,零漂移(此前双 INS 方案两板 yaw
 *   均无绝对参考,差值温漂,长时间工况不成立,弃用)。
 *   符号全自动综合,无需手拨判据:
 *     云台自检(0x1A9 flags)下发"tick 增向=云台 CCW/CW";
 *     底盘自检 chassis_detect_follow_sign() 用小 w 脉冲+底盘 INS 测出
 *     "w 正向=底盘 CCW/CW";两者相乘即跟随环符号
 * 功率:  chassis_control() 的 power_scale 直接缩放四轮输出电流
 * ========================================================================== */
#include "chassis.h"
#include "pid.h"
#include "filter.h"
#include "attitude.h"
#include "can.h"
#include "robot_param.h"
#include "c_comm.h"

dji_motor_t motor_wheel_rf;  // 0x201 右前
dji_motor_t motor_wheel_lf;  // 0x202 左前
dji_motor_t motor_wheel_lr;  // 0x203 左后
dji_motor_t motor_wheel_rr;  // 0x204 右后
dji_motor_t motor_yaw_follow;  // Yaw GM6020 反馈,CAN2 0x205(跟随用,相对角零漂移)

static PID_PositionTypeDef wheel_speed_pid[4];  // 顺序:RF/LF/LR/RR(槽位序)
static PID_PositionTypeDef follow_pid;

/* 跟随环诊断导出(debug_task 打印) */
volatile float dbg_follow_err_deg = 0;
volatile float dbg_follow_out = 0;
volatile float dbg_follow_sign = 0;  // 综合后的跟随符号(0=未定)

static float follow_target_tick = 0;  // 跟随基准(编码器累计值)
static uint8_t follow_latched = 0;
static uint8_t follow_reset_request = 0;

void chassis_motor_init(void)
{
    dji_motor_init(&motor_wheel_rf, &hcan1, 0x201, DJI_MOTOR_3508);
    dji_motor_init(&motor_wheel_lf, &hcan1, 0x202, DJI_MOTOR_3508);
    dji_motor_init(&motor_wheel_lr, &hcan1, 0x203, DJI_MOTOR_3508);
    dji_motor_init(&motor_wheel_rr, &hcan1, 0x204, DJI_MOTOR_3508);
    dji_motor_init(&motor_yaw_follow, &hcan2, 0x205, DJI_MOTOR_6020);

    for (uint8_t i = 0; i < 4; i++)
    {
        PID_PositionStructureInit(&wheel_speed_pid[i], 0);
        PID_PositionSetParameter(&wheel_speed_pid[i], MECANUM_WHEEL_SPEED_KP,
                                 MECANUM_WHEEL_SPEED_KI, MECANUM_WHEEL_SPEED_KD);
        PID_PositionSetOUTRange(&wheel_speed_pid[i], -MECANUM_WHEEL_SPEED_OUT_LIMIT,
                                MECANUM_WHEEL_SPEED_OUT_LIMIT);
    }

    PID_PositionStructureInit(&follow_pid, 0);
    PID_PositionSetParameter(&follow_pid, CHASSIS_FOLLOW_KP, CHASSIS_FOLLOW_KI, CHASSIS_FOLLOW_KD);
    PID_PositionSetOUTRange(&follow_pid, -CHASSIS_FOLLOW_OUT_LIMIT, CHASSIS_FOLLOW_OUT_LIMIT);
}

uint8_t chassis_motor_offline(void)
{
    uint32_t now = HAL_GetTick();
    return (motor_wheel_rf.fdb.last_online_tick == 0 || now - motor_wheel_rf.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_wheel_lf.fdb.last_online_tick == 0 || now - motor_wheel_lf.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_wheel_lr.fdb.last_online_tick == 0 || now - motor_wheel_lr.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_wheel_rr.fdb.last_online_tick == 0 || now - motor_wheel_rr.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
}

void chassis_control(float vx, float vy, float w, float power_scale)
{
    /* 麦轮逆解(legacy Mecanum_PowerMoveControl 的实际生效版本)
     * 换算:w_wheel = (vx±vy±w*(rx+ry)/100)/R * 18143.66 */
    float rear = w * (MECANUM_RX + MECANUM_RY) / 100.0f;
    float rf = (vx - vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
    float lf = (-vx - vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
    float lr = (vx + vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
    float rr = (-vx + vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;

    wheel_speed_pid[0].Need_Value = rf;
    wheel_speed_pid[1].Need_Value = lf;
    wheel_speed_pid[2].Need_Value = lr;
    wheel_speed_pid[3].Need_Value = rr;

    PID_PositionCalc(&wheel_speed_pid[0], motor_wheel_rf.fdb.rotor_speed);
    PID_PositionCalc(&wheel_speed_pid[1], motor_wheel_lf.fdb.rotor_speed);
    PID_PositionCalc(&wheel_speed_pid[2], motor_wheel_lr.fdb.rotor_speed);
    PID_PositionCalc(&wheel_speed_pid[3], motor_wheel_rr.fdb.rotor_speed);

    /* 简易功率限制:按 power_scale 缩放输出电流 */
    int16_t out[4];
    out[0] = (int16_t)(wheel_speed_pid[0].OUT * power_scale);
    out[1] = (int16_t)(wheel_speed_pid[1].OUT * power_scale);
    out[2] = (int16_t)(wheel_speed_pid[2].OUT * power_scale);
    out[3] = (int16_t)(wheel_speed_pid[3].OUT * power_scale);

    dji_motor_send_current_low(&hcan1, out[0], out[1], out[2], out[3]);
}

void chassis_follow_reset(void)
{
    follow_reset_request = 1;  // 下个跟随周期重锁基准并清积分
    PID_PositionClean(&follow_pid);
}

/* 底盘自检:给四轮一个小 w 脉冲,用底盘 INS yaw 响应方向测出
 * "w 正向 = 底盘 CCW(+1)/CW(-1)"。阻塞约 200ms,落地时整车会轻转一下。
 * 返回 1-成功 0-失败(轮离线/姿态未就绪/无响应) */
uint8_t chassis_detect_follow_sign(void)
{
    if (chassis_motor_offline() || !att_ready)
        return 0;

    chassis_output_zero();
    HAL_Delay(50);  // 静默期

    const float yaw_before = att_deg_yaw;
    chassis_control(0, 0, CHASSIS_DETECT_W, 1.0f);
    for (uint16_t i = 0; i < CHASSIS_DETECT_TIME_MS; i++)
        HAL_Delay(1);
    chassis_output_zero();

    const float dyaw = theta_format(att_deg_yaw - yaw_before);
    HAL_Delay(80);  // 回稳

    if (dyaw > CHASSIS_DETECT_YAW_MIN)
    {
        dbg_follow_sign = 1.0f;   // w+ -> 底盘 CCW
        return 1;
    }
    if (dyaw < -CHASSIS_DETECT_YAW_MIN)
    {
        dbg_follow_sign = -1.0f;  // w+ -> 底盘 CW
        return 1;
    }
    return 0;  // 车没动(架空无力矩/被挡),不给结论
}

/* 跟随符号综合:
 *   云台自检给出"tick 增向 = 云台 CCW(+1)/CW(-1)"(ctoc_encoder_ccw)
 *   底盘自检给出"w 正向 = 底盘 CCW(+1)/CW(-1)"
 * 推导:底盘 CCW 转 δ(世界系)且云台被其 yaw 环锁定世界向时,
 *   云台相对底盘 CW 偏 δ。若 tick 增向=云台 CCW,则相对 tick 误差 e
 *   (增=CCW 为正)随底盘 CCW 转动而减小 -> 需要 w 使底盘 CCW:
 *   w = chassis_ccw_sign * K * err_ccw,其中 err_ccw = enc_ccw * err_tick */
float chassis_follow_sign(void)
{
    if (dbg_follow_sign == 0 || ctoc_encoder_ccw == 0)
        return CHASSIS_FOLLOW_SIGN;  // 自检未完成,回退参数
    return dbg_follow_sign * (float)ctoc_encoder_ccw;
}

/* 跟随一步:反馈为 Yaw GM6020 编码器相对角(零漂移),输出 w */
float chassis_follow_w(void)
{
    /* 尚未收到 0x205 反馈时不锁定基准(避免首帧假偏差),输出 0 */
    if (motor_yaw_follow.fdb.last_online_tick == 0)
    {
        follow_latched = 0;
        return 0;
    }

    if (follow_reset_request || !follow_latched)
    {
        follow_reset_request = 0;
        follow_target_tick = (float)motor_yaw_follow.fdb.total_tick;
        follow_latched = 1;
        PID_PositionClean(&follow_pid);
    }

    /* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
    float err_deg = ((float)motor_yaw_follow.fdb.total_tick - follow_target_tick) * 360.0f / 8192.0f;
    if (ctoc_encoder_ccw < 0)
        err_deg = -err_deg;
    if (err_deg > 180.0f)
        err_deg -= 360.0f;
    else if (err_deg < -180.0f)
        err_deg += 360.0f;

    if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND)
        err_deg = 0;

    follow_pid.Need_Value = 0;
    PID_PositionCalc(&follow_pid, chassis_follow_sign() * -err_deg);

    dbg_follow_err_deg = err_deg;
    dbg_follow_out = follow_pid.OUT;
    return follow_pid.OUT;
}

void chassis_output_zero(void)
{
    dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
}