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

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

static int16_t	mag6310_back[3];
static int32_t	timestamp_front[IMU_BUFF_SIZE];
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;
	}
	
	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;
		
		last_timestamp = timestamp_front[i];
		
//		SEGGER_RTT_printf(0,"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);
	}
}

void app_math_TimerCounter(void)
{
	int16_t 				group_num = 0;
	
	//游戏模式
	if(hal_mode_get() == HAL_MODE_GAME){
		
//			CHECK_TIMECONSUMING_START;
		
		rssi = 0-host_get_rssi();
		group_num = IMU_Get_Front_Data_Num();
		for(int i=0;i<group_num;i++){
			IMU_Get_Front_Data(i, gry_front[i], acc_front[i], mag6310_front[i], &timestamp_front[i]);
			SEGGER_RTT_printf(0,"imu_front index:%d------------------------>\n",i);
			SEGGER_RTT_printf(0,"f_ax=%d\r,f_ay=%d\r,f_az=%d\r\n",acc_front[i][0],acc_front[i][1],acc_front[i][2]);
			SEGGER_RTT_printf(0,"f_gx=%d\r,f_gy=%d\r,f_gz=%d\r\n",gry_front[i][0],gry_front[i][1],gry_front[i][2]);
			SEGGER_RTT_printf(0,"f_mx=%d\r,f_my=%d\r,f_mz=%d\r\n",mag6310_front[i][0],mag6310_front[i][1],mag6310_front[i][2]);
			SEGGER_RTT_printf(0,"timestamp=%d\r\n",timestamp_front[i]);
//			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);
		}
		
		IMU_Get_Back_Data(mag6310_back);
		
		SEGGER_RTT_printf(0,"b_mx=%d\r,b_my=%d\r,b_mz=%d\r\n",mag6310_back[0],mag6310_back[1],mag6310_back[2]);
		
		if(mFlash.isHost){
				process_imu_data_front(group_num);
			}else if(Slave_Get7_5ms_interval()){
				process_imu_data_front(group_num);
			}
			
//			CHECK_TIMECONSUMING_END;
	}else{
	 //将状态重设为0
	 IMU_STATUS = 0;
	}
			 
	
	//实时计步模式
	if(hal_mode_get() == HAL_MODE_REALSTEP){
		rssi = 0-host_get_rssi();
		group_num = IMU_Get_Front_Data_Num();
		for(int i=0;i<group_num;i++)IMU_Get_Front_Data(i, gry_front[i], acc_front[i], mag6310_front[i], &timestamp_front[i]);
		IMU_Get_Back_Data(mag6310_back);
		 
		if(app_switchimu_GetGameModeLsm() == USED_FRONT_LSM){
			SEGGER_RTT_printf(0,"app_switchimu_GetGameModeLsm() == USED_FRONT_LSM\r\n");
			//start_cal_step((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc);
		}else if(app_switchimu_GetGameModeLsm() == USED_CENTER_LSM){
			SEGGER_RTT_printf(0,"app_switchimu_GetGameModeLsm() == USED_CENTER_LSM\r\n");
			//start_cal_step((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc);
		}			 
	}
}

static void app_math_DailyStep_Process(void)
{
	int16_t 	acc[3];
	int16_t		mag6310[3];
	 
	if(hal_mode_get() == HAL_MODE_NORMAL){

		for(int i=0; i < IMU_Get_Front_Data_Num(); i++)
		{
			IMU_Get_Front_Data(i, NULL, acc, mag6310, NULL);
			//SEGGER_RTT_printf(0,"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]++;
				  SEGGER_RTT_printf(0,">>>>>>isLoader:%d\r\n",mFlash.mStep.stepCur[0]);
				  break;
			}
		}
	}
}

//static void hal_stepSaveTest_process(void){
//	mFlash.mStep.stepCur[0]++;
////	Flash_SaveStep();
//	SEGGER_RTT_printf(0,"hal step Save one minute:step %d\r\n",mFlash.mStep.stepCur[0]);
//}

void app_math_Init(void)
{
	Process_Start(100,"app_math_DailyStep_Process",app_math_DailyStep_Process);
	Process_Start(10,"app_math_TimerCounter",app_math_TimerCounter);
//	if(mFlash.isHost)Process_Start(10000,"hal_stepSaveTest",hal_stepSaveTest_process);
}