/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "module/cmd.h"
#include <stdbool.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */

/* Exported variables ------------------------------------------------------- */

// 机器人参数配置
Config_RobotParam_t robot_config = {

    .chassis_param = {
    /* DJI3508µç»ú*/
        .motor_param = {
      { 
			.can = BSP_CAN_1, 
			.id = 0x201, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x202, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true
			},
      { 
			.can = BSP_CAN_1, 
			.id = 0x203, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true				
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x204, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true	
			},
    },
		.type = CHASSIS_TYPE_MECANUM,
    /* PID  */
    .pid = {
      /* µ×Å̵ç»ú PID */
      .motor_pid_param = {
        .k = 0.001f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      },

      /* ¸úËæ */
      .follow_pid_param = {
        .k = 0.5f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      },
    },

    .low_pass_cutoff_freq = {
      .in = 50.0f,
      .out = 50.0f,
    },

    .reverse = {
      .yaw = true,
    },
		.limit = {
      .max_vx = 3.0f,     
      .max_vy = 3.0f,
      .max_wz = 2.0f,    
      .max_current = 16000.0f 
    },
  },
    
    .gimbal_param = {
        .pid = {
            .yaw_omega = {
                .k = 1.0f,
                .p = 1.0f,
                .i = 0.3f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .yaw_angle = {
                .k = 8.0f,
                .p = 4.0f,
                .i = 0.0f,
                .d = 0.2f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
            .pit_omega = {
                .k = 0.4f,
                .p = 1.0f,
                .i = 0.0f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .pit_angle = {
                .k = 8.0f,
                .p = 5.0f,
                .i = 2.5f,
                .d = 0.2f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
        },
        .mech_zero = {
            .yaw = 0.0f,
            .pit = 2.98220015,  //0.195206895
        },
        .travel = {
            .yaw = -1.0f,
            .pit = 0.6,
        },
        .low_pass_cutoff_freq = {
            .out = -1.0f,
            .gyro = 1000.0f,
        },
        .pit_motor ={
            .can = BSP_CAN_1,
            .can_id = 0x2,
            .master_id = 0x12,
            .module = MOTOR_DM_J4310,
            .reverse = false,
        },
        .yaw_motor = {
            .can = BSP_CAN_1,
            .can_id = 0x50,
            .master_id = 0x60,
            .module = MOTOR_DM_J4310,
            .reverse = false,
        }
    },

    .shoot_param = {
        .basic={
          .projectileType=SHOOT_PROJECTILE_42MM,
		      .fric_num=6,
		      .extra_deceleration_ratio=1.0f,
          .num_trig_tooth=5,
          .shot_freq=1.0f,
          .shot_burst_num=3,
          .ratio_multilevel = {0.8f, 1.0f},
        },   
        .jamDetection={
            .enable=true,
            .threshold=310.0f, 
            .suspectedTime=0.5f, 
        },
        .motor={
            .fric = {
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x201,
                        .module = MOTOR_M3508,
                        .reverse = false,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x202,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x203,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x204,
                        .module = MOTOR_M3508,
                        .reverse = false,
                        .gear = false,
                    },
                    .level=2,
                },
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x205,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=2,
                },
                {
                    .param = {
                        .can = BSP_CAN_2,
                        .id = 0x206,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=2,
                }
            },
            .trig = {
                .can = BSP_CAN_1,
                .id = 0x207,
                .module = MOTOR_M3508,
                .reverse = false,
                .gear=true,
            },
        },
        .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=2.5f,
                .p=1.0f,
                .i=0.1f,
                .d=0.04f,
                .i_limit=0.4f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=M_2PI,
            },
            .trig_omg_2006 = {
                .k=1.0f,
                .p=1.5f,
                .i=0.3f,
                .d=0.5f,
                .i_limit=0.2f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=-1.0f,
            },
            .trig_3508 = {
                .k=0.5f,
                .p=1.8f,
                .i=0.3f,
                .d=0.1f,
                .i_limit=0.15f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=M_2PI,
            },
            .trig_omg_3508 = {
                .k=1.0f,
                .p=1.0f,
                .i=0.0f,
                .d=0.0f,
                .i_limit=0.0f,
                .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,
            },
        },  
    },
    .cmd_param={
        .sourcePriorityConfigs={
            CMD_SRC_RC,
            CMD_SRC_PC,
            CMD_SRC_NUC,
            CMD_SRC_REF
        },
        .pc.map = {                                   /*1<<CMD_KEY_W记录一下不要忘了这个思路  */
            .key_map[CMD_BEHAVIOR_FORE]             = {CMD_KEY_W,       CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_BACK]             = {CMD_KEY_S,       CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_LEFT]             = {CMD_KEY_A,       CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_RIGHT]            = {CMD_KEY_D,       CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_ACCELERATE]       = {CMD_KEY_SHIFT,   CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_DECELEBRATE]      = {CMD_KEY_CTRL,    CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_FIRE]             = {CMD_L_CLICK,     CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_FIRE_MODE]        = {CMD_R_CLICK,     CMD_ACTIVE_RISING_EDGE},
            .key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35]   = {CMD_KEY_E,       CMD_ACTIVE_RISING_EDGE},
            .key_map[CMD_BEHAVIOR_OPENCOVER]        = {CMD_KEY_F,      CMD_ACTIVE_RISING_EDGE},
            .key_map[CMD_BEHAVIOR_REVTRIG]          = {CMD_KEY_R,       CMD_ACTIVE_RISING_EDGE},
            .key_map[CMD_BEHAVIOR_ROTOR]            = {CMD_KEY_G,       CMD_ACTIVE_PRESSED},
            .key_map[CMD_BEHAVIOR_GIMBAL_MODE]      = {CMD_KEY_V,       CMD_ACTIVE_RISING_EDGE},
         },
         .pc.sensitivity={
            
            .move_sense=0.1f,
            .sens_mouse=3.0f,
            .move_fast_sense=2.4f,
            .move_slow_sense=0.8f
        },


    },

    
};

/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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