/* 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.
 */
#include "vnhsi.h"
#include "vnhsiexampleutil.h"

#define MAXKEPTPOINTS 3200			/* maximum number of points to keep for HSI computation */
#define TXTBUFSIZE 200

int main(int argc, char *argv[])
{
	
	enum VnError r = E_NONE;
	
	VnHsiYmr meas;
	VnHsiYmr kept[MAXKEPTPOINTS]; 		/* Kept data saved for HSI computation. */
	unsigned int numKeptMeas = 0;

	VnHsiConfig c = {	/* configuration for the HSI algorithm run. */
					HSI_MODE_3D, 			/* mode */
					{{1,0,0,0,1,0,0,0,1}},	/* oldSI */
					{{0,0,0}}, 				/* oldHI */
					{{1,0,0,0,1,0,0,0,1}},	/* RFR */
					31.9f,					/* latitude */
					34.8f,					/* longitude */
					100,					/* dataRate */
					122,					/* nPatches */
					10,						/* nDataPerPatch */
					2						/* flagKM */
					};

	VnHsiSol sol; 						/* HSI solution structure. */
	VnHsiFom fom; 						/* figure of merit struct  */
	unsigned int fomScore[8];

	union vn_mat3f hsi_si;		/* final soft-iron compensation matrix */
	union vn_vec3f hsi_hi;		/* final hard-iron compensation vector */
	
	FILE* fp;
	char filename[] = "example3D.dat";
	char txtbuf[TXTBUFSIZE];
	unsigned int meascnt = 0;
	
	struct VnUartPacket packet; 
	packet.data = (const uint8_t*)txtbuf;
	packet.length = 0;		
	
	/* initialize VNHSI parameters */
	vnhsi_init(&c);
		
	/* collect measurements and store in kept-buffer */
	fp = fopen(filename,"r");
	while(fgets(txtbuf,TXTBUFSIZE,fp) && numKeptMeas < MAXKEPTPOINTS)
	{
		r = VnUartPacket_parseVNYMR(&packet,&meas.ypr,&meas.mag,&meas.acc,&meas.gyro);
		if(r != E_NONE) return vnhsi_util_print_error("VnUartPacket_parseVNYMR()", r);

		meas.index = meascnt++;
		if (vnhsi_keep(&c, meas))
		{
			kept[numKeptMeas] = meas;
			numKeptMeas++;
		}
	}
	fclose(fp);
	
	/* solve for HSI solution */
	r = vnhsi_solve(&c, kept, numKeptMeas, &sol);
	if(r != E_NONE) return vnhsi_util_print_error("vnhsi_solve()", r); 
	
	/* compute Figures of Merit */
	vnhsi_figure_of_merit(&c, kept, numKeptMeas, sol, &fom, fomScore, NULL, NULL, NULL);
		
	/* transform HSI results to account for pre-existing HSI and RFR parameters in use when data was collected */
	vnhsi_results(&c, &hsi_si, &hsi_hi, sol);

	/* Print results to screen */
	printf("\n\nFinal Solution:\n");
	vnhsi_util_print_hsi(stdout, hsi_si, hsi_hi);
	vnhsi_util_print_fom(stdout, fom, fomScore);
	
    return E_NONE;
}