#include "app_math.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_ser_imu_mode_manage.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"

#define IMU_MAX_GROUP_NUM 20

static int16_t 	acc_front[IMU_MAX_GROUP_NUM][3];
static int16_t  gry_front[IMU_MAX_GROUP_NUM][3];
static int16_t	mag6310_front[IMU_MAX_GROUP_NUM][3];

static int16_t	mag6310_back[3];
static int32_t	timestamp_front[IMU_MAX_GROUP_NUM];
static uint8_t  rssi;

static int16_t  IMU_STATUS; //记录状态用来重新记录时间戳

static int32_t  timestamp;

static int32_t  last_timestamp;

void process_imu_data_front(int front_index)
{
	if(IMU_STATUS != 1)IMU_STATUS = 1;{
		last_timestamp = timestamp_front[0];
		timestamp = 0;
	}
	
	if(front_index >IMU_MAX_GROUP_NUM)return;
	for(int i = 0; i < front_index; i++)
	{
		int32_t dt = timestamp_front[i] - last_timestamp;
		
		if(dt > 20000 || dt < 0)
		{
			dt = 10000;
		}
		
		timestamp += dt;
		
//		DEBUG_LOG("timestamp_front[i] - last_timestamp : %d;  i = %d NRF_RTC0->COUNTER:%d\r\n", timestamp_front[i] - last_timestamp, i,NRF_RTC0->COUNTER);
		
		last_timestamp = timestamp_front[i];
		
//		DEBUG_LOG("timestamp_front[i] : %d;  i = %d\r\n", timestamp_front[i], i);
		
		IMU_Process_motion_queue(mFlash.isHost, timestamp, acc_front[i],
		gry_front[i],mag6310_front[i], mag6310_back, rssi);
	}
}

//测试使用，游戏模式下 1S亮绿灯20ms.
static void app_game_led(uint8_t gamemode){
	static uint8_t led_state =0;
	static uint8_t temp =0;
	if(gamemode){
		 temp++;
		 if(temp == 100){
			  if(led_state){led_state =0;
					LED_Start(LED_RUN,COLOR_GREEN);
			  }
			  else {led_state =1;
					LED_Start(LED_RUN,COLOR_BLACK);
			  }
	   }
		 if(temp > 102){temp =0;
			 LED_Stop(LED_RUN);
		 }
	}else{
		 if(2 == led_state){
		    LED_Stop(LED_RUN);
		 }
	}
}

void app_math_TimerCounter(int front_num, int back_num, int _rssi)
{
	ser_imu_data_t		data;
	int16_t 				group_num = 0;
	
	//游戏模式
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) != -1){
		
		rssi = _rssi;
		group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		if(group_num >IMU_MAX_GROUP_NUM || front_num == 0)return;
		for(int i=0;i<group_num;i++){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
			gry_front[i][0] = data.gry[0];gry_front[i][1] = data.gry[1];gry_front[i][2] = data.gry[2];
			acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
			mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
			timestamp_front[i] = data.fifo_timestamp;
//			JS_RTT_Print_06(acc_front[i][0],acc_front[i][1],acc_front[i][2],gry_front[i][0],gry_front[i][1],gry_front[i][2]);
//			JS_RTT_Print_06(mag6310_front[i][0],mag6310_front[i][1],mag6310_front[i][2],timestamp_front[i],0,0);
		}
		
		if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
			mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2];
		}
		
		if(mFlash.isHost){
				process_imu_data_front(group_num);
			}else if(Slave_Get7_5ms_interval()){
				process_imu_data_front(group_num);
			}
			app_game_led(1);
	}else{
	 //将状态重设为0
	 IMU_STATUS = 0;
	 set_pdr_status();
	 app_game_led(0);
	}
			 
	
	//实时计步模式
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) != -1){
		rssi = _rssi;
		group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		if(group_num >IMU_MAX_GROUP_NUM || front_num == 0)return;
		for(int i=0;i<group_num;i++){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
			gry_front[i][0] = data.gry[0];gry_front[i][1] = data.gry[1];gry_front[i][2] = data.gry[2];
			acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
			mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
			timestamp_front[i] = data.fifo_timestamp;
		}
		if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
			mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2];
		}
		
//		DEBUG_LOG("======RealTimeStep=====%d\r\n",TIME_GetTicks());
		if(RealTimeStep((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc_front)){
			 mFlash.mStep.stepCur[0]++;
			 DEBUG_LOG("======RealTimeStep=====step %d\r\n",mFlash.mStep.stepCur[0]);
		}		 
	}
}

static void app_math_DailyStep_Process(void)
{
	int16_t 	acc[3];
	int16_t		mag6310[3];
	int16_t 					group_num = 0;
	ser_imu_data_t		data;
	

	uint8_t displaybuf[30]={0};
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1 && hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) == -1){
		
	if(mFlash.isHost)sprintf((char *)displaybuf,"11111 Left DailyStep current step\r\n");
		
		group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		for(int i=0; i < group_num; i++){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
			mag6310[0] = data.mag[0];mag6310[1] = data.mag[1];mag6310[2] = data.mag[2];
			acc[0] = data.acc[0];acc[1] = data.acc[1];acc[2] = data.acc[2];
			
//			DEBUG_LOG("f_mx=%d\r,f_my=%d\r,f_mz=%d\r\n",mag6310[0],mag6310[1],mag6310[2]);
			if(1 == detect_step_by_mag(mag6310,acc[2])){
					mFlash.mStep.stepCur[0]++;
				  DEBUG_LOG("DailyStep current step:%d\r\n",mFlash.mStep.stepCur[0]);
				  break;
			}
		}
	}
}


static uint32_t timeCNT = 0;
static uint8_t FlagFix_process = 0;
//一小时计时
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(sec >= 130000 )DEBUG_LOG("!!!!!!!!!!!!!!! sec over,%d,%d,%d\n",cnt,cnt_b,sec);
	if(cnt >16777216)cnt_b = cnt - 16777216;
	else cnt_b = cnt;
	
	if(sec >= 1800){//1800半小时
		Halfhour_cnt++;
		FlagFix_process =1;
		timeCNT =0;
		DEBUG_LOG("timeCNT(%d)(%d)\n",timeCNT,sec);
	}
	if(Halfhour_cnt >=2){Halfhour_cnt =0;
		app_client_step_SetIsScan();
	}
}

static void app_gyro_Fix_process(void){//陀螺仪零偏矫正
	static uint8_t state =0;
	static int16_t sample_count =0;
	static uint32_t tim =0;
	ser_imu_data_t data;
	
	switch(state){
		case 0:
			    if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) && 1 == FlagFix_process){
							Process_SetHoldOn(app_gyro_Fix_process,1);
							hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_CALIBRATION, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
							state =1;
						  FlagFix_process =0;
						  tim = TIME_GetTicks();
						  DEBUG_LOG("====>>>>HAL_SER_IMU_MODE_MANAGE_CALIBRATION,tim:%d s\r\n",TIME_GetTicks()/1000);
					}
			    break;
		case 1:
			    if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_CALIBRATION)){
						 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(TIME_GetTicks()-tim>=10000){tim = TIME_GetTicks();//进入游戏模式10秒失败
//							 DEBUG_LOG("====cnt >= 3000\r\n");
						   state =3;
						 }
					}
					if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME))state =3;//游戏模式下退出
			    break;
		case 2:
					//读取ACC值6
					if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT) >= 1){
						hal_ser_imu_mode_manage_get_data(SER_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(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME))state =3;//游戏模式下退出
			    break;
		case 3:
			    Process_UpdatePeroid(app_gyro_Fix_process,1000);
		      hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_CALIBRATION, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
		      Process_SetHoldOn(app_gyro_Fix_process,0);
				  state =0;
//		      DEBUG_LOG("====OUT app_gyro_Fix_process\r\n");
			    break;
		default:state =0;break;
	}
	
}

void app_math_Init(void)
{
	Process_Start(0,"app_math_DailyStep_Process",app_math_DailyStep_Process);
//	Process_Start(10,"app_math_TimerCounter",app_math_TimerCounter);
	
	hal_ser_imu_data_update_notify(app_math_TimerCounter);
	
	Process_Start(500,"app_gyro_Fix_process",app_gyro_Fix_process);
	Process_Start(1000,"app_math_Hour",app_math_Hour_process);
}