/********************** Í·Îļþ *************************/
#include "usr.h"
#include "hal_Realstep.h"
#include "system.h"
#include "bsp_time.h"
#include "hal_ble_client.h"
#include "hal_flash.h"
#include "hal_mag.h"
#include "hal_attitude.h"

static uint8_t Realstep_Enable = 0;
static uint8_t ReqRealstep_Enable = 0;
static uint8_t Realstep_temp =0;

enum{
	Realstep_init,
	Realstep_Check_r,
	Realstep_run,
	Realstep_quiet
};

void hal_Realstep_Enable(uint8_t cmd)
{
	ReqRealstep_Enable = cmd;
}

uint8_t hal_Realstep_GetState(void)
{
	return Realstep_Enable;
}

static void hal_Realstep_process(void)
{
	static uint8_t state =Realstep_init;
	
	switch(state){
		 case Realstep_init:
		      if(ReqRealstep_Enable ==1){
				     Process_SetHoldOn(hal_Realstep_process,1);
						 hal_attitude_setMode(Att_GameMode);
						 hal_mag_SetState(Mag_All_l,Mag_Mode_10Hz);
						 state = Realstep_Check_r;
						 Realstep_temp =0;
			    }
		      break;
	    case Realstep_Check_r:
				  if(1 == hal_attitude_GetMode(Att_GameMode) && Mag_Mode_10Hz == hal_mag_GetState(Mag_Front) && Mag_Mode_10Hz == hal_mag_GetState(Mag_Back)){
						 state = Realstep_run;
						 SEGGER_RTT_printf(0,"Realstep_Check_r\n");
						 Realstep_Enable =1;
					}
					else if(Realstep_temp++ >20){
						 SEGGER_RTT_printf(0,"Realstep_Check into fail\n");
						 state = Realstep_init;
						 ReqRealstep_Enable =0;
						 if(0 == hal_attitude_GetMode(Att_GameMode))app_err_Set(ERR_NUM_IMU_BMI160,1);
						 else app_err_Set(ERR_NUM_IMU_QMC6310,1);
					}
		      break;
		  case Realstep_run:
			    if(ReqRealstep_Enable == 0){
				     Process_SetHoldOn(hal_Realstep_process,0);
						 hal_attitude_setMode(Att_Lowpower);
						 hal_mag_SetState(Mag_Front,Mag_Mode_10Hz);
				     hal_mag_SetState(Mag_Back,Mag_Mode_Suspend);
						 state = Realstep_quiet;
			    }
		      break;
		  case Realstep_quiet:
			    if(hal_attitude_GetMode(Att_Lowpower) && Mag_Mode_Suspend == hal_mag_GetState(Mag_Back) && Mag_Mode_10Hz == hal_mag_GetState(Mag_Front)){
						Realstep_Enable = 0; 
						state = Realstep_init;
						SEGGER_RTT_printf(0,"Realstep out ok\n");
					}
					else if(Realstep_temp++ >40){
						Realstep_Enable = 0;
						SEGGER_RTT_printf(0,"Realstep out fail\n");
					  state = Realstep_init;
						if(0 == hal_attitude_GetMode(Att_GameMode))app_err_Set(ERR_NUM_IMU_BMI160,1);
						else app_err_Set(ERR_NUM_IMU_QMC6310,1);
					}
			    break;
			default:
					break;
	}
}

void hal_Realstep_Init(void)
{
	Process_Start(100,"hal_Realstep_process",hal_Realstep_process);
}