#include "app_math.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_imu.h"
#include "math.h"
#include "ble_comm.h"
#include "hal_flash.h"
#include "detect_zero_vel.h"
#include "tool.h"
#include "app_switchimu.h"

#include "pdrStatus.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(1 == time_10ms_flag){
		
		//游戏模式
		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;
		 
		 set_pdr_status();
	 }
				 
		
		//实时计步模式
		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);
			 
			start_cal_step(mag6310_front[0], mag6310_back, acc_front[0]);
					 
		 }
	}
}

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]);
		}
		
	}
}

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);
}