/********************************************************************************
 * File Name:			ProtocolPack.c
 * Function Describe:  协议封包
 * Explain:  
 * Writer:			ShiLiangWen
 * Date:				2019-2-9
 *******************************************************************************/
 #include "includes.h"
  #include "ProtocolPack.h"
/*
 部标协议
 |标识位|消息头|消息体|检验码|标识|
发送消息时：消息封装——>计算并填充校验码——>转义
接收消息时：转义还原——>验证校验码——>解析消息
 */


unsigned char g_ucPropacketBuf[PROPACKET_BUF_LEN]; 

/*********************************************************************************
将封包Buf数据进行转义，头尾加标识0x7e,中间有0x7e 的按如下规则转换
0x7e <————> 0x7d 后紧跟一个 0x02； 
0x7d <————> 0x7d 后紧跟一个 0x01
返回：
0--转义失败，Buf空间不够
>0 转义后Buf有效数据长度
*********************************************************************************/
int PacketConvert(unsigned char *Buf,int BufLen,int DataLen)
{
	int i,j,num,len;
	//先统计Buf中的Data有多少个0x7e、0x7d
	num=0;
	for(i=0;i<DataLen;i++){
		if(Buf[i]==0x7e || Buf[i]==0x7d)num++;
	}
	//扩展后的长度
	len=DataLen+num+2;//加2是因为头尾都要加0x7e
	if(len>BufLen){//扩展后的长度大于缓存长度，转义失败
		return 0;
	}
	//尾部添加0x7e
	Buf[len-1]=0x7e;
	//中间转义
	j=len-2;
	for(i=DataLen-1;i>0;i--){
		if(Buf[i]==0x7e){
			Buf[j--]=0x02; 
			Buf[j--]=0x7d;
		}else if(Buf[i]==0x7d){
			Buf[j--]=0x01; 
			Buf[j--]=0x7d;
		}else{
			Buf[j--]=Buf[i];
		}
	}
	//i=0 的值也要处理一下
	
	if(Buf[0]==0x7e){
		Buf[j--]=0x02; 
		Buf[j--]=0x7d;
	}else if(Buf[0]==0x7d){
		Buf[j--]=0x01; 
		Buf[j--]=0x7d;
	}else{
		Buf[j--]=Buf[0];
	}
	
	//头部添加0x7e
	Buf[0]=0x7e;
	return len;
}

void PacketConvertTest(void)
{
	int i,len;
	unsigned char buf[10]={0x09,0x29,0x7e,0x03,0x7d,0x04};
	printf("PacketConvertTest begin!\r\n");
	printf("buf1=");
	for(i=0;i<6;i++){
		printf("%02X ",buf[i]);
	}
	
	len=PacketConvert(buf,sizeof(buf),6);
	printf("\r\nbuf2=");
	for(i=0;i<len;i++){
		printf("%02X ",buf[i]);
	}
	printf("\r\nPacketConvertTest end!\r\n");
}

/*********************************************************************************
将封包Buf数据进行解转义，去掉头尾标识0x7e,中间有0x7e 的按如下规则转换
0x7e <————> 0x7d 后紧跟一个 0x02； 
0x7d <————> 0x7d 后紧跟一个 0x01

0x7d 0x02<————> 0x7e 
0x7d 0x01<————> 0x7d
返回：
<0  协议错误
>0 解转义后的数据长度
*********************************************************************************/
int PacketDeConvert(unsigned char *pMsg,int DataLen)
{
	unsigned char *w,*p,*n;
	int i,j;
	if(pMsg[0]!=0x7e || pMsg[DataLen-1]!=0x7e)return -1;
	
	i=0;
	j=0;
	w=pMsg;
	p=pMsg;
	n=p+1;
	while(i<(DataLen-1)){
		if(*p==0x7e){
			if(i==0){
				p++;
				n++;
				i++;
			}else{
				return j;
			}
		}else if(*p==0x7d){
			if(*n==0x01){
				*w=0x7d;
				w++;
				j++;
				p+=2;
				n+=2;
				i+=2;
			}else if(*n==0x02){
				*w=0x7e;
				w++;
				j++;
				p+=2;
				n+=2;
				i+=2;
			}else return -3;
		}else{
			*w=*p;
			w++;
			j++;
			p++;
			n++;
			i++;
		}
	}
	return j;
}

void PacketDeConvertTest(void)
{
	int i,len;
	unsigned char buf[]={
		0x7E,0x89,0x29,0x00,0x0E,0x00,0x01,0x90,0x21,0x00,0x03,0x00,0x01,0x19,0x02,0x19,0x17,0x00,0x22,0x01,0x00,0x00,0x68,0x00,0x05,0x00,0x05,0x42,0x7E,
		0x7E,0x89,0x29,0x00,0x0E,0x00,0x01,0x90,0x21,0x00,0x03,0x00,0x01,0x19,0x02,0x19,0x17,0x00,0x46,0x01,0x00,0x00,0x68,0x00,0x05,0x00,0x05,0x26,0x7E
	};
	SlwTrace(INF,"PacketDeConvertTest begin!\r\n");
	TraceData(buf,sizeof(buf));
	len=PacketDeConvert(buf,sizeof(buf));
	if(len>0){
		TraceData(buf,len);
	}else{
		sprintf((char *)buf,"len=%d\r\n",len);
		SlwTrace(DEBUG,(char *)buf);
	}
	SlwTrace(INF,"PacketDeConvertTest end!\r\n");
}
/********************************************************************************
消息封装——>计算并填充校验码-->转义
*********************************************************************************/
int Packet(unsigned short MsgID,unsigned short MsgNum ,unsigned char *pData,int DataLen)
{
	SUTDS ds;
	SUTDL dl;
	unsigned char check=0;
	unsigned char *pBuf=g_ucPropacketBuf;
	int i,j;
	if((DataLen+14)>PROPACKET_BUF_LEN)return 0;

	i=0;
	ds.Data.usData=MsgID;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//消息体属性
	//分包=无，加密=无，长度=DataLen
	ds.Data.usData=DataLen;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//设备编码
	for(j=0;j<6;j++){
		pBuf[i++]=sutProductPara.DeviceID[j];
	}
	//消息流水号
	ds.Data.usData=MsgNum;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//消息体
	if(DataLen>0){
		for(j=0;j<DataLen;j++){
			pBuf[i++]=pData[j];
		}
	}
	//校验码
	for(j=0;j<i;j++){
		check^=pBuf[j];
	}
	pBuf[i++]=check;
	return PacketConvert(pBuf,PROPACKET_BUF_LEN,i);
}
 /********************************************************************************
打包成一个位置点数据包
 *********************************************************************************/
int PacketPosition(unsigned short MsgNum,SUT_POSITION sutPosition)
{
	SUTDS ds;
	SUTDL dl;
	unsigned char *pBuf=g_ucPropacketBuf;
	unsigned char check;
	int i,j;
	i=0; 
	tsk_lock();
	//消息ID
	ds.Data.usData=MSGID_POSITION;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//消息体属性: 分包=无，加密=无，长度暂时设置为0，后续再填充
	pBuf[i++]=0;
	pBuf[i++]=0;
	//设备编码
	for(j=0;j<6;j++){
		pBuf[i++]=sutProductPara.DeviceID[j];
	}
	//消息流水号
	ds.Data.usData=MsgNum;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//以下是消息体
	dl.Data.ulData=*(unsigned int *)&sutPosition.Alarm;//0 报警标志 DWORD 报警标志位定义见 表 24 
	pBuf[i++] = dl.Data.ucData.b4;
	pBuf[i++] = dl.Data.ucData.b3;
	pBuf[i++] = dl.Data.ucData.b2;
	pBuf[i++] = dl.Data.ucData.b1;
	
	dl.Data.ulData=*(unsigned int *)&sutPosition.Status;//4 状态 DWORD 状态位定义见 表 25 
	pBuf[i++] = dl.Data.ucData.b4;
	pBuf[i++] = dl.Data.ucData.b3;
	pBuf[i++] = dl.Data.ucData.b2;
	pBuf[i++] = dl.Data.ucData.b1;
	
	dl.Data.ulData=sutPosition.Latitude;//8 纬度 DWORD 以度为单位的纬度值乘以10的6次方， 精确到百万分之一度 
	pBuf[i++] = dl.Data.ucData.b4;
	pBuf[i++] = dl.Data.ucData.b3;
	pBuf[i++] = dl.Data.ucData.b2;
	pBuf[i++] = dl.Data.ucData.b1;
	
	dl.Data.ulData=sutPosition.Longitude;//12 经度 DWORD 以度为单位的经度值乘以10的6次方， 精确到百万分之一度  
	pBuf[i++] = dl.Data.ucData.b4;
	pBuf[i++] = dl.Data.ucData.b3;
	pBuf[i++] = dl.Data.ucData.b2;
	pBuf[i++] = dl.Data.ucData.b1;
	
	ds.Data.usData=sutPosition.Altitude; //16 高程 WORD 海拔高度，单位为米（m） 
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	
	ds.Data.usData=sutPosition.Speed;//18 速度 WORD 1/10km/h 
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	
	ds.Data.usData=sutPosition.Aspect;//20 方向 WORD 0-359，正北为 0，顺时针 
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	
	pBuf[i++]=sutPosition.Time[0];//22 时间 BCD[6] YY-MM-DD-hh-mm-ss（GMT+8 时及的时间均采用此时区） 
	pBuf[i++]=sutPosition.Time[1];
	pBuf[i++]=sutPosition.Time[2];
	pBuf[i++]=sutPosition.Time[3];
	pBuf[i++]=sutPosition.Time[4];
	pBuf[i++]=sutPosition.Time[5];
	
	//附加信息
	pBuf[i++]=sutPosition.ExtraMsgId;//附加信息ID      --水表项目固定为：0x3A
	pBuf[i++]=sutPosition.ExtraMsgLen;//附加信息长度   --长度固定为从下一项开始到结构体最后的长度
	pBuf[i++]=sutPosition.NetMode;
	pBuf[i++]=sutPosition.CSQ;
  pBuf[i++]=sutPosition.Infrared;
	pBuf[i++]=sutPosition.Camera;
	for(j=0;j<sutPosition.TcLen;j++){
		pBuf[i++]=sutPosition.TcData[j];
	}
	//修正长度
	ds.Data.usData=i-12;
	pBuf[2] = ds.Data.ucData.b2;
	pBuf[3] = ds.Data.ucData.b1;
	//校验码 
	check=0;
	for(j=0;j<i;j++){
		check^=pBuf[j];
	}
	pBuf[i++]=check;
	j=PacketConvert(pBuf,PROPACKET_BUF_LEN,i);
	tsk_unlock();
	return j;
}

 /********************************************************************************
打包成多个位置点数据包
 *********************************************************************************/
//int PacketMorePosition(unsigned short MsgNum)
//{
//	SUTDS ds;
//	SUTDL dl;
//	unsigned char check;
//	unsigned char *pBuf=sutPropacket.pSendBuf;
//	int i,j;
//	SUT_POSITION sutPosition;
//	i=0; 
//	//消息ID
//	ds.Data.usData=MSGID_M_POSITION;
//	pBuf[i++] = ds.Data.ucData.b2;
//	pBuf[i++] = ds.Data.ucData.b1;
//	//消息体属性: 分包=无，加密=无，长度暂时设置为0，后续再填充
//	pBuf[i++]=0;
//	pBuf[i++]=0;
//	//设备编码
//	for(j=0;j<6;j++){
//		pBuf[i++]=sutPropacket.DeviceID[j];
//	}
//	//消息流水号
//	ds.Data.usData=MsgNum;
//	pBuf[i++] = ds.Data.ucData.b2;
//	pBuf[i++] = ds.Data.ucData.b1;
//	//以下是消息体
//	pBuf[i++]=0;//数据项个数高位 先填充0 ，后面修正
//	pBuf[i++]=0;//数据项个数低位 先填充0 ，后面修正
//	pBuf[i++]=1;//位置数据类型  0：正常位置批量汇报，1：盲区补报 
//	for(j=0;j<16;j++){
//		if(!StorageFiFoPop(&sutPosition))break;
//		pBuf[i++]=0;//每项长度 WORD  固定为50bytes
//		pBuf[i++]=50;//每项长度 WORD  固定为50bytes
//		dl.Data.ulData=*(unsigned int *)&sutPosition.Alarm;//0 报警标志 DWORD 报警标志位定义见 表 24 
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		dl.Data.ulData=*(unsigned int *)&sutPosition.Status;//4 状态 DWORD 状态位定义见 表 25 
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		dl.Data.ulData=sutPosition.Latitude;//8 纬度 DWORD 以度为单位的纬度值乘以10的6次方， 精确到百万分之一度 
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		dl.Data.ulData=sutPosition.Longitude;//12 经度 DWORD 以度为单位的经度值乘以10的6次方， 精确到百万分之一度  
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		ds.Data.usData=sutPosition.Altitude; //16 高程 WORD 海拔高度，单位为米（m） 
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		
//		ds.Data.usData=sutPosition.Speed;//18 速度 WORD 1/10km/h 
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		
//		ds.Data.usData=sutPosition.Aspect;//20 方向 WORD 0-359，正北为 0，顺时针 
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		
//		pBuf[i++]=sutPosition.Time[0];//22 时间 BCD[6] YY-MM-DD-hh-mm-ss（GMT+8 时及的时间均采用此时区） 
//		pBuf[i++]=sutPosition.Time[1];
//		pBuf[i++]=sutPosition.Time[2];
//		pBuf[i++]=sutPosition.Time[3];
//		pBuf[i++]=sutPosition.Time[4];
//		pBuf[i++]=sutPosition.Time[5];
//		
//		//附加信息
//		pBuf[i++]=sutPosition.ExtraMsgId;//附加信息ID      --水表项目固定为：0x3A
//		pBuf[i++]=sutPosition.ExtraMsgLen;//附加信息长度   --长度固定为从下一项开始到结构体最后的长度
//		
//		dl.Data.ulData=sutPosition.InstantFlow;//瞬间流速  m3/h 放大1000倍  
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		dl.Data.ulData=sutPosition.TotalFlow;  //累计流量  m3
//		pBuf[i++] = dl.Data.ucData.b4;
//		pBuf[i++] = dl.Data.ucData.b3;
//		pBuf[i++] = dl.Data.ucData.b2;
//		pBuf[i++] = dl.Data.ucData.b1;
//		
//		ds.Data.usData=sutPosition.Ax;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		ds.Data.usData=sutPosition.Ay;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		ds.Data.usData=sutPosition.Az;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		ds.Data.usData=sutPosition.Mx;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		ds.Data.usData=sutPosition.My;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//		ds.Data.usData=sutPosition.Mz;
//		pBuf[i++] = ds.Data.ucData.b2;
//		pBuf[i++] = ds.Data.ucData.b1;
//	}
//	//
//	pBuf[13]=j;//数据项修正 
//	
//	//修正长度
//	ds.Data.usData=i-12;
//	pBuf[2] = ds.Data.ucData.b2;
//	pBuf[3] = ds.Data.ucData.b1;
//	//校验码 
//	check=0;
//	for(j=0;j<i;j++){
//		check^=pBuf[j];
//	}
//	pBuf[i++]=check;
//	return PacketConvert(pBuf,sutPropacket.SendBufLen,i);
//}
 /********************************************************************************
 打包成注册包
消息头+消息体
 *********************************************************************************/
int PacketLogin(unsigned short MsgNum)
{
	SUTDS ds;
	SUTDL dl;
	unsigned char *pBuf=g_ucPropacketBuf;
	unsigned char check;
	int i,j;
	i=0; 
	tsk_lock();
	//消息ID
	ds.Data.usData=MSGID_LOGIN;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//消息体属性: 分包=无，加密=无，长度暂时设置为0，后续再填充
	pBuf[i++]=0;
	pBuf[i++]=0;
	//设备编码
	for(j=0;j<6;j++){
		pBuf[i++]=sutProductPara.DeviceID[j];
	}
	//消息流水号
	ds.Data.usData=MsgNum;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//产品型号  BYTE[8]
	for(j=0;j<8;j++){
		pBuf[i++]=sutProductPara.ProductName[j];
	}
	//硬件版本号 WORD
	ds.Data.usData=sutProductPara.HardwareVer;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//软件版本号 WORD
	ds.Data.usData=sutProductPara.SoftwareVer;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//终端校验码 DWORD
	dl.Data.ulData=sutProductPara.AKEY;
	pBuf[i++]=dl.Data.ucData.b4;
	pBuf[i++]=dl.Data.ucData.b3;
	pBuf[i++]=dl.Data.ucData.b2;
	pBuf[i++]=dl.Data.ucData.b1;
	//物联网卡号 BYTE[20]
	for(j=0;j<20;j++)pBuf[i++]=sutDeviceConfig.CCID[j];
	//采样间隔 WORD
	ds.Data.usData=sutDeviceConfig.SamplingInterval;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//上传间隔 WORD
	ds.Data.usData=sutDeviceConfig.UploadInterval;
	pBuf[i++] = ds.Data.ucData.b2;
	pBuf[i++] = ds.Data.ucData.b1;
	//修正长度
	ds.Data.usData=i-12;
	pBuf[2] = ds.Data.ucData.b2;
	pBuf[3] = ds.Data.ucData.b1;
	//校验码 
	check=0;
	for(j=0;j<i;j++){
		check^=pBuf[j];
	}
	pBuf[i++]=check;
	j=PacketConvert(pBuf,PROPACKET_BUF_LEN,i);
	tsk_unlock();
	return j;
}
/*
检查接收到的数据包是否符合要求
输入：pData/len  如:pData=7E8001000500000000000000000000020000867E  len=20
输出：
校验失败，返回小于0的值,如-1
校验成功，返回应答ID，如0x8001
如果成功，应带消息ID放在AckID，应答消息流水号存放到AckNum
*/
int CheckPacket(unsigned char *pData,unsigned short DataLen)
{
	unsigned short MsgID;
	int i;
	unsigned char checksum=0;
	int len;
	//pData=7E8001000500000000000000000000020000867E
	//TraceData(pData,DataLen);
	len=PacketDeConvert(pData,DataLen);
	if(len<0){
		return -1;
	}
	//TraceData(pData,len);
	//pData=8001 0005 000000000000 0000    0000        0200   00   86
	//      ID   属性 设备编号      流水号  应答流水号   应答ID 结果 校验
	//校验码验证
	for(i=0;i<(len-1);i++){
		checksum^=pData[i];
	}
	if(checksum!=pData[len-1]){
//		char buf[30];
		//printf("sum:%d!=%d\r\n",checksum,pData[len-1]);
//		SlwTrace(DEBUG,buf); 
		return -2;
	}
	//设备编号验证
	for(i=0;i<6;i++){
		if(sutProductPara.DeviceID[i]!=pData[4+i])break;
	}
	if(i<6)return -3;
	MsgID=((unsigned short)pData[0]<<8)|pData[1];
	return MsgID;
}