#include "comm.h"


//	----------------------------------------------------------------------------
//	comm
//	----------------------------------------------------------------------------

comm::comm(QTcpSocket *socket,QSerialPort *serial,QObject *parent) :
	QObject(parent)
{
	sockInit = 0;									// inits
	nPort = 0;										// inits
	sockClient = socket;							// inits
	serialPort = serial;							// inits
	lanPortOpen = 0;								// inits
	serialPortOpen = 0;								// inits
	portHandle = 0;									// inits
//	memset(&commPort,0,sizeof(commPort));			// inits
	baudRate = 115200;								// inits
	memset(portMessage,0,sizeof(portMessage));		// inits
	strcpy(portName,"COM4");						// inits
	goodCount = 0;									// inits
	crcCount = 0;									// inits
	maxlenCount = 0;								// inits
	nodataCount = 0;								// inits
	gimbalTimeout = 0;								// inits
	gimbalConnected = 0;							// inits
	getMsgCount = 0;								// inits
	quit = 0;										// inits
	wait = 500;										// inits
	connected = 0;									// inits
	gimbalPresent = 0;								// inits
	errorCode = 0;									// inits
	verbose = 1;									// inits
	memset(&commBuff,0,sizeof(commBuff));			// inits

	memset(message,0,sizeof(message));				// inits
	oldCount = 0;									// inits
	totalcount = 0;									// inits
	id = 0;											// inits
	len = 0;										// inits
	index = 0;										// inits
	state = 0;										// inits
	sum1 = 0;										// inits
	sum2 = 0;										// inits
	err1 = 0;										// inits
	err2 = 0;										// inits
	err3 = 0;										// inits
	good = 0;										// inits
	fpass = 1;										// inits
	maxlenCount = 0;								// inits
	goodCount = 0;									// inits
	crcCount = 0;									// inits

	if(sockClient){
		sockClient->setSocketOption(QAbstractSocket::LowDelayOption,1);
		sockClient->setReadBufferSize(4096);
	}
}

//	----------------------------------------------------------------------------
//	openLanPort
//	----------------------------------------------------------------------------

int comm::openLanPort(const char *hostNamePar, int portPar)
{
	closePort();

	const int Timeout = 5 * 1000;
//	commPort.portType = PORT_LAN;
	strcpy(portName,hostNamePar);
	nPort = portPar;
	sockClient->connectToHost(portName, nPort);
	if(sockClient->waitForConnected(Timeout)) {
		sockInit = 1;
		lanPortOpen = 1;
		connected = 1;
		return 0;
	}else{
		emit error(-1, sockClient->errorString());
		sockInit = -1;
		lanPortOpen = 0;
		return 1;
	}
}

//	----------------------------------------------------------------------------
//	open232Port
//	----------------------------------------------------------------------------

int comm::open232Port(const char *portNamePar,int baudRatePar)
{
	closePort();

//	commPort.portType = PORT_RS232;
//	commPort.rbufCnt = 0;
//	commPort.rbufPos = 0;
//	commPort.state = STARTING;

	goodCount = 0;
	crcCount = 0;
	maxlenCount = 0;
	nodataCount = 0;

	baudRate = baudRatePar;
	strcpy(portName,portNamePar);

	serialPort->setDataBits(QSerialPort::Data8);
	serialPort->setParity(QSerialPort::NoParity);
	serialPort->setStopBits(QSerialPort::OneStop);

	serialPort->setBaudRate(baudRate);
	serialPort->setPortName(portName);
	serialPort->setFlowControl(QSerialPort::NoFlowControl);
	if(serialPort->open(QIODevice::ReadWrite)){
		serialPortOpen = 1;
		connected = 1;
		return 0;
	}else{
		serialPortOpen = 0;
		connected = 0;
		emit error(serialPort->error(), serialPort->errorString());
		return 1;
	}
}

//	----------------------------------------------------------------------------
//	openPort
//	----------------------------------------------------------------------------

int comm::openPort(const char *portNamePar,int baudRatePar, int portPar)
{
	if(baudRatePar){
		QString ss;
		ss.sprintf("open232Port %s@%i",portNamePar,baudRatePar);
		emit error(-1,ss);
		return open232Port(portNamePar,baudRatePar);
	}else{
		QString ss;
		ss.sprintf("openLanPort %s:%i",portNamePar,portPar);
		emit error(-1,ss);
		return openLanPort(portNamePar,portPar);
	}
}

//	----------
//	Close Port
//	----------

int comm::closePort()
{
	close232Port();
	closeLanPort();
	return 1;
}

//	----------------------------------------------------------------------------
//	closeLanPort
//	----------------------------------------------------------------------------

int comm::closeLanPort()
{
	if(!sockClient)return 0;

	if(sockClient->isOpen()){
		emit error(-1,"closeLanPort");
		sockClient->close();
		lanPortOpen = 0;
	}
	return 0;
}

//	-----------------
//	Close Serial Port
//	-----------------

int comm::close232Port()
{
	if(serialPortOpen){
		emit error(-1,"close232Port");
		serialPort->close();
		serialPortOpen = 0;
	}
	return(0);
}

//	----------------------------------------------------------------------------
//	"get" functions
//	----------------------------------------------------------------------------

int comm::isConnected(){return connected;}
int comm::getBaudRate(){return baudRate;}
char *comm::getPortName(){return portName;}
char *comm::getPortMessage(){return portMessage;}

//	----------------------------------------------------------------------------
//	readByte
//	----------------------------------------------------------------------------

int comm::readByte(char *c)
{
	if(!connected){
		commBuff.rbufHead = 0;
		commBuff.rbufTail = 0;
		return 0;
	}

	if(commBuff.rbufHead > commBuff.rbufTail){
		*c = commBuff.rbuf[commBuff.rbufTail++];
		return 1;
	}else{
		commBuff.rbufHead = 0;
		commBuff.rbufTail = 0;
		unsigned int rLen = 0;

		if(lanPortOpen){
			ReadLAN(commBuff.rbuf,RBLEN, &rLen);
		}else if(serialPortOpen){
			Read232(commBuff.rbuf,RBLEN, &rLen);
		}else{
			return 0;
		}

		if((rLen>0)&&(rLen<=RBLEN)){
			commBuff.rbufHead = rLen;
			*c = commBuff.rbuf[commBuff.rbufTail++];
			return 1;
		}else{
			return 0;
		}
	}
}

//	----------------------------------------------------------------------------
//	ReadLAN
//	----------------------------------------------------------------------------

void comm::ReadLAN(unsigned char *rbuf,unsigned int bytesToRead,unsigned int *bytesRead)
{
	const int Timeout = 20;
	if(sockInit != 1)return;
	if(!lanPortOpen)return;
//	msleep(20);
	if(sockClient->state() != QAbstractSocket::ConnectedState){
		if(connected){
			lanPortOpen = 0;
			connected = 0;
			emit error(-1, "sockClient remote disconnect");
		}
		*bytesRead = 0;
		return;
	}
	if(!sockClient->waitForReadyRead(Timeout)){
		emit error(sockClient->error(), sockClient->errorString());
		*bytesRead = 0;
		return;
	}
	unsigned int count = sockClient->bytesAvailable();
	while (count < 1) {
		usleep(20*1000);
		count = sockClient->bytesAvailable();
	}
	count = (count<bytesToRead)?(count):(bytesToRead);
	int res = sockClient->read((char *)rbuf,count);
	if(res==-1){
		emit error(sockClient->error(), sockClient->errorString());
		*bytesRead = 0;
	}else{
		*bytesRead = res;
		if(verbose){
			QString ss;
			ss.sprintf("ReadLAN %i",(int)*bytesRead);
			emit error(0,ss);
		}
	}
}

//	----------------------------------------------------------------------------
//	Read232
//	----------------------------------------------------------------------------

void comm::Read232(unsigned char *rbuf,unsigned int bytesToRead,unsigned int *bytesRead)
{
	if(!serialPortOpen)return;

	if(!serialPort->waitForReadyRead(wait)){
		*bytesRead = 0;
	}else{
		int res = serialPort->read((char *)rbuf,bytesToRead);
		if(res==-1){
			emit error(serialPort->error(), serialPort->errorString());
			*bytesRead = 0;
		}else{
			*bytesRead = res;
		}
	}
}

//	----------------------------------------------------------------------------
//	WriteLAN
//	----------------------------------------------------------------------------

void comm::WriteLAN(unsigned char *tbuf,int bytesToWrite, int *bytesWritten)
{
	const int Timeout = 5 * 1000;
	if(sockInit != 1)return;
	if(!lanPortOpen)return;
	*bytesWritten = sockClient->write((char *)tbuf,bytesToWrite);
	if(!sockClient->waitForBytesWritten(Timeout)){
		emit error(sockClient->error(), sockClient->errorString());
		*bytesWritten = 0;
	}
}

//	----------------------------------------------------------------------------
//	Write232
//	----------------------------------------------------------------------------

void comm::Write232(unsigned char *tbuf,int bytesToWrite, int *bytesWritten)
{
	const int Timeout = 5 * 1000;
	if(!connected)return;
	if(!serialPortOpen)return;
	serialPort->clear();
	*bytesWritten = serialPort->write((char *)tbuf,bytesToWrite);
	if(!serialPort->waitForBytesWritten(Timeout)){
		emit error(serialPort->error(), serialPort->errorString());
		*bytesWritten = 0;
	}
}

//	----------------------------------------------------------------------------
//	writePort
//	----------------------------------------------------------------------------

void comm::writePort(unsigned char *bufferPtr, int len)
{
	int numWritten;
	if(lanPortOpen){
		WriteLAN(bufferPtr, len, &numWritten);
	}else if(serialPortOpen){
		Write232(bufferPtr, len, &numWritten);
	}
}

int messageLen(int id);

//	----------------------------------------------------------------------------
//	commandCheck
//	----------------------------------------------------------------------------

int comm::commandCheck(msgHeader_t *h, uint32_t *len, uint32_t maxlen)
{
	if(messageLen(h->id)){
		if( (messageLen(h->id)) == (int)(h->len) ){
			*len = (h->len);
			return (h->id);
		}else{
			*len = 0;
			return 0;
		}
	}else{
		if( maxlen >= (h->len) ){
			*len = (h->len);
			return (h->id);
		}else{
			*len = 0;
			return 0;
		}
	}
}
//msgTunings_t *mt = (msgTunings_t *)message;


//	----------------------------------------------------------------------------
//	checkSumX
//	----------------------------------------------------------------------------

int comm::checkSumX(int len, int *sum1,int *sum2)
{
	int len1 = len - sizeof(msgTail_t);
	msgTail_t *t1 = (msgTail_t *)&(message[len1]);;
	msgTail_t t2;
	t2.checksum = 0;
	unsigned char *msg = message;
	for(int i=0;i<len1;i++){
		t2.checksum += *msg++;
	}
	*sum1 = t1->checksum;
	*sum2 = t2.checksum;
	if(t2.checksum == t1->checksum){
		return 1;
	}else{
		return 0;
	}
}

//	----------------------------------------------------------------------------
//	getMessage
//	----------------------------------------------------------------------------

int comm::getMessage()
{
	char c;

	msgHeader_t *g = (msgHeader_t *)message;
	while(1){
		if(1 != readByte(&c)){
			return 0;
		}
		totalcount++;
		message[index++] = c;
		switch(state){
		case 0:	// header0
			index = 0;
			message[index++] = c;
			if(c != 0x57){
				state = 0;
				err1++;
				if(good)maxlenCount++;
			}else{
				state++;
			}
			good = 0;
			break;
		case 1:	// header1
			if(c != 0x19){
				state = 0;
				index = 0;
				err2++;
			}else{
				state++;
			}
			break;

		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			state++;
			break;
		case 11:	// len3
			id = commandCheck(g,&len,RXBUFLEN);
			if(id){
				state++;
			}else{
				state = 0;
				index = 0;
				err3++;
			}
			break;

		case 12:
			if(index>=len){
				index = 0;
				state = 0;
				if(checkSumX(len,&sum1,&sum2)){
					goodCount++;
					good = 1;
					return len;
				}else{
					crcCount++;
					good = 0;
					return 0;
				}
			}
			break;

		}
	}
}
