#include "system.h"
#include "bsp_time.h"
#include "nrf_delay.h"
#include "hal_imu.h"
#include "hal_ble_client.h"
#include "hal_ble_host.h"

//系统函数
void FPS_process(void)
{
	#if DEBUG_FPS
	static uint32_t tem1 = 0;
	static uint32_t tem2 = 0;
	static uint32_t fps_max=0;
	static uint32_t cnt_max=0;
	uint32_t fps;
	
	tem2 = NRF_RTC0->COUNTER;
	if(tem2<tem1) tem2 += 16777216;
	fps = (tem2-tem1)/32.768;
	if(fps_max<fps){
		fps_max = fps;
		cnt_max = tem2-tem1;
	}

	static uint32_t tim=0;
	if(TIME_GetTicks()-tim>=1000){ tim = TIME_GetTicks();
		SEGGER_RTT_printf(0,"fps=%dms,cnt=%d\n",fps_max,cnt_max);
		fps_max = 0;
	}
	tem1 = NRF_RTC0->COUNTER;
	#endif
}

uint16_t lose_pack_all =0;
#if LOSSPACK_ENANBLE
void LossPack_process(void){
		static uint16_t pack_l =0;
		pack_l = lose_pack_all;
//		SEGGER_RTT_printf(0,">>>>>loss pack,%d:%d\n",pack_l,rssi);
		uint8_t buf[8];
		uint8_t L = 0;
		buf[L++] = (uint8_t)(pack_l>>8);
		buf[L++] = (uint8_t)(pack_l);
		buf[L++] = (uint8_t)(IMU_GetHostRssi());
		BLE_Client_Tx_Send(0,0x03,buf,L);
	
	  static uint8_t temp =0;
	  temp++;
		if(temp >= 100){
			lose_pack_all =0;
			temp =0;
		}
}
#endif

//休眠时间 ms
uint32_t systermSleepTime = 1000;
static uint8_t sleep_num = 0;
static Sleep_cb sleep_cb[sleep_cb_max]={0};
int Sleep_Regist(Sleep_cb cb)
{
	for(int i=0;i<sleep_cb_max;i++) {
		if(sleep_cb[i]==cb) return -1;
		if(sleep_cb[i]==0){
			sleep_num++;
			sleep_cb[i] = cb;  //回调函数
			return 0;
		}
	}
	SEGGER_RTT_printf(0,"Sleep_Regist too many!\n");
	return -2;
}

void Sleep_Event(void)
{
	for(int i=0;i<sleep_num;i++) { //SEGGER_RTT_printf(0,"time_cb[%d]=%d\n",i,time_cb[i]);
		if(sleep_cb[i]){
			sleep_cb[i](systermSleepTime);  //回调函数
		}
	}
}

static uint8_t wakeup_num = 0;
static Sleep_cb wakeup_cb[wakeup_cb_max]={0};
int Wakeup_Regist(Sleep_cb cb)
{
	for(int i=0;i<wakeup_cb_max;i++) {
		if(wakeup_cb[i]==cb) return -1;
		if(wakeup_cb[i]==0){
			wakeup_num++;
			wakeup_cb[i] = cb;  //回调函数
			return 0;
		}
	}
//	SEGGER_RTT_printf(0,"Wakeup_Regist too many!\n");
	return -2;
}

void Wakeup_Event(void)
{
	for(int i=0;i<wakeup_num;i++) { //SEGGER_RTT_printf(0,"time_cb[%d]=%d\n",i,time_cb[i]);
		if(wakeup_cb[i]){
			wakeup_cb[i](systermSleepTime);  //回调函数
		}
	}
}

////后台进程管理
//#define process_cb_max 255
//static uint8_t process_num = 0;
//static PROCESS_cb process_cb[process_cb_max]={0};
//int Process_Regist(PROCESS_cb cb)
//{
//	for(int i=0;i<process_cb_max;i++) {
//		if(process_cb[i]==cb) return -1;
//		if(process_cb[i]==0){
//			process_num++;
//			process_cb[i] = cb;  //回调函数
//			return 0;
//		}
//	}
//	SEGGER_RTT_printf(0,"Process_Regist too many!\n");
//	return -2;
//}
//void Process_Background(void)
//{
//	for(int i=0;i<process_num;i++) { //SEGGER_RTT_printf(0,"time_cb[%d]=%d\n",i,time_cb[i]);
//		if(process_cb[i]){
//			process_cb[i]();  //回调函数
//		}
//	}
//}

//app进程管理

static uint16_t process_dex = 0;
static PROCESS_t mProcess[process_max];

static PROCESS_t* head_handle = 0;

int Process_Start(uint32_t peroid,const char *name,PROCESS_cb cb)
{
	for(int i=0;i<process_dex;i++){ //检查是否存在过
		if(mProcess[i].cb==cb){
			mProcess[i].next = head_handle;
			mProcess[i].runflag =1;
			head_handle = &mProcess[i];
			SEGGER_RTT_printf(0,"process was existed(%d)\n",cb);
			return 0;
		}
	}
	
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb) return -1;
		target = target->next ;
	}
	
	if(process_dex>=process_max) return -1;
	mProcess[process_dex].cb = cb;
	mProcess[process_dex].runflag =1;
	mProcess[process_dex].next = head_handle;
	mProcess[process_dex].Peroid = peroid;
	mProcess[process_dex].tim = TIME_GetTicks();
	#if ProcessTime_EN
	sprintf(mProcess[process_dex].name_t,"%s",name);
	#endif
	head_handle = &mProcess[process_dex];
	process_dex++;
//	SEGGER_RTT_printf(0,"process num(%d)\n",process_dex);
	return 0;
}

void Process_Stop(PROCESS_cb cb)
{
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb){	//存在
			target->Peroid = 0;
			target->runflag = 0;
			return;
		}
		target = target->next ;
	}
}

void Process_SetHoldOn(PROCESS_cb cb,uint8_t holdon)
{
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb){	//存在
			if(target->holdon!=holdon) target->holdon = holdon;
			return;
		}
		target = target->next ;
	}
}

void Process_UpdatePeroid(PROCESS_cb cb,uint16_t Peroid)
{
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb){	//存在
			target->Peroid = Peroid;
			target->tim = TIME_GetTicks();
			return;
		}
		target = target->next ;
	}
}

uint16_t Process_GetPeroid(PROCESS_cb cb)
{
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb){	//存在
			return target->Peroid;
		}
		target = target->next ;
	}
	return 0;
}
void Process_SetError(PROCESS_cb cb,uint8_t error)
{
	PROCESS_t* target = head_handle;
	while(target){					//检查是否已经存在
		if(target->cb==cb){	//存在
			target->error = error;
		}
		target = target->next ;
	}
}

int Process_App(void)
{
	PROCESS_t* target;
	int ret = 0;
	for(target=head_handle;target;target=target->next){
		if(target->cb && target->runflag) {
			#if ProcessTime_EN
			target->cnt1_rtc = NRF_RTC0->COUNTER;
			#endif
			if(0 == target-> Peroid)target->cb();
			else if(TIME_GetTicks()-target->tim >= target-> Peroid ){
				target->tim = TIME_GetTicks();
				target->cb();
			}
			#if ProcessTime_EN
			target->cnt2_rtc = NRF_RTC0->COUNTER;
			#endif
		}
		if(target->holdon) ret = 1;	//不能进入低功耗
	}
	
	#if ProcessTime_EN
	char Display[100]={0};
	static uint32_t tim =0;
	if(TIME_GetTicks()-tim>=DisInterval){ tim = TIME_GetTicks();
		for(target=head_handle;target;target=target->next){
			if(target->cb && target->cnt2_rtc >= (target->cnt1_rtc +2)) {
				sprintf(Display,"%s,  time:%02f us\n",target->name_t,(target->cnt2_rtc-target->cnt1_rtc)/32.768 * 1000);
				SEGGER_RTT_printf(0,"%s",Display);
				memset(Display,0,sizeof(Display));
			}
		}
  }
	#endif
	return ret;
}

//大循环
void USR_Process(void)
{
	//app进程调度
//  static uint32_t time_t =0;
	if(Process_App()==0){	//进入低功耗模式
//		SEGGER_RTT_printf(0,"Sleep(%d)...\n",TIME_GetTicks()-time_t);
//		SEGGER_RTT_printf(0,"process_dex:%d,sleep_cb_max:%d,wakeup_cb_max:%d...\n",process_dex,sleep_num,wakeup_num);
		Sleep_Event();
//		nrf_delay_ms(systermSleepTime);	//模拟低功耗
		systermSleepTime = rtc_sleep();
		if(systermSleepTime==0){ 
			systermSleepTime = 13;
			nrf_delay_ms(systermSleepTime);
		}
		//time_t = TIME_GetTicks();
		Wakeup_Event();
		//SEGGER_RTT_printf(0,"Wakeup(%d)...\n",systermSleepTime);
	}
	
	FPS_process();
}