#include "app_math.h"
#include "bsp_time.h"
#include "system.h"
#include "math.h"
#include "ble_comm.h"
#include "app_flash.h"
#include "detect_zero_vel.h"
#include "tool.h"
#include "app_switchimu.h"
#include "detect_step_by_mag.h"
#include "pdrStatus.h"
#include "detect_step_by_mag.h"
#include "app_client_step.h"
#include "hal_mt.h"
#include "hal_led.h"
#include "bll_imu.h"
#include "app_game.h"

static uint32_t timeCNT = 0;
static uint8_t  FlagFix_process = 0;

//校准时间
void app_math_calit_time(uint8_t appminute){
	timeCNT = (appminute)*60*32768;
}

//一小时计时
static void app_math_Hour_process(void){
	static uint8_t Halfhour_cnt =0;
	uint32_t sec = 0;
	static uint32_t cnt_b=0;
	uint32_t cnt = NRF_RTC0->COUNTER;
	if(cnt<cnt_b) cnt += 16777216;
	timeCNT += cnt - cnt_b;
	sec = (timeCNT)/32768;
  
	if(cnt >16777216)cnt_b = cnt - 16777216;
	else cnt_b = cnt;
	
	if(sec >=1800){//半小时
		if(1 != Halfhour_cnt){
			Halfhour_cnt =1;
			FlagFix_process =1;
		}
	}
	
	if(sec >= 3600){//一小时
		timeCNT =0;
		app_client_step_SetIsScan();
		FlagFix_process =1;
		Halfhour_cnt =0;
		DEBUG_LOG("timeCNT(%d)(%d)\n",timeCNT,sec);
	}

}

//算法校准模式
static const bll_imu_one_way_param_t calibration_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_ON,											//前脚 - 时间戳开启
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_104HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_104HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_104HZ,	
};

static const bll_imu_one_way_param_t calibration_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,											  //后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_200HZ,										//后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
};

static const bll_imu_param_t calibration_bll_imu_param_t={
	.config_param[BLL_IMU_DIR_FRONT] = &calibration_front_param,
	.config_param[BLL_IMU_DIR_BACK] =  &calibration_back_param,
};


static void app_gyro_Fix_process(void){//陀螺仪零偏矫正
	static uint8_t state =0;
	static int16_t sample_count =0;
	static uint32_t tim =0;
	static uint8_t Setimu_config =0;
	bll_imu_data_t data= {0};
	uint8_t front_CS =0,back_CS =0;
	
	
	switch(state){
		case 0:
			    if(!app_game_GetGameMode() && 1 == FlagFix_process){
							Process_SetHoldOn(app_gyro_Fix_process,1);
							bll_imu_Resume_config_param(&calibration_bll_imu_param_t);
							state =1;
						  FlagFix_process =0;
						  Setimu_config =0;
						  tim = TIME_GetTicks();
						  DEBUG_LOG("====>>>>HAL_SER_IMU_MODE_MANAGE_CALIBRATION,tim:%d s\r\n",TIME_GetTicks()/1000);
					}
			    break;
		case 1: 
			    front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&calibration_bll_imu_param_t);
	        back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&calibration_bll_imu_param_t);
			    if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){
						 state =2;
						 Process_UpdatePeroid(app_gyro_Fix_process,10);
						 sample_count =0;
						 tim = TIME_GetTicks();
//						 MT_Run(500);
//						 DEBUG_LOG("====INTO HAL_SER_IMU_MODE_MANAGE_CALIBRATION\r\n");
					}
					else  if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
								 bll_imu_Resume_config_param(&calibration_bll_imu_param_t);
								 if(++Setimu_config >= 20){Setimu_config =0;
										bll_imu_Resume_unregister_config_param(&calibration_bll_imu_param_t);
									  state =3;
										Except_TxError(EXCEPT_GAME,"shoes into CALIBRATION mode fail");
								 }
					}
					if(app_game_GetGameMode())state =3;//游戏模式下退出
			    break;
		case 2:
					//读取ACC值6
					if(bll_imu_get_data_num(BLL_IMU_DIR_FRONT) >= 1){
						bll_imu_get_data(BLL_IMU_DIR_FRONT, 0, &data);
						estimate_gyr_bias_interface(data.gry,sample_count);
//						DEBUG_LOG("====>>>>gry:%d,%d,%d,tim:%d\r\n",data.gry[0],data.gry[1],data.gry[2],sample_count);
						sample_count++;
					}
					if(TIME_GetTicks()-tim>=10000){ //跑完10秒退出
						 state =3;
//						 DEBUG_LOG("====>>>>HAL_SER_IMU_MODE_MANAGE_CALIBRATION\r\n");
					}
					if(app_game_GetGameMode())state =3;//游戏模式下退出
			    break;
		case 3:
			    Process_UpdatePeroid(app_gyro_Fix_process,1000);
		      bll_imu_Resume_unregister_config_param(&calibration_bll_imu_param_t);
		      Process_SetHoldOn(app_gyro_Fix_process,0);
				  state =0;
		      Setimu_config =0;
//		      DEBUG_LOG("====OUT app_gyro_Fix_process\r\n");
			    break;
		default:state =0;Setimu_config =0;break;
	}
	
}

void app_math_Init(void)
{
	Process_Start(500,"app_gyro_Fix_process",app_gyro_Fix_process);
	Process_Start(1000,"app_math_Hour",app_math_Hour_process);
}