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

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

#define VN3D_GLSDC_JTHRESH		2E-6f	/* mean per-measurement error threshold */
#define VN3D_COND_THRESH		1E6f	/* max condition number on HTH during GLSDC */
#define VN3D_GLSDC_CMAX			10		/* Max number of iterations */
#define VN3D_ALIGNMENT_MINEIG	1E-4f	/* minimum eigenvalue allowed in Alignment HTH */
#define VN3D_ELLIPSE_MINEIG		1E-4f	/* minimum eigenvalue allowed in Ellipsoid HTH */

typedef struct {
	bool initFlag;
	union vn_mat3f DCMold;
} VnHsiKeep3DParam;

static VnHsiKeep3DParam kp;
  
/** \brief Fit the measured magnetic vectors to an ellipsoid.
  *
  * The 3D HSI calibration starts with fitting an ellipsoid to the 3D magnetic data.  
  * The solution of that ellipsoid fit provides the best estimate of the hard iron, 
  * along with the best estimate of a symmetrical soft-iron matrix.
  
  * \param[in] dipole The magnetic dipole. 
  * \param[in] meas  The YMR measurement.
  * \param[in] hsisolnIN  HSI solution to apply to measurements prior to solving.  Used for calculating accurate FOM. Set to NULL for uncorrected solution.
  * \param[in] count The number of YMR measurements.
  * \param[out] SI0  The symmetric soft iron solution.
  * \param[out] HI0  The hard iron solution.
  * \param[out] FOMn  The figures of merit.
  * \return Error code. */
  
static enum VnError vnhsi_fit_ellipsoid(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float [], float HI0[], float* FOMn);

/** \brief Estimate the ellipsoid rotation matrix.
  *
  * This function estimates a rotation matrix (R) which will be used to calculate
  * the asymmetric soft-iron matrix.
  
  * \param[in] dipole The magnetic dipole. 
  * \param[in] c The HSI configuration.
  * \param[in] meas  The YMR measurement.
  * \param[in] hsisolnIN  HSI solution to apply to measurements prior to solving.  Used for calculating accurate FOM. Set to NULL for uncorrected solution.
  * \param[in] numMeas The number of YMR measurements.
  * \param[in] SI0  The symmetric soft iron solution.
  * \param[in] HI0  The hard iron solution.
  * \param[out] Rmat  The ellipsoid rotation matrix.
  * \param[out] magInc  The vertical external field component.
  * \param[in/out] FOMn  The figures of merit.
  * \return Error code. */
  
static enum VnError vnhsi_find_rotation_3d(VnHsiDipole dipole, VnHsiConfig* c, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, union vn_mat3f SI0, float HI0[3], float [], float* magInc, float* FOM);

/** Estimate the 4 parameters (3 rotation angles and magnetic inclination) using GLSDC.

  * This function iterates the 3 rotation angles and the vertical external field component, 
  * for a total of 4 parameters through the Gaussian Least Squares Differential Correction (GLSDC).

  * \param[in] dipole The magnetic dipole. 
  * \param[in] meas  The YMR measurement.
  * \param[in] hsisolnIN  HSI solution to apply to measurements prior to solving.  Used for calculating accurate FOM. Set to NULL for uncorrected solution.
  * \param[in] numMeas The number of YMR measurement.
  * \param[in] X0  Current 4 parameter estimate.
  * \param[in] SI0  The symmetric soft iron solution.
  * \param[out] HTH  The states sensitivity matrix.
  * \param[out] HTY  The states/meas sensitivity vector.
  * \return Error none. */
  
static void vnhsi_glsdc_iter(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, union vn_vec4f X0 , union vn_mat3f SI0, float HI0[], float HTH[4][4], float HTY[4], float* J);

void vnhsi_3d_init(VnHsiDipole dipole, VnHsiConfig* c)
{

	kp.initFlag = false;
	vn_eye_m3((float*)kp.DCMold.e);

#ifdef MAXPATCHES
	vnhsi_patches_init(c);
#endif /* MAXPATCHES */
}


bool vnhsi_3d_keep(VnHsiDipole dipole, VnHsiConfig* c, union vn_vec3f ypr)
{

	bool keepMeas = false;
	
	union vn_mat3f  DCM, DCMrot;
	union vn_vec3f e;
	float deltaTheta, angDelta = 2*VN_PIF/c->nPatches;
	
#ifdef MAXPATCHES
	VnHsiPatch Npatch, Epatch;

	Epatch = vnhsi_patches_adddata(dipole,c,ypr,EASTPATCHTYPE);
	Npatch = vnhsi_patches_adddata(dipole,c,ypr,NORTHPATCHTYPE);
	
	if (c->flagKM == 1)
	{	/* Use the gravity vectors to choose the patches */
		if(Npatch.patchInd >= 0) {
			keepMeas = true;
		} 
	}
	
	if (c->flagKM == 2)
	{	/* Use the gravity and east vectors to choose the patches */
		if(Npatch.patchInd >= 0) {
			keepMeas = true;
		} 
		else if(Epatch.patchInd >= 0) {
			keepMeas = true;
		} 
	}
	
	if (c->flagKM == 3)
#endif /* MAXPATCHES */
	{  /* Use delta angle to keep meas. */
		vn_mult_v3s(ypr.c,VN_PIF/180.0f,e.c);
		Euler3212C(DCM.e, e.c);
		if(!kp.initFlag) {
			vn_tranpose_m((float*)DCM.e,3,3,(float*)kp.DCMold.e);
			kp.initFlag = true;
			keepMeas = true;
		} else {
			vn_mult_mn((float*)DCM.e,(float*)kp.DCMold.e,3,3,3,(float*)DCMrot.e);
			deltaTheta = acosf(0.5f*(DCMrot.e[0]+DCMrot.e[4]+DCMrot.e[8]-1.0f));
			if(deltaTheta > angDelta) {
				vn_tranpose_m((float*)DCM.e,3,3,(float*)kp.DCMold.e);
				keepMeas = true;
			}
		}
	}
	
	return keepMeas;
}


enum VnError vnhsi_3d_solve(VnHsiDipole dipole, VnHsiConfig* c, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, VnHsiSol* hsisoln, VnHsiFom* FOM)
{
	
	union vn_mat3f SI0, Rmat;

	enum VnError code = E_NONE;
	
	float *ElCov = NULL;
	float *AlCov = NULL;
	
	if(FOM != NULL)
	{
		ElCov = &FOM->ellipsoidCoverage;
		AlCov = &FOM->alignmentCoverage;
	}
	
	/* initialize the SI and HI */
    vn_eye_m((float*) hsisoln->SI.e,3);		
	vn_eye_m((float*) SI0.e,3);
	vn_eye_m((float*) Rmat.e,3);
	vn_init_v3(hsisoln->HI.c, 0, 0, 0);

	/* Find HI and symmetric SI using ellipsoid fit */
	code = vnhsi_fit_ellipsoid(dipole, meas, hsisolnIN, numMeas, SI0.e, hsisoln->HI.c,ElCov);

	if (code != E_NONE)
		return code;	
	
	/* Find rotation matrix on SI using vertical component constraint*/
	code = vnhsi_find_rotation_3d(dipole, c, meas, hsisolnIN, numMeas, SI0, hsisoln->HI.c, Rmat.e,&hsisoln->magInc,AlCov);
		
	if (code != E_NONE)
		return code;	
	
	vn_mult_mn((float*) Rmat.e, (float*) SI0.e, 3, 3, 3, (float*) hsisoln->SI.e);
	
	return code;
	
}

enum VnError vnhsi_find_rotation_3d(VnHsiDipole dipole, VnHsiConfig* c, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, union vn_mat3f SI0, float HI0[3], float RM[], float* magInc, float* FOM)
{
	float Jthresh, dJthresh;
	float J, dJ, Jold, minEig;

	int k, c1;
	union vn_vec4f X0, dX, evalr, evali, HTdy;
	union vn_vec3f Xr; 
	float HTH[4][4], HTHinv[4][4], evec[4][4];

	enum VnError code = E_NONE;

	Jthresh = VN3D_GLSDC_JTHRESH * numMeas;		/* minimum cost threshold */
	dJthresh = VN3D_GLSDC_JTHRESH;				/*  minimum change in cost threshold */

	/* initialize GLSDC Parameters */
	vn_zero_m(X0.c,4,1);
	X0.c[3] = dipole.Bdown/dipole.Bnorth;
	vn_zero_m(dX.c,4,1);
	
	J = (float)numMeas*10000.0f;
	Jold = J;
	dJ = 1.0;
	
	c1=0;
	
	/* Main estimation Loop */
	while (J > Jthresh && dJ > dJthresh)
	{
				
		vnhsi_glsdc_iter(dipole,meas,hsisolnIN,numMeas, X0, SI0, HI0, HTH, HTdy.c, &J);
		
		code = vn_inverse_m((float*) HTH, 4, (float*) HTHinv);
		if (code != E_NONE)
			return code;
		if (vn_cond((float*)HTH, (float*)HTHinv, 4) > VN3D_COND_THRESH)
			return E_ILL_CONDITIONED;
		
		vn_mult_mn((float*) HTHinv, (float*) HTdy.c, 4, 4, 1, (float*) dX.c);
		vn_add_v(X0.c,dX.c,4,X0.c);
	
		dJ = Jold - J;
		Jold = J;
	
		if (c1++ == VN3D_GLSDC_CMAX)
			return E_EXCEEDED_MAX_ITERATIONS;
	}
	
	vnhsi_glsdc_iter(dipole, meas, hsisolnIN, numMeas, X0, SI0, HI0, HTH, HTdy.c, &J);

	/* get the eigen values and vectors for HTH */
	n_eigeng((float*)HTH, 4, evalr.c, evali.c, (float *)evec);

	/* find min eigenvalue to get the index for the eigen vector */
	minEig = evalr.c[0];
	for (k = 1; k < 4; k++) {
		if(minEig>evalr.c[k]) 
			 minEig=evalr.c[k];
	}
	
	if (minEig < VN3D_ALIGNMENT_MINEIG) { 
		code = E_INSUFFICIENT_DATA;
		if(FOM) *FOM = 100*dipole.Bnorth;	
		return code;
	}

	if(FOM) *FOM = dipole.Bnorth/sqrtf(minEig);


	/* Format output */
	for (k =0; k<3;k++) Xr.c[k] = X0.c[k];
	Euler3212C(RM , Xr.c); 
	*magInc = X0.c[3]*dipole.Bnorth;

		
	return code;
}


void vnhsi_glsdc_iter(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, union vn_vec4f X0 , union vn_mat3f SI0, float HI0[4], float HTH[4][4], float HTY[4], float* J)
{
	unsigned int i, j ,k;
	union vn_vec3f mg, yprl, Xr, M0, dM, tmp1, R1M0, R2M0, R3M0;
	union vn_mat3f Rmat, Cmat, dRdx1, dRdx2,dRdx3;
	
	float tmp, tr1, tr2, tr3, Htemp[4];
	float c1, c2, c3, s1, s2, s3, dy;
	
		
	vn_zero_m((float*)HTH,4,4);
	vn_zero_m(HTY,4,1);
	*J = 0.0;
	
	for ( i=0 ; i < numMeas ; i++){
		mg = vnhsi_ApplyHSI(hsisolnIN, meas[i].mag);
		for ( k = 0; k < 3 ; k++){
			yprl.c[k] = meas[i].ypr.c[k]*VN_PIF/180.0f;
			Xr.c[k] = X0.c[k];
			dM.c[k]= mg.c[k] - HI0[k];
		}
		
		Euler3212C(Rmat.e , Xr.c); 				
		Euler3212C(Cmat.e , yprl.c);				
		
		vn_mult_mn((float*) SI0.e, (float*) dM.c, 3, 3, 1, (float*) M0.c);    
		vn_mult_mn((float*) Rmat.e, (float*) M0.c, 3, 3, 1, (float*) tmp1.c); 
		
		tmp=0;
		for (j =0;j<3; j++) 
			tmp = tmp + Cmat.e[2 + 3*j]*tmp1.c[j];
		
		dy = X0.c[3]*dipole.Bnorth - tmp;
		
		c1= cosf(X0.c[0]); c2= cosf(X0.c[1]); c3= cosf(X0.c[2]);
		s1= sinf(X0.c[0]); s2= sinf(X0.c[1]); s3= sinf(X0.c[2]);
	
	    dRdx1.e[0] = -c2*s1;  			dRdx1.e[1] = c2*c1; 				dRdx1.e[2]= 0; 
		dRdx1.e[3] = -c3*c1-s3*s2*s1;   dRdx1.e[4] = -c3*s1+s3*s2*c1;   	dRdx1.e[5]= 0; 
		dRdx1.e[6] = s3*c1-c3*s2*s1; 	dRdx1.e[7] = s3*s1+c3*s2*c1; 		dRdx1.e[8]= 0;
		
        dRdx2.e[0] = -s2*c1;			dRdx2.e[1] = -s2*s1; 				dRdx2.e[2] =-c2; 
		dRdx2.e[3] = s3*c2*c1;			dRdx2.e[4] = s3*c2*s1; 				dRdx2.e[5] = -s3*s2; 
		dRdx2.e[6] = c3*c2*c1; 			dRdx2.e[7] = c3*c2*s1; 				dRdx2.e[8] = -c3*s2;
		
        dRdx3.e[0] = 0;					dRdx3.e[1] = 0;						dRdx3.e[2] = 0; 
		dRdx3.e[3] = s3*s1+c3*s2*c1;   	dRdx3.e[4] = -s3*c1+c3*s2*s1;		dRdx3.e[5] = c3*c2; 
		dRdx3.e[6] = c3*s1-s3*s2*c1;	dRdx3.e[7] = -c3*c1-s3*s2*s1;		dRdx3.e[8] = -s3*c2;
		
		vn_mult_mn((float*) dRdx1.e, (float*) M0.c, 3, 3, 1, (float*) R1M0.c); 
		vn_mult_mn((float*) dRdx2.e, (float*) M0.c, 3, 3, 1, (float*) R2M0.c); 
		vn_mult_mn((float*) dRdx3.e, (float*) M0.c, 3, 3, 1, (float*) R3M0.c); 
		
		tr1=0; tr2=0; tr3=0;
		for (j =0;j<3; j++) {
			tr1 = tr1 + Cmat.e[2 + 3*j]*R1M0.c[j];
			tr2 = tr2 + Cmat.e[2 + 3*j]*R2M0.c[j];
			tr3 = tr3 + Cmat.e[2 + 3*j]*R3M0.c[j];
		}
		
		Htemp[0] = tr1; 
		Htemp[1] = tr2; 
		Htemp[2] = tr3; 
		Htemp[3] = -dipole.Bnorth; 
		
		for(j=0;j<4;j++) {
			HTY[j] += Htemp[j]*dy;
			for(k=0;k<4;k++) {
			HTH[j][k] += Htemp[j]*Htemp[k];
			}
		}
		*J += dy*dy;
	}

}

enum VnError vnhsi_fit_ellipsoid(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float SV[], float HI0[], float* FOMn)
{
	
	enum VnError code = E_NONE;
	
	float Ym, Hmat[9], HTH[9][9], HTY[9], SIsq[3][3], XX[9];
	unsigned int j, k, m;
	
	float Rdesired = dipole.sphereRad;
	union vn_vec3f evalrS, evaliS, tmpVs, tmp1;
	union vn_mat3f evecS, evecST, tmpM2, tmp2, tmp3;

	float scale, EllPar[10];
			
	vn_zero_m((float*)HTH,9,9);
	vn_zero_m((float*)HTY,9,1);
	for ( j=0;j<count;j++){
		
		tmp1 = vnhsi_ApplyHSI(hsisolnIN, meas[j].mag);

		Ym = -(tmp1.c[0]*tmp1.c[0]+tmp1.c[1]*tmp1.c[1]+tmp1.c[2]*tmp1.c[2]);  
		Hmat[0] = tmp1.c[0]*tmp1.c[0] + tmp1.c[1]*tmp1.c[1] - 2*tmp1.c[2]*tmp1.c[2];
		Hmat[1] = tmp1.c[0]*tmp1.c[0] - 2*tmp1.c[1]*tmp1.c[1] + tmp1.c[2]*tmp1.c[2];
		Hmat[2] = 2*tmp1.c[0]*tmp1.c[1]; 			
		Hmat[3] = 2*tmp1.c[0]*tmp1.c[2];       
		Hmat[4] = 2*tmp1.c[1]*tmp1.c[2];
		Hmat[5] = Rdesired*tmp1.c[0];            
		Hmat[6] = Rdesired*tmp1.c[1];          
		Hmat[7] = Rdesired*tmp1.c[2];	     
		Hmat[8] = Rdesired*Rdesired;			
			
		for(k=0;k<9;k++) {
			HTY[k] += Hmat[k]*Ym;
			for(m=0;m<9;m++) {
				HTH[k][m] += Hmat[k]*Hmat[m];
			}
		}
	}
	
	code = vnhsi_solveLLS((float*)HTH,HTY,9,VN3D_ELLIPSE_MINEIG,XX,FOMn);
	if (code != E_NONE)
		return code;

	if(FOMn) *FOMn *= Rdesired*Rdesired;	

	/* Get the ellipsoid parameters */
	EllPar[0] = XX[0] + XX[1] + 1.0f;
	EllPar[1] = EllPar[0] - 3.0f*XX[1] ;
	EllPar[2] = EllPar[0] - 3.0f*XX[0] ;
	
	for (j = 3; j < 10; j++) 
		EllPar[j] = XX[j-1];
	
	/* Form the algebraic form of the ellipsoid */
	
	SIsq[0][0]=EllPar[0]; SIsq[0][1]=EllPar[3]; SIsq[0][2]=EllPar[4]; 
	SIsq[1][0]=EllPar[3]; SIsq[1][1]=EllPar[1]; SIsq[1][2]=EllPar[5]; 
	SIsq[2][0]=EllPar[4]; SIsq[2][1]=EllPar[5]; SIsq[2][2]=EllPar[2]; 
	
	n_eigeng(SIsq[0], 3, (float *)evalrS.c, (float *)evaliS.c, (float *)evecS.e);
			 
	for (k = 0; k < 3; k++) {
		if (evalrS.c[k] <= 0.0f){
			return E_DATA_NOT_ELLIPTICAL;
		}
		tmpVs.c[k] = EllPar[6 + k];
	}
	vn_inverse_m((float*) evecS.e, 3, (float*) evecST.e);
	vn_mult_mn((float*) evecST.e, (float*) tmpVs.c, 3, 3, 1, (float*) tmp1.c);
	
	for (j = 0; j < 3; j++)
		for (k = 0; k < 3; k++) 
			tmpM2.e[3*j + k] = -0.5f*Rdesired*evecS.e[3*j + k] / evalrS.c[k];
	
	/* find the center of the ellipsoid (Hard Iron)*/
	vn_mult_mn((float*) tmpM2.e, (float*) tmp1.c, 3, 3, 1, (float*) HI0);
	
	/* compute scaling on SI matrix*/
	scale = 1.0f/sqrtf(-EllPar[9] - (0.5f/Rdesired)*(tmpVs.c[0]*HI0[0] + tmpVs.c[1]*HI0[1] + tmpVs.c[2]*HI0[2]));
	
	/* compute SI matrix*/
	for (j = 0; j < 3; j++)
		for (k = 0; k < 3; k++) 
			tmp3.e[3*j + k] = evecST.e[3*j + k] * sqrtf(evalrS.c[j]);
		
	vn_mult_mn((float*) evecS.e, (float*) tmp3.e, 3, 3, 3, (float*) tmp2.e);	
	vn_scale_ms((float*) tmp2.e, scale, 3 , 3 , (float*) SV);

 
	return code;

}