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

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

#ifdef DEBUG

ConfigParam_t param_chassis ={

#else
	
static const ConfigParam_t param_chassis ={

#endif

   .chassis = {/**/	 
    .C6020Angle_param = {
      .p = 30.0f,
			.i = 0.05f,
			.d = 2.0f,
			.i_limit = 20.0f,
			.out_limit = 5000.0f,
    },
    .C6020Omega_param = {
      .p = 45.0f,
			.i = 0.4f,
			.d = 6.0f,
			.i_limit = 2000.0f,
			.out_limit = 25000.0f  
    },
		.Chassis_AngleAdjust_param={
			.p = 10.0f,     
			.i = 0.0f,   
			.d = 0.0f,    
			.i_limit = 0.0f,
			.out_limit =6000.0f,							
		},
		// 家里雷达pid
//		 .RadarAngle_param = {
//       .p = 22.0f,
//			 .i = 20.0f,
//			 .d = 800.0f,
//			 .i_limit = 100.0f,
//		   .out_limit =2000.0f,
//	 },	
//		 .RadarSpeed_param = {
//       .p = 7.0f,
//			 .i = 0.1f,
//			 .d = 50.0f,
//	    .i_limit = 1000.0f,
//			.out_limit =8000.0f, 
//	 },

 //备馆雷达pid		
//   .RadarAngle_param = {
//       .p = 20.0f,
//			 .i = 0.06f,
//			 .d = 650.0f,
//			 .i_limit = 30.0f,
//		   .out_limit =2000.0f,
//	 },	
//		 .RadarSpeed_param = {
//       .p = 7.0f,
//			 .i = 0.1f,
//			 .d = 200.0f,
//	    .i_limit = 1000.0f,
//			.out_limit =8000.0f, 
//	 },

//第一次调
//   .RadarAngle_param = {
//       .p = 22.0f,
//			 .i = 1.2f,
//			 .d = 700.0f,
//			 .i_limit = 30.0f,
//		   .out_limit =4000.0f,
//	 },	
//		 .RadarSpeed_param = {
//       .p = 9.0f,
//			 .i = 0.15f,
//			 .d = 450.0f,
//	    .i_limit = 1200.0f,
//			.out_limit =8000.0f, 
//	 },

//第二次调
//   .RadarAngle_param = {
//       .p = 30.0f,
//			 .i = 0.9f,
//			 .d = 650.0f,
//			 .i_limit = 30.0f,
//		   .out_limit =4000.0f,
//	 },	
//		 .RadarSpeed_param = {
//       .p = 7.8f,
//			 .i = 0.3f,
//			 .d = 250.0f,
//	    .i_limit = 15.0f,
//			.out_limit =8000.0f, 
//	 },	

//第三次调
   .RadarAngle_param = {
       .p = 23.0f,
			 .i = 0.09f,
			 .d = 75.0f,
			 .i_limit = 30.0f,
		   .out_limit =8000.0f,
	 },	
		 .RadarSpeed_param = {
       .p = 8.3f,
			 .i = 0.005f,
			 .d = 900.0f,
	    .i_limit = 10.0f,
			.out_limit =8000.0f, 
	 },	
		 
	 .SickVx_param = {
			 .p =12.0f,
			 .i =0.3f,
			 .d =1000.0f,
			 .i_limit =2500.0f,   
			 .out_limit =15000.0f,				
		},
		.SickVy_param = {
			 .p =15.0f,
			 .i =0.3f,
			 .d =1200.0f,
			 .i_limit =2500.0f,  
			 .out_limit =15000.0f,				
		},
		.SickVw_param = {
			.p =20.0f,
			.i =0.3f,
			.d =800.0f,
			.i_limit = 2500.0f,
			.out_limit = 8000.0f				
		},		
		
		.M3508_param = {
		 .p = 15.0f,
		 .i = 0.0f,
		 .d = 1.5f,
		 .i_limit = 0.0f,
		 .out_limit = 5000.0f,
	 },
//    .C6020pitAngle_param = {
//			.p = 10.0f,
//			.i = 0.025f,
//			.d =5.0f,
//			.i_limit = 2000.0f,
//			.out_limit = 3000.0f,
//		},
//		.C6020pitOmega_param = {
//			.p =10.0f,
//			.i =0.0f,
//			.d =0.0f,
//			.i_limit = 2000.0f,
//			.out_limit = 3000.0f,				
//		}, 
	},

           /*码盘导航*/	 
	 
	 .ops={
		 
		.ops_pid={
			/*路径速度*/    
			//不要盲目的为了提高速度直接给大out，一定要慢慢往上加并根据情况调整pid参数

			/*原本的pid，速度不快，很稳定，适合作为基础速度*/
			.path_speed_pid_param=
			{
			 .p = 20.0f,
			 .i = 0.02f,
			 .d = 3.2f,
			 .i_limit = 200.0f,
			 .out_limit =6000.0f,				
			},
			.pid_PosSpeed_xy_param={
			   .p =25.0f,
			   .i =0.1f,
			   .d =0.0f,
			   .i_limit =5.0f,
			   .out_limit =6000.0f,					 
			},
//			//比赛pid
//			.path_speed_pid_param=
//			{
//			 .p = 0.5f,
//			 .i = 0.0f,
//			 .d = 0.0f,
//			 .i_limit = 0.0f,
//			 .out_limit =1000.0f,				
//			},
//			
//			/*远距离xy纠正*/
//			.pid_PosSpeed_xy_param={
//			   .p =25.0f,
//			   .i =0.1f,
//			   .d =500.0f,
//			   .i_limit =500.0f,
//			   .out_limit =20000.0f,					 
//			}, 
  		/*远距离角度纠正*/ 
			.pid_OutAngle_param={ 
				 .p = 20.0f,
				 .i = 0.0f,
				 .d = 0.0f,
				 .i_limit = 0.0f,
		     .out_limit =2000.0f,	 
			},
			.pid_InnerAngle_param={
				 .p = 20.0f,
				 .i = 0.04f,
				 .d = 0.0f,
			   .i_limit = 0.0f,
					.out_limit =16000.0f, 
			},
			
			/*近距离xy纠正*/
			.pid_pos_xy_outer_param={
			  .p =4.0f,
				.i =0.1f,
				.d =0.5f,
				.i_limit =5.0f,
				.out_limit =1000.0f,				 
			},	
			.pid_pos_xy_inner_param={
				.p =4.0f,
				.i =0.1f,
				.d =0.5f,
				.i_limit =5.0f,
				.out_limit =6000.0f,				 
			 },
			/*近距离角度纠正*/	
//原本的pid				
//			.pid_OutAngle_hold_param={
//			   .p = 50.0f,
//				 .i = 0.3f,
//				 .d = 3.5f,
//				 .i_limit = 100.0f,
//		     .out_limit =1000.0f,
//		   },
//			   .pid_InnerAngle_hold_param={
//				 .p = 30.0f,
//				 .i = 0.0f,
//				 .d = 0.5f,
//			   .i_limit = 0.0f,
//				 .out_limit =7000.0f, 
//		   },
      //比赛pid			
			 .pid_OutAngle_hold_param={
			   .p = 25.0f,
				 .i = 0.1f,
				 .d = 5.0f,
				 .i_limit = 100.0f,
		     .out_limit =1000.0f,
		   },
			 .pid_InnerAngle_hold_param={
				 .p = 20.0f,
				 .i = 0.1f,
				 .d = 0.5f,
			   .i_limit = 400.0f,
				 .out_limit =7000.0f, 
		   },
			 
				/*sick校准pid*/
			 .pid_sick_out_param={
				  .p =8.0f,
			   .i =0.1f,
			   .d =0.5f,
			   .i_limit =5.0f,
			   .out_limit =4000.0f,			 					
				},
			 .pid_sick_inner_param={
			  .p =5.0f,
				.i =0.1f,
				.d =0.5f,
				.i_limit =5.0f,
				.out_limit =4000.0f,		
				},
		},
		
	},
   .can = {
       .chassis5065 = BSP_CAN_2,
       .chassis6020 = BSP_CAN_1,
		   
   },
};

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

/**
 * \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();
}