#include "app_math.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_imu.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;
	int16_t 				front_index = 0;
	
		
	//游戏模式
	if(IMU_GetSensorEnable() > 0 && IMU_GetGameMode()>0 && IMU_GetCurrentMode() == STATE_FULL_POWER_MODE){
		
//			CHECK_TIMECONSUMING_START;
		
		rssi = IMU_Get_Rssi();
		group_num = IMU_Get_Front_Update_Data_GroupNum();
		group_num = group_num >= IMU_BUFF_SIZE?IMU_BUFF_SIZE:group_num;
		front_index = IMU_Get_Front_Full_Power_Data((int16_t*)gry_front, (int16_t*)acc_front, (int16_t*)mag6310_front, timestamp_front, group_num);
		
		IMU_Get_Back_Mag(mag6310_back);
		
		if(mFlash.isHost){
				process_imu_data_front(front_index);
			}else if(Slave_Get7_5ms_interval()){
				process_imu_data_front(front_index);
			}
			
//			CHECK_TIMECONSUMING_END;
	}else{
	 //将状态重设为0
	 IMU_STATUS = 0;
	}
			 
	
	//实时计步模式
	if(IMU_GetSensorEnable() > 0 && IMU_GetRealTimeStepMode()>0 && IMU_GetCurrentMode() == STATE_FULL_POWER_MODE){
		rssi = 0-host_get_rssi();
		 
		group_num = IMU_Get_Front_Update_Data_GroupNum();
		group_num = group_num >= IMU_BUFF_SIZE?IMU_BUFF_SIZE:group_num;
		front_index = IMU_Get_Front_Full_Power_Data((int16_t*)gry_front, (int16_t*)acc_front, (int16_t*)mag6310_front, timestamp_front, group_num);
		 
		IMU_Get_Back_Mag(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 group_num = 0;
	int16_t front_index = 0;
	 
	if(IMU_GetCurrentMode() == STATE_LOW_POWER_MODE){
		group_num = IMU_Get_Front_Update_Data_GroupNum();
		group_num = group_num >= IMU_BUFF_SIZE?IMU_BUFF_SIZE:group_num;
		front_index = IMU_Get_Front_Low_Power_Data((int16_t*)acc_front, (int16_t*)mag6310_front, timestamp_front, group_num);
		
		for(int i=0; i < front_index; i++)
		{
			//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]);
			if(1 == detect_step_by_mag(mag6310_front[i])){
					mFlash.mStep.stepCur[0]++;
				  SEGGER_RTT_printf(0,">>>>>>isLoader:\r\n");
				  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);
}