#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)) //角度转弧度


/*导航地图*/
#define point_num 4
 
point_t map_point[point_num] ={

 	  {0.0, 0, DEG_TO_RAD(90.0), 5.0},
		
    {0,  0, DEG_TO_RAD(180.0), 5.0},
//		
    {0.0, 0.0, DEG_TO_RAD(270.0), 5.0},
		
    {0.0, 0.0, DEG_TO_RAD(360.0), 5.0},
		
		
};


#ifdef DEBUG

ConfigParam_t param_chassis ={

#else
	
static const ConfigParam_t param_chassis ={

#endif

   .chassis = {/**/
    .C6020pitAngle_param = {
			.p = 15.0f,
			.i = 0.3f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f,
		},
		.C6020pitOmega_param = {
			.p =30.0f,
			.i =0.3f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f				
		},
		
		.Gimbal_yawAngle_param = {
			.p =8.0f,
			.i =0.0f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f
		},
		
		.Gimbal_yawOmega_param = {
			.p =18.0f,
			.i =0.0f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f
		},
		
		.Gimbal_pitchAngle_param = {
			.p =8.0f,
			.i =0.0f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f
		},
		
		.Gimbal_pitchOmega_param = {
			.p =18.0f,
			.i =0.0f,
			.d =0.0f,
			.i_limit = 200.0f,
			.out_limit = 3000.0f
		},
     .AngleCor_param = {
			.p =0.8f,
			.i =0.0f,
			.d =1.0f,
			.i_limit = 0.0f,
			.out_limit =5000.0f,
     },
		 
     .OmegaCor_param = {
			.p =23.5f,
			.i =0.0f,
			.d =0.05f,
			.i_limit = 0.0f,
			.out_limit =5000.0f,
     },
		 
   .ImuCor_param = {
	    .p =95.0f,
	 		.i =0.0f,
			.d =0.0f,
			.i_limit = 0.0f,
			.out_limit =200.0f,
    }, 
	 
	 .DisCamera_param = {
       .p =80.0f,
		   .i =0.1f,
		   .d =0.0f,
		   .i_limit = 0.0f,
		   .out_limit =5000.0f,
	 },
	 
	 	 .M3508_param = {
		 .p = 15.1f,
		 .i = 0.02f,
		 .d = 3.2f,
		 .i_limit = 200.0f,
		 .out_limit =6000.0f,
	 },	
//	 	 .M3508_param = {
//		 .p = 10.0f,
//		 .i = 0.0f,
//		 .d = 0.0f,
//		 .i_limit = 0.0f,
//		 .out_limit =10000.0f,
//	 },	

////高速那一套	 
//    .NaviVx_param ={
//			.p = 1.15f,
//			.i = 0.0f,
//			.d = 0.15f,
//			.i_limit = 0.0f,
//			.out_limit =5000.0f,			
//		},
//		 .NaviVy_param ={
//			 .p = 1.15f,
//			.i = 0.0f,
//			.d = 0.15f,
//			.i_limit = 0.0f,
//			.out_limit =5000.0f,	
//		 },
//		 .NaviVw_param ={
//			.p = 1.15f,
//			.i = 0.0f,
//			.d = 0.15f,
//			.i_limit = 0.0f,
//			.out_limit =5000.0f,	 
//		 },
//  

//低速那一套	 
    .NaviVx_param ={
			.p = 1.04f,
			.i = 0.0f,
			.d = 0.15f,
			.i_limit = 0.0f,
			.out_limit =5000.0f,			
		},
		 .NaviVy_param ={
			 .p = 1.1f,
			.i = 0.0f,
			.d = 0.15f,
			.i_limit = 0.0f,
			.out_limit =5000.0f,	
		 },
		 .NaviVw_param ={
			.p = 1.5f,
			.i = 0.0f,
			.d = 0.15f,
			.i_limit = 0.0f,
			.out_limit =5000.0f,	 
		 },
		 .Sick_CaliYparam ={
			 .p =2.5f,
			 .i =0.001f,
			 .d =0.1f,
			 .i_limit =15.0f,
			 .out_limit =500.0f,		 
		 },
		 .Sick_CaliXparam ={
			 .p =2.5f,
			 .i =0.001f,
			 .d =0.1f,
			 .i_limit =15.0f,
			 .out_limit =500.0f,	
      			 
		 }

   },
	 
	  
   .can = {
		   .pitch6020 =  BSP_CAN_1,
       .motor3508 = BSP_CAN_1,  
		   .chassis6020 = BSP_CAN_2,
		   .sick = BSP_CAN_2,
   },

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