#include "stdafx.h"
#include <stdint.h>
#include "gimbalDefs.h"
#include "commPort.h"
#include "serial.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

//	----------------------------------------------------------------------------
//	commPort_t
//	----------------------------------------------------------------------------

#define RBLEN 200000

	typedef struct {
		HANDLE fd;
		unsigned char rbuf[RBLEN];
		int rbufHead;
		int rbufTail;
	} commPort_t;

//	----------------------------------------------------------------------------
//	Globals
//	----------------------------------------------------------------------------

	HANDLE portHandle = INVALID_HANDLE_VALUE;
	commPort_t commPort;
	int baudRate = 115200;
	char portMessage[250];
	char portName[250] = "COM4";
	int goodCount = 0;
	int goodGeoCount = 0;
	int badGeoCount = 0;
	int crcCount = 0;
	int maxlenCount = 0;
	int nodataCount = 0;
	int gimbalTimeout = 0;
	int gimbalConnected = 0;

//	----------------------------------------------------------------------------
//	functions
//	----------------------------------------------------------------------------

	void lastError(int res);
	extern int commandCheck(msgHeader_t *h, uint32_t *len, uint32_t maxlen);
	extern void toLog(char *string);
	extern void toLog(char *string, int val);

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

int openPort(int portIndexPar,int baudRatePar)
{
	DCB dcb;
	COMMTIMEOUTS commTimeouts;
	int commTime = 2;
	char portname[20];

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

	if(portHandle != INVALID_HANDLE_VALUE)closePort();

	sprintf_s(portName,sizeof(portName),"COM%d",portIndexPar+1);
	if(portIndexPar>8){
		sprintf_s(portname,sizeof(portname),"\\\\.\\COM%d",portIndexPar+1);
	}else{
		sprintf_s(portname,sizeof(portname),"COM%d",portIndexPar+1);
	}
	baudRate = baudRatePar;

	//	----------------
	//	Open Serial Port
	//	----------------

	portHandle = CreateFile(portname, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
	
	if(portHandle == INVALID_HANDLE_VALUE ){
		sprintf_s(portMessage,sizeof(portMessage),"Error: Unable to open port %s", portName);
		toLog(portMessage);
		return(1);
	}else{
		sprintf_s(portMessage,sizeof(portMessage),"Opened port %s", portName);
		toLog(portMessage);
	}
	commPort.fd = portHandle;

	memset(&dcb, 0, sizeof(DCB));
	dcb.DCBlength = sizeof(DCB);
	dcb.BaudRate = baudRate;
	dcb.fBinary = TRUE;
	dcb.fParity = FALSE;
	if(0){
		dcb.fOutxCtsFlow = TRUE;
		dcb.fRtsControl = RTS_CONTROL_TOGGLE;
	}else{
		dcb.fOutxCtsFlow = FALSE;
		dcb.fRtsControl = RTS_CONTROL_ENABLE;
	}
	dcb.fOutxDsrFlow = FALSE;
	dcb.fDtrControl = DTR_CONTROL_ENABLE;
	dcb.fDsrSensitivity = FALSE;
	dcb.fOutX = FALSE;
	dcb.fInX = FALSE;
	dcb.fAbortOnError = FALSE;
	dcb.ByteSize = 8;
	dcb.Parity = NOPARITY;
	dcb.StopBits = ONESTOPBIT;
	SetCommState(portHandle, &dcb);

	if(1){
		commTimeouts.WriteTotalTimeoutMultiplier = 0;
		commTimeouts.WriteTotalTimeoutConstant = commTime;
		commTimeouts.ReadIntervalTimeout = 0;
		commTimeouts.ReadTotalTimeoutMultiplier = 0;
		commTimeouts.ReadTotalTimeoutConstant = commTime;
		SetCommTimeouts(portHandle, &commTimeouts);
	}else{
		commTimeouts.WriteTotalTimeoutMultiplier = MAXDWORD;
		commTimeouts.WriteTotalTimeoutConstant = commTime;
		commTimeouts.ReadIntervalTimeout = MAXDWORD;	// Return Immediately (polling mode)
		commTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
		commTimeouts.ReadTotalTimeoutConstant = commTime;
		SetCommTimeouts(portHandle, &commTimeouts);
	}
	SetupComm(portHandle, 4096, 4096);
	PurgeComm(portHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);

	commPort.rbufHead = 0;
	commPort.rbufTail = 0;

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

	return(0);
}

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

int closePort()
{
	if(portHandle == INVALID_HANDLE_VALUE)return(1);
	CloseHandle(portHandle);
	portHandle = INVALID_HANDLE_VALUE;
	commPort.fd = portHandle;
	sprintf_s(portMessage,sizeof(portMessage),"Closed port %s", portName);
	toLog(portMessage);
	return(0);
}

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

	int getBaudRate(){return baudRate;}
	int getPortState(){return portHandle == INVALID_HANDLE_VALUE;}
	char *getPortName(){return portName;}
	char *getPortMessage(){return portMessage;}

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

void writePort(unsigned char *bufferPtr, int len)
{
	if(commPort.fd==INVALID_HANDLE_VALUE)return;
	unsigned long numWritten;
	int res = WriteFile(commPort.fd, bufferPtr, len, &numWritten, NULL);
	if(res==-1){
		lastError(res);
	}
}

//	----------------------------------------------------------------------------
//	lastError
//	----------------------------------------------------------------------------

void lastError(int res)
{
	CString s;
	LPVOID lpMsgBuf;
	DWORD dw = GetLastError(); 
	FormatMessage(
		FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM,
		NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),(LPTSTR) &lpMsgBuf, 0, NULL );
	s.Format("res = %i, err = %Li, %s",res,dw,lpMsgBuf);
	OutputDebugString(s);
	LocalFree(lpMsgBuf);
}

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

int readByte(char *c)
{
	if(portHandle==INVALID_HANDLE_VALUE){
		return(0);
	}

	if(commPort.rbufHead > commPort.rbufTail){
		*c = commPort.rbuf[commPort.rbufTail++];
		return 1;
	}else{
		commPort.rbufHead = 0;
		commPort.rbufTail = 0;
		DWORD rLen;
		int res = ReadFile(commPort.fd,commPort.rbuf,RBLEN,&rLen,0);
		if(res==-1){
			rLen = 0;
			lastError(res);
		}
		if(rLen){
			static int errc = 0;

			if(rLen==RBLEN){
				CString s;
				s.Format("rLen = %i,count = %i\n",rLen,errc++);
				OutputDebugString(s);
			}

			commPort.rbufHead = rLen;
			*c = commPort.rbuf[commPort.rbufTail++];
			return 1;
		}else{
			return 0;
		}
	}
}

//	----------------------------------------------------------------------------
//	message
//	----------------------------------------------------------------------------

#define RXBUFLEN 200000
unsigned char message[RXBUFLEN];
__int16 *message16 = (__int16 *)message;

//	----------------------------------------------------------------------------
//	checksumOk
//	----------------------------------------------------------------------------

inline int checksumOk(int len)
{
	unsigned char *msg = (unsigned char *)message;
	int len1 = len - sizeof(msgTail_t);
	int16_t checksum = 0;
	msgTail_t *tail = (msgTail_t *)&(msg[len1]);
	for (int i = 0; i < len1; i++){
		checksum += *msg++;
	}
	return (tail->checksum == checksum);
}
//	----------------------------------------------------------------------------
//	readByteX
//	----------------------------------------------------------------------------

	int indexX = 0;
	int lenX = 0;

int readByteX(char *c)
{
	if(lenX>0){
		lenX--;
		*c = message[indexX++];
		return 1;
	}else{
		return readByte(c);
	}

}

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

char *msgStringS[MSG_LAST] = {MSG_STRINGS};

int getMessage()
{
	char c;
	static __int16 oldCount = 0;
	static int totalcount = 0;
	static int id = 0;
	static uint32_t len = 0;
	static uint32_t index = 0;
	static int state = 0;
	static int err1 = 0;
	static int err2 = 0;
	static int err3 = 0;
	static int unframed = 0;

	msgHeader_t *g = (msgHeader_t *)message;
	while(1){
		if(1 != readByteX(&c)){
			return 0;
		}
		totalcount++;
		if(index>(RXBUFLEN-1))index = 0;
		message[index++] = c;
		switch(state){
		case 0:	// header0
			index = 0;
			message[index++] = c;
			if(c != 0x57){
				state = 0;
				err1++;
				unframed++;
			}else{
				state++;
			}
			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){
				if(lenX>0){
					char s[40];
					sprintf_s(s,sizeof(s),"message recovered! %s",msgStringS[(g->id)-(MSG_FIRST)]);
					toLog(s);
				}
				if(unframed>0){
					char s[40];
					sprintf_s(s,sizeof(s),"unframed %i",unframed);
					toLog(s);
					unframed = 0;
				}
				state++;
			}else{
				state = 0;
				index = 0;
				err3++;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				state = 0;
				if(checksumOk(len)){
					gimbalConnected = 1;
					goodCount++;
				}else{
					lenX = len-1;
					indexX = 1;
					len = 0;
					crcCount++;
					if(1){
						char s[40];
						sprintf_s(s,sizeof(s),"checksum fail %s",msgStringS[(g->id)-(MSG_FIRST)]);
						toLog(s);
					}
				}
				return len;
			}
			break;
		}
	}
}