/**
 ******************************************************************************
 *
 * @file       MagOrAccelSensorCal.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      3 axis sensor cal from six measurements taken in a constant field.
 *             Call SixPointInConstFieldCal(FieldMagnitude, X, Y, Z, S, b)
 *             - FieldMagnitude is the constant field, e.g. 9.81 for accels
 *             - X, Y, Z are vectors of six measurements from different orientations
 *             - returns, S[3] and b[3], that are the scale and offsett for the axes
 *             - i.e. Measurementx = S[0]*Sensorx + b[0]
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */


#include <math.h>
#include "stdint.h"

//Function Prototypes
int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution);


int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
{
  int16_t i;
  double A[5][5];
  double f[5], c[5];
  double xp, yp, zp, Sx;

  // Fill in matrix A -
  // write six difference-in-magnitude equations of the form
  // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
  // or in other words
  // 2*Sx*bx*(x2-x1)/Sx^2  + Sy^2(y2^2-y1^2)/Sx^2  + 2*Sy*by*(y2-y1)/Sx^2  + Sz^2(z2^2-z1^2)/Sx^2  + 2*Sz*bz*(z2-z1)/Sx^2  = (x1^2-x2^2)
  for (i=0;i<5;i++){
      A[i][0] = 2.0 * (x[i+1] - x[i]);
      A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
      A[i][2] = 2.0 * (y[i+1] - y[i]);
      A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
      A[i][4] = 2.0 * (z[i+1] - z[i]);
      f[i]    = x[i]*x[i] - x[i+1]*x[i+1];
  }

  // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
  if (  !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;

  // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
  xp = x[0]; yp = y[0]; zp = z[0];
  Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));

  S[0] = Sx;
  b[0] = Sx*c[0];
  S[1] = sqrt(c[1]*Sx*Sx);
  b[1] = c[2]*Sx*Sx/S[1];
  S[2] = sqrt(c[3]*Sx*Sx);
  b[2] = c[4]*Sx*Sx/S[2];

  return 1;
}

//*****************************************************************

int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution)
{
  double fMaxElem;
  double fAcc;

  int16_t i , j, k, m;


  for(k=0; k<(nDim-1); k++) // base row of matrix
  {
    // search of line with max element
    fMaxElem = fabs( pfMatr[k*nDim + k] );
    m = k;
    for(i=k+1; i<nDim; i++)
    {
      if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
      {
        fMaxElem = pfMatr[i*nDim + k];
        m = i;
      }
    }

    // permutation of base line (index k) and max element line(index m)
    if(m != k)
    {
      for(i=k; i<nDim; i++)
      {
        fAcc               = pfMatr[k*nDim + i];
        pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
        pfMatr[m*nDim + i] = fAcc;
      }
      fAcc = pfVect[k];
      pfVect[k] = pfVect[m];
      pfVect[m] = fAcc;
    }

    if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!

    // triangulation of matrix with coefficients
    for(j=(k+1); j<nDim; j++) // current row of matrix
    {
      fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
      for(i=k; i<nDim; i++)
      {
        pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
      }
      pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
    }
  }

  for(k=(nDim-1); k>=0; k--)
  {
    pfSolution[k] = pfVect[k];
    for(i=(k+1); i<nDim; i++)
    {
      pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
    }
    pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
  }

  return 1;
}