/* VectorNav HSI Calibration Library v1.0.0.2
 *
 * Copyright (c) 2018 VectorNav Technologies, LLC
 *
 * This source code is proprietary to and copyrighted by VectorNav Technologies, LLC and is
 * available solely under license from VectorNav. All rights reserved. If you do not have a
 * license to use this source code from VectorNav, any use hereof is strictly prohibited by
 * U.S. law and other applicable law. This source code is restricted for use solely with the
 * products of VectorNav and as otherwise set forth in any agreement with VectorNav. Any
 * disclosure of this source code to the public or to any third party is also prohibited.
 */
/* Functions for loading raw data logs, loading configuration data files,
*  and writing final hard/soft iron results out. */
#include "vnhsiexampleutil.h"

#define FILE_LOAD_BUF_SIZE 20
#define ERR_STR_LEN 40

#ifndef INFINITY
	#ifdef HUGE_VALF
		#define INFINITY HUGE_VALF
	#else
		#include <float.h>
		#define INFINITY FLT_MAX
	#endif
#endif

static void vnhsi_util_ymr_handler(void *userData, struct VnUartPacket *packet, size_t runningIndex);

typedef struct VnHsiUtilDataCollection {
	unsigned int maxKeptMeas;
	unsigned int numKeptMeas;
	VnHsiYmr* kept;
	VnHsiConfig* c;
	VnHsiUtilConfig* uc;
	FILE* rawdatafp;
} VnHsiUtilDataCollection;


enum VnError vnhsi_util_main_options(int argc, char *argv[], VnHsiUtilConfig* uc)
{
	enum VnError r = E_NONE;

	if(argc == 1)
		uc->helpFlag = true;
	else {
		unsigned int c1;
		for(c1=1;c1<strlen(argv[1]);c1++) {
			switch(argv[1][c1]) {
				case 'P': case 'p':
					uc->liveFlag = false;
					break;
				case 'L': case 'l':
					uc->liveFlag = true;
					break;
				case 'S': case 's':
					uc->seqFlag = true;
					break;
				case 'H': case 'h':
					uc->helpFlag = true;
					break;
				default:
					return vnhsi_util_print_error("Invalid command option",E_NOT_SUPPORTED);
			}
		}
	}
	
	if(uc->helpFlag)
	{
		/* read the README.txt file here */
		char buf[1024];
		size_t nread;
		FILE* f = fopen("README.txt","r");
		if (!f)	
			return vnhsi_util_print_error("Couldn't open the README file!", E_UNKNOWN);		
		else{
			while ((nread = fread(buf, 1, sizeof buf, f)) > 0)
				fwrite(buf, 1, nread, stdout);
		}
		fclose(f);
	}
	else if(uc->liveFlag)
	{
		switch(argc) {
			case 7:
				uc->measFilename = argv[6];
			case 6:
				uc->outputFilename = argv[5];
			case 5:
				uc->configFilename = argv[4];
				uc->baudRate = atoi(argv[3]);
				uc->comPort = argv[2];
				break;
			default:
				r = vnhsi_util_print_error("Invalid arguments.",E_NOT_SUPPORTED);
		}
	}
	else
	{
		switch(argc) {
			case 5:
				uc->outputFilename = argv[4];
			case 4:
				uc->configFilename = argv[3];
				uc->measFilename = argv[2];
				break;
			default:
				r = vnhsi_util_print_error("Invalid arguments.",E_NOT_SUPPORTED);
		}
	} 
	
	return r;
}

enum VnError vnhsi_util_load_config(const char* fn, VnHsiConfig* c)
{
	char readbuf[256];
	char* loc;
		
	FILE* f = fopen(fn, "rb");
	if (!f)	
		return vnhsi_util_print_error("Couldn't open the configuration file!", E_UNKNOWN);		
		
	while ((loc = fgets(readbuf, sizeof(readbuf), f)) != NULL)
	{
		char name[100], val[100];
		sscanf(loc, "%s = %s", name, val);

		if (!strcmp("mode", name))
		{
			sscanf(val, "%u", &c->mode);
			if (c->mode >= HSI_MODE_MAX)
				return vnhsi_util_print_error("The mode exceeds the maximum value!", E_INVALID_VALUE);		
		}
		else if (!strcmp("flagKM", name))
		{
			sscanf(val, "%u", &c->flagKM);
			if (c->flagKM != 1 && c->flagKM != 2 && c->flagKM != 3)
				return vnhsi_util_print_error("Invalid keep measurement flag!", E_INVALID_VALUE);		
		}
		else if (!strcmp("Latitude", name))
		{
			sscanf(val, "%f", &c->latitude);
			if (c->latitude < -90.f || c->latitude > 90.f)
				return vnhsi_util_print_error("Invalid latitude value!", E_INVALID_VALUE);		
		}
		else if (!strcmp("Longitude", name))
		{
			sscanf(val, "%f", &c->longitude);
			if (c->longitude < -180.f || c->longitude > 180.f)
				return vnhsi_util_print_error("Invalid longitude value!", E_INVALID_VALUE);		
		}
		else if (!strcmp("DataRate", name))
		{
			sscanf(val, "%u", &c->dataRate);
			if (c->dataRate < 0 || c->dataRate > 1000)
				return vnhsi_util_print_error("Invalid data rate value!", E_INVALID_VALUE);		
		}
		else if (!strcmp("nPatches", name))
		{
			sscanf(val, "%u", &c->nPatches);
			if(c->nPatches < 32)
				return vnhsi_util_print_error("Invalid number of patches value!", E_INVALID_VALUE);		
		}
		else if (!strcmp("nDataPerPatch", name))
		{
			sscanf(val, "%u", &c->nDataPerPatch);
			if (c->nDataPerPatch < 0 || c->nDataPerPatch > 10)
				return vnhsi_util_print_error("Invalid number of points per patch value!", E_INVALID_VALUE);		
		}
		else if (!strncmp("RFR", name, 3) && strlen(name) == 5)
		{
			float rfr_val;
			int i = name[3] - '1';
			int j = name[4] - '1';

			if (i < 0 || i > 2 || j < 0 || j > 2)
				return vnhsi_util_print_error("Invalid RFR tag!", E_INVALID_VALUE);		
			sscanf(val, "%f", &rfr_val);
			c->RFR.e[i + 3*j] = rfr_val;
			if (rfr_val < -1.f || rfr_val > 1.f)
				return vnhsi_util_print_error("Invalid RFR value!", E_INVALID_VALUE);	
		}
		else if (!strncmp("SI", name, 2) && strlen(name) == 4)
		{
			int i = name[2] - '1';
			int j = name[3] - '1';

			if (i < 0 || i > 2 || j < 0 || j > 2)
				return vnhsi_util_print_error("Invalid SI tag!", E_INVALID_VALUE);		

			sscanf(val, "%f", &c->oldSI.e[i + 3*j]);
		}
		else if (!strncmp("HI", name, 2) && strlen(name) == 3)
		{
			size_t i = name[2] - '1';

			if (i < 0 || i > 2)
				return vnhsi_util_print_error("Invalid HI tag!", E_INVALID_VALUE);		

			sscanf(val, "%f", &c->oldHI.c[i]);
		}
		else
			return vnhsi_util_print_error("Invalid config item name!", E_INVALID_VALUE);		
	}
	
	fclose(f);

	return E_NONE;
}

void vnhsi_util_save_results(VnHsiUtilConfig uc, VnHsiYmr* meas, size_t numMeas, union vn_mat3f si, union vn_vec3f hi, float sphereResiduals[], float vertResiduals[], VnHsiFom fom, unsigned int fomScore[8])
{
	unsigned int j;

	/* Write results to output file */
	FILE* f = fopen(uc.outputFilename,"w+");
	fprintf(f, "The HSI results for the measured data from '%s' using the configuration file '%s'\n", uc.measFilename, uc.configFilename);

	vnhsi_util_print_hsi(f, si, hi);
	vnhsi_util_print_fom(f, fom, fomScore);

	fprintf(f,"\n The selected measurement data are \n");
	if(sphereResiduals == NULL || vertResiduals == NULL) {
		fprintf(f, "\nIndex\t\t\tYaw(deg)\t\tPitch(deg)\t\tRoll(deg)\t\tMagX(Gauss)\t\tMagY(Gauss)\t\tMagZ(Gauss)\t\tAngRateX(deg/s)\tAngRateY(deg/s)\tAngRateZ(deg/s)\n");
		for ( j=0; j < numMeas ; j++){
			fprintf(f, "%4u\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\n",(unsigned int)meas[j].index, meas[j].ypr.c[0], meas[j].ypr.c[1],meas[j].ypr.c[2],meas[j].mag.c[0],meas[j].mag.c[1],meas[j].mag.c[2], meas[j].gyro.c[0],meas[j].gyro.c[1],meas[j].gyro.c[2]);
		}
	} else {
		fprintf(f, "\nIndex\t\t\tResSphere\t\tResVert\t\t\tYaw(deg)\t\tPitch(deg)\t\tRoll(deg)\t\tMagX(Gauss)\t\tMagY(Gauss)\t\tMagZ(Gauss)\t\tAngRateX(deg/s)\tAngRateY(deg/s)\tAngRateZ(deg/s)\n");
		for ( j=0; j < numMeas ; j++){
			fprintf(f, "%4u\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\t%14.3f\n",(unsigned int)meas[j].index, sphereResiduals[j], vertResiduals[j], meas[j].ypr.c[0], meas[j].ypr.c[1],meas[j].ypr.c[2],meas[j].mag.c[0],meas[j].mag.c[1],meas[j].mag.c[2], meas[j].gyro.c[0],meas[j].gyro.c[1],meas[j].gyro.c[2]);
		}
	}
	
    fclose(f);
}

enum VnError vnhsi_util_sensor_init(struct VnSensor* vs, char* comPort, unsigned int baudRate, VnHsiConfig* config)
{
	enum VnError error = E_NONE;
	
	struct VnMagnetometerCalibrationControlRegister hsiConfigOff = {
				VNHSIOUTPUT_NOONBOARD, 	/*hsiOutput*/
				VNHSIMODE_OFF, 			/*hsiMode*/
				5 						/*convergeRate*/
				}; 
	
	struct VnMagnetometerCompensationRegister magcomp;
	union vn_mat3f	RFR;
	uint32_t adof;
	
	unsigned int c1;
	unsigned int sleepNsec = 5;
	
	/* Set response timeouts and retransmit delays */
	error = VnSensor_setResponseTimeoutMs(vs, 1000);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_setResponseTimeoutMs()", error);	
		
	error = VnSensor_setRetransmitDelayMs(vs, 300);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_setRetransmitDelayMs()", error);	
	
	/* Connect to the sensor */  
	printf("\n Connecting to the sensor...\n");
	error = VnSensor_connect(vs,comPort,baudRate);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_connect()", error);
	VnThread_sleepMs(100);
	
	/*   Restore Factory Settings and change comPort baud rate to factory default 115200*/
	printf("\n Issuing Restore Factory Settings command...\n");
	if ((error = VnSensor_restoreFactorySettings(vs, true)) != E_NONE){
		vnhsi_util_print_error("VnSensor_restoreFactorySettings()", error);
		return error;}
	VnThread_sleepMs(1000);
	
	printf("\n Changing comPort to 115200 baud...\n");
	error = VnSensor_changeBaudrate_noSensorCommand(vs, 115200);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_changeBaudrate_noSensorCommand()", error);
	VnThread_sleepMs(1000);
		
	/* Turn off the Async Data Output Type  */
	printf("\n Turning off the Async Data Output Type  \n");
	error = VnSensor_writeAsyncDataOutputType(vs, VNOFF , true);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_writeAsyncDataOutputType()", error);
	VnThread_sleepMs(100);

	/* Read the RFR, HSI, and Async Data Freq registers and store in config struct */
	printf("\n Reading Reference Frame Rotation and storing in config struct...\n");
	error = VnSensor_readReferenceFrameRotation(vs,&RFR);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_readReferenceFrameRotation()", error);
	VnThread_sleepMs(100);

	printf("\n Reading current hard- and soft-iron and storing in config struct...\n");
	error = VnSensor_readMagnetometerCompensation(vs,&magcomp);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_readMagnetometerCompensation()", error);
	VnThread_sleepMs(100);

	printf("\n Reading async data output frequency and storing in config struct...\n");
	error = VnSensor_readAsyncDataOutputFrequency(vs,&adof);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_readAsyncDataOutputFrequency()", error);
	VnThread_sleepMs(100);	
	config->dataRate = (unsigned int)adof;
	
	config->RFR = RFR;
	config->oldSI = magcomp.c;
	config->oldHI =  magcomp.b;		
	
	/* Turn off on-board real-time HSI */
	printf("\n Turning off on-board real-time HSI...\n");
	error = VnSensor_writeMagnetometerCalibrationControl(vs, hsiConfigOff, true);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_writeMagnetometerCalibrationControl()", error);
			
	/* Setting Async Data Output to YMR... */
	printf("\n Setting Async Data Output to YMR... \n\n");
	error = VnSensor_writeAsyncDataOutputType(vs, VNYMR , true);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_writeAsyncDataOutputType()", error);
	VnThread_sleepMs(100);
	
	for (c1 = 0; c1 <= sleepNsec; c1++) {
		printf(" Keep sensor stationary to stabilize for %d seconds ... " , sleepNsec-c1);
		VnThread_sleepMs(1000); 
		fflush(stdout);	printf("\r");		
	}

	printf("\n");

	return error;	
}

enum VnError vnhsi_util_sensor_finalize(struct VnSensor* vs, union vn_mat3f si, union vn_vec3f hi)
{
	enum VnError error = E_NONE;

	bool flagSaveHSI;
	char saveYN;
	
	printf("\n Write HSI solution to sensor and save settings to non-volatile memory? (y/n): ");
	saveYN = getchar();
	switch(saveYN) {
		case 'y':
		case 'Y':
			flagSaveHSI = true;
			break;
		case 'n':
		case 'N':
			flagSaveHSI = false;
			break;
		default:
			flagSaveHSI = false;
			printf("\n Invalid entry. HSI solution not saved to sensor.\n");
	}

	if (flagSaveHSI)
	{
		struct VnMagnetometerCompensationRegister magcomp;
		init_m3f_fa(&magcomp.c, si.e);
		init_v3f_fa(&magcomp.b, hi.c);
    
		printf("\n Writing the new HSI results to the sensor...\n");
		error = VnSensor_writeMagnetometerCompensation(vs, magcomp, true);
		if (error != E_NONE)
			return vnhsi_util_print_error("VnSensor_writeMagnetometerCompensation()", error);
	 
		printf("\n Writing settings to non-volatile memory...\n");
		error = VnSensor_writeSettings(vs, true);
		if (error != E_NONE)
			return vnhsi_util_print_error("VnSensor_writeSettings()", error);
		 
		printf("\n Resetting the sensor...\n");
		error = VnSensor_reset(vs, true);
		if (error != E_NONE)
			return vnhsi_util_print_error("VnSensor_reset()", error);

		VnThread_sleepMs(1000);
	}

	printf("\n Disconnecting from the sensor...\n");
	error = VnSensor_disconnect(vs);
	if (error != E_NONE)
		return vnhsi_util_print_error("VnSensor_disconnect()", error);

	return error;	
}

enum VnError vnhsi_util_print_error(char* errorMessage, enum VnError errorCode)
{
	char errorCodeStr[ERR_STR_LEN];
	
	to_string_VnError(errorCodeStr, sizeof(errorCodeStr), errorCode);
	printf("\tERROR (%04d):\t%s\n\t\t\t%s\n", errorCode, errorCodeStr, errorMessage);
	
	return errorCode;
}

void vnhsi_util_print_fom(FILE* fp, VnHsiFom fom, unsigned int fomScore[8])
{
	const char *fomEvalText[] = {"Great","Good","Fair","Poor"};

	fprintf(fp,"\nEllipsoid Fit Residuals (Percentage)");
	fprintf(fp,"\nMean:              \t%6.2f %%\t\t%-10s", fom.ellipsoidFitResidualsMean*100, fomEvalText[fomScore[0]]);
	fprintf(fp,"\nMaximum:           \t%6.2f %%\t\t%-10s", fom.ellipsoidFitResidualsMax*100, fomEvalText[fomScore[1]]);
	fprintf(fp,"\nStandard Deviation:\t%6.2f %%\t\t%-10s", fom.ellipsoidFitResidualsStd*100, fomEvalText[fomScore[2]]);

	fprintf(fp,"\n\n\nAlignment Fit Residuals (Percentage)");
	fprintf(fp,"\nMean:              \t%6.2f %%\t\t%-10s", fom.alignmentFitResidualsMean*100, fomEvalText[fomScore[3]]);
	fprintf(fp,"\nMaximum:           \t%6.2f %%\t\t%-10s", fom.alignmentFitResidualsMax*100, fomEvalText[fomScore[4]]);
	fprintf(fp,"\nStandard Deviation:\t%6.2f %%\t\t%-10s", fom.alignmentFitResidualsStd*100, fomEvalText[fomScore[5]]);

	fprintf(fp,"\n\nEllipsoid fit coverage multiplier:\t%8.2f\t\t%-10s", fom.ellipsoidCoverage, fomEvalText[fomScore[6]]);
	fprintf(fp,"\nAlignment fit coverage multiplier:\t%8.2f\t\t%-10s\n", fom.alignmentCoverage, fomEvalText[fomScore[7]]);
}

void vnhsi_util_print_hsi(FILE* fp, union vn_mat3f si, union vn_vec3f hi)
{
	unsigned int c1;
	
	fprintf(fp, "\n The SI Matrix is \n");
	for (c1=0; c1 < 3 ; c1++) 
		fprintf(fp, "%10.6f   %10.6f   %10.6f\n", si.e[c1], si.e[c1 + 3], si.e[c1 + 6]);
	
	fprintf(fp, "\n The HI Vector is \n");
	for (c1=0; c1 < 3 ; c1++) 
		fprintf(fp, "%10.6f  \n", hi.c[c1]);
}

enum VnError vnhsi_util_collect_meas(struct VnSensor* vs, VnHsiConfig* c, VnHsiUtilConfig* uc, VnHsiYmr* kept, unsigned int* numKept)
{
	enum VnError r = E_NONE;
	
	VnHsiUtilDataCollection dc;


	FILE* datainfile;
	uint8_t readBuf[FILE_LOAD_BUF_SIZE];

	dc.numKeptMeas = 0;
	dc.maxKeptMeas = *numKept;
	dc.kept = kept;
	dc.rawdatafp = NULL;
	dc.c = c;
	dc.uc = uc;
	
	if(uc->liveFlag)
	{
		if(uc->measFilename)			/* save the YMR to a text file */
			dc.rawdatafp = fopen(uc->measFilename,"w+");
	
		printf("\n Press any key to start collecting measurements....\n");
		getchar();
		
		printf("\n Issuing Known Magnetic Disturbance Command...\n");
		r = VnSensor_magneticDisturbancePresent(vs, true, true);
		if ((r != E_NONE))
			return vnhsi_util_print_error("VnSensor_magneticDisturbancePresent()", r);
	}
	else
	{
		datainfile = fopen(uc->measFilename,"rb");
		if (!datainfile) return vnhsi_util_print_error("Invalid raw data filename.",E_UNKNOWN);
	}
		
	r = VnSensor_registerAsyncPacketReceivedHandler(vs, vnhsi_util_ymr_handler, &dc);
    if (r != E_NONE ) 
		return vnhsi_util_print_error("VnSensor_registerAsyncPacketReceivedHandler()", r); 
	
#ifndef _MSC_VER
	if (uc->seqFlag == 1)
		printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
	if (uc->liveFlag == 1)
		printf("\n");
	printf("\n\n\n");
	fflush(stdout);  
#endif

	if(uc->liveFlag)
	{
		getchar();
		
		r = VnSensor_unregisterAsyncPacketReceivedHandler(vs);
		if (r != E_NONE ) 
			return vnhsi_util_print_error("VnSensor_unregisterAsyncPacketReceivedHandler()", r); 
		
		if (dc.rawdatafp)			
			fclose(dc.rawdatafp);
	}
	else
	{
		while(true)
		{
			size_t bytesRead = fread(readBuf, sizeof(uint8_t), FILE_LOAD_BUF_SIZE, datainfile);
			
			if (bytesRead == 0)
				break;
			VnUartPacketFinder_processData(&vs->packetFinder, readBuf, bytesRead);
		}
		fclose(datainfile);
	}
	
	*numKept = dc.numKeptMeas;
		
	return r;	
}

void vnhsi_util_ymr_handler(void *userData, struct VnUartPacket *packet, size_t runningIndex)
{

	enum VnError r = E_NONE;	

	VnHsiYmr data;
	static unsigned int Curindex = 0;
	
	VnHsiUtilDataCollection* dc = userData;
	
	/* Parse the VNYMR message. */
	if (VnUartPacket_determineAsciiAsyncType(packet) == VNYMR)
	{
		r = VnUartPacket_parseVNYMR(packet,&data.ypr,&data.mag,&data.acc,&data.gyro);
		if(r != E_NONE) return;
		data.index = Curindex;
		Curindex++;
	}
	else
		return;
	
	if(dc->rawdatafp)
	{
		unsigned int c1;
		for(c1=0;c1<packet->length;c1++)
			fprintf(dc->rawdatafp,"%c",packet->data[c1]);
	}
	
	if (vnhsi_keep(dc->c, data))
	{
		dc->kept[dc->numKeptMeas] = data;
		dc->numKeptMeas++;
			
		if (dc->uc->seqFlag)
		{
			VnHsiSol sol; 						/* HSI solution structure. */
			VnHsiFom fom; 						/* figure of merit struct  */
			VnHsiFom fomFail = {INFINITY, INFINITY, INFINITY, INFINITY, INFINITY, INFINITY, INFINITY, INFINITY};
			unsigned int fomScore[8];
			char errorCodeStr[ERR_STR_LEN];
			
			r = vnhsi_solve(dc->c, dc->kept, dc->numKeptMeas, &sol);
			if (r != E_NONE)
			{ 
				unsigned int c1;
				for(c1=0;c1<8;c1++) 
					fomScore[c1] = vnhsi_get_max_fomScore();
				fom = fomFail;
			}
			else 
			{ 
				/* Compute Figures of Merit */
				vnhsi_figure_of_merit(dc->c, dc->kept, dc->numKeptMeas, sol, &fom, fomScore, NULL, NULL, NULL);
			}
			
			to_string_VnError(errorCodeStr, sizeof(errorCodeStr), r);
#ifndef _MSC_VER
			printf("\033[20A\33[2K\r");
#endif
			printf("\n Number of measurements: \n\tParsed: %4d\tKept: %4d\n", Curindex,dc->numKeptMeas);
			printf("\n vnhsi_solve error code: %-50s \n",errorCodeStr);
			
			vnhsi_util_print_fom(stdout,fom,fomScore);
			printf("\n");
		}
		else {
#ifndef _MSC_VER
			printf("\033[4A\33[2K\r");	
#endif
			printf("\n Number of measurements: \n\tParsed: %4d\tKept: %4d\n\n", Curindex,dc->numKeptMeas);
		}	
		if(dc->uc->liveFlag)
			printf(" Press any key to stop the measurements...");
		fflush(stdout);
	}
}