/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "device/rc_can.h"

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */

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

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

    .imu_param = {
        .can = BSP_CAN_2,
        .can_id = 0x6FF,      
        .device_id = 0x42,   
        .master_id = 0x43,   
    },

    .chassis_param = {
        .yaw={
            .k=1.0f,
            .p=1.2f,
            .i=0.0f,
            .d=0.05f,
            .i_limit=0.0f,
            .out_limit=1.0f,
            .d_cutoff_freq=30.0f,
            .range=M_2PI, /* 2*PI，用于处理角度循环误差 */
        },

        .roll={
            .k=1.0f,
            .p=5.0f,           /* 横滚角比例系数 */
            .i=0.0f,           /* 横滚角积分系数 */
            .d=0.2f,           /* 横滚角微分系数 */
            .i_limit=0.0f,     /* 积分限幅 */
            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
            .d_cutoff_freq=30.0f, /* 微分截止频率 */
            .range=-1.0f,      /* 不使用循环误差处理 */
        },

        .leg_length={
            .k = 20.0f,
            .p = 10.0f,
            .i = 0.0f,
            .d = 0.0f,
            .i_limit = 0.0f,
            .out_limit = 100.0f,
            .d_cutoff_freq = -1.0f,
            .range = -1.0f,
        },
        .leg_theta={
            .k=1.0f,
            .p=5.0f,           /* 摆角比例系数 */
            .i=0.0f,           /* 摆角积分系数 */
            .d=0.0f,           /* 摆角微分系数 */
            .i_limit=0.0f,     /* 积分限幅 */
            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
            .d_cutoff_freq=30.0f, /* 微分截止频率 */
            .range=-1.0f,      /* 不使用循环误差处理 */
        },

        .tp={
            .k=1.0f,
            .p=2.0f,          /* 俯仰角比例系数 */
            .i=0.0f,           /* 俯仰角积分系数 */
            .d=0.0f,           /* 俯仰角微分系数 */
            .i_limit=0.0f,     /* 积分限幅 */
            .out_limit=2.0f,   /* 输出限幅，腿长差值限制 */
            .d_cutoff_freq=30.0f, /* 微分截止频率 */
            .range=-1.0f,      /* 不使用循环误差处理 */
        },
        

        .low_pass_cutoff_freq = {
            .in = 30.0f,
            .out = 30.0f,
        },
        .joint_motors = {
            { // 左髋关节
                .can = BSP_CAN_1,
                .motor_id = 124,
                .host_id = 0xFF,
                .module = MOTOR_LZ_RSO3,
                .reverse = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 左膝关节
                .can = BSP_CAN_1,
                .motor_id = 125,
                .host_id = 0xFF,
                .module = MOTOR_LZ_RSO3,
                .reverse = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 右膝关节
                .can = BSP_CAN_1,
                .motor_id = 126,
                .host_id = 0xFF,
                .module = MOTOR_LZ_RSO3,
                .reverse = true,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 右髋关节
                .can = BSP_CAN_1,
                .motor_id = 127,
                .host_id = 0xFF,
                .module = MOTOR_LZ_RSO3,
                .reverse = true,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
        },
        .wheel_motors = {
            { // 左轮电机
                .can = BSP_CAN_1,
                .id = 0x141,
                .module = MOTOR_LK_MF9025,
                .reverse = false,
            },
            { // 右轮电机
                .can = BSP_CAN_1,
                .id = 0x142,
                .module = MOTOR_LK_MF9025,
                .reverse = true,
            },
        },
        .yaw_motor = { // 云台Yaw轴电机
            .can = BSP_CAN_2,
            .can_id = 0x1,
            .master_id = 0x11,
            .module = MOTOR_DM_J4310,
            .reverse = false,
        },

        .mech_zero_yaw = 1.21085676f, /* 250.5度，机械零点 */
        
        .vmc_param = {
            { // 左腿
                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m)
                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m)
                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m)
                .hip_length = 0.0f    // 髋宽 (L5) (m)
            },
            { // 右腿
                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m)
                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m)
                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m)
                .hip_length = 0.0f    // 髋宽 (L5) (m)
            }
        },
        .lqr_gains = {
            .k11_coeff = { -1.518517857889991e+02f, 1.947427875897828e+02f, -1.148120822481098e+02f, 2.924883559935997e+00f }, // theta
            .k12_coeff = { 1.210535496345407e+01f, -9.650112118530574e+00f, -7.795614823592060e+00f, 4.657327020761534e-01f },  // d_theta
            .k13_coeff = { -1.165837444184349e+02f, 1.210192009641869e+02f, -4.426716583574083e+01f, 2.303172586380976e+00f },  // x
            .k14_coeff = { -1.286052871916038e+02f, 1.353354867883034e+02f, -5.203199795488686e+01f, 2.622579832615006e+00f },  // d_x
            .k15_coeff = { -1.105374347374162e+01f, 3.229730763109554e+01f, -2.423106279412239e+01f, 6.195204290845447e+00f }, // phi
            .k16_coeff = { 8.260175276799853e-01f, 2.318023492852569e+00f, -2.927935219327240e+00f, 8.556202879801391e-01f },  // d_phi
            .k21_coeff = { 5.970052798954075e+02f, -5.263867571125131e+02f, 1.275655583254851e+02f, 1.140189988320814e+01f }, // theta
            .k22_coeff = { 7.451585479025395e+01f, -7.785445709151199e+01f, 2.827386050355351e+01f, -1.578620610065337e-01f },  // d_theta
            .k23_coeff = { 4.622306333044981e+01f, 4.325117706159210e+00f, -3.407203207833582e+01f, 1.455475256803718e+01f },  // x
            .k24_coeff = { 6.870578295004115e+01f, -1.301773374139566e+01f, -3.211146583952794e+01f, 1.644195610244500e+01f },  // d_x
            .k25_coeff = { 4.493994591642744e+02f, -4.563158073471503e+02f, 1.605396531175411e+02f, -3.691208683727941e+00f }, // phi
            .k26_coeff = { 5.771012099214764e+01f, -5.915171391239870e+01f, 2.096920183531924e+01f, -1.205283154896878e+00f },  // d_phi
        },
        .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
        .lqr_param.max_wheel_torque = 4.5f,  // 轮毂电机最大力矩 2.5Nm
    },

    .rc_can_param = {
        .can = BSP_CAN_2,
        .mode = RC_CAN_MODE_SLAVE,
        .joy_id = 0x250,
        .sw_id = 0x251,
        .mouse_id = 0x252,
        .keyboard_id = 0x253,
    },
};

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

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