#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 = 18.0f,
			.i = 0.0f,
			.d =6.0f,
	    .f=0.1f,
			.i_limit = 200.0f,
			.out_limit = 5000.0f,
    },
    .C6020Omega_param = {
      .p =70.0f,
			.i =0.015f,
			.d =2.0f,
			.f=0.1f,
			.i_limit = 150.0f,
			.out_limit = 25000.0f  
    },
		 .CameraAngle_param = {
       .p = 50.0f,
			 .i = 0.3f,
			 .d = 3.5f,
			 .i_limit = 100.0f,
		   .out_limit =1000.0f,
	 },	
		 .CameraSpeed_param = {
       .p = 30.0f,
			 .i = 0.0f,
			 .d = 0.5f,
	    .i_limit = 0.0f,
			.out_limit =7000.0f, 
	 },			 
		.M3508_param = {
		 .p = 15.0f,
		 .i = 0.0f,
		 .d = 0.8f,
		 .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参数
			.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 =5.0f,
			   .i =0.1f,
			   .d =0.5f,
			   .i_limit =5.0f,
			   .out_limit =6000.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, 
			},
				
			
			.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_pos_xy_inner_param={
				.p =4.0f,
				.i =0.1f,
				.d =0.5f,
				.i_limit =5.0f,
				.out_limit =4000.0f,				 
			 },
				.pid_pos_xy_outer_param={
			  .p =4.0f,
				.i =0.1f,
				.d =0.5f,
				.i_limit =5.0f,
				.out_limit =4000.0f,				 
			},
				/*sick校准*/
				.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();
}