#include "config.h"
#include "string.h"

#define DEBUG

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

#ifdef DEBUG
Config_Param_t config = {

#else

static const Config_Param_t config = {

#endif

    .chassis = {

        .motor_3508_param[0] = {BSP_CAN_1, 0x201, MOTOR_M3508, false, false},
        .motor_3508_param[1] = {BSP_CAN_1, 0x202, MOTOR_M3508, false, false},
        .motor_3508_param[2] = {BSP_CAN_1, 0x203, MOTOR_M3508, false, false},
        .motor_3508_param[3] = {BSP_CAN_1, 0x204, MOTOR_M3508, false, false},

        .motor_6020_param[0] = {BSP_CAN_1, 0x205, MOTOR_GM6020, false, false},
        .motor_6020_param[1] = {BSP_CAN_1, 0x206, MOTOR_GM6020, false, false},
        .motor_6020_param[2] = {BSP_CAN_1, 0x207, MOTOR_GM6020, false, false},
        .motor_6020_param[3] = {BSP_CAN_1, 0x208, MOTOR_GM6020, false, false},

        .C6020Omega_param = {
            .k = 1.0f,
            .p = 0.5f,
            .i = 0.1f,
            .d = 0.0f,
            .i_limit = 1.0f,
            .out_limit = 1.0f,
            .d_cutoff_freq = -1.0f,
            .range = -1.0f},

        .C6020Angle_param = {
            .k = 0.2f,
            .p = 0.21f,
            .i = 0.1f,
            .d = 0.0f,
            .i_limit = 1.0f,
            .out_limit = 30.0f,
            .d_cutoff_freq = -1.0f,
            .range = 360
        },

        .M3508v_param = {
            // .k=0.2f,
            // .p=0.25f,
            // .i=0.1f,
            // .d=0.0f,
            // .i_limit=1.0f,
            // .out_limit=1.0f,
            // .d_cutoff_freq= -1.0f,
            // .range=-1.0f
            .k = 1.0f,
            .p = 4.0f,
            .i = 0.0f,
            .d = 0.0f,
            .i_limit = 1.0f,
            .out_limit = 1.0f,
            .d_cutoff_freq = -1.0f,
            .range = -1.0f
        },
        .chassis_follow_gimbalPID = {
            .k = 0.7f, 
            .p = 0.5f, 
            .i = 0.1f, 
            .d = 0.0f, 
            .i_limit = 1.0f, 
            .out_limit = 0.5f, 
            .d_cutoff_freq = -1.0f, 
            .range = M_2PI
        },

        /*这两个数据是云台6020的零点和机械限位*/
        .mech_zero = 1.31f,
        .travel = 1.4f,
        .mech_zero_4310=2.06f,


    },

    .gimbal = {
        .yaw_6020_motor ={BSP_CAN_2, 0x209, MOTOR_GM6020, false, true},

        .yaw_4310_motor = {
            .can = BSP_CAN_2,
             .can_id = 0x1, 
             .master_id = 0x11,
             .module = MOTOR_DM_J4310,
             .reverse = false
            },

        .pitch_4310_motor = {
            .can = BSP_CAN_2, 
            .can_id = 0x2, 
            .master_id = 0x12, 
            .module = MOTOR_DM_J4310, 
            .reverse = true
        },

        .pid.pitch_4310_motor_angle = {
            .k = -2.0f, 
            .p = 3.1f, //2.0
            .i = 0.1f, //0.0
            .d = 2.5f, //0.0
            .i_limit = 1.0f, 
            .out_limit = 10.0f, 
            .d_cutoff_freq = -1.0f, 
            .range = M_2PI
        },

        .pid.pitch_4310_motor_omega = {
            .k = 0.3f, 
            .p = 0.3f, 
            .i = 0.0f, 
            .d = 0.0f, 
            .i_limit = 0.0f, 
            .out_limit = 1.0f, 
            .d_cutoff_freq = -1.0f, 
            .range = -1.0f
        },

        .pid.yaw_4310_motor_angle = {
            // .k = 2.0f, 
            // .p = 6.0f, 
            // .i = 0.01f, 
            // .d = 0.1f, 
            // .i_limit = 0.0f, 
            // .out_limit = 7.0f, 
            // .d_cutoff_freq = 20.0f, 
            // .range = M_2PI
            .k = 2.0f, 
            .p = 5.0f, 
            .i = 0.0f, 
            .d = 1.0f, 
            .i_limit = 0.0f, 
            .out_limit = 10.0f, 
            .d_cutoff_freq = 20.0f, 
            .range = M_2PI
        },

        .pid.yaw_4310_motor_omega = {
            .k = 0.0f, 
            .p = 0.3f, 
            .i = 0.0f, 
            .d = 0.0f, 
            .i_limit = 0.0f, 
            .out_limit = 1.0f, 
            .d_cutoff_freq = -1.0f, 
            .range = -1.0f
        },

        .pid.yaw_6020_motor_angle = {
            // .k = 2.0f, 
            // .p = 2.5f, 
            // .i = 0.0f, 
            // .d = 0.0f, 
            // .i_limit = 0.0f, 
            // .out_limit = 10.0f, 
            // .d_cutoff_freq = 0.0f, 
            // .range = M_2PI
            /*上面一组pid会导致电机超调，可能是因为pid计算错误，正在进行重新修改pid*/
            // .k = 2.0f, 
            // .p = 5.0f, 
            // .i = 0.0f, 
            // .d = 0.0f, 
            // .i_limit = 1.0f, 
            // .out_limit = 10.0f, 
            // .d_cutoff_freq = 0.0f, 
            // .range = M_2PI

            .k = 2.0f, 
            .p = 7.0f, 
            .i = 0.5f, 
            .d = 1.0f, 
            .i_limit = 1.0f, 
            .out_limit = 10.0f, 
            .d_cutoff_freq = 0.0f, 
            .range = M_2PI
        },

        .pid.yaw_6020_motor_omega = {
            // .k = 0.30f, 
            // .p = 0.4f, 
            // .i = 0.01f, 
            // .d = 0.0f, 
            // .i_limit = 1.0f, 
            // .out_limit = 1.0f, 
            // .d_cutoff_freq = -1.0f, 
            // .range = -1.0f

            // .k = 0.35f, 
            // .p = 0.3f, 
            // .i = 0.0f, 
            // .d = 0.0f, 
            // .i_limit = 1.0f, 
            // .out_limit = 1.0f, 
            // .d_cutoff_freq = -1.0f, 
            // .range = -1.0f

            .k = 0.35f, 
            .p = 0.35f, 
            .i = 0.0f, 
            .d = 0.0f, 
            .i_limit = 1.0f, 
            .out_limit = 1.0f, 
            .d_cutoff_freq = -1.0f, 
            .range = -1.0f
        },

        .mech_zero = {
            .yaw_6020 = 1.31f,
            .yaw_4310 = 2.06f, /*大yaw零点*/
            .pitch_4310 = 0.93f,
        },
        .travel = {
            .yaw_6020 = 1.4f,
            .yaw_4310 = -1.0f, /*大yaw无限位*/
            .pitch_4310 = 0.9f,
            // .pitch_4310=-1.0f,/*pitch无限位*/
        },
        .low_pass_cutoff_freq={
            .out = -1.0f,
            .gyro = 20.0f,
        }

    },
    .shoot = {
        .motor.fric[0] = {{BSP_CAN_2, 0x201, MOTOR_M3508, false, false},.level=1},
        .motor.fric[1] = {{BSP_CAN_2, 0x202, MOTOR_M3508, true, false},.level=1}, 
        .motor.trig = {BSP_CAN_2, 0x203, MOTOR_M2006, false, true},

              .basic.projectileType = SHOOT_PROJECTILE_17MM,
              .basic.fric_num = 2,
              .basic.extra_deceleration_ratio = 1,
              .basic.ratio_multilevel[0] = 1.0f,
              .basic.num_trig_tooth = 8,
              .basic.shot_freq = 5.0f,
              .basic.shot_burst_num = 1,

              .jamDetection.enable = true,
              .jamDetection.threshold = 120.0f,
              .jamDetection.suspectedTime = 0.5f,

              .pid = {
                .fric_follow = {
                .k = 1.0f,
                .p = 1.5f,
                .i = 0.3f,
                .d = 0.0f,
                .i_limit = 0.2f,
                .out_limit = 0.9f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
                },

                .fric_err={
                .k=0.0f,
                .p=4.0f,
                .i=0.4f,
                .d=0.0f,
                .i_limit=0.25f,
                .out_limit=0.25f, 
                .d_cutoff_freq=-1.0f,
                .range=-1.0f, 
                },

                .trig_2006={
                .k=0.7f,
                .p=0.7f,
                .i=0.5f,
                .d=0.05f,
                .i_limit=0.2f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=M_2PI,
                },

                .trig_omg_2006={
                .k=3.0f,
                .p=2.0f,
                .i=0.2f,
                .d=0.5f,
                .i_limit=0.2f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=-1.0f,
                },
              },
            .filter = {
                .fric = {
                    .in = 30.0f,
                    .out = 30.0f,
                },
                .trig = {
                    .in = 30.0f,
                    .out = 30.0f,
                },
            }

    }

};

/**
 * @brief 获取机器人配置参数
 * @return 机器人配置参数指针
 */
Config_Param_t *Config_GetRobotParam(void)
{
    return &config;
}