#ifndef __MAGENTO_1_4__
#define __MAGENTO_1_4__

// In https://github.com/jremington/MPU-9250-AHRS
// magneto 1.4 magnetometer/accelerometer calibration code
// from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html
// tested and works with Code::Blocks 10.05 through 20.03

// Command line version slightly modified from original, sjames_remington@gmail.com
// Now includes option to reject outliers in units of sigma, deviation of data vector length
// from mean values of the sample. Suggest using 2 as the rejection criterion

// comma separated ASCII input data file expected, three columns x, y, z

#include <stdio.h>
#include <math.h>
#include <malloc.h>
#include <string.h>
#include <float.h>

// Forward declarations of mymathlib.com routines
void Multiply_Self_Transpose(double*, double*, int, int);
void Get_Submatrix(double*, int, int, double*, int, int, int);
int Choleski_LU_Decomposition(double*, int);
int Choleski_LU_Inverse(double*, int);
void Multiply_Matrices(double*, double*, int, int, double*, int);
void Identity_Matrix(double*, int);

int Hessenberg_Form_Elementary(double*, double*, int);
void Hessenberg_Elementary_Transform(double*, double*, int[], int);

void Copy_Vector(double*, double*, int);

int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int);
void One_Real_Eigenvalue(double[], double[], double[], int, double);
void Two_Eigenvalues(double*, double*, double[], double[], int, int, double);
void Update_Row(double*, double, double, int, int);
void Update_Column(double*, double, double, int, int);
void Update_Transformation(double*, double, double, int, int);
void Double_QR_Iteration(double*, double*, int, int, int, double*, int);
void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int);
int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double);
void Double_QR_Step(double*, int, int, int, double, double, double*, int);
void BackSubstitution(double*, double[], double[], int);
void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int);
void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int);
void Calculate_Eigenvectors(double*, double*, double[], double[], int);
void Complex_Division(double, double, double, double, double*, double*);

void Transpose_Square_Matrix(double*, int);

int Lower_Triangular_Solve(double* L, double B[], double x[], int n);
int Lower_Triangular_Inverse(double* L, int n);
int Upper_Triangular_Solve(double* U, double B[], double x[], int n);

void Interchange_Rows(double* A, int row1, int row2, int ncols);
void Interchange_Columns(double* A, int col1, int col2, int nrows, int ncols);
void Identity_Matrix(double* A, int n);
void Copy_Vector(double* d, double* s, int n);

void Hessenberg_Elementary_Transform(double* H, double* S, int perm[], int n);

void One_Real_Eigenvalue(double Hrow[], double eigen_real[], double eigen_imag[], int row, double shift);
void Two_Eigenvalues(double* H, double* S, double eigen_real[], double eigen_imag[], int n, int k, double t);
void Update_Row(double* Hrow, double cos, double sin, int n, int k);
void Update_Column(double* H, double cos, double sin, int n, int k);
void Update_Transformation(double* S, double cos, double sin, int n, int k);
void Double_QR_Iteration(double* H, double* S, int row, int min_row, int n, double* shift, int iteration);
void Product_and_Sum_of_Shifts(double* H, int n, int max_row, double* shift, double* trace, double* det, int iteration);
int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, int max_row, int n, double trace, double det);
void Double_QR_Step(double* H, int min_row, int max_row, int min_col, double trace, double det, double* S, int n);
void Complex_Division(double x, double y, double u, double v, double* a, double* b);
void BackSubstitution(double* H, double eigen_real[], double eigen_imag[], int n);
void BackSubstitute_Real_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
void BackSubstitute_Complex_Vector(double* H, double eigen_real[], double eigen_imag[], int row, double zero_tolerance, int n);
void Calculate_Eigenvectors(double* H, double* S, double eigen_real[], double eigen_imag[], int n);

void CalculateCalibration(float *buf, int sampleCount, float BAinv[4][3]);

#endif // __MAGENTO_1_4__