/*
 * 配置相关
 */

#include "config.h"

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

#include "bsp/flash.h"

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

#define JOINT_CALF_OFFSET (2.7321f * 12.66)

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

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

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

        .mech_param = {
            .length = {
                .hip = 0.0861f,
                .thigh = 0.20f,
                .calf = 0.20f, 
            },
            .limit = {
                .max = {
                    .named = {
                        .lf_hip = 0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
                        .lf_thigh = 2.50f, /* 左前腿大腿最大角度，单位：弧度 */
                        .lf_calf = 2.60f,  /* 左前腿小腿最大角度，单位：弧度 */
                        .rf_hip = 0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
                        .rf_thigh = 0.0f, /* 右前腿大腿最大角度，单位：弧度 */
                        .rf_calf = -1.2f,  /* 右前腿小腿最大角度，单位：弧度 */
                        .lr_hip = 0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
                        .lr_thigh = 2.50f, /* 左后腿大腿最大角度，单位：弧度 */
                        .lr_calf = 2.60f,  /* 左后腿小腿最大角度，单位：弧度 */
                        .rr_hip = 0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
                        .rr_thigh = 0.0f, /* 右后腿大腿最大角度，单位：弧度 */
                        .rr_calf = -1.2f,  /* 右后腿小腿最大角度，单位：弧度 */
                    }
                },
                .min = {
                    .named = {
                        .lf_hip = -0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
                        .lf_thigh = 0.0f, /* 左前腿大腿最大角度，单位：弧度 */
                        .lf_calf = 1.20f,  /* 左前腿小腿最大角度，单位：弧度 */
                        .rf_hip = -0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
                        .rf_thigh = -2.50f, /* 右前腿大腿最大角度，单位：弧度 */
                        .rf_calf = -2.6f,  /* 右前腿小腿最大角度，单位：弧度 */
                        .lr_hip = -0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
                        .lr_thigh = 0.0f, /* 左后腿大腿最大角度，单位：弧度 */
                        .lr_calf = 1.20f,  /* 左后腿小腿最大角度，单位：弧度 */
                        .rr_hip = -0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
                        .rr_thigh = -2.50f, /* 右后腿大腿最大角度，单位：弧度 */
                        .rr_calf = -2.6f,  /* 右后腿小腿最大角度，单位：弧度 */
                    }
                }
            },


            .zero_point = {
                .named = {
                    .lf_hip = 0.02f,   /* 左前腿髋关节零点角度，单位：弧度 */
                    .lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
                    .lf_calf = 0.72f - JOINT_CALF_OFFSET,  /* 左前腿小腿零点角度，单位：弧度 */

                    .rf_hip = 5.37f,   /* 右前腿髋关节零点角度，单位：弧度 */
                    .rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
                    .rf_calf = 4.96f + JOINT_CALF_OFFSET,  /* 右前腿小腿零点角度，单位：弧度 */

                    .lr_hip = 4.5f,   /* 左后腿髋关节零点角度，单位：弧度 */
                    .lr_thigh = -3.2f, /* 左后腿大腿零点角度，单位：弧度 */
                    .lr_calf = 1.73f - JOINT_CALF_OFFSET,  /* 左后腿小腿零点角度，单位：弧度 */

                    .rr_hip = 2.7f,   /* 右后腿髋关节零点角度，单位：弧度 */
                    .rr_thigh = 10.58f, /* 右后腿大腿零点角度，单位：弧度 */
                    .rr_calf = 3.58f + JOINT_CALF_OFFSET,  /* 右后腿小腿零点角度，单位：弧度 */
                }
            },



            .sign = {
                .leg = {
                    [0] = { .hip = 1, .thigh = 1, .calf = -1 }, /* 左前腿 */
                    [1] = { .hip = -1, .thigh = -1, .calf = 1 },   /* 右前腿 */
                    [2] = { .hip = -1, .thigh = 1, .calf = -1 }, /* 左后腿 */
                    [3] = { .hip = 1, .thigh = -1, .calf = 1 },   /* 右后腿 */
                }
            },
            .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, /* 右后腿小腿减速比 */
                }
            },
            .leg_offset = {
                .x = 0.19625f, /* 腿偏移X轴，单位：米 */
                .y = 0.0825f, /* 腿偏移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
}