// Copyright (c) 2014, Freescale Semiconductor, Inc.
// All rights reserved.
// vim: set ts=4:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Freescale Semiconductor, Inc. nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// This file contains magnetic calibration functions.  It is STRONGLY RECOMMENDED
// that the casual developer NOT TOUCH THIS FILE.  The mathematics behind this file
// is extremely complex, and it will be very easy (almost inevitable) that you screw
// it up.
//
// Haha - This file has been edited!  Please do not blame or pester NXP (formerly
//        Freescale) about the "almost inevitable" issues!

#include "imuread.h"

#define FXOS8700_UTPERCOUNT  0.1f
#define DEFAULTB 50.0F				// default geomagnetic field (uT)
#define X 0                         // vector components
#define Y 1
#define Z 2
#define ONETHIRD 0.33333333F        // one third
#define ONESIXTH 0.166666667F       // one sixth
#define MINMEASUREMENTS4CAL 40      // minimum number of measurements for 4 element calibration
#define MINMEASUREMENTS7CAL 100     // minimum number of measurements for 7 element calibration
#define MINMEASUREMENTS10CAL 150    // minimum number of measurements for 10 element calibration
#define MINBFITUT 22.0F             // minimum geomagnetic field B (uT) for valid calibration
#define MAXBFITUT 67.0F             // maximum geomagnetic field B (uT) for valid calibration
#define FITERRORAGINGSECS 7200.0F   // 2 hours: time for fit error to increase (age) by e=2.718

static void fUpdateCalibration4INV(MagCalibration_t *MagCal);
static void fUpdateCalibration7EIG(MagCalibration_t *MagCal);
static void fUpdateCalibration10EIG(MagCalibration_t *MagCal);



// run the magnetic calibration
int MagCal_Run(void)
{
	int i, j;			// loop counters
	int isolver;		// magnetic solver used
	int count=0;
	static int waitcount=0;

	// only do the calibration occasionally
	if (++waitcount < 20) return 0;
	waitcount = 0;

	// count number of data points
	for (i=0; i < MAGBUFFSIZE; i++) {
		if (magcal.valid[i]) count++;
	}

	if (count < MINMEASUREMENTS4CAL) return 0;

	if (magcal.ValidMagCal) {
		// age the existing fit error to avoid one good calibration locking out future updates
		magcal.FitErrorAge *= 1.02f;
	}

	// is enough data collected
	if (count < MINMEASUREMENTS7CAL) {
		isolver = 4;
		fUpdateCalibration4INV(&magcal); // 4 element matrix inversion calibration
		if (magcal.trFitErrorpc < 12.0f) magcal.trFitErrorpc = 12.0f;
	} else if (count < MINMEASUREMENTS10CAL) {
		isolver = 7;
		fUpdateCalibration7EIG(&magcal); // 7 element eigenpair calibration
		if (magcal.trFitErrorpc < 7.5f) magcal.trFitErrorpc = 7.5f;
	} else {
		isolver = 10;
		fUpdateCalibration10EIG(&magcal); // 10 element eigenpair calibration
	}

	// the trial geomagnetic field must be in range (earth is 22uT to 67uT)
	if ((magcal.trB >= MINBFITUT) && (magcal.trB <= MAXBFITUT))	{
		// always accept the calibration if
		//  1: no previous calibration exists
		//  2: the calibration fit is reduced or
		//  3: an improved solver was used giving a good trial calibration (4% or under)
		if ((magcal.ValidMagCal == 0) ||
				(magcal.trFitErrorpc <= magcal.FitErrorAge) ||
				((isolver > magcal.ValidMagCal) && (magcal.trFitErrorpc <= 4.0F))) {
			// accept the new calibration solution
			//printf("new magnetic cal, B=%.2f uT\n", magcal.trB);
			magcal.ValidMagCal = isolver;
			magcal.FitError = magcal.trFitErrorpc;
			if (magcal.trFitErrorpc > 2.0f) {
				magcal.FitErrorAge = magcal.trFitErrorpc;
			} else {
				magcal.FitErrorAge = 2.0f;
			}
			magcal.B = magcal.trB;
			magcal.FourBsq = 4.0F * magcal.trB * magcal.trB;
			for (i = X; i <= Z; i++) {
				magcal.V[i] = magcal.trV[i];
				for (j = X; j <= Z; j++) {
					magcal.invW[i][j] = magcal.trinvW[i][j];
				}
			}
			return 1; // indicates new calibration applied
		}
	}
	return 0;
}



// 4 element calibration using 4x4 matrix inverse
static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
{
	float fBp2;					// fBp[X]^2+fBp[Y]^2+fBp[Z]^2
	float fSumBp4;				// sum of fBp2
	float fscaling;				// set to FUTPERCOUNT * FMATRIXSCALING
	float fE;					// error function = r^T.r
	int16_t iOffset[3];			// offset to remove large DC hard iron bias in matrix
	int16_t iCount;				// number of measurements counted
	int i, j, k;				// loop counters

	// working arrays for 4x4 matrix inversion
	float *pfRows[4];
	int8_t iColInd[4];
	int8_t iRowInd[4];
	int8_t iPivot[4];

	// compute fscaling to reduce multiplications later
	fscaling = FXOS8700_UTPERCOUNT / DEFAULTB;

	// the trial inverse soft iron matrix invW always equals
	// the identity matrix for 4 element calibration
	f3x3matrixAeqI(MagCal->trinvW);

	// zero fSumBp4=Y^T.Y, vecB=X^T.Y (4x1) and on and above
	// diagonal elements of matA=X^T*X (4x4)
	fSumBp4 = 0.0F;
	for (i = 0; i < 4; i++) {
		MagCal->vecB[i] = 0.0F;
		for (j = i; j < 4; j++) {
			MagCal->matA[i][j] = 0.0F;
		}
	}

	// the offsets are guaranteed to be set from the first element but to avoid compiler error
	iOffset[X] = iOffset[Y] = iOffset[Z] = 0;

	// use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices
	iCount = 0;
	for (j = 0; j < MAGBUFFSIZE; j++) {
		if (MagCal->valid[j]) {
			// use first valid magnetic buffer entry as estimate (in counts) for offset
			if (iCount == 0) {
				for (k = X; k <= Z; k++) {
					iOffset[k] = MagCal->BpFast[k][j];
				}
			}

			// store scaled and offset fBp[XYZ] in vecA[0-2] and fBp[XYZ]^2 in vecA[3-5]
			for (k = X; k <= Z; k++) {
				MagCal->vecA[k] = (float)((int32_t)MagCal->BpFast[k][j]
					- (int32_t)iOffset[k]) * fscaling;
				MagCal->vecA[k + 3] = MagCal->vecA[k] * MagCal->vecA[k];
			}

			// calculate fBp2 = Bp[X]^2 + Bp[Y]^2 + Bp[Z]^2 (scaled uT^2)
			fBp2 = MagCal->vecA[3] + MagCal->vecA[4] + MagCal->vecA[5];

			// accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
			fSumBp4 += fBp2 * fBp2;

			// now we have fBp2, accumulate vecB[0-2] = X^T.Y =sum(Bp2.Bp[XYZ])
			for (k = X; k <= Z; k++) {
				MagCal->vecB[k] += MagCal->vecA[k] * fBp2;
			}

			//accumulate vecB[3] = X^T.Y =sum(fBp2)
			MagCal->vecB[3] += fBp2;

			// accumulate on and above-diagonal terms of matA = X^T.X ignoring matA[3][3]
			MagCal->matA[0][0] += MagCal->vecA[X + 3];
			MagCal->matA[0][1] += MagCal->vecA[X] * MagCal->vecA[Y];
			MagCal->matA[0][2] += MagCal->vecA[X] * MagCal->vecA[Z];
			MagCal->matA[0][3] += MagCal->vecA[X];
			MagCal->matA[1][1] += MagCal->vecA[Y + 3];
			MagCal->matA[1][2] += MagCal->vecA[Y] * MagCal->vecA[Z];
			MagCal->matA[1][3] += MagCal->vecA[Y];
			MagCal->matA[2][2] += MagCal->vecA[Z + 3];
			MagCal->matA[2][3] += MagCal->vecA[Z];

			// increment the counter for next iteration
			iCount++;
		}
	}

	// set the last element of the measurement matrix to the number of buffer elements used
	MagCal->matA[3][3] = (float) iCount;

	// store the number of measurements accumulated
	MagCal->MagBufferCount = iCount;

	// use above diagonal elements of symmetric matA to set both matB and matA to X^T.X
	for (i = 0; i < 4; i++) {
		for (j = i; j < 4; j++) {
			MagCal->matB[i][j] = MagCal->matB[j][i]
				= MagCal->matA[j][i] = MagCal->matA[i][j];
		}
	}

	// calculate in situ inverse of matB = inv(X^T.X) (4x4) while matA still holds X^T.X
	for (i = 0; i < 4; i++) {
		pfRows[i] = MagCal->matB[i];
	}
	fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4);

	// calculate vecA = solution beta (4x1) = inv(X^T.X).X^T.Y = matB * vecB
	for (i = 0; i < 4; i++) {
		MagCal->vecA[i] = 0.0F;
		for (k = 0; k < 4; k++) {
			MagCal->vecA[i] += MagCal->matB[i][k] * MagCal->vecB[k];
		}
	}

	// calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
	// = fSumBp4 - 2 * vecA^T.vecB + vecA^T.matA.vecA
	// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = SumBp4 - 2 * vecA^T.vecB
	fE = 0.0F;
	for (i = 0; i < 4; i++) {
		fE += MagCal->vecA[i] * MagCal->vecB[i];
	}
	fE = fSumBp4 - 2.0F * fE;

	// set vecB = (X^T.X).beta = matA.vecA
	for (i = 0; i < 4; i++) {
		MagCal->vecB[i] = 0.0F;
		for (k = 0; k < 4; k++) {
			MagCal->vecB[i] += MagCal->matA[i][k] * MagCal->vecA[k];
		}
	}

	// complete calculation of P by adding beta^T.(X^T.X).beta = vecA^T * vecB
	for (i = 0; i < 4; i++) {
		fE += MagCal->vecB[i] * MagCal->vecA[i];
	}

	// compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
	for (k = X; k <= Z; k++) {
		MagCal->trV[k] = 0.5F * MagCal->vecA[k];
	}

	// compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
	MagCal->trB = sqrtf(MagCal->vecA[3] + MagCal->trV[X] * MagCal->trV[X] +
			MagCal->trV[Y] * MagCal->trV[Y] + MagCal->trV[Z] * MagCal->trV[Z]);

	// calculate the trial fit error (percent) normalized to number of measurements
	// and scaled geomagnetic field strength
	MagCal->trFitErrorpc = sqrtf(fE / (float) MagCal->MagBufferCount) * 100.0F /
			(2.0F * MagCal->trB * MagCal->trB);

	// correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
	for (k = X; k <= Z; k++) {
		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB
			+ (float)iOffset[k] * FXOS8700_UTPERCOUNT;
	}

	// correct the geomagnetic field strength B to correct scaling (result in uT)
	MagCal->trB *= DEFAULTB;
}










// 7 element calibration using direct eigen-decomposition
static void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
{
	float det;					// matrix determinant
	float fscaling;				// set to FUTPERCOUNT * FMATRIXSCALING
	float ftmp;					// scratch variable
	int16_t iOffset[3];			// offset to remove large DC hard iron bias
	int16_t iCount;				// number of measurements counted
	int i, j, k, m, n;			// loop counters

	// compute fscaling to reduce multiplications later
	fscaling = FXOS8700_UTPERCOUNT / DEFAULTB;

	// the offsets are guaranteed to be set from the first element but to avoid compiler error
	iOffset[X] = iOffset[Y] = iOffset[Z] = 0;

	// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix matA
	for (m = 0; m < 7; m++) {
		for (n = m; n < 7; n++) {
			MagCal->matA[m][n] = 0.0F;
		}
	}

	// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix matA
	iCount = 0;
	for (j = 0; j < MAGBUFFSIZE; j++) {
		if (MagCal->valid[j]) {
			// use first valid magnetic buffer entry as offset estimate (bit counts)
			if (iCount == 0) {
				for (k = X; k <= Z; k++) {
					iOffset[k] = MagCal->BpFast[k][j];
				}
			}

			// apply the offset and scaling and store in vecA
			for (k = X; k <= Z; k++) {
				MagCal->vecA[k + 3] = (float)((int32_t)MagCal->BpFast[k][j]
					- (int32_t)iOffset[k]) * fscaling;
				MagCal->vecA[k] = MagCal->vecA[k + 3] * MagCal->vecA[k + 3];
			}

			// accumulate the on-and above-diagonal terms of
			// MagCal->matA=Sigma{vecA^T * vecA}
			// with the exception of matA[6][6] which will sum to the number
			// of measurements and remembering that vecA[6] equals 1.0F
			// update the right hand column [6] of matA except for matA[6][6]
			for (m = 0; m < 6; m++) {
				MagCal->matA[m][6] += MagCal->vecA[m];
			}
			// update the on and above diagonal terms except for right hand column 6
			for (m = 0; m < 6; m++) {
				for (n = m; n < 6; n++) {
					MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
				}
			}

			// increment the measurement counter for the next iteration
			iCount++;
		}
	}

	// finally set the last element matA[6][6] to the number of measurements
	MagCal->matA[6][6] = (float) iCount;

	// store the number of measurements accumulated
	MagCal->MagBufferCount = iCount;

	// copy the above diagonal elements of matA to below the diagonal
	for (m = 1; m < 7; m++) {
		for (n = 0; n < m; n++) {
			MagCal->matA[m][n] = MagCal->matA[n][m];
		}
	}

	// set tmpA7x1 to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 7);

	// find the smallest eigenvalue
	j = 0;
	for (i = 1; i < 7; i++) {
		if (MagCal->vecA[i] < MagCal->vecA[j]) {
			j = i;
		}
	}

	// set ellipsoid matrix A to the solution vector with smallest eigenvalue,
	// compute its determinant and the hard iron offset (scaled and offset)
	f3x3matrixAeqScalar(MagCal->A, 0.0F);
	det = 1.0F;
	for (k = X; k <= Z; k++) {
		MagCal->A[k][k] = MagCal->matB[k][j];
		det *= MagCal->A[k][k];
		MagCal->trV[k] = -0.5F * MagCal->matB[k + 3][j] / MagCal->A[k][k];
	}

	// negate A if it has negative determinant
	if (det < 0.0F) {
		f3x3matrixAeqMinusA(MagCal->A);
		MagCal->matB[6][j] = -MagCal->matB[6][j];
		det = -det;
	}

	// set ftmp to the square of the trial geomagnetic field strength B
	// (counts times FMATRIXSCALING)
	ftmp = -MagCal->matB[6][j];
	for (k = X; k <= Z; k++) {
		ftmp += MagCal->A[k][k] * MagCal->trV[k] * MagCal->trV[k];
	}

	// calculate the trial normalized fit error as a percentage
	MagCal->trFitErrorpc = 50.0F *
		sqrtf(fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) / fabs(ftmp);

	// normalize the ellipsoid matrix A to unit determinant
	f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));

	// convert the geomagnetic field strength B into uT for normalized
	// soft iron matrix A and normalize
	MagCal->trB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));

	// compute trial invW from the square root of A also with normalized
	// determinant and hard iron offset in uT
	f3x3matrixAeqI(MagCal->trinvW);
	for (k = X; k <= Z; k++) {
		MagCal->trinvW[k][k] = sqrtf(fabs(MagCal->A[k][k]));
		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
	}
}





// 10 element calibration using direct eigen-decomposition
static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
{
	float det;					// matrix determinant
	float fscaling;				// set to FUTPERCOUNT * FMATRIXSCALING
	float ftmp;					// scratch variable
	int16_t iOffset[3];			// offset to remove large DC hard iron bias in matrix
	int16_t iCount;				// number of measurements counted
	int i, j, k, m, n;			// loop counters

	// compute fscaling to reduce multiplications later
	fscaling = FXOS8700_UTPERCOUNT / DEFAULTB;

	// the offsets are guaranteed to be set from the first element but to avoid compiler error
	iOffset[X] = iOffset[Y] = iOffset[Z] = 0;

	// zero the on and above diagonal elements of the 10x10 symmetric measurement matrix matA
	for (m = 0; m < 10; m++) {
		for (n = m; n < 10; n++) {
			MagCal->matA[m][n] = 0.0F;
		}
	}

	// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix matA
	iCount = 0;
	for (j = 0; j < MAGBUFFSIZE; j++) {
		if (MagCal->valid[j]) {
			// use first valid magnetic buffer entry as estimate for offset
			// to help solution (bit counts)
			if (iCount == 0) {
				for (k = X; k <= Z; k++) {
					iOffset[k] = MagCal->BpFast[k][j];
				}
			}

			// apply the fixed offset and scaling and enter into vecA[6-8]
			for (k = X; k <= Z; k++) {
				MagCal->vecA[k + 6] = (float)((int32_t)MagCal->BpFast[k][j]
					- (int32_t)iOffset[k]) * fscaling;
			}

			// compute measurement vector elements vecA[0-5] from vecA[6-8]
			MagCal->vecA[0] = MagCal->vecA[6] * MagCal->vecA[6];
			MagCal->vecA[1] = 2.0F * MagCal->vecA[6] * MagCal->vecA[7];
			MagCal->vecA[2] = 2.0F * MagCal->vecA[6] * MagCal->vecA[8];
			MagCal->vecA[3] = MagCal->vecA[7] * MagCal->vecA[7];
			MagCal->vecA[4] = 2.0F * MagCal->vecA[7] * MagCal->vecA[8];
			MagCal->vecA[5] = MagCal->vecA[8] * MagCal->vecA[8];

			// accumulate the on-and above-diagonal terms of matA=Sigma{vecA^T * vecA}
			// with the exception of matA[9][9] which equals the number of measurements
			// update the right hand column [9] of matA[0-8][9] ignoring matA[9][9]
			for (m = 0; m < 9; m++) {
				MagCal->matA[m][9] += MagCal->vecA[m];
			}
			// update the on and above diagonal terms of matA ignoring right hand column 9
			for (m = 0; m < 9; m++) {
				for (n = m; n < 9; n++) {
					MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
				}
			}

			// increment the measurement counter for the next iteration
			iCount++;
		}
	}

	// set the last element matA[9][9] to the number of measurements
	MagCal->matA[9][9] = (float) iCount;

	// store the number of measurements accumulated
	MagCal->MagBufferCount = iCount;

	// copy the above diagonal elements of symmetric product matrix matA to below the diagonal
	for (m = 1; m < 10; m++) {
		for (n = 0; n < m; n++) {
			MagCal->matA[m][n] = MagCal->matA[n][m];
		}
	}

	// set MagCal->vecA to the unsorted eigenvalues and matB to the unsorted
	// normalized eigenvectors of matA
	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 10);

	// set ellipsoid matrix A from elements of the solution vector column j with
	// smallest eigenvalue
	j = 0;
	for (i = 1; i < 10; i++) {
		if (MagCal->vecA[i] < MagCal->vecA[j]) {
			j = i;
		}
	}
	MagCal->A[0][0] = MagCal->matB[0][j];
	MagCal->A[0][1] = MagCal->A[1][0] = MagCal->matB[1][j];
	MagCal->A[0][2] = MagCal->A[2][0] = MagCal->matB[2][j];
	MagCal->A[1][1] = MagCal->matB[3][j];
	MagCal->A[1][2] = MagCal->A[2][1] = MagCal->matB[4][j];
	MagCal->A[2][2] = MagCal->matB[5][j];

	// negate entire solution if A has negative determinant
	det = f3x3matrixDetA(MagCal->A);
	if (det < 0.0F) {
		f3x3matrixAeqMinusA(MagCal->A);
		MagCal->matB[6][j] = -MagCal->matB[6][j];
		MagCal->matB[7][j] = -MagCal->matB[7][j];
		MagCal->matB[8][j] = -MagCal->matB[8][j];
		MagCal->matB[9][j] = -MagCal->matB[9][j];
		det = -det;
	}

	// compute the inverse of the ellipsoid matrix
	f3x3matrixAeqInvSymB(MagCal->invA, MagCal->A);

	// compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
	for (k = X; k <= Z; k++) {
		MagCal->trV[k] = 0.0F;
		for (m = X; m <= Z; m++) {
			MagCal->trV[k] += MagCal->invA[k][m] * MagCal->matB[m + 6][j];
		}
		MagCal->trV[k] *= -0.5F;
	}

	// compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
	MagCal->trB = sqrtf(fabs(MagCal->A[0][0] * MagCal->trV[X] * MagCal->trV[X] +
			2.0F * MagCal->A[0][1] * MagCal->trV[X] * MagCal->trV[Y] +
			2.0F * MagCal->A[0][2] * MagCal->trV[X] * MagCal->trV[Z] +
			MagCal->A[1][1] * MagCal->trV[Y] * MagCal->trV[Y] +
			2.0F * MagCal->A[1][2] * MagCal->trV[Y] * MagCal->trV[Z] +
			MagCal->A[2][2] * MagCal->trV[Z] * MagCal->trV[Z] - MagCal->matB[9][j]));

	// calculate the trial normalized fit error as a percentage
	MagCal->trFitErrorpc = 50.0F * sqrtf(
		fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) /
		(MagCal->trB * MagCal->trB);

	// correct for the measurement matrix offset and scaling and
	// get the computed hard iron offset in uT
	for (k = X; k <= Z; k++) {
		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
	}

	// convert the trial geomagnetic field strength B into uT for
	// un-normalized soft iron matrix A
	MagCal->trB *= DEFAULTB;

	// normalize the ellipsoid matrix A to unit determinant and
	// correct B by root of this multiplicative factor
	f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));
	MagCal->trB *= powf(det, -(ONESIXTH));

	// compute trial invW from the square root of fA (both with normalized determinant)
	// set vecA to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
	// where matA holds the 3x3 matrix fA in its top left elements
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			MagCal->matA[i][j] = MagCal->A[i][j];
		}
	}
	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 3);

	// set MagCal->matB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
	//   matB . diag(sqrt(sqrt(vecA))
	for (j = 0; j < 3; j++) { // loop over columns j
		ftmp = sqrtf(sqrtf(fabs(MagCal->vecA[j])));
		for (i = 0; i < 3; i++) { // loop over rows i
			MagCal->matB[i][j] *= ftmp;
		}
	}

	// set trinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
	//   matB * matB^T = sqrt(fA) (guaranteed symmetric)
	// loop over rows
	for (i = 0; i < 3; i++) {
		// loop over on and above diagonal columns
		for (j = i; j < 3; j++) {
			MagCal->trinvW[i][j] = 0.0F;
			// accumulate the matrix product
			for (k = 0; k < 3; k++) {
				MagCal->trinvW[i][j] += MagCal->matB[i][k] * MagCal->matB[j][k];
			}
			// copy to below diagonal element
			MagCal->trinvW[j][i] = MagCal->trinvW[i][j];
		}
	}
}