#include "hal_imu.h"
#include "system.h"
#include "bsp_time.h"
#include "bsp_spi.h"
#include "vl53l1.h"
#include "drv_qmc5883L_f.h"
#include "nrf_drv_timer.h"
#include "nrf_delay.h"
#include "hal_mt.h"
#include "hal_ble_client.h"
#include "hal_ble_host.h"
#include "main.h"
#include "app_game.h"
#include "ble_comm.h"
#include "hal_flash.h"
#include "app_err.h"
#include "hal_led.h"
#include "hal_mempress.h"
#include "motion_interface.h"
#include "app_connect_manage.h"
#include "IOI2C.h"
#include "detect_step_by_acc.h"
#include "arm_math.h"
#include "tool.h"
#include "hal_step.h"
#include "hal_mag.h"
#include "hal_attitude.h"
#include "app_ImuCalibration.h"
#include "drv_qmc6310.h"
#include "hal_Realstep.h"
#include "detect_zero_vel.h"
#include "drv_mpu9250.h"

#define ACC_FIFO_SIZE				10

/********************** 变量区 *************************/
static volatile uint8_t  rssi = 0;
static volatile int32_t  distand = 0;

static uint8_t isGameMode = 0;
static uint8_t sensorEnable = 0;

/********************** 外部接口声明区 *************************/

void send_protocol(uint8_t index,uint8_t cmd,uint8_t* p,uint8_t datLen)
{
	BLE_Client_Tx_Send(0,cmd,p,datLen);
}

void IMU_SetGameMode(uint8_t isgame)
{
	isGameMode = isgame;
}

uint8_t IMU_GetGameMode(void)
{
	return sensorEnable;
}

uint8_t IMU_GetHostRssi(void)
{
	return rssi;
}

__weak void process_motion(int16_t* _acc, int16_t* _gry, int16_t* front_mag, int16_t* back_mag, uint8_t _rssi)
{
	static uint32_t tim=0;
	if(TIME_GetTicks()-tim>=1000){ tim = TIME_GetTicks();
		SEGGER_RTT_printf(0,"__weak void process_motion\n");
	}
}
	
uint8_t left_buff[64];
uint8_t left_len;
uint8_t right_buff[64];
uint8_t right_len;
uint8_t inter_buff[128];
uint8_t inter_buff_len;

void IMU_SetSlaveData(uint8_t* pdat,uint8_t len)
{
	if(pdat[3]==BLE_Host_R_TRACK){
		IMU_Rec_data(pdat,len);
	}
	app_game_SetClientGameMode();
}

void IMU_GetSensorData(void)
{
	hal_att_read_Acc_Gry_temp();
	hal_Mag_ReadXYZ();
	
	#if DEBUG_IMU
	int16_t *acc = hal_att_GetAcc();
	int16_t *gry = hal_att_GetGry();
	SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",acc[0],acc[1],acc[2]);
	SEGGER_RTT_printf(0,"h_gx=%d\r,h_gy=%d\r,h_gz=%d\r\n",gry[0],gry[1],gry[2]);
	float *p1 =NULL,*p2=NULL;
	p1 =hal_Mag_GetXYZ(Mag_Back);
	p2 =hal_Mag_GetXYZ(Mag_Front);
	static char string[200];
	sprintf(string,"Mag_Front=%f,%f,%f,Mag_Back=%f,%f,%f\n",p2[0],p2[1],p2[2],p1[0],p1[1],p1[2]);
	SEGGER_RTT_printf(0,"%s",string);
	SEGGER_RTT_printf(0,"h_rssi=%d\n",rssi);
	#endif

}

void IMU_GetSensorDataLowPower(void)
{
	static uint32_t 				tim=0;
	if(TIME_GetTicks()-tim>=50){ tim = TIME_GetTicks();

	  hal_att_read_acc();
		hal_Mag_ReadXYZ();

		#if DEBUG_IMU
		int16_t *acc = hal_att_GetAcc();
	  SEGGER_RTT_printf(0,"MPU9250 h_ax[1]=%d\r,h_ay[1]=%d\r,h_az[1]=%d\n",acc[0],acc[1],acc[2]);
		
		float *p1 =NULL,*p2=NULL;
		p1 =hal_Mag_GetXYZ(Mag_Back);
		p2 =hal_Mag_GetXYZ(Mag_Front);
		static char string[200];
		sprintf(string,"Mag_Front=%f,%f,%f,Mag_Back=%f,%f,%f\n",p2[0],p2[1],p2[2],p1[0],p1[1],p1[2]);
		SEGGER_RTT_printf(0,"%s",string);
		#endif
	}
}

/********************** 函数声明区 *************************/
void IMU_TimerCounter(void* t)
{
	static int16_t front_mag_int[3];
	static int16_t back_mag_int[3];
	float *p1 =NULL,*p2=NULL;
	int16_t *acc =NULL,*gry=NULL;
	if(1 == time_10ms_flag){
			   #if CALIBRATION_ENANBLE
			   if(app_imucal_getstate())IMU_GetSensorData();
			   #else
				 if(sensorEnable>0 || hal_Realstep_GetState()){
							IMU_GetSensorData();		 
					    p1= hal_Mag_GetXYZ(Mag_Front);
					    p2= hal_Mag_GetXYZ(Mag_Back);
					    acc = hal_att_GetAcc();
	            gry = hal_att_GetGry();
							for(uint8_t i = 0; i < 3; i ++)
							{
								front_mag_int[i] = (int16_t) (p1[i] *10);
								back_mag_int[i] = (int16_t) (p2[i] *10);
							}
							if(sensorEnable>0 ){
									if(mFlash.isHost){
									 IMU_Process_motion_queue(mFlash.isHost,(int16_t*)acc,(int16_t*)gry,(int16_t*)front_mag_int,(int16_t*)back_mag_int,rssi);
									}else  if(Slave_Get7_5ms_interval()){
									 IMU_Process_motion_queue(mFlash.isHost,(int16_t*)acc,(int16_t*)gry,(int16_t*)front_mag_int,(int16_t*)back_mag_int,rssi);
									}
							}
							else start_cal_step((int16_t*)front_mag_int, (int16_t*)back_mag_int, (int16_t*)acc);
			   }
			   #endif
	}
}

static void hal_imu_readrssi_process(void){
	if(sensorEnable>0 || hal_Realstep_GetState()){
			rssi = 0-host_get_rssi();
	}
}

static void IMU_process(void)
{
	enum {
		STATE_lOWPOWERMODE = 0,
		STATE_ENTER_GAMEMODE,
		STATE_GAMEMODE,
		STATE_EXIT_PRE,
		STATE_EXIT_GAMEMODE,
	};
	static uint8_t state = 0;
	static uint32_t tim =0;
	static uint8_t n = 0;

	switch(state){
		case STATE_lOWPOWERMODE:{
			if(isGameMode >0 ){ Process_SetHoldOn(IMU_process,1);
				state = STATE_ENTER_GAMEMODE;
				hal_attitude_setMode(Att_GameMode);
				hal_mag_SetState(Mag_All_l,Mag_Mode_100Hz);
				Process_UpdatePeroid(IMU_process,200);
				break;
			}
		  #if CALIBRATION_ENANBLE
		  if(!app_imucal_getstate())IMU_GetSensorDataLowPower();
		  #else
		  if(!hal_Realstep_GetState())IMU_GetSensorDataLowPower();
		  #endif
			break;}
		case STATE_ENTER_GAMEMODE:{
			if(1 == hal_attitude_GetMode(Att_GameMode)){
				if(Mag_Mode_100Hz == hal_mag_GetState(Mag_Front) && Mag_Mode_100Hz == hal_mag_GetState(Mag_Back)){
					sensorEnable = 1;
					n = 0;
					Process_SetHoldOn(IMU_process,1);
					state = STATE_GAMEMODE;
					Process_UpdatePeroid(IMU_process,0);
					#if LOSSPACK_ENANBLE
					if(mFlash.isHost)Process_Start(10,"LossPack_process",LossPack_process);
					#endif
					Process_Start(10,"ReadRSSI",hal_imu_readrssi_process);
					SEGGER_RTT_printf(0,"into game mode\n");
				}
				else {
					if(n++ >= 20){
						SEGGER_RTT_printf(0,"ERR_NUM_IMU_QMC6310\n");
						isGameMode =0;
						app_err_Set(ERR_NUM_IMU_QMC6310,1);
						Process_SetHoldOn(IMU_process,0);
						state = STATE_lOWPOWERMODE;
						Process_UpdatePeroid(IMU_process,0);
					}
				}
			}
			else {
//				SEGGER_RTT_printf(0,"ERR_NUM_IMU_BMI160\n");
				if(n++ >= 20){
					app_err_Set(ERR_NUM_IMU_BMI160,1);
					Process_SetHoldOn(IMU_process,0);
					state = STATE_lOWPOWERMODE;
					Process_UpdatePeroid(IMU_process,0);
				}
			}
			break;}
		case STATE_GAMEMODE:{
			#if !GAME_ENANBLE
			if(isGameMode ==0 ){
				sensorEnable = 0;
				n =0;
				state = STATE_EXIT_PRE;
				hal_attitude_setMode(Att_Lowpower);
				hal_mag_SetState(Mag_Front,Mag_Mode_10Hz);
				hal_mag_SetState(Mag_Back,Mag_Mode_Suspend);
				Process_UpdatePeroid(IMU_process,500);
				#if LOSSPACK_ENANBLE
				if(mFlash.isHost)Process_Stop(LossPack_process);
				#endif
				#if LASER_ENABLE
				hal_laser_Enable(0);
				#endif
				break;
			} 
		  #endif
			if(TIME_GetTicks()-tim>=500){tim = TIME_GetTicks();
				  if( 0 == slave_isconnect())advertising_start();
				  if(mFlash.isHost && 0 == host_isconnect())scan_start();
			}			
			IMU_Dtalige();
			
			break;}
		case STATE_EXIT_PRE:{
				if(hal_attitude_GetMode(Att_Lowpower)){
					state = STATE_EXIT_GAMEMODE;
					Process_UpdatePeroid(IMU_process,500);
					n = 0;
				}
				else if(++n >10){ n = 0;
					Process_SetHoldOn(IMU_process,0);
					app_err_Set(ERR_NUM_IMU_BMI160,1);
					state = STATE_EXIT_GAMEMODE;
					Process_UpdatePeroid(IMU_process,500);
				}
			break;}
		case STATE_EXIT_GAMEMODE:{
				 if(Mag_Mode_Suspend == hal_mag_GetState(Mag_Back) && Mag_Mode_10Hz == hal_mag_GetState(Mag_Front)){
					 n = 0;
					 Process_SetHoldOn(IMU_process,0);
					 Process_UpdatePeroid(IMU_process,0);
					 state = STATE_lOWPOWERMODE;
					 Process_Stop(hal_imu_readrssi_process);
					 SEGGER_RTT_printf(0,"out game mode\n");
				 }else if(++n>10){ n=0;
					 app_err_Set(ERR_NUM_IMU_QMC5883L,1);
					 state = STATE_lOWPOWERMODE;
					 Process_UpdatePeroid(IMU_process,0);
					 Process_SetHoldOn(IMU_process,0);
					 Process_Stop(hal_imu_readrssi_process);
				 }
			break;}
		default:state = 0;break;
	}
}

void IMU_CancelTimerCounter(void)
{
	TIME_UnRegist(IMU_TimerCounter);
}



void IMU_Initialize(void)
{
  hal_step_init();
	hal_Realstep_Init();
	hal_attitude_init();
	hal_mag_init();
	#if LASER_ENABLE
	hal_laser_init();
	#endif
	Process_Start(10,"IMU_process",IMU_process);
  TIME_Regist(IMU_TimerCounter);
	
}