#include "hal_ble_host.h"
#include "system.h"
#include "bsp_time.h"
#include "hal_flash.h"
#include "hal_imu.h"

/************************ 函数声明 ***********************************/
extern int send_bytes_server(uint8_t * bytes, uint16_t len);
void BLE_Host_Tx_Process(void);

/************************ 变量 ***********************************/

/********************** 环形缓存区 *************************/
static const int  RxLen = 1024;
static volatile unsigned char RxBuf[RxLen];
static volatile unsigned char* RxW=RxBuf;
static volatile unsigned char* RxR=RxBuf;

void BLE_Host_Push(unsigned char* p,int len)
{
	volatile unsigned char *W=RxW; //这里要与上面指针相同
	if(len<5) return;
	unsigned char L = p[1];
	unsigned char LF = ~p[2];
	if(p[0]==0xAA&&L==LF&&L==len&&p[3]<5){
		IMU_SetSlaveData(p,len);
	}else{
		for(int i=0;i<len;i++){
			W=RxW+1; if(W>=RxBuf+RxLen) W=RxBuf;        //取下一位置(到顶转到底)
			if(W!=RxR){*RxW=*(p+i); RxW=W;}
			else break;
		}
	}
}

static unsigned int CheckLen(void) //检查RX接收了多少数据
{   
	unsigned int Len; //short 
	volatile unsigned char *W=RxW; volatile unsigned char *R=RxR; 
	if(W>=R)Len=W-R;else Len=(W+RxLen)-R;         //这样正确(中途中断改变也变不了结果)
    return Len; 
}

//static unsigned char ReadByte(void)   //读RX中数锯,地指加一,和丢弃
//{	
//	unsigned char R=*RxR;       //读数
//	if(RxR!=RxW){	if(RxR+1>=(RxBuf+RxLen))RxR =RxBuf; else RxR++;}//下标
//	return R; 
//}

static unsigned char CheckByte(unsigned short n) //看RX中数锯,地指不变, 
{  	
	volatile unsigned char *R=RxR+n; 
	if(R>=(RxBuf+RxLen))R-=RxLen; 
	return *R; 
}	

static void Discard(unsigned short n) //丢弃RX数据几位  
{	
	while(n){ n--;  
		if(RxR==RxW) return; 
		if(RxR+1>=RxBuf+RxLen){RxR=RxBuf;} else RxR++; //下标 
	}	
}

//********************* 接收 **********************//
#define BLE_Host_RX_CMD_LEN 256
static BLE_Host_Rx_t mBLE_Host_Rx[BLE_NUM_OF];
static unsigned char cmdDatBuf[BLE_Host_RX_CMD_LEN]; //存放一条完整命令
static unsigned int rxNum = 0; 
int BLE_Host_Rx_Regist(BLE_CMD_n cmd,BLE_Host_Callback cb) //注册
{
	if(rxNum>=BLE_NUM_OF) return -1;
	mBLE_Host_Rx[rxNum].cb = cb;
	mBLE_Host_Rx[rxNum].cmd = cmd;
	mBLE_Host_Rx[rxNum].pDat = cmdDatBuf;
	rxNum++;
	return 0;
}

//********************* 接收协议 **********************//
//协议(1位头+ 1位长度+ 1位长度反码+ 1命令+ N数据+ 1效验) 
static void Protocol(unsigned char len) //协议处理
{
	#if DEBUG_BLE_Host
	SEGGER_RTT_printf(0,"Rx_BLE_Host(%d):",CheckByte(1)); for(int i=0;i<len;i++){SEGGER_RTT_printf(0,"%02X ",CheckByte(i));} SEGGER_RTT_printf(0,"\r\n");
	#endif
	uint8_t cmd = CheckByte(3);
	if(cmd<5){
		uint8_t buf[BLE_Host_RX_CMD_LEN];
		for(int i=0;i<len-5;i++) buf[i] = CheckByte(i+4);
		BLE_Host_Tx_Send(0,(BLE_CMD_n)cmd,buf,len-5);
	}
	if(len<5) return;
	for(int j=0;j<rxNum;j++){
		if(mBLE_Host_Rx[j].cmd==cmd&&mBLE_Host_Rx[j].cb){
			for(int i=0;i<len-5;i++) mBLE_Host_Rx[j].pDat[i] = CheckByte(i+4);
			mBLE_Host_Rx[j].datLen = len-5;
			mBLE_Host_Rx[j].cb((BLE_Host_Rx_t*)&mBLE_Host_Rx[j]);
			break;
		}
	}
}

//协议(1位头+ 1位长度+ 1位长度反码+ 1命令+ N数据+ 1效验) 
void BLE_Host_Rx_Process(void)
{	
	static unsigned char R=0; 
  static unsigned char L=0; 

	//接收缓存有数据，就全部处理
    while(1){  
        switch( R ){  
        case  0:    if( CheckLen()<3 )return; else{  
                        if(CheckByte(0)!=0xAA){ Discard(1);}
                        else{unsigned char LF=CheckByte(2);LF=~LF; L=CheckByte(1); if((LF==L)&&(L>=5)){ R++;}else { Discard(1);}}
                    }   break;  
        // 多收数据 7 = 3位头+ 1位长度负数+ 1位长度+ 2效验
        case  1:    if( CheckLen()<L) { return; }else{  //SEGGER_RTT_printf(0,"Rx:"); for(int i=0;i<L;i++){SEGGER_RTT_printf(0,"%02X ",CheckByte(i));} SEGGER_RTT_printf(0,"\r\n");
						unsigned char i,ver=0;
						for(i=0;i<L-1;i++){  ver += CheckByte(i); }
                        if(CheckByte(L-1)==ver){ Protocol(L);Discard(L);//丢弃整条命令
                        } else Discard(1); 
						R=0;
					}	break;    
        default:    R=0;break; }  
    }
}

//********************* 发送协议 **********************//
//协议(1位头+ 1位长度+ 1位长度反码+ 1命令+ N数据+ 1效验) 
void BLE_Host_Send(BLE_CMD_n cmd,unsigned char *pDat,unsigned char datLen) //协议发送
{	
	unsigned char buf[255];
	unsigned char ver=0; 
	unsigned char i,L=0,Len=datLen+5;  
	
	buf[L]=0xAA;	ver+=buf[L++];		// 头
	buf[L]=Len;		ver+=buf[L++]; 		//1位长度
	buf[L]=~Len;	ver+=buf[L++];	 	// 1位长度反码
	buf[L]=cmd; 	ver+=buf[L++];    	//1命令
	for(i=0;i<datLen; i++){ buf[L]=pDat[i];ver+=buf[L++];} //数据
	buf[L++]=ver;      //效验
	#if DEBUG_BLE_Host
	SEGGER_RTT_printf(0,"Tx_BLE_Host:"); for(int i=0;i<L;i++){SEGGER_RTT_printf(0,"%02X ",buf[i]);} SEGGER_RTT_printf(0,"\r\n");
	#endif

	send_bytes_server(buf,L);   //压入发送缓存
}

static BLE_Host_Tx_t* head_handle = 0;
void BLE_Host_Tx_Send(BLE_Host_Tx_t* handle,BLE_CMD_n cmd,unsigned char *pDat,unsigned char datLen)
{
	BLE_Host_Tx_t* target = head_handle;
	if(handle){
		handle->cmd = cmd;
		handle->pDat = pDat;
		handle->datLen = datLen;
		handle->tcnt = handle->t;
		if(handle->n>0) handle->ncnt = handle->n-1;
		else handle->ncnt = 0;
		handle->holdon = 1;
		while(target){					//检查是否已经存在
			if(target==handle){
//				SEGGER_RTT_printf(0,"handle(%d)\n",handle);
				return;
			}
			target = target->next ;
		}
//		SEGGER_RTT_printf(0,"add handle(%d)\n",handle);
		handle->next = head_handle;
		head_handle = handle;
	}
	BLE_Host_Send(cmd,pDat,datLen);
}

void BLE_Host_Tx_Clear(BLE_Host_Tx_t* handle)
{
	BLE_Host_Tx_t** curr;
	for(curr=&head_handle;*curr;){
		BLE_Host_Tx_t* entry = *curr;
		if(entry==handle){
			handle->holdon = 0;
			*curr = entry->next;
		}else{
			curr = &entry->next;
		}
	}
}

void BLE_Host_Tx_Process(void)
{
	BLE_Host_Tx_t* target;
	uint8_t holdon = 0;
	
	for(target=head_handle; target; target=target->next) {
		if(target->tcnt > 0){
			if(target->tcnt >= HeartTime_Interval)target->tcnt -= HeartTime_Interval;
			else target->tcnt =0;
			if(target->tcnt==0){
				if(target->ncnt>0){target->ncnt--;
					target->tcnt = target->t;
					BLE_Host_Send((BLE_CMD_n)target->cmd,target->pDat,target->datLen);
				}else{
					BLE_Host_Tx_Clear(target);
					if(target->cb){
						target->cb(target);
					}
				}
			}
		}
		holdon |= target->holdon;
	}
	Process_SetHoldOn(BLE_Host_Tx_Process,holdon);
}

void BLE_Host_Initialize(void)
{
	Process_Start(0,"BLE_Host_Rx",BLE_Host_Rx_Process);
	Process_Start(1,"BLE_Host_Tx",BLE_Host_Tx_Process);
}