#include "config.h"
#include "flash.h"
#include "string.h"
#define DEBUG

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度


#ifdef DEBUG

ConfigParam_t param_chassis ={

#else
	
static const ConfigParam_t param_chassis ={


#endif
	.up={
		
		
		/*上层pid参数*/
.Shoot_M2006_angle_param = {  
    .p = 25.0f,        
    .i = 0.0f,         
    .d = 1.5f,         
    .i_limit = 1000.0f,
    .out_limit = 3000.0f,
},
.Shoot_M2006_speed_param = {  
    .p = 5.0f,         
    .i = 0.3f,         
    .d = 0.0f,         
    .i_limit = 2000.0f,
    .out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {  
    .p = 25.0f,        
    .i = 0.0f,         
    .d = 1.5f,         
    .i_limit = 1000.0f,
    .out_limit = 3000.0f,
},
.Pitch_M2006_speed_param={
    .p = 5.0f,         
    .i = 0.3f,         
    .d = 0.0f,         
    .i_limit = 2000.0f,
    .out_limit = 3000.0f,
},


.go_cmd={
    .id =0,
    .mode = 1,
    .K_P = 1.0f,
    .K_W = 0.05,
    .Pos = 0,
    .W = 0,
    .T = 0,

},


/*上层其他参数*/
    /*运球*/
    .DribbleConfig_Config = {
    .m3508_init_angle = 50,
    .m3508_high_angle = 1200,
    .go2_init_angle =  0,
    .go2_flip_angle = -250,
    .flip_timing = 200,
    .go2_release_threshold = -550.0f,
},
    /*投球*/
    .PitchConfig_Config = {
    .m2006_init_angle =-170,    
	  .m2006_trigger_angle =0,
    .go1_init_position = -50,   
	  .go1_release_threshold =-210,
		.m2006_Screw_init=0,	
},
	
	
	},
	 
   .can = {
		   .pitch6020 =  BSP_CAN_1,
       .motor3508 = BSP_CAN_1,  
		   .chassis6020 = BSP_CAN_1,
       .chassis5065 = BSP_CAN_1,
		 
		   .sick = BSP_CAN_2,
   },

};
const ConfigParam_t *Config_ChassisGet(void)
{
  return &param_chassis;
}

///*获取导航地图*/
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
//    config->ops.path = path;
//    config->ops.path_num = path_num;
//}
/**
 * \brief 从Flash读取配置信息
 *
 * \param cfg 配置信息
 */
void Config_Get(Config_t *cfg) {
  BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
//  /* 防止第一次烧写后访问NULL指针 */
	 cfg->chassis_config = &param_chassis;
  if (cfg->chassis_config == NULL) cfg->chassis_config = &param_chassis;
  /* 防止擦除后全为1 */
  if ((uint32_t)(cfg->chassis_config) == UINT32_MAX)
    cfg->chassis_config = &param_chassis;
	
	  if (memcmp(&cfg->cali_088.gyro_offset.x, "\xFF\xFF\xFF\xFF", 4) == 0) {
        cfg->cali_088.gyro_offset.x = 0.0f;
    }
    if (memcmp(&cfg->cali_088.gyro_offset.y, "\xFF\xFF\xFF\xFF", 4) == 0) {
        cfg->cali_088.gyro_offset.y = 0.0f;
    }
    if (memcmp(&cfg->cali_088.gyro_offset.z, "\xFF\xFF\xFF\xFF", 4) == 0) {
        cfg->cali_088.gyro_offset.z = 0.0f;
    }
}

/**
 * \brief 将配置信息写入Flash
 *
 * \param cfg 配置信息
 */
void Config_Set(Config_t *cfg) {
  osKernelLock();
  BSP_Flash_EraseSector(11);
  BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
  osKernelUnlock();
}