/* 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 "vnhsidipole.h"
#include "vnhsi2d.h"
#include "vnhsi3d.h"
#include "vnhsimath.h"
#include "vnamath.h"

static const VnHsiFom fomEvalThresholds[] = {
		{0.0001f, 0.015f, 0.005f, 0.0001f, 0.015f, 0.005f, 0.1f, 0.1f},	/* Great FOMs values defined in Readme*/
		{0.0005f, 0.03f, 0.01f, 0.0005f, 0.03f, 0.01f, 0.2f, 0.2f},		/* Good FOMs values defined in Readme*/
		{0.002f, 0.06f, 0.02f, 0.002f, 0.06f, 0.02f, 0.5f, 0.5f}};		/* Fair FOMs values defined in Readme*/

#ifdef MAXPATCHES
	#include "vnhsipatches.h"
#endif

/** \brief Evaluate the figures of merit based on pre-defined values.
*
* \param[in] fom  The figures of merit.
* \param[out] fomScore  Score 0-3 for each FOM based on predefined thresholds (0-Great,3-Poor).
* \return The indices of the FOM evaluations. */
static void vnhsi_evaluate_fom(VnHsiFom* fom, unsigned int fomScore[8]);

void vnhsi_init(VnHsiConfig* c)
{
	VnHsiDipole dp;

	if(c->mode==HSI_MODE_3D_NOLOCATION || c->mode==HSI_MODE_2D_NOLOCATION)
	{
		c->latitude = 0.0f;
		c->longitude = 0.0f;
	}
		
	dp = vnhsi_dipole_init(c);
	
	switch(c->mode) {
		case HSI_MODE_3D: 					/* standard 3D */
		case HSI_MODE_3D_NOLOCATION: 		/* no location 3D */
			vnhsi_3d_init(dp,c);
			break;
		case HSI_MODE_2D: 					/* standard 2D */
		case HSI_MODE_2D_NOLOCATION: 		/* no location 2D */
		case HSI_MODE_2D_SCALE_S33: 		/* same as standard 2D with scale S33 2D */
			vnhsi_2d_init(dp,c);
			break;
		default:
			return;
	}
	
}

bool vnhsi_keep(VnHsiConfig* c, VnHsiYmr meas)
{
	VnHsiDipole dp = vnhsi_dipole_get_dipole(false);

	switch(c->mode)
	{
		case HSI_MODE_3D: 				/* standard 3D */
		case HSI_MODE_3D_NOLOCATION: 	/* no location 3D */
		{
			return vnhsi_3d_keep(dp, c, meas.ypr);
		}
		case HSI_MODE_2D: 				/* standard 2D */
		case HSI_MODE_2D_NOLOCATION: 	/* no location 2D */
		case HSI_MODE_2D_SCALE_S33: 	/* same as standard 2D with scale S33 2D */
		{
			return vnhsi_2d_keep(dp, c, meas.ypr, meas.gyro);
		}
		default:
			return false;
	}
}

enum VnError vnhsi_solve(VnHsiConfig* c, VnHsiYmr* meas, size_t numMeas, VnHsiSol* hsisoln)
{
	enum VnError ret;
	VnHsiDipole dp = vnhsi_dipole_get_dipole(false);

	switch(c->mode)
	{
		case HSI_MODE_3D: /* standard 3D */
		case HSI_MODE_3D_NOLOCATION: /* no location 3D */
			ret = vnhsi_3d_solve(dp, c, meas, NULL, numMeas, hsisoln, NULL);
			if(ret==E_NONE) vnhsi_dipole_update(hsisoln->magInc,false);
			break;
		case HSI_MODE_2D: /* standard 2D */
		case HSI_MODE_2D_NOLOCATION: /* no location 2D */
		case HSI_MODE_2D_SCALE_S33: /* scale S33 2D */
			ret = vnhsi_2d_solve(dp, c, meas, NULL, numMeas, hsisoln, NULL);
			if(ret==E_NONE) vnhsi_dipole_update(hsisoln->magInc,true);
			break;
		default:
			ret = E_UNKNOWN;
			break;
	}
	
	return ret;
}
	
void vnhsi_figure_of_merit(VnHsiConfig* c, VnHsiYmr* meas, size_t numMeas, VnHsiSol hsisoln, VnHsiFom* FOM, unsigned int fomScore[8], float* sphereResiduals, float* vertResiduals, float(*correctedMag)[3])
{
	unsigned int c1 ,k;
	union vn_mat3f DCM;
	union vn_vec3f mhat,e, gr, gravityVec;
	float sphereRes, vertRes;
	float sphereVar = 0.0, vertVar = 0.0, dif = 0.0;
	VnHsiDipole dp = vnhsi_dipole_get_dipole(true);
	
	VnHsiSol hsisoln2;
	switch(c->mode)
	{
		case HSI_MODE_3D: /* standard 3D */
		case HSI_MODE_3D_NOLOCATION: /* no location 3D */
			vnhsi_3d_solve(dp, c, meas, &hsisoln, numMeas, &hsisoln2, FOM);
			break;
		case HSI_MODE_2D: /* standard 2D */
		case HSI_MODE_2D_NOLOCATION: /* no location 2D */
		case HSI_MODE_2D_SCALE_S33: /* scale S33 2D */
			vnhsi_2d_solve(dp, c, meas, &hsisoln, numMeas, &hsisoln2, FOM);
			break;
		default:
			break;
	}
	
	FOM->ellipsoidFitResidualsMean = 0.0f;
	FOM->ellipsoidFitResidualsMax = 0.0f;
	FOM->ellipsoidFitResidualsStd = 0.0f;
	
	FOM->alignmentFitResidualsMean = 0.0f;
	FOM->alignmentFitResidualsMax = 0.0f;
	FOM->alignmentFitResidualsStd = 0.0f;

	gravityVec.c[0]=gravityVec.c[1]=0.0f;
	gravityVec.c[2]= 1.0f;

	for ( c1=0; c1<numMeas;c1++){
		/* calculate the gravity vector from the ypr */
		for ( k=0;k<3;k++){
			e.c[k]=meas[c1].ypr.c[k] * VN_PIF/180.0f;			/* the YPR in rad */
		}
		Euler3212C(DCM.e, e.c);
		vn_mult_mn((float*)DCM.e, gravityVec.c, 3, 3, 1, gr.c);

		mhat = vnhsi_ApplyHSI(&hsisoln, meas[c1].mag);
		
		sphereRes = sqrtf(mhat.c[0]*mhat.c[0] + mhat.c[1]*mhat.c[1] + mhat.c[2]*mhat.c[2])/dp.sphereRad - 1.0f;
		
		vertRes = (mhat.c[0]*gr.c[0] + mhat.c[1]*gr.c[1] + mhat.c[2]*gr.c[2] - dp.Bdown)/dp.sphereRad;
		
		if(sphereResiduals != NULL)
			sphereResiduals[c1] = sphereRes;
		
		if(vertResiduals != NULL)
			vertResiduals[c1] = vertRes;

		if(correctedMag != NULL) {
			correctedMag[c1][0] = mhat.c[0];
			correctedMag[c1][1] = mhat.c[1];
			correctedMag[c1][2] = mhat.c[2];
		}
		
		dif = sphereRes - FOM->ellipsoidFitResidualsMean;
		sphereVar = (sphereVar*(float)c1 + dif*dif*(float)c1/(float)(c1+1))/(float)(c1+1);
		FOM->ellipsoidFitResidualsMean += dif/(float)(c1+1);
		if(fabsf(sphereRes) > FOM->ellipsoidFitResidualsMax)
			FOM->ellipsoidFitResidualsMax = fabsf(sphereRes);
		
		dif = vertRes - FOM->alignmentFitResidualsMean;
		vertVar = (vertVar*(float)c1 + dif*dif*(float)c1/(float)(c1+1))/(float)(c1+1);
		FOM->alignmentFitResidualsMean += dif/(float)(c1+1);
		if(fabsf(vertRes) > FOM->alignmentFitResidualsMax)
			FOM->alignmentFitResidualsMax = fabsf(vertRes);

	}
	FOM->ellipsoidFitResidualsStd = sqrtf(sphereVar);
	FOM->alignmentFitResidualsStd = sqrtf(vertVar);
	
	vnhsi_evaluate_fom(FOM,fomScore);
}

void vnhsi_results(VnHsiConfig* c, union vn_mat3f *si, union vn_vec3f *hi, VnHsiSol hsisoln)
{
	union vn_vec3f tmphi1, tmphi2;
	union vn_mat3f RFRT, tmp, tmp2;
	
	vn_tranpose_m3((float*)c->RFR.e, (float*)RFRT.e);
		
	vn_mult_m3_m3((float*)RFRT.e, (float*)hsisoln.SI.e, (float*)tmp.e);
	vn_mult_m3_m3((float*)tmp.e, (float*)c->RFR.e, (float*)tmp2.e);
	vn_mult_m3_m3((float*)tmp2.e, (float*)c->oldSI.e, (float*)si->e);

	vn_inverse_m3((float*)c->oldSI.e, (float*)tmp.e);
	vn_mult_mn((float*)RFRT.e, hsisoln.HI.c, 3, 3, 1, tmphi1.c);
	vn_mult_mn((float*)tmp.e, tmphi1.c, 3, 3, 1, tmphi2.c);
	vn_add_v3(c->oldHI.c, tmphi2.c, hi->c);

}

static void vnhsi_evaluate_fom(VnHsiFom* fom, unsigned int fomScore[8])
{
	int c1;
	unsigned int maxScore = vnhsi_get_max_fomScore();
	
	for(c1=0;c1<8;c1++)
		fomScore[c1] = maxScore;

	for(c1=maxScore-1;c1>=0;c1--)
	{
		if(fabsf(fom->ellipsoidFitResidualsMean) <= fomEvalThresholds[c1].ellipsoidFitResidualsMean)
			fomScore[0] = c1;
		if(fabsf(fom->ellipsoidFitResidualsMax) <= fomEvalThresholds[c1].ellipsoidFitResidualsMax)
			fomScore[1] = c1;
		if(fabsf(fom->ellipsoidFitResidualsStd) <= fomEvalThresholds[c1].ellipsoidFitResidualsStd)
			fomScore[2] = c1;
		if(fabsf(fom->alignmentFitResidualsMean) <= fomEvalThresholds[c1].alignmentFitResidualsMean)
			fomScore[3] = c1;
		if(fabsf(fom->alignmentFitResidualsMax) <= fomEvalThresholds[c1].alignmentFitResidualsMax)
			fomScore[4] = c1;
		if(fabsf(fom->alignmentFitResidualsStd) <= fomEvalThresholds[c1].alignmentFitResidualsStd)
			fomScore[5] = c1;
		if(fabsf(fom->ellipsoidCoverage) <= fomEvalThresholds[c1].ellipsoidCoverage)
			fomScore[6] = c1;
		if(fabsf(fom->alignmentCoverage) <= fomEvalThresholds[c1].alignmentCoverage)
			fomScore[7] = c1;
	}
}

unsigned int vnhsi_get_max_fomScore(void)
{
	return sizeof(fomEvalThresholds)/sizeof(fomEvalThresholds[0]);
}

union vn_vec3f vnhsi_unit_mag_ref(VnHsiDipole dp) 
{
	union vn_vec3f out;
	
	out.c[0] = dp.Bnorth/dp.sphereRad;
	out.c[1] = 0.0;
	out.c[2] = dp.Bdown/dp.sphereRad;
	
	return out;
}

VnHsiDipole vnhsi_get_dipole(bool updatedFlag)
{
	return vnhsi_dipole_get_dipole(updatedFlag);
}

#ifdef MAXPATCHES
VnHsiPatchFill vnhsi_get_patch_fill(VnHsiPatchType patchType)
{
	return vnhsi_patches_fill(patchType);
}

VnHsiPatchDef vnhsi_get_patch_def()
{
	return vnhsi_patches_def();
}

int vnhsi_get_last_patch()
{
	return vnhsi_patches_last_patch();
}
#endif /* MAXPATCHES */