#include "app_step.h"
#include "nrf_gpio.h"
#include "usr_config.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_mt.h"
#include "app_host.h"
#include "app_charge.h"
#include "hal_ble_client.h"
#include "hal_ble_host.h"
#include "nrf_delay.h"
#include "app_flash.h"
#include "hal_ser_imu_mode_manage.h"
#include "ble_comm.h"
#include "exception.h"

static uint8_t  realStepMode = BLE_REALTIMESTEP_EXIT;       //是否为实时计步模式
static uint32_t realStepCur_L = 0;      //左鞋步数记录点
static uint32_t realStepAdd_R = 0;      //右鞋步数
static uint8_t  realStepTimCnt = 0;     //右鞋实时计步的心跳
static uint16_t  realStepHeartCnt = 0;   //实时计步模式心跳计时
static uint8_t  RealStepFlag =BLE_REALTIMESTEP_EXIT;        //实时计步的信号。

/********************** 函数声明区 *************************/
uint32_t app_step_GetStep_L(void)
{
	return mFlash.mStep.stepCur[0];
}

uint32_t app_step_GetStep_R(void)
{ 
	return mFlash.mStep.stepCur[1];
}

//右鞋实时计步发送
static void app_step_RealSendProcess(void)
{
	if(realStepMode == BLE_REALTIMESTEP_ENTER){
		uint8_t buf[8]={0};
		uint8_t L = 0;
		buf[L++] = BLE_REALTIMESTEP_ENTER;
		buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>24);
		buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>16);
		buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>8);
		buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>0);
		buf[L++] = (uint8_t)(realStepAdd_R>>24);
		buf[L++] = (uint8_t)(realStepAdd_R>>16);
		buf[L++] = (uint8_t)(realStepAdd_R>>8);
		buf[L++] = (uint8_t)(realStepAdd_R>>0);
//		DEBUG_LOG("mFlash.mStep.stepCur[0]:%d,realStepCur_L:%d,realStepAdd_R:%d\n",mFlash.mStep.stepCur[0],realStepCur_L,realStepAdd_R);
		BLE_Client_Tx_Send(0,BLE_REALTIMESTEP,buf,L);
	}
}

//右鞋实时计步连接管理
void app_step_RealConnectProcess(void)
{
	 static uint8_t state =0;
	 static uint32_t tim =0;
	 uint8_t mod =0;
	
	 switch(state){
		 case 0://空闲
			     if(RealStepFlag == BLE_REALTIMESTEP_ENTER){
						 state =1;
						 tim = TIME_GetTicks();
						 hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
					 }else{
						 if(realStepTimCnt>0){ realStepTimCnt = 0;
						    BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&RealStepFlag,1);
				     }
					 }
			     break;
		 case 1://配置IMU
			     if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) == 0 ){
						 state =2;
						 realStepCur_L = mFlash.mStep.stepCur[0]; //重新计数
		         realStepAdd_R = 0;
		         realStepHeartCnt = 0;
						 realStepMode = BLE_REALTIMESTEP_ENTER;
					 }
					 else{
						 if(TIME_GetTicks()-tim >= 10000){tim = TIME_GetTicks();
							  hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
							  state =0;
							  RealStepFlag = BLE_REALTIMESTEP_EXIT;
							  realStepMode = BLE_REALTIMESTEP_EXIT;
						 }
						 else hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
					 }
			     break;
		 case 2://进入实时计步
			     realStepHeartCnt++;
					 if(realStepHeartCnt >= 600){realStepHeartCnt=0;
							hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
							mod = BLE_REALTIMESTEP_EXIT;
							BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&mod,1); //退出从机实时步数
						  state =3;
						  tim = TIME_GetTicks();
					 }else{
							if(realStepTimCnt > 0) realStepTimCnt--;
							if(realStepTimCnt==0) {
								mod = BLE_REALTIMESTEP_ENTER;
								BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&mod,1);
							}
					 }
					 if(RealStepFlag == BLE_REALTIMESTEP_EXIT){
							 state =3;
						   realStepMode = BLE_REALTIMESTEP_EXIT;
							 hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
					 }
			     break;
		 case 3://退出实时计步模式
			     if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) != 0 || TIME_GetTicks()-tim >= 10000){
						  state =0;
						  realStepMode = BLE_REALTIMESTEP_EXIT;
					 }
					 else hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
			     break;
		 default:RealStepFlag = BLE_REALTIMESTEP_EXIT;realStepMode = BLE_REALTIMESTEP_EXIT;state =0;break;
	 }
}

//接收右鞋实时步数并发送到手机
void cb_BLE_Host_R_REALTIMESTEP(void* handle)
{
	BLE_Client_Rx_t* target = handle;
	if(target->pDat[0]==BLE_REALTIMESTEP_ENTER){
		  if(realStepMode == BLE_REALTIMESTEP_ENTER){
				realStepAdd_R = ((uint32_t)target->pDat[1]<<24) | (uint32_t)(target->pDat[2]<<16) | (uint32_t)(target->pDat[3]<<8) | ((uint32_t)target->pDat[4]<<0);
				app_step_RealSendProcess();
//		    DEBUG_LOG(">>>>>>realStepCur_L:%ld,realStepAdd_R:%ld,\n",realStepCur_L,realStepAdd_R);
			}
			realStepTimCnt = 3;
	}
}

//接收手机命令
void cb_BLE_Client_R_REALTIMESTEP(void* handle)
{
	BLE_Client_Rx_t* target = handle;
	RealStepFlag = target->pDat[0];

	if(RealStepFlag == BLE_REALTIMESTEP_ENTER){
		realStepHeartCnt = 0;
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
	}
	else{
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_REALSTEP, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
	}
	
	BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&RealStepFlag,1);
}

void app_step_Init(void)
{
	if(0 == mFlash.isHost){
		Process_Start(100,"step_RealSend",app_step_RealSendProcess);
	}
	BLE_Client_Rx_Regist(BLE_REALTIMESTEP,cb_BLE_Client_R_REALTIMESTEP);
	Process_Start(100,"step_RealConnect",app_step_RealConnectProcess);
	
	
  BLE_Host_Rx_Regist(BLE_REALTIMESTEP,cb_BLE_Host_R_REALTIMESTEP);
}