#include "to_stretch.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "message_center.h"
#include "referee_task.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"
#include "user_lib.h"

//to_stretch

static Publisher_t *to_stretch_pub;                    // 用于发布伸缩的数据
static Subscriber_t *to_stretch_sub;                   // 用于订阅伸缩的控制命令
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv;         // 伸缩接收到的控制命令
static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据

static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd;

void To_stretchInit() {
    // 上下
    Motor_Init_Config_s ud_config = {
            .can_init_config = {
                    .can_handle = &hcan1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 0.0f,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 100,
                            .MaxOut = 500,
                    },
                    .speed_PID = {
                            .Kp = 0,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 2500,
                            .MaxOut = 20000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
                    .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
                    .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
            },
            .motor_type = M3508,
    };
    // 前后
    Motor_Init_Config_s fb_config = {
            .can_init_config = {
                    .can_handle = &hcan1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 0.01f,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 100,
                            .MaxOut = 500,
                    },
                    .speed_PID = {
                            .Kp = 10,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 2500,
                            .MaxOut = 20000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
                    .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
                    .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
            },
            .motor_type = M3508,
    };
    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动

    ud_config.can_init_config.tx_id = 5;
    ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_lu = DJIMotorInit(&ud_config);

    ud_config.can_init_config.tx_id = 6;
    ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_ru = DJIMotorInit(&ud_config);

    fb_config.can_init_config.tx_id = 7;
    fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_ld = DJIMotorInit(&fb_config);

    fb_config.can_init_config.tx_id = 8;
    fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_rd = DJIMotorInit(&fb_config);

    to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
    to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));

}

/* 机器人伸缩控制核心任务 */
void To_stretchTask()
{

    SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv);

    if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE)
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        DJIMotorStop(motor_lu);
        DJIMotorStop(motor_ru);
        DJIMotorStop(motor_ld);
        DJIMotorStop(motor_rd);
    }
    else
    { // 正常工作
        DJIMotorEnable(motor_lu);
        DJIMotorEnable(motor_ru);
        DJIMotorEnable(motor_ld);
        DJIMotorEnable(motor_rd);
    }

    // 根据控制模式设定旋转速度
    switch (to_stretch_cmd_recv.to_stretch_mode)
    {
        case TO_STRETCH_ZERO_FORCE:
            DJIMotorStop(motor_lu);
            DJIMotorStop(motor_ru);
            DJIMotorStop(motor_ld);
            DJIMotorStop(motor_rd);
            break;
        case TO_STRETCH_MODE:
            DJIMotorEnable(motor_lu);
            DJIMotorEnable(motor_ru);
            DJIMotorEnable(motor_ld);
            DJIMotorEnable(motor_rd);

            DJIMotorSetRef(motor_lu, to_stretch_cmd_recv.ud);
            DJIMotorSetRef(motor_ru, to_stretch_cmd_recv.ud);
            DJIMotorSetRef(motor_ld, to_stretch_cmd_recv.fb);
            DJIMotorSetRef(motor_rd, to_stretch_cmd_recv.fb);
            break;
        default:
            break;
    }

    // 推送反馈消息
#ifdef ONE_BOARD
    PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data);
#endif
#ifdef CHASSIS_BOARD
    CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
#endif // CHASSIS_BOARD
}