/* =============================================================================
 * gimbal.c - 云台双轴控制(串级 PID)
 * Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
 *   位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
 *   速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电压(限幅)
 *   稳定性条件:SIGN*G>0,G 为"电压->陀螺 z 角速度"的装配增益,
 *   由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
 * Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
 * 发送:CAN2 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
 * ========================================================================== */
#include "gimbal.h"
#include "shoot.h"
#include "pid.h"
#include "attitude.h"
#include "bmi088.h"
#include "can.h"
#include "robot_param.h"
#include "filter.h"
#include "robot_typedef.h"
#include "dr16.h"

#ifndef RAD_TO_ANGLE
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
#endif

dji_motor_t motor_yaw_gimbal;
dji_motor_t motor_pitch_gimbal;
dji_motor_t motor_friction_l;
dji_motor_t motor_friction_r;
dji_motor_t motor_rammer;

static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid;
static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;

static float yaw_target_deg;    // 世界系目标角(°)
static float pitch_target_deg;  // 目标角(°)
static float yaw_voltage;
static float pitch_voltage;

/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
static float yaw_sign_f = 0.0f;
static uint8_t yaw_sign_valid = 0;

/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
static int16_t rammer_current_shared = 0;

void shoot_share_rammer_current(int16_t current);

void gimbal_motor_init(void)
{
    dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
    dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
    dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
    dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
    dji_motor_init(&motor_rammer, &hcan1, 0x207, DJI_MOTOR_2006);

    /* Yaw 位置环:° -> °/s */
    PID_PositionStructureInit(&yaw_angle_pid, 0);
    PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD);
    PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
    PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);

    /* Yaw 速度环:°/s -> 电压 */
    PID_PositionStructureInit(&yaw_speed_pid, 0);
    PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
    PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);

    /* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
    PID_PositionStructureInit(&pitch_angle_pid, 0);
    PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
    PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
    PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);

    PID_PositionStructureInit(&pitch_speed_pid, 300);
    PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
    PID_PositionSetEkRange(&pitch_speed_pid, -5, 5);
    PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
}

uint8_t gimbal_detect_yaw_sign(void)
{
    if (gimbal_motor_offline())
        return 0;

    /* 静默期:输出清零等待机械/陀螺稳定 */
    dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
    HAL_Delay(50);

    /* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
    float sum = 0.0f;
    dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
    for (uint16_t i = 0; i < 60; i++)
    {
        sum += bmi088_gyro[2];  // rad/s(z 轴向上为正)
        HAL_Delay(1);
    }
    dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
    HAL_Delay(80);  // 回稳,随后 gimbal_task 的首次控制会同步目标角

    const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
    if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
    {
        yaw_sign_f = 1.0f;  // 正电压->陀螺正转,装配增益 G>0
        yaw_sign_valid = 1;
    }
    else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
    {
        yaw_sign_f = -1.0f;  // 皮带交叉等反向装配
        yaw_sign_valid = 1;
    }
    else
        return 0;  // 被按住/没转:不给结论,由调用方决定重试或回退

    return 1;
}

float gimbal_yaw_sign(void)
{
    return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK;
}

int8_t gimbal_encoder_ccw(void)
{
    /* 自检:电压正脉冲 -> 陀螺 z 正转(云台 CCW),而电压正方向即编码器
     * tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
    if (!yaw_sign_valid)
        return 0;
    return (yaw_sign_f > 0.0f) ? 1 : -1;
}

void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
{
    /* 双环共用装配符号 */
    const float sign = gimbal_yaw_sign();
    const float yaw_meas = sign * att_deg_yaw;                                   // °
    const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE);          // °/s

    /* 首次进入控制或复位:将目标角设定为设定的编码器零位朝向(类似 Pitch 上电归位) */
    static uint8_t first_run = 1;
    if (first_run || reset_flag)
    {
        first_run = 0;

        /* 计算当前编码器距离设定零位的最短差值(-4096 ~ +4096) */
        int16_t ecd_diff = (int16_t)motor_yaw_gimbal.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
        if (ecd_diff > 4096)
            ecd_diff -= 8192;
        else if (ecd_diff < -4096)
            ecd_diff += 8192;

        /* 将编码器 tick 差折算为机械角度偏差(°)，补偿给世界系目标角 */
        const float ecd_err_deg = (float)ecd_diff * (360.0f / 8192.0f);
        yaw_target_deg = yaw_meas - ecd_err_deg;
        pitch_target_deg = 0;

        if (reset_flag)
        {
            PID_PositionClean(&yaw_angle_pid);
            PID_PositionClean(&yaw_speed_pid);
            PID_PositionClean(&pitch_angle_pid);
            PID_PositionClean(&pitch_speed_pid);
        }
    }

    yaw_target_deg += yaw_delta;
    pitch_target_deg += pitch_delta;

    /* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */
    yaw_angle_pid.Need_Value = yaw_target_deg;
    PID_PositionCalc(&yaw_angle_pid, yaw_meas);

    /* Yaw 速度环:°/s -> 电压(反馈为带符号陀螺角速度,双环符号自洽) */
    yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
    PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);

    /* 小陀螺底盘自旋解耦前馈:根据底盘相对转速补偿动摩擦与反电动势拖拽，消除同向/反向手感不对称 */
    float spin_feedforward = 0.0f;
    if (cmd_robot.spin_flag || dr16.s2 == 1 || dr16.s2 == 2)
    {
        spin_feedforward = GIMBAL_YAW_SPIN_FEEDFORWARD_K * (float)motor_yaw_gimbal.fdb.rotor_speed;
    }
    yaw_voltage = yaw_speed_pid.OUT + spin_feedforward;
    if (yaw_voltage > GIMBAL_YAW_SPEED_OUT_LIMIT)
        yaw_voltage = GIMBAL_YAW_SPEED_OUT_LIMIT;
    else if (yaw_voltage < -GIMBAL_YAW_SPEED_OUT_LIMIT)
        yaw_voltage = -GIMBAL_YAW_SPEED_OUT_LIMIT;

    /* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
    pitch_angle_pid.Need_Value = pitch_target_deg;
    PID_PositionCalc(&pitch_angle_pid, att_deg_roll);
    pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
    PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
    pitch_voltage = pitch_speed_pid.OUT;

    /* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
    dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
    dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
}

void shoot_share_rammer_current(int16_t current)
{
    rammer_current_shared = current;
}

uint8_t gimbal_motor_offline(void)
{
    uint32_t now = HAL_GetTick();
    uint8_t rammer_off = 0;
#if GIMBAL_RAMMER_INSTALLED
    rammer_off = (motor_rammer.fdb.last_online_tick == 0 ||
                  now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
#endif
    return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
            rammer_off);
}

void gimbal_output_zero(void)
{
    dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
    dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
    dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
}