#include "app_Calibration.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_imu.h"
#include "hal_flash.h"
#include "hal_ble_client.h"
#include "arm_math.h"
#include "drv_calibration.h"
#include "nrf_gpio.h"
#include "app_charge.h"

static int16_t Acc[3]={0};
static int16_t Gry[3]={0};
static int16_t FrontMag[3]={0};

/***********************************
 *未校准：灭灯
 *校准中：指示灯常亮
 *校准完成：指示灯慢闪，亮10ms,灭灯2S
 *校准错误：指示灯快闪，亮10ms,灭灯100ms
************************************/
static void app_calibration_CalLed_Process(void){
	
		static uint32_t tim =0;
	
		if(tim == 0){
			tim = TIME_GetTicks();
		}else if(TIME_GetTicks()-tim >= 60000){								//亮1分钟
			tim = 0;
			Process_Stop(app_calibration_CalLed_Process);				//停止该任务	
		}
		
		
		
		nrf_gpio_pin_write(PIN_LED_RUN,0); 
		nrf_delay_ms(10);
		nrf_gpio_pin_write(PIN_LED_RUN,1);
	  
	  char printdata[200]={0};
	  sprintf(printdata,"caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);
	  SEGGER_RTT_printf(0,"%s\n",printdata);
		Mahony_PRINT("caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);
		
		

}

#define USED_MAG_CHECK_BAREBOARD_VALUE_START		0
#define USED_MAG_CHECK_BAREBOARD_VALUE_END			300
void app_calibration_is_BareBoard_Process(void)
{
	if(IMU_GetCurrentMode() == STATE_IDLE_MODE)
	{
		IMU_GetMagFront(FrontMag);
		if(USED_MAG_CHECK_BAREBOARD_VALUE_START < FrontMag[0] &&  FrontMag[0] < USED_MAG_CHECK_BAREBOARD_VALUE_END){
			IMU_SetBareBoardMode(1);		//进入板子模式
		}else{
			IMU_SetBareBoardMode(0);		//不进入板子模式
		}
	}
}

void app_calibration_is_Calibration_Process(void)
{
	if(IMU_GetCurrentMode() == STATE_BAREBOARD_MODE)
	{
		
		if(IMU_GetBareBoardMode() == 1)IMU_SetBareBoardMode(0);		//进入板子模式后，将进入板子模式的标志位清零。
		
		IMU_GetAcc(Acc);
		IMU_GetGry(Gry);
		IMU_GetMagFront(FrontMag);
		
		ImuCalibration_pcs(Acc, Gry, FrontMag);
		if(ImuCalibration_GetState() == ImuCal_GetData){
				nrf_gpio_pin_write(PIN_LED_RUN,0);
				IMU_SetCalibrationMode(1);														//进入校准模式
		}else{
			if(FrontMag[0] >= USED_MAG_CHECK_BAREBOARD_VALUE_END || USED_MAG_CHECK_BAREBOARD_VALUE_START >= FrontMag[0]){
				IMU_SetLowPowerMode(1);																//进入低功耗模式。
			}
		}
	}
}

void app_calibration_Calibration_in_Process(void)
{
	if(IMU_GetCurrentMode() == STATE_CALIBRATION_MODE)
	{
		
		if(IMU_GetCalibrationMode() == 1)IMU_SetCalibrationMode(0);		//进入板子校准模式后，将进入板子校准模式的标志位清零。
		
		IMU_GetAcc(Acc);
		IMU_GetGry(Gry);
		IMU_GetMagFront(FrontMag);
		
		ImuCalibration_pcs(Acc, Gry, FrontMag);
		if(ImuCalibration_GetState() == ImuCal_finish){
			Flash_SaveBackup();
			Mahony_PRINT("ImuCal_state ImuCal_finish");
			Process_Start(2000,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
			Process_UpdatePeroid(app_calibration_CalLed_Process,2000);
			IMU_SetLowPowerMode(1);																																														//进入低功耗模式。
		}else if(ImuCalibration_GetState() == ImuCal_error || ImuCalibration_GetState() == ImuCal_quiet){
			Mahony_PRINT("ImuCal_state ImuCal_error");
			if(app_charge_Getstate() == BLE_Client_T_CHARGE_INSERT ||	app_charge_Getstate() == BLE_Client_T_CHARGE_DONE){			//若是充电，重新校准
				ImuCalibration_SetState(ImuCal_init);
				IMU_SetBareBoardMode(1);																																												//进入板子模式
			}else{
				Process_Start(100,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
				Process_UpdatePeroid(app_calibration_CalLed_Process,100);
				IMU_SetLowPowerMode(1);																																													//进入低功耗模式。
			}
		}
	}
	
}

void app_calibration_Init(void)
{
	Process_Start(1000,"app_calibration_is_BareBoard_Process",app_calibration_is_BareBoard_Process);				//是否进入板子模式
	Process_Start(200,"app_calibration_is_Calibration_Process",app_calibration_is_Calibration_Process);			//是否进入板子校准模式
	Process_Start(10,"app_calibration_Calibration_in_Process",app_calibration_Calibration_in_Process);			//校准处理中
}