#include "app_game.h"
#include "usr_config.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_mt.h"
#include "hal_battery.h"
#include "app_charge.h"
#include "hal_ble_client.h"
#include "hal_ble_host.h"
#include "nrf_delay.h"
#include "app_flash.h"
#include "ble_comm.h"
#include "exception.h"
#include "bll_imu.h"
#include "hal_led.h"
#include "pdrStatus.h"

/********************** 函数声明区 *************************/

//游戏模式
static const bll_imu_one_way_param_t game_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_ON,											//前脚 - 时间戳开启
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_416HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_416HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_416HZ,	
};

static const bll_imu_one_way_param_t game_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,											  //后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_200HZ,										//后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
};

static const bll_imu_param_t game_bll_imu_param_t={
	.config_param[FML_IMU_DIR_FRONT] = &game_front_param,
	.config_param[FML_IMU_DIR_BACK] =  &game_back_param,
};

typedef struct game_manager{
 uint8_t clientCnt;       //右鞋游戏模式的标志位
 uint8_t right_discnt;    //右鞋断开连接计数器
 uint8_t client_discnt;   //主机连接断开计数器
 uint8_t Signal;          //收到主机的游戏指令
 uint8_t HeartCnt;        //游戏模式心跳维护,左鞋
 uint8_t GameMode;        //是否在游戏
}game_manager_t;

static game_manager_t game_manager ={0};

#define IMU_MAX_GROUP_NUM 10

static int16_t 	acc_front[IMU_MAX_GROUP_NUM][3];
static int16_t  gry_front[IMU_MAX_GROUP_NUM][3];
static int16_t	mag6310_front[IMU_MAX_GROUP_NUM][3];

static int16_t	mag6310_back[3];
static int32_t	timestamp_front[IMU_MAX_GROUP_NUM];
static uint8_t  rssi;

static uint8_t  IMU_STATUS; //记录状态用来重新记录时间戳

static uint32_t  timestamp;

static int32_t  last_timestamp;

static void process_imu_data_front(int front_index)
{
	if(IMU_STATUS != 1){
		IMU_STATUS = 1;
		last_timestamp = timestamp_front[0];
		timestamp = 0;
	}
	
	if(front_index >IMU_MAX_GROUP_NUM)return;
	for(int i = 0; i < front_index; i++)
	{
		int32_t dt = timestamp_front[i] - last_timestamp;
	
		last_timestamp = timestamp_front[i];
		
		DEBUG_LOG("dt : %d\n", dt);
		
		if(dt == 0)
		{
			DEBUG_LOG("as dt == 0, return\n", dt);
			
			continue;
		}
		
		if(dt < 0)
		{
			dt = 2425;
		}
		
		timestamp += dt;
		
		DEBUG_LOG("timestamp : %ud\n", timestamp);
		DEBUG_LOG("timestamp_front[i] : %d\n", timestamp_front[i]);
		
		IMU_Process_motion_queue(mFlash.isHost, timestamp, acc_front[i],
		gry_front[i],mag6310_front[i], mag6310_back, rssi);
	}
}

void app_game_SetClientGameMode(void)
{
	if(game_manager.clientCnt != 3) game_manager.clientCnt = 3;
}

uint8_t app_game_GetGameMode(void)
{
	return game_manager.GameMode;
}

static void cb_BLE_Client_R_GAMEMODE(void* handle)
{
	BLE_Client_Rx_t* target = handle;
	game_manager.Signal = target->pDat[0];
	if(game_manager.Signal){
		game_manager.HeartCnt =0;
		bll_imu_Resume_config_param(&game_bll_imu_param_t);
		//是否上传原始数据
		if(1 == game_manager.Signal)IMU_Dtalige_Rowdata_OFF();
		else if(2 == game_manager.Signal)IMU_Dtalige_Rowdata_ON();
	}else{
		bll_imu_Resume_unregister_config_param(&game_bll_imu_param_t);
	}
	
	BLE_Host_Tx_Send(0,BLE_GAMEMODE,&game_manager.Signal,1);
	DEBUG_LOG(">>>>>>>>>>cb_BLE_Client_R_GAMEMODE:%d\r\n",game_manager.Signal);
}

//通知右鞋进入或者退出游戏模式
static void app_game_notify_host(uint8_t mode){
	uint8_t clientMode = mode;
	BLE_Host_Tx_Send(0,BLE_GAMEMODE,&clientMode,1);
}

//测试使用，游戏模式下 1S亮绿灯20ms.
static void app_game_led(uint8_t gamemode){
	static uint8_t led_state =0;
	static uint8_t temp =0;
	if(gamemode){
		 temp++;
		 if(temp == 100){
			  if(led_state){led_state =0;
					LED_Start(LED_RUN,COLOR_GREEN);
			  }
			  else {led_state =1;
					LED_Start(LED_RUN,COLOR_BLACK);
			  }
	   }
		 if(temp > 102){temp =0;
			 LED_Stop(LED_RUN);
		 }
	}else{
		 if(2 == led_state){
		    LED_Stop(LED_RUN);
		 }
	}
}

//IMU数据回调
static void gamemode_data_notify_cb(uint32_t dir_bit)
{
	int16_t 				group_num = 0;
	bll_imu_data_t data={0};
	
	if(0 == app_game_GetGameMode())return;
	
	if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01){
		rssi = 0-host_get_rssi();
		group_num = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
		if(group_num >IMU_MAX_GROUP_NUM)return;
		for(int i=0;i<group_num;i++){
			bll_imu_get_data(BLL_IMU_DIR_FRONT, i, &data);
			gry_front[i][0] = data.gry[0];gry_front[i][1] = data.gry[1];gry_front[i][2] = data.gry[2];
			acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
			mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
			timestamp_front[i] = data.fifo_timestamp;
		}
		
		if(bll_imu_get_data_num(BLL_IMU_DIR_BACK) >= 1){
			bll_imu_get_data(BLL_IMU_DIR_BACK, 0, &data);
			mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2];
		}
		
		if(mFlash.isHost){
				process_imu_data_front(group_num);
			}else if(Slave_Get7_5ms_interval()){
				process_imu_data_front(group_num);
			}
			app_game_led(1);
	}
	
}

static void app_game_Process(void)
{
	static uint8_t configcnt = 0;
  static uint8_t state = 0;
	uint8_t front_CS =0,back_CS =0;
	if(game_manager.clientCnt>0) game_manager.clientCnt--;
	switch(state){
		case 0:{
						if(game_manager.Signal){
								front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t);
								back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t);
								DEBUG_LOG("front_CS:%d back_CS:%d\r\n",front_CS,back_CS);
								if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){
									state =1;
									game_manager.GameMode =1;
								}
								else  if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
									 bll_imu_Resume_config_param(&game_bll_imu_param_t);
									 if(++configcnt >= 20){configcnt =0;
											game_manager.Signal =0;
											bll_imu_Resume_unregister_config_param(&game_bll_imu_param_t);
											Except_TxError(EXCEPT_GAME,"shoes into game mode fail");
									 }
								}
								if(0 != game_manager.HeartCnt)game_manager.HeartCnt =0;
					  }
						if(game_manager.clientCnt >0 && mFlash.isHost) app_game_notify_host(0);
						break;
					}
		case 1:{
					 if(mFlash.isHost){//管理右鞋状态
							 if(host_isconnect()){
								 if(0 == game_manager.clientCnt){
										app_game_notify_host(1);
										if(++configcnt >= 10){configcnt =0;
											DEBUG_LOG("ERR_NUM_GAME app_game_Process\r\n");
											Except_TxError(EXCEPT_GAME,"no get right shoes data");
										}
								 }
								 else if(configcnt>0) configcnt = 0;
								 if(0 != game_manager.right_discnt)game_manager.right_discnt =0;
							 }
							 else game_manager.right_discnt++;
						}
						
						if(0 == slave_isconnect())game_manager.client_discnt++;
						else if(0 != game_manager.client_discnt)game_manager.client_discnt =0;
							
						if(0 == game_manager.Signal  ||      //收到退出游戏的指令
							game_manager.client_discnt > 5 ||  //与主机断开5秒后，退出游戏模式
							game_manager.right_discnt  > 10    //与右鞋断开10秒后，退出游戏模式
						){
								if(game_manager.right_discnt > 10)Except_TxError(EXCEPT_GAME,"In game mode,right shoes disconnted long time");
								if(game_manager.client_discnt > 5)Except_TxError(EXCEPT_GAME,"In game mode,client disconnted long time");
								configcnt =0;
								state =0;
								game_manager.Signal = 0;
								game_manager.client_discnt =0;
								game_manager.right_discnt =0;
								game_manager.GameMode =0;
								bll_imu_Resume_unregister_config_param(&game_bll_imu_param_t);
								if(mFlash.isHost)app_game_notify_host(0);

								IMU_STATUS = 0;
								set_pdr_status();
								app_game_led(0);
							
						}
						break;
					}
		default: state =0;game_manager.Signal = 0;game_manager.client_discnt =0;break;
	  }
}

static void app_AutoOutgame_Process(void){
	uint8_t front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t);
	uint8_t back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t);
	
	if(mFlash.isHost && app_game_GetGameMode()){
		DEBUG_LOG("game_manager.GameModeHeartCnt:%d\r\n",game_manager.HeartCnt);
		if(game_manager.HeartCnt++ >= 5){
			 game_manager.Signal = 0;
			 bll_imu_Resume_unregister_config_param(&game_bll_imu_param_t);
			 BLE_Host_Tx_Send(0,BLE_GAMEMODE,&game_manager.Signal,1); 
		}
	}
}

void app_game_Init(void)
{
	Process_Start(1000,"app_game",app_game_Process);
	BLE_Client_Rx_Regist(BLE_GAMEMODE,cb_BLE_Client_R_GAMEMODE);
	Process_Start(60000,"app_AutoOutgame",app_AutoOutgame_Process);
	bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, gamemode_data_notify_cb);
}