#include "shoot.h"
#include "robot_def.h"

#include "dji_motor.h"
#include "message_center.h"
#include "bsp_dwt.h"
#include "general_def.h"
#include "servo_motor.h"

/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
static ServoInstance *lid;

static Publisher_t *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息

// dwt定时,计算冷却用
static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;

void ShootInit()
{
    // 左摩擦轮
    Motor_Init_Config_s friction_config = {
            .can_init_config = {
                    .can_handle = &hcan2,
            },
            .controller_param_init_config = {
                    .speed_PID = {
                            .Kp = 1.5f,
                            .Ki = 0.2f,
                            .Kd = 0,
                            .Improve = PID_Integral_Limit,
                            .IntegralLimit = 10000,
                            .MaxOut = 15000,
                    },
                    .current_PID = {
                            .Kp = 0,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Integral_Limit,
                            .IntegralLimit = 10000,
                            .MaxOut = 15000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = SPEED_LOOP,
                    .close_loop_type = SPEED_LOOP,
            },
            .motor_type = M3508};
    friction_config.can_init_config.tx_id = 2,
    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    friction_l = DJIMotorInit(&friction_config);

    friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行
    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    friction_r = DJIMotorInit(&friction_config);

    // 拨盘电机
    Motor_Init_Config_s loader_config = {
            .can_init_config = {
                    .can_handle = &hcan2,
                    .tx_id = 1
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
                            .Kp = 500,
                            .Ki = 100,
                            .Kd = 0,
                            .MaxOut = 5000,
                    },
                    .speed_PID = {
                            .Kp = 2, // 3
                            .Ki = 0, // 1
                            .Kd = 0,
                            .Improve = PID_Integral_Limit,
                            .IntegralLimit = 5000,
                            .MaxOut = 10000,
                    },
                    .current_PID = {
                            .Kp = 0, // 0.7
                            .Ki = 0, // 0.1
                            .Kd = 0,
                            .Improve = PID_Integral_Limit,
                            .IntegralLimit = 5000,
                            .MaxOut = 5000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
                    //.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
            },
            .motor_type = M2006 // 英雄使用m3508
    };
    loader = DJIMotorInit(&loader_config);

    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

    Servo_Init_Config_s lid_config = {
            .htim = &htim1,
            .Channel = TIM_CHANNEL_1,
            .Servo_type = Servo180,
            .Servo_Angle_Type = Free_Angle_mode,
    };

    lid = ServoInit(&lid_config);

//    SPI_Init_Config_s rgb_config = {
//           .spi_handle = ;
//           .GPIOx = GPIOx;
//           .cs_pin = cs_pin;
//           .spi_work_mode = spi_work_mode;
//           .callback = callback;
//           .id = 1;
//    }
}

//卡弹检测
void stalled_detect()
{
    static float last_detect_time = 0,detect_time = 0;
    static float last_total_angle = 0;
    static uint8_t stalled_cnt = 0;

    detect_time = DWT_GetTimeline_ms();

    //last_detect_time + 200 > detect_time
    if(detect_time - last_detect_time < 200)  // 200ms 检测一次
        return;
//    reference_angle = (detect_time - last_detect_time) * loader->motor_controller.speed_PID.Ref * 1e-3f;
//    real_angle = loader->measure.total_angle - last_total_angle;
    if(shoot_cmd_recv.loader_mode == LOAD_BURSTFIRE)
    {
        //if(real_angle < reference_angle * 0.2f)
        if(abs(loader->measure.real_current)>=9500)
        {
            //stalled_cnt ++;
            shoot_feedback_data.stalled_flag = 1;

        }
        last_detect_time = DWT_GetTimeline_ms();
    }
//    last_detect_time = DWT_GetTimeline_ms();
//    last_total_angle = loader->measure.total_angle;
}

// 热量控制
//void Heat_control()
//{
//    if(shoot_cmd_recv.heat>=(0.9*shoot_cmd_recv.heat_limit))
//    {
//        DJIMotorStop(loader);
//    }
//    else
//    {
//        DJIMotorEnable(loader);
//    }
//}

/* 机器人发射机构控制核心任务 */
void ShootTask()
{
    //从cmd获取控制数据
    SubGetMessage(shoot_sub, &shoot_cmd_recv);

    // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
    {
        DJIMotorSetRef(friction_l, 0);
        DJIMotorSetRef(friction_r, 0);
        DJIMotorStop(loader);
    }
    else // 恢复运行
    {
        DJIMotorEnable(friction_l);
        DJIMotorEnable(friction_r);
        DJIMotorEnable(loader);
    }

    // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
    // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
    if (hibernate_time + dead_time > DWT_GetTimeline_ms())
        return;

    // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
    switch (shoot_cmd_recv.loader_mode)
    {
        // 停止拨盘
        case LOAD_STOP:
            DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
            DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
            break;
            // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
        case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
            DJIMotorOuterLoop(loader, ANGLE_LOOP);                              // 切换到角度环
            DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
            hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
            dead_time = 150;                                                                    // 完成1发弹丸发射的时间
            break;
            // 三连发,如果不需要后续可能删除
        case LOAD_3_BULLET:
            DJIMotorOuterLoop(loader, ANGLE_LOOP);                                     // 切换到角度环
            DJIMotorSetRef(loader, loader->measure.total_angle - 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
            hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
            dead_time = 1000;                                                                       // 完成3发弹丸发射的时间
            break;
            // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
        case LOAD_BURSTFIRE:
            DJIMotorOuterLoop(loader, SPEED_LOOP);
            DJIMotorSetRef(loader, -(shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8));

            // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
            break;
            // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
            // 也有可能需要从switch-case中独立出来
        case LOAD_REVERSE:
            DJIMotorOuterLoop(loader, SPEED_LOOP);
            DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8);             // 固定速度反转
            hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
            dead_time = 100;                                                                    // 翻转500ms
            shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
            // ...
            break;
        default:
            while (1)
                ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
    }

    // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
    if (shoot_cmd_recv.friction_mode == FRICTION_ON)
    {
        // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
//        switch (shoot_cmd_recv.bullet_speed)
//        {
//            case SMALL_AMU_15:
//                DJIMotorSetRef(friction_l, 0);
//                DJIMotorSetRef(friction_r, 0);
//                break;
//            case SMALL_AMU_18:
//                DJIMotorSetRef(friction_l, 0);
//                DJIMotorSetRef(friction_r, 0);
//                break;
//            case SMALL_AMU_30:
//                DJIMotorSetRef(friction_l, 0);
//                DJIMotorSetRef(friction_r, 0);
//                break;
        //default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
        DJIMotorSetRef(friction_l, 40000);
        DJIMotorSetRef(friction_r, 40000);
//                break;
//        }
    }
    else // 关闭摩擦轮
    {
        DJIMotorSetRef(friction_l, 0);
        DJIMotorSetRef(friction_r, 0);
    }

    // 开关弹舱盖
    if (shoot_cmd_recv.lid_mode == LID_CLOSE)
    {
        Servo_Motor_FreeAngle_Set(lid,107);// 小107 大115
    }
    else if (shoot_cmd_recv.lid_mode == LID_OPEN)
    {
        Servo_Motor_FreeAngle_Set(lid,10);
    }

    // 开关热控
    if(shoot_cmd_recv.heat_mode == HEAT_OPEN)
    {
        if(shoot_cmd_recv.heat>=(0.75*shoot_cmd_recv.heat_limit))
        {
            DJIMotorStop(loader);
        }
        else
        {
            DJIMotorEnable(loader);
        }
    }
    else if(shoot_cmd_recv.heat_mode == HEAT_CLOSE) {
        DJIMotorEnable(loader);
    }
    //卡弹检测
    stalled_detect();
    // 热量控制
    //Heat_control();

    // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
    PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
}