#include "stdafx.h"
#include "gimbalDefs.h"
#include "commPort.h"
#include "gps.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

//	----------------------------------------------------------------------------
//	Global functions
//	----------------------------------------------------------------------------

	void sendSteering(steering_t *s);

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

	extern double novAtelLat;
	extern double novAtelLon;
	extern float novAtelhMsl;
	extern float novAtelAlt;
	extern int16_t novAtelMode;
	extern int16_t novAtelCount;
	extern steering_t steering;

	extern int gpsConnected;
	extern int quit;

//	----------------------------------------------------------------------------
//	gps
//	----------------------------------------------------------------------------

namespace gps {

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

#define RBLEN 4000

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

	enum { maxlen = 200 };

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

	HANDLE portHandle = INVALID_HANDLE_VALUE;
	commPort_t commPort;
	int baudRate = 460800;
	char portMessage[250];
	char portName[250] = "COM28";
	int goodCount = 0;
	int crcCount = 0;
	int maxlenCount = 0;
	int nodataCount = 0;
	int gimbalTimeout = 0;
	int gimbalConnected = 0;
	uintptr_t gpsThreadHandle = NULL;
	int quit = 0;
	int index = 0;
	char message[maxlen];
	int mstate;

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

	void lastError(int res);

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

int openPort(char *portNamePar, int baudRatePar)
{
	DCB dcb;
	COMMTIMEOUTS commTimeouts;
	int commTime = 2;

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

	if(portHandle != INVALID_HANDLE_VALUE)closePort();
	strcpy_s(portName, sizeof(portName), portNamePar);
	baudRate = baudRatePar;

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

	sprintf_s(portMessage, sizeof(portMessage), "%.10s", portName);
	portHandle = CreateFile("\\\\.\\COM28", 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: %.10s", portName);
		return(1);
	}
	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;
	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(unsigned 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;
		}
	}
}

//	-----------------------------------------------------------
//	gpsThread
//	-----------------------------------------------------------

void gpsThread(void *)
{
	while (!quit){
		readGps();
		//	Sleep(1);
	}
	gpsThreadHandle = NULL;
	_endthread();
}

//	-----------------------------------------------------------
//	startGpsThread
//	-----------------------------------------------------------

void startGpsThread()
{
	if(gpsThreadHandle)return;
	CWnd *pActiveWindow = CWnd::GetActiveWindow();
	gpsThreadHandle = _beginthread(gpsThread, 0, NULL);
}

//	----------------------------------------------------------------------------
//	GpsThread - Novatel
//	----------------------------------------------------------------------------

#define ptrToDouble(ptr) *(double *) (ptr)

void readGps()
{
	const char header[3] = { (char)0xaa,0x44,0x12 };
	int headerLength;
	int length;
	int totalLength;

	while (!quit){
		char c;
		int numread = readByte((unsigned char *)(&c));
		if(numread == 1){
			if(index < maxlen){
				message[index++] = c;
			}else{
				mstate = 0;
				index = 0;
			}
			switch(mstate){
				case 0:
				case 1:
				case 2:
					if(mstate == 0){
						index = 0;
						message[index++] = c;
					}
					if(c == header[mstate]){
						mstate++;
					}else{
						mstate = 0;
						index = 0;
					}
					break;
				case 3:
					mstate++;
					break;
				case 4:
					if(c == 42){
						mstate++;
					}else{
						mstate = 0;

					}
					break;
				case 5:
					if(index > 28){
						headerLength = message[3];
						length = *(int *)(&message[8]);
						if(headerLength < 28)break;
						if(length != 72)break;
						totalLength = headerLength + length;
						if(index > totalLength){
							novAtelLat = ptrToDouble(&message[headerLength + 8]);
							novAtelLon = ptrToDouble(&message[headerLength + 16]);
							novAtelAlt = (float)(ptrToDouble(&message[headerLength + 24]));
							novAtelhMsl = novAtelAlt;
							novAtelMode = 1;
							novAtelCount++;
							gpsConnected = 1;
							mstate++;
						}
					}
					break;
				case 6:
					headerLength = message[3];
					length = *(int *)(&message[8]);
					totalLength = headerLength + length + 4;
					if(index >= totalLength){
						mstate = 0;
						index = 0;
					}
					break;
				default:
					mstate = 0;
					index = 0;
					break;
			}
		}
		//		if(numread==1){
		//			emit newMsg(cons->message,numread);
		//		}
		//		if(numtosend){
		//			transmit(msgtosend,numtosend);
		//			numtosend = 0;
		//		}
	}
	closePort();

}

	pvtMessage_t pvtMessage;

//	----------------------------------------------------------------------------
//	GpsThread - uBlox
//	----------------------------------------------------------------------------

void readGps2()
{
	const unsigned char header[6] = {
		(unsigned char)0xB5,			// 0 header 1
		(unsigned char)0x62,			// 1 header 2
		(unsigned char)0x01,			// 2 command 1
		(unsigned char)0x07,			// 3 command 2
		(unsigned char)92,				// 4 len(l) = 92	
		(unsigned char)0				// 5 len(h) = 0
	};

	while (!quit){
		unsigned char c;
		int numread = readByte(&c);
		if(numread == 1){
			if(index < maxlen){
				message[index++] = c;
			}else{
				mstate = 0;
				index = 0;
			}
			switch (mstate){
				case 0:
				case 1:
				case 2:
				case 3:
				case 4:
				case 5:
					if(mstate == 0){
						index = 0;
						message[index++] = c;
					}
					if(c == header[mstate]){
						mstate++;
					}else{
						mstate = 0;
						index = 0;
					}
					break;
				case 6:
					if(index >= 100){
						memcpy(&pvtMessage, message, sizeof(pvtMessage));
						mstate = 0;
						index = 0;
						novAtelLat = pvtMessage.lat*(1.0/10000000);
						novAtelLon = pvtMessage.lon*(1.0/10000000);
						novAtelhMsl = pvtMessage.hMSL*(1.0f/1000);
						novAtelAlt = pvtMessage.height*(1.0f/1000);
						novAtelCount++;
						novAtelMode = (pvtMessage.fixType)?(1):(0);
						gpsConnected = 1;
					}
					break;
				default:
					mstate = 0;
					index = 0;
					break;
			}
		}

	}
	closePort();
}

} // gps

// VectorNav external GPS message

/*
Asynchronous Stream (5Hz) Formats:
1. Position/Velocity in Earth-Centered Earth-Fixed Frame (ECEF):

	$VNGPE,TOW,Week,GpsFix,NumSats,PosX,PosY,PosZ,VelX,VelY,VelZ,PosAcX,PosAccY,PosAccZ,SpeedAcc,TimeAcc*XX<CR><LF>

	a. Position and Velocity data are expected in the ECEF coordinate frame
	b. Position uncertainty (1-sigma level confidence) is expected in the local North East Down (NED) coordinate frame (note: only the first two components of position uncertainty (North & East values) are used internally, the third component (Down value) is automatically scaled on-board.
	c. The velocity uncertainty is expected as a scalar magnitude (1-sigma confidence level)
	d. All uncertainty values must be positive, greater than zero

2. Position in Latitude, Longitude, Altitude (LLA), velocity in NED coordinate frame

	$VNGPS,TOW,Week,GpsFix,NumSats,Lat,Long,Alt,VelX,VelN,VelE,VelD,PosAccY,PosAccZ,SpeedAcc,TimeAcc*XX<CR><LF>

	a. Position data is expected as LLA in WGS84 coordinates
	b. Velocity is expected in the local NED frame.
	c. Position uncertainty (1-sigma level confidence) is expected in the local NED coordinate frame (note: only the first two components of position uncertainty (North & East values) are used internally, the third component (Down value) is automatically scaled on-board.
	d. The velocity uncertainty is expected as a scalar magnitude (1-sigma confidence level)
	e. All uncertainty values must be positive, greater than zero

Field Descriptions:

	TOW = Time of Week (GPS seconds)
	Week = GPS week number
	GpsFix = 3 if data packet contains a valid 3D GPS fix. Set to 0 if the data is not valid.
	NumSats = Number of visible satellites used in the GPS solution. If unavailable or not applicable set to 0.
	PosXYZ = ECEF position in units of meters
	VelXYZ = ECEF velocity in units of meters/second
	PosAccXYZ = 1-Sigma position uncertainty in NED frame (3 components), units of meters
	SpeedAcc = Velocity uncertainty (1-sigma scalar magnitude), units of meters/second
	TimeAcc = Uncertainty on timestamp in seconds. (Typically in the nanosecond range for GPS). If unavailable set to 0.
	PosLLA = Latitude, Longitude and Altitude in units of degrees, degrees and meters respectively
	VelNED = North, East and Down velocity components in units of meters/second

Example package:

	$VNGPE,414167.400007,1745,3,13,-0611931.210,-5327005.350,+3442499.850,+004.470,-017.160,-026.000,+001.920,+001.711,+001.671,+000.096,1.00E-09*XXXX<CR><LF>

	Note that the 16 Bit CRC was utilized here.
*/

//	$VNGPS,          TOW, Week,GpsFix,NumSats,         Lat,        Long,         Alt,    VelN,    VelE,    VelD,PosAccY,PosAccZ,SpeedAcc,TimeAcc*XX<CR><LF>
//	$VNGPE,          TOW, Week,GpsFix,NumSats,        PosX,        PosY,        PosZ,    VelX,    VelY,    VelZ,  PosAcX, PosAccY, PosAccZ,SpeedAcc, TimeAcc*XX<CR><LF>
//	$VNGPE,414167.400007, 1745,     3,     13,-0611931.210,-5327005.350,+3442499.850,+004.470,-017.160,-026.000,+001.920,+001.711,+001.671,+000.096,1.00E-09*XXXX<CR><LF>
