/*
 * 配置相关
 */

#include "config.h"

#include <stdint.h>
#include <string.h>

#include "bsp/flash.h"

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

Config_t param_default = {
    
    .uart = {
        .left_leg = BSP_UART_LEFT_LEG,
        .right_leg = BSP_UART_RIGHT_LEG,
    },
    .cali = {
        .bmi088 = {
            .gyro_offset = {0.0f, 0.0f, 0.0f},
        },
    },
    .chassis = {
        .type = CHASSIS_TYPE_QUADRUPED,

        .torque_pid_param = {
            .k = 1.0f, /* 控制器增益 */
            .p = 1.0f, /* 比例项增益 */
            .i = 0.0f, /* 积分项增益 */
            .d = 0.0f, /* 微分项增益 */
            .i_limit = 0.0f, /* 积分项上限 */
            .out_limit = 0.0f, /* 输出绝对值限制 */
            .d_cutoff_freq = 10.0f, /* D项低通截止频率 */
            .range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
        },
        
        .low_pass_cutoff_freq = {
            .in = -1.0f,
            .out = -1.0f,
        },

        .mech_param = {
            .length = {
                .named = {
                    .lf_hip = 0.0861f,   /* 左前腿髋关节长度，单位：米 */
                    .lf_thigh = 0.20f, /* 左前腿大腿长度，单位：米 */
                    .lf_calf = 0.20f,  /* 左前腿小腿长度，单位：米 */
                    .rf_hip = 0.0861f,   /* 右前腿髋关节长度，单位：米 */
                    .rf_thigh = 0.20f, /* 右前腿大腿长度，单位：米 */
                    .rf_calf = 0.20f,  /* 右前腿小腿长度，单位：米 */
                    .lr_hip = 0.0861f,   /* 左后腿髋关节长度，单位：米 */
                    .lr_thigh = 0.20f, /* 左后腿大腿长度，单位：米 */
                    .lr_calf = 0.20f,  /* 左后腿小腿长度，单位：米 */
                    .rr_hip = 0.0861f,   /* 右后腿髋关节长度，单位：米 */
                    .rr_thigh = 0.20f, /* 右后腿大腿长度，单位：米 */
                    .rr_calf = 0.20f,  /* 右后腿小腿长度，单位：米 */
                }
            },
            .zero_point = {
                .named = {
                    .lf_hip = 1.98f,   /* 左前腿髋关节零点角度，单位：弧度 */
                    .lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
                    .lf_calf = -32.367f,  /* 左前腿小腿零点角度，单位：弧度 */
                    .rf_hip = 5.56f,   /* 右前腿髋关节零点角度，单位：弧度 */
                    .rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
                    .rf_calf = 37.047f,  /* 右前腿小腿零点角度，单位：弧度 */
                    .lr_hip = 4.76f,   /* 左后腿髋关节零点角度，单位：弧度 */
                    .lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
                    .lr_calf = -35.847f,  /* 左后腿小腿零点角度，单位：弧度 */
                    .rr_hip = 2.42f,   /* 右后腿髋关节零点角度，单位：弧度 */
                    .rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
                    .rr_calf = 38.55f,  /* 右后腿小腿零点角度，单位：弧度 */
                }
            },
            .ratio = {
                .named = {
                    .lf_hip = 6.33f,   /* 左前腿髋关节减速比 */
                    .lf_thigh = 6.33f, /* 左前腿大腿减速比 */
                    .lf_calf = 12.66f, /* 左前腿小腿减速比 */
                    .rf_hip = 6.33f,   /* 右前腿髋关节减速比 */
                    .rf_thigh = 6.33f, /* 右前腿大腿减速比 */
                    .rf_calf = 12.66f, /* 右前腿小腿减速比 */
                    .lr_hip = 6.33f,   /* 左后腿髋关节减速比 */
                    .lr_thigh = 6.33f, /* 左后腿大腿减速比 */
                    .lr_calf = 12.66f, /* 左后腿小腿减速比 */
                    .rr_hip = 6.33f,   /* 右后腿髋关节减速比 */
                    .rr_thigh = 6.33f, /* 右后腿大腿减速比 */
                    .rr_calf = 12.66f, /* 右后腿小腿减速比 */
                }
            },
            .mech_center = {
                .x = 0.0f, /* 机械中心X坐标，单位：米 */
                .y = 0.0f, /* 机械中心Y坐标，单位：米 */
                .z = 0.0f, /* 机械中心Z坐标，单位：米 */
            }, /* 机械中心位置 */

        },


    },
}; /* param_default */



/**
 * \brief 获取配置信息
 *
 * \param cfg 配置信息
 */
void Config_Get(Config_t *cfg) {
    //直接把param_default传进去
    if (cfg == NULL) return; // 检查参数是否为NULL
    memcpy(cfg, &param_default, sizeof(Config_t)); // 复制默认配置到cfg
}