/* 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 SAVERESIDUALS				/* saves array of residual values and prints to file - comment out if not desired */

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

	VnHsiConfig c;						/* configuration for the HSI algorithm run. */
	VnHsiSol sol; 						/* HSI solution structure. */
	VnHsiFom fom; 						/* figure of merit struct  */
	unsigned int fomScore[8];

	union vn_mat3f hsi_si;		/* computed soft-iron compensation matrix */
	union vn_vec3f hsi_hi;		/* computed hard-iron compensation vector */
	
	VnHsiUtilConfig uc = {NULL,NULL,NULL,NULL,0,false,false,false};
	
	struct VnSensor vs;
	
	#ifdef SAVERESIDUALS
		float sphereResiduals[MAXKEPTPOINTS];
		float vertResiduals[MAXKEPTPOINTS];
		float correctedMagMeas[MAXKEPTPOINTS][3];
	#else
		float *sphereResiduals = NULL;
		float *vertResiduals = NULL;
		float (*correctedMagMeas)[3] = NULL;
	#endif	

	/* parse input arguments */
	r = vnhsi_util_main_options(argc, argv, &uc);
	if (r != E_NONE) return vnhsi_util_print_error("processInputArguments()",r); 

	if(uc.helpFlag) 
		return 0;
		
	/* load calibration configuration from file (see README for definition) */
	r = vnhsi_util_load_config(uc.configFilename,&c);
	if (r != E_NONE) return vnhsi_util_print_error("vnhsi_util_load_config()",r); 
	
	/* initialize VNHSI parameters */
	vnhsi_init(&c);
		
	/* initialize sensor struct to enable data collection & parsing */
	VnSensor_initialize(&vs);

	/* for a live data-collection, initialize sensor settings */
	if(uc.liveFlag) 
	{
		r = vnhsi_util_sensor_init(&vs,uc.comPort,uc.baudRate,&c);
		if(r != E_NONE) return vnhsi_util_print_error("initializeVNsensor()", r);	
	}
	
	/* collect measurements and store in kept-buffer */
	r = vnhsi_util_collect_meas(&vs,&c,&uc,kept,&numKeptMeas);
	if(r != E_NONE) return vnhsi_util_print_error("collectMeasurements()", r);	
	
	/* 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, sphereResiduals, vertResiduals, correctedMagMeas);
		
	/* 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);
	
	/* save results to a file if desired */
	if(uc.outputFilename)
		vnhsi_util_save_results(uc, kept, numKeptMeas, hsi_si, hsi_hi, sphereResiduals, vertResiduals, fom, fomScore);
   
	/* for a live data-collection, finalize sensor settings and/or disconnect */
	if(uc.liveFlag) 
	{
		r = vnhsi_util_sensor_finalize(&vs, hsi_si, hsi_hi);
		if(r != E_NONE)	return vnhsi_util_print_error("finalizeVNsensor()", r);
	}
	
    return E_NONE;
}