#include "pch.h"
#include "Devices.h"

/************************************************/
/*	串口数据解析								*/
/*	deviceID:									*/
/*		HFOG:哈工程光纤, OFOG:圆形光纤,			*/
/*		STIM300, PSINSBoard,						*/
/*		C201:旧接收机, NC504:新接收机			*/
/************************************************/
void serialDataProc(DeviceID device, CHAR* buffer, IMUDATA* imudata)
{
	switch (device)
	{
	case DeviceID::HFOG:				// 哈工程光纤解析程序
		memcpy(&imudata->CNT, &buffer[0],  sizeof(UCHAR));
		memcpy(&imudata->CS,  &buffer[1],  sizeof(char));
		memcpy(&imudata->Ax,  &buffer[2],  sizeof(int));
		memcpy(&imudata->Ay,  &buffer[6],  sizeof(int));
		memcpy(&imudata->Az,  &buffer[10], sizeof(int));
		memcpy(&imudata->Gx,  &buffer[14], sizeof(int));
		memcpy(&imudata->Gy,  &buffer[18], sizeof(int));
		memcpy(&imudata->Gz,  &buffer[22], sizeof(int));
		memcpy(&imudata->Ax1, &buffer[26], sizeof(int));
		memcpy(&imudata->Ay1, &buffer[30], sizeof(int));
		memcpy(&imudata->Az1, &buffer[34], sizeof(int));
		memcpy(&imudata->Gx1, &buffer[38], sizeof(int));
		memcpy(&imudata->Gy1, &buffer[42], sizeof(int));
		memcpy(&imudata->Gz1, &buffer[46], sizeof(int));
		imudata->wx = imudata->Gx1 / (float)100000.0;
		imudata->wy = imudata->Gy1 / (float)100000.0;
		imudata->wz = imudata->Gz1 / (float)100000.0;
		imudata->fx = imudata->Ax1 / (float)100000.0;
		imudata->fy = imudata->Ay1 / (float)100000.0;
		imudata->fz = imudata->Az1 / (float)100000.0;
		break;
	case DeviceID::OFOG:				// 圆形惯导解析程序
		memcpy(&imudata->Gx,  &buffer[0],  sizeof(int));
		memcpy(&imudata->Gy,  &buffer[4],  sizeof(int));
		memcpy(&imudata->Gz,  &buffer[8],  sizeof(int));
		memcpy(&imudata->Tp1, &buffer[12], sizeof(char));
		memcpy(&imudata->Ax,  &buffer[13], sizeof(short));
		memcpy(&imudata->Ay,  &buffer[15], sizeof(short));
		memcpy(&imudata->Az,  &buffer[17], sizeof(short));
		memcpy(&imudata->Gx1, &buffer[19], sizeof(int));
		memcpy(&imudata->Gy1, &buffer[23], sizeof(int));
		memcpy(&imudata->Gz1, &buffer[27], sizeof(int));
		memcpy(&imudata->Tp2, &buffer[31], sizeof(char));
		memcpy(&imudata->Ax1, &buffer[32], sizeof(short));
		memcpy(&imudata->Ay1, &buffer[34], sizeof(short));
		memcpy(&imudata->Az1, &buffer[36], sizeof(short));
		memcpy(&imudata->CNT, &buffer[45], sizeof(UCHAR));
		memcpy(&imudata->CS, &buffer[46],  sizeof(char));
		// 原始数据转换成角增量并且放大kScal倍
		FLOAT vec3x, vec3y, vec3z;
		vec3x = Kg[0][0] * imudata->Gx + Kg[0][1] * imudata->Gy + Kg[0][2] * imudata->Gz - eb[0];
		vec3y = Kg[1][0] * imudata->Gx + Kg[1][1] * imudata->Gy + Kg[1][2] * imudata->Gz - eb[1];
		vec3z = Kg[2][0] * imudata->Gx + Kg[2][1] * imudata->Gy + Kg[2][2] * imudata->Gz - eb[2];
		imudata->wx =  vec3y * kScal;
		imudata->wy = -vec3z * kScal;
		imudata->wz = -vec3x * kScal;
		vec3x = Ka[0][0] * imudata->Ax + Ka[0][1] * imudata->Ay + Ka[0][2] * imudata->Az - db[0];
		vec3y = Ka[1][0] * imudata->Ax + Ka[1][1] * imudata->Ay + Ka[1][2] * imudata->Az - db[1];
		vec3z = Ka[2][0] * imudata->Ax + Ka[2][1] * imudata->Ay + Ka[2][2] * imudata->Az - db[2];
		imudata->fx =  vec3y * kScal;
		imudata->fy = -vec3z * kScal;
		imudata->fz = -vec3x * kScal;
		vec3x = Kg[0][0] * imudata->Gx1 + Kg[0][1] * imudata->Gy1 + Kg[0][2] * imudata->Gz1 - eb[0];
		vec3y = Kg[1][0] * imudata->Gx1 + Kg[1][1] * imudata->Gy1 + Kg[1][2] * imudata->Gz1 - eb[1];
		vec3z = Kg[2][0] * imudata->Gx1 + Kg[2][1] * imudata->Gy1 + Kg[2][2] * imudata->Gz1 - eb[2];
		imudata->wx1 =  vec3y * kScal;
		imudata->wy1 = -vec3z * kScal;
		imudata->wz1 = -vec3x * kScal;
		vec3x = Ka[0][0] * imudata->Ax1 + Ka[0][1] * imudata->Ay1 + Ka[0][2] * imudata->Az1 - db[0];
		vec3y = Ka[1][0] * imudata->Ax1 + Ka[1][1] * imudata->Ay1 + Ka[1][2] * imudata->Az1 - db[1];
		vec3z = Ka[2][0] * imudata->Ax1 + Ka[2][1] * imudata->Ay1 + Ka[2][2] * imudata->Az1 - db[2];
		imudata->fx1 =  vec3y * kScal;
		imudata->fy1 = -vec3z * kScal;
		imudata->fz1 = -vec3x * kScal;
		break;
	case DeviceID::STIM300:			// STIM300解析程序
		memcpy3B(&imudata->Gx, &buffer[0]);
		memcpy3B(&imudata->Gy, &buffer[3]);
		memcpy3B(&imudata->Gz, &buffer[6]);
		memcpy3B(&imudata->Ax, &buffer[10]);
		memcpy3B(&imudata->Ay, &buffer[13]);
		memcpy3B(&imudata->Az, &buffer[16]);
		
		imudata->wx = imudata->Gx / 256 * (float)0.00006103515625;
		imudata->wy = imudata->Gy / 256 * (float)0.00006103515625;
		imudata->wz = imudata->Gz / 256 * (float)0.00006103515625;
		imudata->fx = imudata->Ax / 256 * (float)0.00006103515625;
		imudata->fy = imudata->Ay / 256 * (float)0.00006103515625;
		imudata->fz = imudata->Az / 256 * (float)0.00006103515625;
		break;
	case DeviceID::PSINSBoard:		// 开发板解析程序
		memcpy(&imudata->CNT, &buffer[0], sizeof(int));
		memcpy(&imudata->wx, &buffer[4],  sizeof(int));
		memcpy(&imudata->wy, &buffer[8],  sizeof(int));
		memcpy(&imudata->wz, &buffer[12], sizeof(int));
		memcpy(&imudata->fx, &buffer[16], sizeof(int));
		memcpy(&imudata->fy, &buffer[20], sizeof(int));
		memcpy(&imudata->fz, &buffer[24], sizeof(int));
		break;
	case DeviceID::Xsens_MTi:
		memcpy2B(&imudata->CNT, &buffer[3]);
		memcpy4B(&imudata->wx, &buffer[30]);
		memcpy4B(&imudata->wy, &buffer[34]);
		memcpy4B(&imudata->wz, &buffer[38]);
		memcpy4B(&imudata->fx, &buffer[15]);
		memcpy4B(&imudata->fy, &buffer[19]);
		memcpy4B(&imudata->fz, &buffer[23]);
		/*memcpy(&imudata->Tp1, &buffer[60], sizeof(FLOAT));*/		// Xsens和OFOG温度格式不一样
		imudata->wx = imudata->wx / GLV_DEG;
		imudata->wy = imudata->wy / GLV_DEG;
		imudata->wz = imudata->wz / GLV_DEG;
		break;
	default:
		break;
	}
}

/************************************************/
/*	3B 数据解析，主要用于STIM300					*/
/************************************************/
void memcpy3B(int* dst, char* src)
{
	memcpy((char*)dst + 3, &src[0], sizeof(char));
	memcpy((char*)dst + 2, &src[1], sizeof(char));
	memcpy((char*)dst + 1, &src[2], sizeof(char));
	*((char*)dst + 0) = 0;
}

/************************************************/
/*	4B 数据解析，主要用于XSens(小端格式)			*/
/************************************************/
void memcpy4B(float* dst, char* src)
{
	memcpy((char*)dst + 3, &src[0], sizeof(char));
	memcpy((char*)dst + 2, &src[1], sizeof(char));
	memcpy((char*)dst + 1, &src[2], sizeof(char));
	memcpy((char*)dst + 0, &src[3], sizeof(char));
}

void memcpy2B(int* dst, char* src)
{
	memcpy((char*)dst + 1, &src[0], sizeof(char));
	memcpy((char*)dst + 0, &src[1], sizeof(char));
}

/************************************************/
/*	GPGGA 数据处理函数							*/
/************************************************/
void serialDataProc(DeviceID device, CHAR* buffer, GPGGADATA* gpgga)
{
	sscanf_s(buffer, ",%d.00,%lf,%c,%lf,%c,%hd,%hd,%f,%f,M,%f,M,%f,%hd*%hd\r",
		&gpgga->time, &gpgga->Lat, &gpgga->NS, 1, &gpgga->Lng, &gpgga->WE, 1, &gpgga->state, &gpgga->SatNum, &gpgga->HDOP,
		&gpgga->Alt, &gpgga->Herr, &gpgga->xx, &gpgga->BaseNum, &gpgga->chk);
}

/************************************************/
/*	BESTVELA 数据处理函数						*/
/************************************************/
void serialDataProc(DeviceID device, CHAR* buffer, BESTVELDATA* bestvel)
{
	sscanf_s(buffer, ",%[^;];%[^,],%[^,],%f,%f,%lf,%lf,%lf,%f*%s\r",
		bestvel->header, 128, bestvel->SOL_STATUS, 32, bestvel->VEL_TYPE, 32, &bestvel->latency, &bestvel->age, &bestvel->vel_h,
		&bestvel->heading, &bestvel->vel_v, &bestvel->xx, bestvel->CRC, 16);

	sscanf_s(bestvel->header, "COM1,%d,%f,%[^,],%d,%d.000,%d,%d,%d",
		&bestvel->ph.sequence, &bestvel->ph.Idletime, &bestvel->ph.FINEclk, 32, &bestvel->ph.week,
		&bestvel->ph.second, &bestvel->ph.rev0, &bestvel->ph.rev1, &bestvel->ph.rev2);

	if (!strcmp(bestvel->SOL_STATUS, "SOL_COMPUTED"))
		bestvel->SS = 0;
	else if (!strcmp(bestvel->SOL_STATUS, "INSUFFICIENT_OBS"))
		bestvel->SS = 1;
	else if (!strcmp(bestvel->SOL_STATUS, "NO_CONVERGENCE"))
		bestvel->SS = 2;
	else if (!strcmp(bestvel->SOL_STATUS, "COV_TRACE"))
		bestvel->SS = 3;
}

/************************************************/
/*	RANGAEA数据处理函数							*/
/************************************************/
void serialDataProc(CHAR* buffer, RANGETABLE* rngtab)
{
	char head[256] = { 0 }, data[4096] = { 0 };
	
	sscanf_s(buffer, "%[^;];%s", head, 256, data, 4096);		// 读取字符串的时候一定要写上读取字符串的最大长度
	readRangeHead(head, rngtab);
	decodeRangeData(data, rngtab);
}

/************************************************/
/*	SATEPOS数据处理函数							*/
/************************************************/
void serialDataProc(CHAR* buffer, SATEPOSTABLE* sattab)
{
	char head[256] = { 0 }, data[4096] = { 0 };

	sscanf_s(buffer, "%[^;];%s", head, 256, data, 4096);		// 读取字符串的时候一定要写上读取字符串的最大长度
	readSateposHead(head, sattab);
	decodeSateposData(data, sattab);
}

/************************************************/
/*	EPHEMERISE数据处理函数						*/
/************************************************/
void serialDataProc(CHAR* buffer, EPHEMRISETABLE* ephtab)
{
	char head[256] = { 0 }, data[4096] = { 0 };

	sscanf_s(buffer, "%[^;];%s", head, 256, data, 4096);		// 读取字符串的时候一定要写上读取字符串的最大长度
	readEphemerisHead(head, ephtab);
	decodeEphemerisData(data, ephtab);
}


void readRangeHead(CHAR* buffer, RANGETABLE* rngtab)
{
	sscanf_s(buffer, ",COM2,%d,%f,%[^,],%d,%d.000,%d,%d,%d",
		&rngtab->ph.sequence, &rngtab->ph.Idletime, &rngtab->ph.FINEclk, 32, &rngtab->ph.week,
		&rngtab->ph.second, &rngtab->ph.rev0, &rngtab->ph.rev1, &rngtab->ph.rev2);
}

void decodeRangeData(CHAR* data, RANGETABLE* rngtab)
{
	CHAR buffer[1024];
	char* w = buffer;
	char* r = data;
	int len = 0;

	while (true)
	{
		*(w++) = *(r++);
		if (*(w - 1) == ',')
		{
			*w = 0;
			rngtab->ph.len = atoi(buffer);		// BUG:此时buffer中除了数字还有逗号
			break;
		}
	}

	INT ksat = 0;		// 卫星数序号
	for (int i = 0; i < rngtab->ph.len; i++)
	{
		int NC = 0;
		w = buffer;
		while (true)
		{
			*(w++) = *(r++);
			if (*(w - 1) == ',')
			{
				NC++;
				if (NC == 9)
				{
					*w = 0;
					sscanf_s(buffer, "%d,%d,%lf,%f,%lf,%f,%f,%f,%f,", &rngtab->ranges[ksat].PRN, 
						&rngtab->ranges[ksat].glof,		&rngtab->ranges[ksat].psr,
						&rngtab->ranges[ksat].psrstd,	&rngtab->ranges[ksat].adr,
						&rngtab->ranges[ksat].adrstd,	&rngtab->ranges[ksat].Dopp,
						&rngtab->ranges[ksat].CNO,		&rngtab->ranges[ksat].Locktime);
					ksat++;
					r = r + 9;
					break;
				}
			}
		}
	}
}

void readSateposHead(CHAR* buffer, SATEPOSTABLE* sattab)
{
	sscanf_s(buffer, ",COM2,%d,%f,%[^,],%d,%d.000,%d,%d,%d",
		&sattab->ph.sequence, &sattab->ph.Idletime, &sattab->ph.FINEclk, 32, &sattab->ph.week,
		&sattab->ph.second, &sattab->ph.rev0, &sattab->ph.rev1, &sattab->ph.rev2);
}

void decodeSateposData(CHAR* data, SATEPOSTABLE* sattab)
{
	CHAR buffer[1024];
	char* w = buffer;
	char* r = data;
	int len = 0;

	while (true)					// read the number of satellites
	{
		*(w++) = *(r++);
		if (*(w - 1) == ',')
		{
			*w = 0;
			sattab->ph.len = atoi(buffer);
			break;
		}
	}

	INT ksat = 0;
	for (int i = 0; i < sattab->ph.len; i++)
	{
		w = buffer;
		while (true)					// read system name
		{
			*(w++) = *(r++);
			if (*(w - 1) == ',')
			{
				*w = 0;
				if (!strcmp(buffer, "GPS,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::GPS;
				else if (!strcmp(buffer, "GLONASS,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::GLONASS;
				else if (!strcmp(buffer, "SBAS,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::SBAS;
				else if (!strcmp(buffer, "Galileo,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::Galileo;
				else if (!strcmp(buffer, "BeiDou,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::BeiDou;
				else if (!strcmp(buffer, "QZSS,"))
					sattab->sateposs[ksat].sysnum = (INT)GNSSSYS::QZSS;
				break;
			}
		}

		int NC = 0;
		w = buffer;
		while (true)
		{
			*(w++) = *(r++);
			if (*(w - 1) == ',')
			{
				NC++;
				if (NC == 7)
				{
					*w = 0;
					sscanf_s(buffer, "%d,%lf,%lf,%lf,%lf,%lf,%lf", 
						&sattab->sateposs[ksat].satid,			&sattab->sateposs[ksat].x,			
						&sattab->sateposs[ksat].y,				&sattab->sateposs[ksat].z,			
						&sattab->sateposs[ksat].clk_corr,		&sattab->sateposs[ksat].iono_delay, 
						&sattab->sateposs[ksat].tropo_delay);
					sattab->sateposs[ksat].satid = sattab->sateposs[ksat].sysnum * 65536 + sattab->sateposs[ksat].satid;		// composed the sys info
					ksat++;
					r = r + 8;
					break;
				}
			}
		}
	}
}

void readEphemerisHead(CHAR* buffer, EPHEMRISETABLE* ephtab)
{
	sscanf_s(buffer, ",COM2,%d,%f,%[^,],%d,%d.000,%d,%d,%d;",
		&ephtab->ph.sequence, &ephtab->ph.Idletime, &ephtab->ph.FINEclk, 32, &ephtab->ph.week,
		&ephtab->ph.second, &ephtab->ph.rev0, &ephtab->ph.rev1, &ephtab->ph.rev2);
}


void decodeEphemerisData(CHAR* data, EPHEMRISETABLE* ephtab) 
{
	CHAR buffer[1024];
	char* w = buffer;
	char* r = data;
	int NC = 0;
	AEPHEMRISEDATA ephdata;

	while (true)
	{
		*(w++) = *(r++);
		if (*(w - 1) == ',')
		{
			NC++;
			if (NC == 29)
			{
				*w = 0;
				sscanf_s(buffer, "%d,%lf,%d,%d,%d,%d,%d,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%lf,%lf,%lf,%lf,%lf,",
					&ephdata.PRN,	&ephdata.tow,	&ephdata.health,		&ephdata.IODE1, &ephdata.IODE2,
					&ephdata.week,	&ephdata.zweek, &ephdata.toe,		&ephdata.A,		&ephdata.dltN,
					&ephdata.M0,		&ephdata.ecc,	&ephdata.omg,		&ephdata.cuc,	&ephdata.cus,
					&ephdata.crc,	&ephdata.crs,	&ephdata.cic,		&ephdata.cis,	&ephdata.I0,
					&ephdata.dotI,	&ephdata.omg0,	&ephdata.dotomg,		&ephdata.iodc,	&ephdata.toc,
					&ephdata.tgd,	&ephdata.af0,	&ephdata.af1,		&ephdata.af2);
				break;
			}
		}
	}

	if (*r == 'T')
	{
		r = r + 5;
		ephdata.AS = 1;
	}
	else if (*r == 'F')
	{
		r = r + 6;
		ephdata.AS = 0;
	}

	w = buffer;
	NC = 0;
	while (true)
	{
		*(w++) = *(r++);
		if (*(w - 1) == ',')
		{
			NC++;
			if (NC == 1)
			{
				*w = 0;
				sscanf_s(buffer, "%lf,%lf,", &ephdata.N, &ephdata.URA);
				break;
			}
		}
	}
	memcpy(&(ephtab->ephemrises[ephdata.PRN]), &ephdata, sizeof(AEPHEMRISEDATA));
}