#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 ={

#else
	
static const ConfigParam_t param ={


#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 = 1600.0f,        
    .i = 0.0f,         
    .d = 3.0f,         
    .i_limit = 4000.0f,
    .out_limit = 20000.0f,
},
.Pitch_M2006_speed_param={
    .p = 8.5f,         
    .i = 0.03f,         
    .d = 0.0f,         
    .i_limit = 2000.0f,
    .out_limit = 15000.0f,
},
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
    .p = 20.0f,         
    .i = 0.05f,         
    .d = 0.0f,         
    .i_limit = 500.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,

},


/*上层其他参数,一些只是初始值，在运行过程中会被更改*/			

    /*投球*/
    .LaunchCfg = {
    .m2006_init = -150.0f,    // M2006初始角度
    .m2006_trig = 0.0f,       // M2006触发角度
    .go_pull_pos = -214.0f,   // go上升去合并扳机，扳机位置
    .go_up_speed = 12.0f,     // 上升速度
    .go_down_speed = 6.0f,     // 下降速度
		.Pitch_angle = 66,	           //俯仰角
		.go_init	 = -50
},
    .PitchCfg = {

		.go_init = -50,       //仅用在go上电，初始位置
	  .go_release_pos=-200,
		.Pitch_angle  =66,

}, 
		.PassCfg={
		.go_wait  =-10,   
	  .go_release_pos	=-150,		
	  .Pitch_angle=66, 
    .go_up_speed	=12,
    .go_down_speed =5,			
		},			
	},
	 .chassis = {/**/
		

	 	 .M3508_param = {
		 .p = 15.1f,
		 .i = 0.02f,
		 .d = 3.2f,
		 .i_limit = 200.0f,
		 .out_limit =6000.0f,
	 },	
		 /*视觉*/
		 .chassis_PICKWzPID_HIGN_param ={ 
			.p = 1.0f,
			.i = 0.03f,
			.d = 0.03f,
			.i_limit = 100.0f,
			.out_limit =2000.0f,	 
		 },
		 	.chassis_PICKWzPID_LOW_param ={ 
			.p = 0.5f,     //1.0 0.5
			.i = 0.5f,    //0.01 0.04
			.d = 0.0f,    //0.02 0.02
			.i_limit = 50.0f,
			.out_limit =1000.0f,	 
		 },
			.SickCali_WInparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
			.SickCali_WOutparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
			.SickCali_YInparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
			.SickCali_YOutparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
			.SickCali_XInparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
			.SickCali_XOutparam = {
				.p = 0.0f,
				.i = 0.0f,
				.d = 0.0f,
				.i_limit = 0.0f,
				.out_limit = 0.0f,
			},
					.Chassis_AngleAdjust_param={
			.p = 10.0f,     
			.i = 0.0f,   
			.d = 0.0f,    
			.i_limit = 0.0f,
			.out_limit =1000.0f,			
				
			},
			.sickparam={
				.w_error=5,
				.xy_error=5,
				.x_set=600,
				.y_set=100,
				
			},

				 
   },


   .can = {
       .motor_CHASSIS3508 = BSP_CAN_1, 
       . motor_UP3508_shoot=  BSP_CAN_2, 
		   .motor_UP3508_Dribble=  BSP_CAN_2, 
      		 
       .chassis5065 = BSP_CAN_1,
		 
		   .sick = BSP_CAN_2,
		 	 .encoder=BSP_CAN_2,

   },

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

///*获取导航地图*/
//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->config = &param;
  if (cfg->config == NULL) cfg->config = &param;
  /* 防止擦除后全为1 */
  if ((uint32_t)(cfg->config) == UINT32_MAX)
    cfg->config = &param;
	
	  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();
}