/* 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.
 */
/** \file
*
* \section Description
* This header file contains declarations for using special HSI mathematics functions.
*/
#ifndef VNHSIMATH_H_INCLUDED
#define VNHSIMATH_H_INCLUDED

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <float.h> 
#include "vnamath.h"
#include "vnhsi.h"

#define TRUE   1
#define FALSE  0
#define ZERO   0.0f
#define ONE    1.0f
#define TWO    2.0f
#define VEKTOR 0
#define MACH_EPS DBL_EPSILON

#define ABS(x) (((x) >= 0.0f)? (x) : -(x))
#define SQRT(x) sqrt(x)
#define SQR(x) ((x) * (x))
#define SWAP(typ, a, b) { typ t; t = (a); (a) = (b); (b) = t; }
#define BASIS basis()
#define MAXIT 50     

#define SIGN(a,b) ((b) > 0.0f ? fabsf(a) : - fabsf(a))

#define MAXLLSSTATES 			9 		/* maximum number of states for linear least squares solver */

typedef struct {
	int n, max;
	float *mem;
} vmblock_t;

typedef struct vncomplex{
	float Re;
	float Im;
} vncomplex;

void n_eigeng(float *_a, int n, float *evalr, float *evali, float *_evec);
int svdcmp(float a[][3], int nRows, int nCols, float w[], float v[][3]);

/** \brief Calculate the attitude direction cosine matrix from the YPR Euler angles.
*
* \param[in] ypr The YPR Euler angles vector (in Radians).
* \param[out] c  The DCM matrix (as a 9 elements vector).
* \return None. */
void Euler3212C(float [], float []);

/** \brief Calculate YPR Euler angles from the attitude direction cosine matrix.
*
* \param[in] c  The DCM matrix (as a 9 elements vector).
* \return The YPR Euler angles vector (in Radians). */
union vn_vec3f C2Euler321(float C[]);

/** \brief Solve linear least squares for the selected data and calculates the FOM.
*
* \param[in] HTH A matrix formulated from the selected data measurements.
* \param[in] HTY A vector formulated from the selected data measurements.
* \param[in] n The size of the matrix HTH (n x n).
* \param[out] FOM A pointer to the FOM array.
* \return Error code. */
enum VnError vnhsi_solveLLS(float* HTH, float* HTY, unsigned int n, float FOMthreshold, float* X, float* FOM);


/** \brief Apply the calculated HSI to a magnetic measurement vector.
*
* \param[in] hsisoln The HSI solution structure.
* \param[in] mag The uncorrected magnetic vector.
* \return The corrected magnetic vector. */
union vn_vec3f vnhsi_ApplyHSI(VnHsiSol* hsisoln, union vn_vec3f mag);

#endif /*VNHSIMATH_H_INCLUDED*/