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

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

#define VN2D_GYRO_TOL_FIRST_MEAS	vn_deg2rad(5.0f)	/* tolerance on first measurement gyro reading (should be zero if stationary and filter converged) */
#define VN2D_PITCH_ROLL_TOLERANCE	vn_deg2rad(10.0f)	/* tolerance on pitch and roll w.r.t. gravVec (rads) */

#define VN2D_COND_THRESH		1E6f		/* condition number value threshold */
#define VN2D_SQRT_THRESH		1E-8f	/* square root value threshold */
#define VN2D_RFR_MINEIG			1E-4f	/* minimum eigenvalue allowed in RFR HTH */
#define VN2D_PLANE_MINEIG		1E-4f	/* minimum eigenvalue allowed in Plane HTH */
#define VN2D_ELLIPSE_MINEIG		1E-4f	/* minimum eigenvalue allowed in Ellipse HTH */

typedef struct {
	unsigned int gravcnt;
	float gravVec[3];
	float deltaTheta;	
} VnHsiKeep2DParam;

static VnHsiKeep2DParam kp;

/** \brief Fit the measured magnetic vectors to a 2D ellipse.
  *
  * The 2D HSI calibration starts with fitting an ellipse to the measured magnetic data.  
  
  * \param[in] dipole The magnetic dipole structure. 
  * \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[in] R Transformation matrix.
  * \param[out] ellipseParam  Ellipse parameters.
  * \param[out] FOMn  The figures of merit.
  * \return Error code. */
static enum VnError vnhsi_fit_ellipse(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float R[], float* ellipseParam, float* FOMn);

/** \brief Calculate ellipse plane normal.
  *
  * \param[in] dipole The magnetic dipole structure. 
  * \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[in] R Transformation matrix.
  * \param[out] Zn  ellipse normal.
  * \param[out] FOMn  The figures of merit.
  * \return Error code. */
static enum VnError vnhsi_fit_plane(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float R[], float* Zn, float* FOMn);

/** \brief Estimate the reference frame rotation (RFR) error.
  *
  * \param[in] meas  The YMR measurement.
  * \param[in] count The number of YMR measurements.
  * \param[out] ypr  Yaw, pitch and roll of the RFR error.
  * \return Error code. */
static enum VnError vnhsi_calc_rfr_error(VnHsiYmr* meas, int count, float* ypr);

/** \brief Convert ellipse parameters to HSI parameters.
  *
  * \param[in] dipole The magnetic dipole structure. 
  * \param[in] c The configuration structure. 
  * \param[in] EP  The 6 ellipse parameters.
  * \param[in] count The number of YMR measurements.
  * \param[in] Zn  ellipse normal.
  * \param[out] SI  The soft iron matrix.
  * \param[out] HIb  The hard iron vector.
  * \return Error code. */
static enum VnError vnhsi_ellipse2HSI(VnHsiDipole dipole, VnHsiConfig* c, float EP[6],float Zn[3],float* SI, float* HIb);

void vnhsi_2d_init(VnHsiDipole dipole, VnHsiConfig* c)
{
	kp.gravcnt = 0;
	kp.gravVec[0] = kp.gravVec[1] = kp.gravVec[2] = 0.0;
	kp.deltaTheta = 0.0;
	
#ifdef MAXPATCHES
	vnhsi_patches_init(c);
#endif /* MAXPATCHES */
}

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

	float angDelta = 2.0f*VN_PIF/c->nPatches;
	float dt = 1.0f/c->dataRate;
	union vn_mat3f DCM;
	union vn_vec3f e, grav, gV;
	
	float normGyro, thetadot, gg;
	unsigned int c1;
	bool keepMeas = false;

	gV.c[0]=gV.c[1]=0.0f;
	gV.c[2]=-1.0f;
	vn_mult_v3s(ypr.c,VN_PIF/180.0f,e.c);
	Euler3212C(DCM.e, e.c);
	normGyro = vn_norm_v(gyro.c, 3);
	
	/* calculate the body magnetic reference vector from the ypr */
	vn_mult_mn((float*) DCM.e, gV.c, 3, 3, 1, grav.c);
	
	thetadot = vn_dot(kp.gravVec,grav.c);
	gg = vn_dot(gyro.c,kp.gravVec);
	
	/* Check for whether measurement should be kept
	 * Check if any measurements have been collected and if the vehicle is at rest */
   if (kp.gravcnt == 0 && normGyro < VN2D_GYRO_TOL_FIRST_MEAS)
	{
		/* Initialize gravVec and add first data item */
		vn_copy_v3(grav.c,kp.gravVec);
		kp.gravcnt++;

		keepMeas = true; 
	}
	/* Check if not first measurement that the gravity vector is within tolerance of previous measurements */
	else if (kp.gravcnt != 0 && acos(thetadot) < VN2D_PITCH_ROLL_TOLERANCE)
	{
		/* Maintain running average on gravity vector */
		for ( c1=0;c1<3;c1++){  
			kp.gravVec[c1] = ((float)kp.gravcnt*kp.gravVec[c1] + grav.c[c1])/(float)(kp.gravcnt + 1);
		}
		kp.gravcnt++;
		
		vn_normalize_v3(kp.gravVec,kp.gravVec);
		
		/* Update change in angle about axis parallel to gravity vector */
		kp.deltaTheta += fabsf(gg) * dt;

		/* Check if change in angle exceeds threshold for keeping measurement */
		if (kp.deltaTheta > angDelta) {
			/* Set flag to collect measurement, reset deltaTheta */
			keepMeas = true; 
			kp.deltaTheta = 0.0;
		}
	}

#ifdef MAXPATCHES
	if(keepMeas) {
		union vn_vec3f refYPR, ypr2;
		union vn_mat3f RFR, temp33, DCM2;
		
		refYPR.c[0] = 0.0f;
		refYPR.c[1] = asinf(kp.gravVec[0])*180.0f/VN_PIF;
		refYPR.c[2] = atan2f(-kp.gravVec[1],-kp.gravVec[2])*180.0f/VN_PIF;

		vnhsi_patches_set_refYPR(refYPR);

		vn_mult_v3s(refYPR.c,VN_PIF/180.0f,e.c);
		Euler3212C(RFR.e, e.c);

		vn_tranpose_m3((float*)RFR.e, (float*)temp33.e);
		vn_mult_m3_m3((float*)temp33.e, (float*)DCM.e, (float*)DCM2.e);
		
		ypr2 = C2Euler321((float*)DCM2.e);

		vnhsi_patches_adddata(dipole,c,ypr2,TWODPATCHTYPE);
	}
#endif /* MAXPATCHES */
	
	return keepMeas;
}

enum VnError vnhsi_2d_solve(VnHsiDipole dipole, VnHsiConfig* c, VnHsiYmr* meas, VnHsiSol* hsisolnIN, size_t numMeas, VnHsiSol* hsisoln, VnHsiFom* FOM)
{
	
	union vn_vec3f e, HI, HIb,Zn;
	union vn_mat3f Rest, SI, RestT, tmp33;
	float EP[6];
		
	enum VnError code = E_NONE;
			
	float ElCov0, AlCov0;
	float *ElCov = &ElCov0;
	float *AlCov = &AlCov0;
	
	if(FOM != NULL)
	{
		ElCov = &FOM->ellipsoidCoverage;
		AlCov = &FOM->alignmentCoverage;
	}

	code = vnhsi_calc_rfr_error(meas, numMeas, e.c);
	if (code != E_NONE)
		return code;	

	Euler3212C(Rest.e, e.c);
	vn_tranpose_m3((float*) Rest.e, (float*) RestT.e);
		
	code = vnhsi_fit_plane(dipole,meas,hsisolnIN,numMeas,RestT.e,Zn.c,AlCov);	
	if (code != E_NONE)
		return code;	
	
	code = vnhsi_fit_ellipse(dipole, meas,hsisolnIN, numMeas, RestT.e, EP, ElCov); 
	if (code != E_NONE)
		return code;	
	
	code = vnhsi_ellipse2HSI(dipole, c, EP, Zn.c, (float*)SI.e, (float*)HIb.c);
	if (code != E_NONE)
		return code;	
	
	if(c->mode == HSI_MODE_2D_NOLOCATION)
		hsisoln->magInc = SI.e[8]/Zn.c[2];
	else
		hsisoln->magInc = dipole.Bdown;		

		/* Transform estimates back to body frame using estimated RFR error */
	vn_mult_m3_m3((float*) Rest.e, (float*) SI.e, (float*)tmp33.e);
	vn_mult_m3_m3((float*) tmp33.e, (float*) RestT.e, (float*) hsisoln->SI.e);
	
	code = vn_inverse_m((float*) SI.e, 3, (float*) tmp33.e);
	if (code != E_NONE)
		return code;	
	if(vn_cond((float*)SI.e, (float*)tmp33.e, 3) > VN2D_COND_THRESH)
		return E_ILL_CONDITIONED;

	vn_mult_m3_v3((float*) tmp33.e, (float*) HIb.c, (float*) HI.c);
	vn_mult_m3_v3((float*) Rest.e, (float*) HI.c, (float*) hsisoln->HI.c);

	return E_NONE;
	
}

enum VnError vnhsi_fit_ellipse(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float R[], float* ellipseParam, float* FOMn)
{
	float X1[5];
	int c1, j, k;
	float measR[3];
	float Rref = dipole.Bnorth;
	float HTH[5][5], HTY[5], Hmat[5], Ym;
	enum VnError code = E_NONE;
	union vn_vec3f magc;

	vn_zero_m((float*)HTH,5,5);	
	vn_zero_m((float*)HTY,5,1);	
	
	for (c1=0 ; c1<count; c1++){
		magc = vnhsi_ApplyHSI(hsisolnIN, meas[c1].mag);

		/* Rotate mag measurements to account for RFR error*/
		vn_mult_m3_v3((float*) R, (float*) magc.c, (float*) measR);
		
		Ym = -measR[0]*measR[0]-measR[1]*measR[1];
		Hmat[0] = measR[0]*measR[0]-measR[1]*measR[1];		
		Hmat[1] = measR[0]*measR[1];
		Hmat[2] = measR[0]*Rref;	
		Hmat[3] = measR[1]*Rref;
		Hmat[4] = Rref*Rref;
		
		for(k=0;k<5;k++) {
			HTY[k] += Hmat[k]*Ym;
			for(j=0;j<5;j++) {
				HTH[k][j] += Hmat[k]*Hmat[j];
			}
		}
	}
	
	code = vnhsi_solveLLS((float*)HTH, HTY, 5, VN2D_ELLIPSE_MINEIG, X1, FOMn);
	if (code != E_NONE)
		return code;	
	if(FOMn) *FOMn *= Rref*Rref;	 
	
	/* Get the second Ellipse parameters (should keep or remove?)*/
	ellipseParam[0] = (X1[0] + 1.0f)/2.0f; 	
	ellipseParam[1] = X1[1]/2.0f;
    ellipseParam[2] = 1.0f - ellipseParam[0];
	if(4.0*ellipseParam[0]*ellipseParam[2] - ellipseParam[1]*ellipseParam[1] < 0.0f)
		return E_DATA_NOT_ELLIPTICAL;
		
	for (j = 0; j < 3; j++) 
		ellipseParam[j + 3] = X1[j + 2]/2.0f*Rref;
	ellipseParam[5] *= Rref;
	
	return code;
	 
}

enum VnError vnhsi_calc_rfr_error(VnHsiYmr* meas, int count, float* ypr)
{

	union vn_vec3f tmp3, naR, HTY;
	union vn_mat3f tmp33;
	float HTH[3][3];
	
	unsigned int c1,c2;
	enum VnError code = E_NONE;
	
	vn_zero_m((float*)HTH,3,3);
	vn_zero_m((float*)HTY.c,3,1);	
	
	for (c1=0 ; c1<count; c1++){
		for (c2=0;c2<3;c2++)	
			tmp3.c[c2] = meas[c1].ypr.c[c2] * VN_PIF/180.0f;
		
		Euler3212C(tmp33.e, tmp3.c);
		
		for (c2=0;c2<3;c2++) 
			tmp3.c[c2] = tmp33.e[2 + 3*c2];

		vn_mult_mn((float*) tmp3.c, (float*)tmp3.c, 3, 1, 3, (float*)tmp33.e);
		vn_add_m((float*) HTH, (float*)tmp33.e, 3, 3, (float*)HTH);
		vn_add_v3((float*) HTY.c, (float*) tmp3.c, (float*) HTY.c);
	}
	
	code = vnhsi_solveLLS((float*)HTH, HTY.c, 3, VN2D_RFR_MINEIG, naR.c, tmp3.c);
	if(code == E_INSUFFICIENT_DATA) { /* the ground is flat/level*/
		for (c2=0;c2<3;c2++) 
			naR.c[c2] = HTY.c[c2];		
		code = E_NONE;
	}
	
	ypr[0] = 0.0f;		
	ypr[1] = -asinf(naR.c[0]/sqrt(naR.c[0]*naR.c[0]+naR.c[1]*naR.c[1]+naR.c[2]*naR.c[2]));
	ypr[2] = atan2f(naR.c[1],naR.c[2]);

	return code;
}

enum VnError vnhsi_fit_plane(VnHsiDipole dipole, VnHsiYmr* meas, VnHsiSol* hsisolnIN, int count, float R[], float* Zn, float* FOMn)
{
	float HTH[3][3], HTY[3];
	float tmp3[3],tmp33[3][3];
	unsigned int c1;
	enum VnError code = E_NONE;
	union vn_vec3f magc;
	
	vn_zero_m((float*)HTH,3,3);
	vn_zero_m((float*)HTY,3,1);	

	for (c1=0 ; c1<count; c1++){
		magc = vnhsi_ApplyHSI(hsisolnIN, meas[c1].mag);
		vn_mult_mn((float*) magc.c, (float*)magc.c, 3, 1, 3, (float*)tmp33);
		vn_add_m ((float*)HTH, (float*)tmp33,3 ,3,  (float*)HTH);
		vn_add_v3( (float*) HTY, (float*) magc.c, (float*) HTY);	
	}
	
	code = vnhsi_solveLLS((float*)HTH, HTY, 3, VN2D_PLANE_MINEIG, tmp3, FOMn);
	if(FOMn) *FOMn *= dipole.sphereRad*dipole.sphereRad;	 
	vn_mult_mn((float*)R,(float*)tmp3,3,3,1,Zn);
	
	return code;
}

enum VnError vnhsi_ellipse2HSI(VnHsiDipole dipole, VnHsiConfig* c, float EP[6],float Zn[3],float* SI, float* HIb)
{	
	float A, B, C, gam, delta, alpha, n1, n2, tmp, tmp2;
	float a[3], b[3];
	unsigned int c1;
	enum VnError code = E_NONE;
	
	A = EP[1]*EP[1] + (EP[2]-EP[0])*(EP[2]-EP[0]);
	B = -2*EP[0]*(EP[2]-EP[0])*(EP[2]-EP[0]) - EP[1]*EP[1]*(3*EP[0]-EP[2])/2;
	C = EP[1]*EP[1]*EP[1]*EP[1]/16 - EP[0]*EP[1]*EP[1]/2*(EP[2]-EP[0]) + EP[0]*EP[0]*(EP[2]-EP[0])*(EP[2]-EP[0]);
	
	tmp = B*B-4.0f*A*C;
	if (fabsf(tmp) < VN2D_SQRT_THRESH)
		tmp = 0.0f;
	
	delta = (-B + sqrtf(tmp))/(2*A);
	a[0] = sqrtf(delta);
	
	tmp2 = a[0]*a[0]+ EP[2]-EP[0];

	if (fabsf(tmp2) < VN2D_SQRT_THRESH)
		tmp2 = 0.0f;
	b[1] = sqrtf(tmp2);
	
	a[1] = EP[1]/(2.0f*(a[0] + b[1]));
	b[0] = a[1];
	b[2] = (a[1]*EP[3]-a[0]*EP[4])/(2*(a[1]*a[1]-a[0]*b[1]));
	a[2] = (EP[3]/2 - a[1]*b[2])/a[0];

	gam  = sqrtf(a[2]*a[2] + b[2]*b[2] - EP[5]);
	
	alpha = dipole.Bnorth/gam;
	
	for (c1=0;c1<3;c1++) {
		a[c1] *= alpha;
		b[c1] *= alpha;
		if (a[c1] != a[c1])
			return E_ILL_CONDITIONED;
		if (b[c1] != b[c1])
			return E_ILL_CONDITIONED;
	}
	
	if (fabsf(Zn[2]) < VN2D_SQRT_THRESH)
		return E_ILL_CONDITIONED;
	
	n1 = Zn[0]/Zn[2];
	n2 = Zn[1]/Zn[2];

	A = n1*n1*n1*n1 + n2*n2*n2*n2 - 2.0f;
	B = 2.0f*(a[0]*n1*n1 + b[1]*n2*n2);
	C = a[0]*a[0] + b[1]*b[1];
	
	tmp = B*B-4.0f*A*C;
	if (fabsf(tmp) < VN2D_SQRT_THRESH)
		tmp = 0.0f;

	if(c->mode == HSI_MODE_2D_NOLOCATION || c->mode == HSI_MODE_2D_SCALE_S33)
		SI[8] = (-B - sqrtf(tmp))/(2*A);
	else
		SI[8] = 1.0f;
	
	SI[0] = a[0] + SI[8]*n1*n1;
	SI[4] = b[1] + SI[8]*n2*n2;
	SI[1] = a[1] + SI[8]*n1*n2;
	SI[6] = SI[8]*n1;
	SI[7] = SI[8]*n2;
	SI[2] = SI[6];
	SI[3] = SI[1];
	SI[5] = SI[7];
   
	HIb[0] = SI[8]*n1/Zn[2] - a[2];
	HIb[1] = SI[8]*n2/Zn[2] - b[2];
	if(c->mode == HSI_MODE_2D_NOLOCATION)
		HIb[2] = 0.0f;
	else
		HIb[2] = SI[8]/Zn[2] - dipole.Bdown;

	return code;
}