/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/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,   
    },
    .joint_motors[0] = { // 左膝关节
        .can = BSP_CAN_1,
        .motor_id = 124,
        .host_id = 130,
        .module = MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION,
    },
    .joint_motors[1] = { // 左髋关节
        .can = BSP_CAN_1,
        .motor_id = 125,
        .host_id = 130,
        .module = MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION,
    },
    .joint_motors[2] = { // 右髋关节
        .can = BSP_CAN_1,
        .motor_id = 126,
        .host_id = 130,
        .module = MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION,
    },
    .joint_motors[3] = { // 右膝关节
        .can = BSP_CAN_1,
        .motor_id = 127,
        .host_id = 130,
        .module = MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION,
    },
    .wheel_motors[0] = { // 左轮电机
        .can = BSP_CAN_1,
        .id = 1,  // 电机ID为1，对应命令ID=0x141，反馈ID=0x181
        .module = MOTOR_LK_MF9025,
        .reverse = false,
    },
    .wheel_motors[1] = { // 右轮电机
        .can = BSP_CAN_1,
        .id = 2,  // 电机ID为2，对应命令ID=0x142，反馈ID=0x182
        .module = MOTOR_LK_MF9025,
        .reverse = true,
    },
    
    .chassis_param = {
        .leglength={
            .k=1.0f,
            .p=0.10f,
            .i=0.0f,
            .d=0.0f,
            .i_limit=0.0f,
            .out_limit=100.0f,
            .d_cutoff_freq=-1.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 = 130,
                .module = MOTOR_LZ_RSO3,
                .reverse = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 左膝关节
                .can = BSP_CAN_1,
                .motor_id = 125,
                .host_id = 130,
                .module = MOTOR_LZ_RSO3,
                .reverse = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 右膝关节
                .can = BSP_CAN_1,
                .motor_id = 126,
                .host_id = 130,
                .module = MOTOR_LZ_RSO3,
                .reverse = true,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 右髋关节
                .can = BSP_CAN_1,
                .motor_id = 127,
                .host_id = 130,
                .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,
            },
        },
        .mech_zero_yaw = 0.0f,
        
        .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 = { -61.932040681074966f,	70.963671642086396f,	-37.730841182566571f,	-0.296475458388679f }, // theta
            .k12_coeff = { -0.586710094600108f,	0.886736272521581f,	-3.626144273475104f,	0.057861910518974f },  // d_theta
            .k13_coeff = { -17.297031110481019f,	16.286794934166178f,	-5.176451154477850f,	-0.867260018374823f },  // x
            .k14_coeff = { -14.893387150006664f,	14.357020815277332f,	-5.244645181873441f,	-0.869862096507486f},  // d_x
            .k15_coeff = { -75.327876471378886f,	79.786699741548944f,	-31.183500053811208f,	5.450635661115236f}, // phi
            .k16_coeff = { -3.572234723237025f,	4.164905011076312f,	-1.874828497771750f,	0.477913222933688f},  // d_phi
            .k21_coeff = { 9.327090608559319f,	1.362675928111105f,	-8.092777598567881f,	4.351387652359104f}, // theta
            .k22_coeff = { 3.872517778351810f,	-3.344077414726880f,	0.589693555828904f,	0.310036629174646f},  // d_theta
            .k23_coeff = { -71.615766251649134f,	74.748309711530837f,	-28.370490124006626f,	4.483586941100197f },  // x
            .k24_coeff = { -68.751866288568962f,	71.204785013044102f,	-26.812636604518396f,	4.238479323700952f },  // d_x
            .k25_coeff = { 205.6887659080132f,	-195.1219729060621f,	62.890188701113303f,	7.452313695653162f }, // phi
            .k26_coeff = { 16.162999842656344f,	-15.926932704437410f,	5.474613395300429f,	-0.002856083635449f },  // d_phi
        },
        .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
        .lqr_param.max_wheel_torque = 2.5f,  // 轮毂电机最大力矩 2.5Nm
    }
};

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

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