#include "gimbal.h"
#include "robot_def.h"
#include "dmmotor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"

static INS_t *gimbal_IMU_data;

static Publisher_t *gimbal_pub;
static Subscriber_t *gimbal_sub;
static Gimbal_Upload_Data_s gimbal_feedback_data;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;

static DMMotorInstance *yaw_motor;   // DM-G6220
static DMMotorInstance *pitch_motor; // DM-J4310-2EC

void GimbalInit()
{
    gimbal_IMU_data = INS_Init();

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

    /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
    DM_Motor_Init_Config_s yaw_config = {
        .motor_config = {
            .can_init_config = {
                .can_handle = &hcan1,
                .tx_id      = 0x01,
                .rx_id      = 0xF1,
            },
            .controller_param_init_config = {
                .angle_PID = {
                    .Kp = 30, .Ki = 80, .Kd = 0.2f,
                    .IntegralLimit = 1000,
                    .CoefA = 20, .CoefB = 10,
                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
                               PID_Derivative_On_Measurement | PID_ChangingIntegrationRate |
                               PID_DerivativeFilter | PID_OutputFilter,
                    .MaxOut = 2000,
                },
                .other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle,
                .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2],
            },
            .output_reverse = OUTPUT_DIRECTION_NORMAL,
            .motor_type = DM_MOTOR,
        },
        .ctrl_mode = DM_MODE_VELOCITY,
        .mit_kp = 0, .mit_kd = 0,
    };
    yaw_motor = DMMotorInit(&yaw_config);

    /* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */
    DM_Motor_Init_Config_s pitch_config = {
        .motor_config = {
            .can_init_config = {
                .can_handle = &hcan1,
                .tx_id      = 0x02,
                .rx_id      = 0xF2,
            },
            .controller_param_init_config = {
                .angle_PID = {
                    .Kp = 20, .Ki = 50, .Kd = 0.1f,
                    .IntegralLimit = 500,
                    .CoefA = 15, .CoefB = 5,
                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
                               PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
                    .MaxOut = 1000,
                },
                .other_angle_feedback_ptr = &gimbal_IMU_data->Pitch,
                .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],
            },
            .output_reverse = OUTPUT_DIRECTION_NORMAL,
            .motor_type = DM_MOTOR,
        },
        .ctrl_mode = DM_MODE_VELOCITY,
        .mit_kp = 0, .mit_kd = 0,
    };
    pitch_motor = DMMotorInit(&pitch_config);
}

void GimbalTask()
{
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
    {
        /* 零力模式: 保持当前IMU角度, 电机不主动出力 */
        DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle);
        DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch);
    }
    else
    {
        DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
        DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
    }

    gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}