/**
  ******************************************************************************
  * @file           : KLink.c
  * @brief          : K-Link Protocol program body
  ******************************************************************************
	*/
#include "KLink.h"
#include "functions.h"
#include "string.h"
#include "PLCFunctions.h"
#include "stm32f0xx_hal.h"

unsigned char KLPacketBuf1[256];
unsigned char KLPacketBuf2[256];

unsigned char KLBufferIn[16]={0};
unsigned char KLBufferOut[16]={0};

unsigned  char nKLAddr=1;
unsigned  char nKLSeq=0;

unKLStat nKLStatus ={0};

int KLThisuS=0;
int KLRecvTimeuS=0;

unsigned char KLBCC(void const * pData, int nSize)
{
	unsigned char k;
	k=0;
	for (int i=0;i<nSize;i++)
	{
		k^=((unsigned char *)pData)[i];
	}
	return k;	
}
int KLMakeRplyPacket(void * pBuf, uchar dst, uchar Status, uchar nCmd, uchar DataLen,const void * pData )
{
	pKLRplyPktHdr p1 = (pKLRplyPktHdr)pBuf;
	int PacketLenth=0;
	p1->RplyStSgn=KLSignReply;
	p1->DstHost=dst;
	p1->nStatus=Status;
	p1->nRplyCMD=nCmd;
	p1->nSize1=DataLen;
	memcpy(p1->Datas,pData,DataLen);
	p1->Datas[DataLen]=KLBCC(p1,sizeof(stKLRplyPktHdr)+DataLen-1);
	PacketLenth=sizeof(stKLRplyPktHdr)+DataLen;	
	

	switch (nCmd)
	{
		case KLCmdNone:
			PacketLenth=0;
			break;
		case KLCmdPing:
//		case KLCmdPingReply:
			//p1->data[DataLen+1]=KLEndSign;		
			//PacketLenth=sizeof(stKLPacket)+DataLen+1;		
			break;
		case KLCmdRead:
//		case KLCmdReadReply:
			//p1->data[DataLen+1]=KLEndSign;
			//PacketLenth=sizeof(stPacket)+DataLen+1;
			break;
		case KLCmdWrite:
//		case KLCmdWriteReply:
		break;
		case KLCmdErrRply:
		
		break;
		default:
			break;
	}
	
	return PacketLenth;
}

int KLCheckPacket(int nChn, void * pBuf, int nLen1)
{
	pKLReqPktHdr p1 = (pKLReqPktHdr) pBuf;
	if (p1->ReqStSgn != KLSignStart)
	{
		return 1;
	}
	
	int DataLen=0;	//p1->LoadLen;
	if (DataLen>KLMaxPacketLength) 
	{
		return 2;
	}
	if (nLen1<DataLen+sizeof(stKLReqPktHdr))
	{
		//len4=sprintf(str3,"%d < %d + %d \r\n",len2,DataLen,sizeof(stPacket));
		//PutStr(str3,len4);
		return 3;	//not long enough					
	}
//	if (p1->data[DataLen+1] != EndSign)
//	{
//		ChnStats[nCurPollId].NoEndErr++;
//		Uart2Stat.LengthErr++;		
//		return -2;
//	}
	unsigned char thisBCC=KLBCC(p1,nLen1-1);
	if (thisBCC != ((uchar *)pBuf)[nLen1-1]) 
	{//BCC Error;
		return 4;
	} 		
	return 0;
}

inline void SetAddrBit(unsigned short * pW, unsigned char bitAddr)
{
	(*pW)|=1<<(bitAddr&0xf);
}

inline void ResetBit(unsigned short * pW, unsigned char bitAddr)
{
	(*pW)&=~(1<<(bitAddr&0xf));
}

static inline void SetBitValue(unsigned short * pW, unsigned char bitAddr, unsigned char Value)
{
	if (Value)	{	SetAddrBit(pW, bitAddr);}
	else {ResetBit(pW, bitAddr);}
}

int KLParseReqPacket(int nChn, void * pBuf, int Len1)
{
	pKLReqPktHdr p1 = (pKLReqPktHdr)pBuf;

	int nDstHost=p1->DstHost;
	KLRecvTimeuS=KLThisuS;
	int nDataType=p1->nType1;
	int nAddr=0;
	int DataLen=0;	//p1->LoadLen;
//int nSrcAddr=p1->SrcAddr;

	nKLStatus.nSEQ = ((pKLStat)(&(p1->Stat)))->nSEQ;;
	
	void * pData=0;
	pKLRplyPktHdr p2=(pKLRplyPktHdr)KLPacketBuf2;
	char rData[4];

	int PacketLen=0;
	KLRecvTimeuS=KLThisuS;
	if (nDstHost==nKLAddr || nDstHost==0xff)
	{
		switch (p1->nCMD)
		{
			case KLCmdNone:
				break;
			case KLCmdPing:
				DataLen=nDataType;
//				KLBufferIn[0]=p1->Params[0];
//				p1->Params[0]=KLBufferOut[0];
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdPing,DataLen,p1->Params);
				SendPacket(nChn, p2, PacketLen);
				break;
//			case KLCmdPingReply:
//				break;
			case KLCmdInfo:
				//if (nDataType == KLDataTypeDT)
				DataLen= sizeof(KMInfoBlock);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,&KMInfoBlock);
				SendPacket(nChn, p2, PacketLen);				
				break;
			case KLCmdGetUid:
				//if (nDataType == KLDataTypeDT)
				DataLen= 12;
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,(uint32_t *)UID_BASE);
				SendPacket(nChn, p2, PacketLen);				
				break;
			case KLCmdGetSN:
				//if (nDataType == KLDataTypeDT)
				DataLen= 4;
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,&((pFactoryData)FACTORY_DATA_BASE)->SN1);
				SendPacket(nChn, p2, PacketLen);				
				break;
			case KLCmdGetFactoryData:
				DataLen= sizeof(stFactoryData);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,(pFactoryData)FACTORY_DATA_BASE);
				SendPacket(nChn, p2, PacketLen);				
				break;
			case KLCmdWriteFactoryData:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2];
				//DataLen=16;
				for (int i=0;i<DataLen;i++)
				{KLPacketBuf2[i]=p1->Params[4+i];}
				WriteFactoryData(KLPacketBuf2, DataLen);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;
			
			case KLCmdRead:
			case KLCmdReadDataWord:
			case KLCmdReadDataByte:				
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2];
				if (nDataType ==KLDataTypeDT)				{			pData=KMem.DT+nAddr;		}
				else if (nDataType == KLDataTypeSDT)	{		pData=KMem.SDT+nAddr;	}
				else if (nDataType == KLDataTypeWSR)	{		pData=KMem.WSR+nAddr;	}
				else if (nDataType == KLDataTypeWX)		{		pData=KMem.WX+nAddr;		}
				else if (nDataType == KLDataTypeWY)		{		pData=KMem.WY+nAddr;		}
				else if (nDataType == KLDataTypeWR)		{		pData=KMem.WR+nAddr;		}
				else if (nDataType == KLDataTypeWLX)		{		pData=KMem.WLX+nAddr;		}
				else if (nDataType == KLDataTypeWLY)		{		pData=KMem.WLY+nAddr;		}
				else if (nDataType == KLDataTypeSV)		{		pData=KMem.SV+nAddr;	}
				else if (nDataType == KLDataTypeEV)		{		pData=KMem.EV+nAddr;	}
				else if (nDataType == KLDataTypeTest)	{		pData=KMem.SDT+nAddr;	}
				else if (nDataType == KLDataSysCfg) { pData = (unsigned short *)&storedKMSysCfg + nAddr;}
				else if (nDataType == KLDataTypeFlash) { pData = (unsigned short *)FLASH_BASE + nAddr;}
				else				{					pData=KLBufferOut+nAddr;				}
				
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,pData);
				SendPacket(nChn, p2, PacketLen);
				
				break;
//			case KLCmdReadReply:
//				break;
			case KLCmdWrite:
			case KLCmdWriteDataWord:
			case KLCmdWriteDataByte:
				
				//memcpy(DispBuf,p1->data,DataLen);
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2];
					if (nDataType ==KLDataTypeDT)				{		pData=KMem.DT+nAddr;		}
				else if (nDataType == KLDataTypeSDT)	{		pData=KMem.SDT+nAddr;	}
				else if (nDataType == KLDataTypeWSR)	{		pData=KMem.WSR+nAddr;	}
				else if (nDataType == KLDataTypeWX)		{		pData=KMem.WX+nAddr;		}
				else if (nDataType == KLDataTypeWY)		{		pData=KMem.WY+nAddr;		}
				else if (nDataType == KLDataTypeWR)		{		pData=KMem.WR+nAddr;		}
				else if (nDataType == KLDataTypeWLX)		{		pData=KMem.WLX+nAddr;		}
				else if (nDataType == KLDataTypeWLY)		{		pData=KMem.WLY+nAddr;		}
				else if (nDataType == KLDataTypeSV)		{		pData=KMem.SV+nAddr;	DataLen=0;}
				else if (nDataType == KLDataTypeEV)		{		pData=KMem.EV+nAddr;	DataLen=0;}
				else if (nDataType == KLDataTypeTest)	{		pData=KMem.SDT+nAddr;	DataLen=0;}
				else if (nDataType == KLDataSysCfg)		{ pData = (unsigned short *)&storedKMSysCfg + nAddr;}
				else if (nDataType == KLDataTypeFlash) { pData = (unsigned short *)FLASH_BASE + nAddr;}
				else				{					pData=KLBufferOut+nAddr;	DataLen=0;			}
						
				memcpy(pData,p1->Params+4,DataLen);					
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,0,0);
				SendPacket(nChn, p2, PacketLen);					
				break;
//			case KLCmdWriteReply:
//				break;

			case KLCmdRead1Bit:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen = 1;
				if (nDataType == KLCoilTypeX)			 { rData[0] = ((KMem.WX[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				else if (nDataType == KLCoilTypeY) { rData[0] = ((KMem.WY[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				else if (nDataType == KLCoilTypeR) { rData[0] = ((KMem.WR[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				else if (nDataType == KLCoilTypeLX) { rData[0] = ((KMem.WLX[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				else if (nDataType == KLCoilTypeLY) { rData[0] = ((KMem.WLY[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				else if (nDataType == KLCoilTypeT) { rData[0] = KMem.Timers[nAddr].bTon;}
				else if (nDataType == KLCoilTypeC) { rData[0] = KMem.Timers[nAddr].bTon;}
				else if (nDataType == KLCoilTypeSR) {rData[0] = ((KMem.WSR[nAddr>>4]&(1<<(nAddr&0x0f)))>0);}
				
				else if (nDataType == KLCoilTypeLR) { rData[0] = 0;}
				else {rData[0]=0;}
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdRead1Bit,DataLen,rData);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdWrite1Bit:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				if (nDataType == KLCoilTypeX)			 { SetBitValue( &KMem.WX[nAddr>>4],nAddr&0x0f,p1->Params[2]);}
				else if (nDataType == KLCoilTypeY) { SetBitValue( &KMem.WY[nAddr>>4],nAddr&0x0f,p1->Params[2]);}
				else if (nDataType == KLCoilTypeR) { SetBitValue( &KMem.WR[nAddr>>4],nAddr&0x0f,p1->Params[2]);}
				else if (nDataType == KLCoilTypeLX) {SetBitValue( &KMem.WLX[nAddr>>4],nAddr&0x0f,p1->Params[2]);}
				else if (nDataType == KLCoilTypeLY) {SetBitValue( &KMem.WLY[nAddr>>4],nAddr&0x0f,p1->Params[2]);}
				else if (nDataType == KLCoilTypeT) { KMem.Timers[nAddr].bTon = p1->Params[2];}
				else if (nDataType == KLCoilTypeC) { KMem.Timers[nAddr].bTon = p1->Params[2];}
				else if (nDataType == KLCoilTypeC) { KMem.Timers[nAddr].bTon = p1->Params[2];}
				else if (nDataType == KLCoilTypeLR) { SetBitValue( &KMem.WSR[nAddr>>4],nAddr&0x0f,p1->Params[2]);;}
				else {rData[0]=0;}
				DataLen=0;
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdWrite1Bit,DataLen,rData);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdReadBits:
				break;
			case KLCmdWriteBits:
				break;
			case KLCmdChgMode:
				
				break;
			case KLCmdReadProgram:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2];
				 if (nDataType==0){
						pData = (unsigned short *)STORE_PRG_BASE + nAddr;
				 } else if (nDataType==1){
						pData = (unsigned short *)ALT_PRG_BASE + nAddr;
				 } else if (KMRunStat.nBinProgBank == 0) {
						pData = (unsigned short *)STORE_PRG_BASE + nAddr;
				 }else {
						pData = (unsigned short *)ALT_PRG_BASE + nAddr;
				 }
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,pData);
				SendPacket(nChn, p2, PacketLen);
				
				break;
			case KLCmdStartProgram:
				DataLen=nDataType;
//				KLBufferIn[0]=p1->Params[0];
//				p1->Params[0]=KLBufferOut[0];
				if (PLCMem.bPLCRunning) PLCMem.bPLCRunning=0;
			
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdStartProgram,0,0);
				SendPacket(nChn, p2, PacketLen);
				
				break;
			case KLCmdWriteProgram:
				if (PLCMem.bPLCRunning) PLCMem.bPLCRunning=0;
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2];

				//DataLen=16;
				for (int i=0;i<DataLen;i++)
				{KLPacketBuf2[i]=p1->Params[4+i];}
				WriteProgram(nAddr, KLPacketBuf2, DataLen,nDataType);
				DataLen=4;
				*((int *)(&rData[0]))=(long)(p1->Params+4);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdWriteProgram,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdFinishProgram:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);		//Program Size;
				DataLen=nDataType;
				KMRunStat.nBinProgSize=nAddr;
				if (KMRunStat.nBinProgBank ==0) {KMRunStat.nBinProgBank=1;}
				else {KMRunStat.nBinProgBank=0;}
				SaveRunStat(&KMRunStat);				
				PLCMem.bPLCRunning=1;
//				KLBufferIn[0]=p1->Params[0];
//				p1->Params[0]=KLBufferOut[0];
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdFinishProgram,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdReadRunStat:
				DataLen= sizeof(stRunStat);
				pData=&KMRunStat;
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,pData);
				SendPacket(nChn, p2, PacketLen);	
				break;
			case KLCmdSaveSysCfg:
				WriteSysCfgToFlash(&storedKMSysCfg);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdSaveSysCfg,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;
			case 	KLCmdSaveRunStat:
				SaveRunStat(&KMRunStat);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdSaveRunStat,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdGetEventLogCount:
				DataLen= 4;
				pData=&KMem.nEventCount;
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,pData);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdGetEventLog:
				nAddr=p1->Params[0]+ (p1->Params[1]<<8);
				DataLen= p1->Params[2] * sizeof(stEventLog);
				pData=GetEventLogAddr(nAddr);
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,p1->nCMD,DataLen,pData);
				SendPacket(nChn, p2, PacketLen);
				break;
			case KLCmdClearEventLog:
				ClearEventLog();
				PacketLen=KLMakeRplyPacket(p2,nDstHost,nKLStatus.StatByte,KLCmdClearEventLog,0,0);
				SendPacket(nChn, p2, PacketLen);
				break;			
			default:
			DataLen=1;
			rData[0]=KL_UNKNOWN;
			PacketLen=KLMakeRplyPacket(p2,nKLAddr,nKLStatus.StatByte,KLCmdErrRply,DataLen,rData);
			SendPacket(nChn, p2, PacketLen);			
				break;		
		}
	}	
	return 0;
}
int KLParsePacket(int nChn, void * pBuf, int Len1)
{
//	pKLReqPktHdr p1 = (pKLReqPktHdr)pBuf;
//	KLThisuS=GetuS();
//	pKLRplyPktHdr p2=(pKLRplyPktHdr)KLPacketBuf2;
	int PacketLen=Len1;
	int Result=KLCheckPacket(nChn, pBuf, Len1);
	if (Result != KL_OK)
	{
		int DataLen=4;
		char rData[4];
		rData[0]=Result;
		
		PacketLen=KLMakeRplyPacket(KLPacketBuf2,nKLAddr,nKLStatus.StatByte,KLCmdErrRply,DataLen,rData);
//		PacketLen=KLMakePacket(p2,0,nAddr,cmdKLPing,p1->nSEQ,DataLen,rData);
		SendPacket(nChn, KLPacketBuf2, PacketLen);
		return Result;
	}
//	if (bKBusMaster) Result=ParseMasterPacket(p1,Len1);
//	memcpy(KLPacketBuf2,pBuf,Len1);
//	SendPacket(nChn, KLPacketBuf2, PacketLen);
	
	Result=KLParseReqPacket(nChn, pBuf, Len1);
	
	//int len1=p1->PacketLen;
//	if (p1->DstHost!=255&&p1->DstHost!=2) return -3;
//	pPacket p2=(pPacket)PacketBuf2;
//			Uart2Stat.OKPacket++;

	return Result;
}