/* =============================================================================
 * shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
 * 逻辑(与 legacy 一致):
 *   摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
 *   开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
 *   拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
 *   拨弹电流经 shoot_send_cmd() 经 CAN2 0x200 帧直接发送(ID 1)
 * ========================================================================== */
#include "shoot.h"
#include "gimbal.h"
#include "pid.h"
#include "tim.h"
#include "robot_param.h"

extern TIM_HandleTypeDef htim3;

uint8_t shoot_friction_flag = 0;

static PID_PositionTypeDef friction_l_pid, friction_r_pid, rammer_pid;

/* 激光:PC8 = TIM3_CH3(10kHz PWM,ARR=8399),占空比 0/满 对应关/开 */
#define LASER_ON() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 8399)
#define LASER_OFF() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0)

static void laser_init(void)
{
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
}

void shoot_init(void)
{
    PID_PositionStructureInit(&friction_l_pid, 0);
    PID_PositionSetParameter(&friction_l_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
    PID_PositionSetEkRange(&friction_l_pid, -5, 5);
    PID_PositionSetOUTRange(&friction_l_pid, -15000, 15000);

    PID_PositionStructureInit(&friction_r_pid, 0);
    PID_PositionSetParameter(&friction_r_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
    PID_PositionSetEkRange(&friction_r_pid, -5, 5);
    PID_PositionSetOUTRange(&friction_r_pid, -15000, 15000);

    PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED);
    PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD);
    PID_PositionSetEkRange(&rammer_pid, -20, 20);
    PID_PositionSetOUTRange(&rammer_pid, -10000, 10000);

    laser_init();
}

void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on)
{
    /* 摩擦轮(与 legacy 逻辑一致:发射机构上电才允许开启;
     * 台架调试时可经 DEBUG_SHOOT_WITHOUT_REFEREE 绕过裁判系统检查) */
    if (friction_on && (shooter_power_on || DEBUG_SHOOT_WITHOUT_REFEREE))
    {
        friction_l_pid.Need_Value = -GIMBAL_FRICTION_WHEEL_SPEED;
        friction_r_pid.Need_Value = GIMBAL_FRICTION_WHEEL_SPEED;
        LASER_ON();
        shoot_friction_flag = 1;
    }
    else
    {
        friction_l_pid.Need_Value = 0;
        friction_r_pid.Need_Value = 0;
        LASER_OFF();
        shoot_friction_flag = 0;
    }

    /* 拨弹:仅在摩擦轮开启后允许 */
    if (shoot_friction_flag)
    {
        if (fire)
            rammer_pid.Need_Value = GIMBAL_RAMMER_SPEED;
        else if (eject)
            rammer_pid.Need_Value = -GIMBAL_RAMMER_SPEED;
        else
            rammer_pid.Need_Value = 0;
    }
    else
        rammer_pid.Need_Value = 0;

    PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed);
    PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
    PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
}

void shoot_send_cmd(void)
{
    /* 摩擦轮 2×M3508:CAN1 0x200 槽1/2 */
    dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
    /* 拨弹盘 M2006(ID 1):CAN2 0x200 槽1 */
    dji_motor_send_current_low(&hcan2, (int16_t)rammer_pid.OUT, 0, 0, 0);
}

void shoot_output_zero(void)
{
    dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
    dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
    LASER_OFF();
    shoot_friction_flag = 0;
}