#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"

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

static int16_t	mag6310_back[3];
static int32_t	timestamp_front[20];
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)
{
	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){
		
//			CHECK_TIMECONSUMING_START;
		
		rssi = 0-host_get_rssi();
		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);
			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);
			}
			
//			CHECK_TIMECONSUMING_END;
	}else{
	 //将状态重设为0
	 IMU_STATUS = 0;
	 set_pdr_status();
	}
			 
	
	//实时计步模式
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) != -1){
		rssi = 0-host_get_rssi();
		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);
			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(RealTimeStep((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc_front)){
			 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;
	 
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1){
		
		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];
			
			//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,"curren step:%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);
}