/*
 * 配置相关
 */

/* 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 = {
        .follow_pid_param = {
            .k = 1.0f,
            .p = 20.0f,
            .i = 0.0f,
            .d = 0.0f,
            .i_limit = 0.0f,
            .out_limit = 500.0f,
            .d_cutoff_freq = 30.0f,
            .range = 0.0f,
        },
        .motor_pid_param = {
            .k = 1.0f,
            .p = 0.5f,
            .i = 0.0f,
            .d = 0.0f,
            .i_limit = 0.0f,
            .out_limit = 12.0f,
            .d_cutoff_freq = 30.0f,
            .range = 0.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 = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
            { // 右髋关节
                .can = BSP_CAN_1,
                .motor_id = 127,
                .host_id = 130,
                .module = MOTOR_LZ_RSO3,
                .reverse = false,
                .mode = MOTOR_LZ_MODE_MOTION,
            },
        },
        .wheel_motors = {
            { // 左轮电机
                .can = BSP_CAN_1,
                .id = 1,
                .module = MOTOR_LK_MF9025,
                .reverse = false,
            },
            { // 右轮电机
                .can = BSP_CAN_1,
                .id = 2,
                .module = MOTOR_LK_MF9025,
                .reverse = true,
            },
        },
        .mech_zero_yaw = 0.0f,
    }
};

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

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