// 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 matrix manipulation functions.
//

#include <inttypes.h>
#include <math.h>

// compile time constants that are private to this file
#define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix

// vector components
#define X 0
#define Y 1
#define Z 2

// function sets the 3x3 matrix A to the identity matrix
void f3x3matrixAeqI(float A[][3]) {
  float *pAij; // pointer to A[i][j]
  int8_t i, j; // loop counters

  for (i = 0; i < 3; i++) {
    // set pAij to &A[i][j=0]
    pAij = A[i];
    for (j = 0; j < 3; j++) {
      *(pAij++) = 0.0F;
    }
    A[i][i] = 1.0F;
  }
}

// function sets the matrix A to the identity matrix
void fmatrixAeqI(float *A[], int16_t rc) {
  // rc = rows and columns in A

  float *pAij; // pointer to A[i][j]
  int8_t i, j; // loop counters

  for (i = 0; i < rc; i++) {
    // set pAij to &A[i][j=0]
    pAij = A[i];
    for (j = 0; j < rc; j++) {
      *(pAij++) = 0.0F;
    }
    A[i][i] = 1.0F;
  }
}

// function sets every entry in the 3x3 matrix A to a constant scalar
void f3x3matrixAeqScalar(float A[][3], float Scalar) {
  float *pAij; // pointer to A[i][j]
  int8_t i, j; // counters

  for (i = 0; i < 3; i++) {
    // set pAij to &A[i][j=0]
    pAij = A[i];
    for (j = 0; j < 3; j++) {
      *(pAij++) = Scalar;
    }
  }
}

// function multiplies all elements of 3x3 matrix A by the specified scalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar) {
  float *pAij; // pointer to A[i][j]
  int8_t i, j; // loop counters

  for (i = 0; i < 3; i++) {
    // set pAij to &A[i][j=0]
    pAij = A[i];
    for (j = 0; j < 3; j++) {
      *(pAij++) *= Scalar;
    }
  }
}

// function negates all elements of 3x3 matrix A
void f3x3matrixAeqMinusA(float A[][3]) {
  float *pAij; // pointer to A[i][j]
  int8_t i, j; // loop counters

  for (i = 0; i < 3; i++) {
    // set pAij to &A[i][j=0]
    pAij = A[i];
    for (j = 0; j < 3; j++) {
      *pAij = -*pAij;
      pAij++;
    }
  }
}

// function directly calculates the symmetric inverse of a symmetric 3x3 matrix
// only the on and above diagonal terms in B are used and need to be specified
void f3x3matrixAeqInvSymB(float A[][3], float B[][3]) {
  float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
  float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
  float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
  float ftmp;           // determinant and then reciprocal

  // calculate useful products
  fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
  fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
  fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];

  // set ftmp to the determinant of the input matrix B
  ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 +
         B[0][2] * fB01B12mB11B02;

  // set A to the inverse of B for any determinant except zero
  if (ftmp != 0.0F) {
    ftmp = 1.0F / ftmp;
    A[0][0] = fB11B22mB12B12 * ftmp;
    A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
    A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
    A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
    A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
    A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
  } else {
    // provide the identity matrix if the determinant is zero
    f3x3matrixAeqI(A);
  }
}

// function calculates the determinant of a 3x3 matrix
float f3x3matrixDetA(float A[][3]) {
  return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
          A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
          A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
}

// function computes all eigenvalues and eigenvectors of a real symmetric matrix
// A[0..n-1][0..n-1] stored in the top left of a 10x10 array A[10][10] A[][] is
// changed on output. eigval[0..n-1] returns the eigenvalues of A[][].
// eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
// the eigenvectors are not sorted by value
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) {
  // maximum number of iterations to achieve convergence: in practice 6 is
  // typical
#define NITERATIONS 15

  // various trig functions of the jacobi rotation angle phi
  float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
  // scratch variable to prevent over-writing during rotations
  float ftmp;
  // residue from remaining non-zero above diagonal terms
  float residue;
  // matrix row and column indices
  int8_t ir, ic;
  // general loop counter
  int8_t j;
  // timeout ctr for number of passes of the algorithm
  int8_t ctr;

  // initialize eigenvectors matrix and eigenvalues array
  for (ir = 0; ir < n; ir++) {
    // loop over all columns
    for (ic = 0; ic < n; ic++) {
      // set on diagonal and off-diagonal elements to zero
      eigvec[ir][ic] = 0.0F;
    }

    // correct the diagonal elements to 1.0
    eigvec[ir][ir] = 1.0F;

    // initialize the array of eigenvalues to the diagonal elements of m
    eigval[ir] = A[ir][ir];
  }

  // initialize the counter and loop until converged or NITERATIONS reached
  ctr = 0;
  do {
    // compute the absolute value of the above diagonal elements as exit
    // criterion
    residue = 0.0F;
    // loop over rows excluding last row
    for (ir = 0; ir < n - 1; ir++) {
      // loop over above diagonal columns
      for (ic = ir + 1; ic < n; ic++) {
        // accumulate the residual off diagonal terms which are being driven to
        // zero
        residue += fabsf(A[ir][ic]);
      }
    }

    // check if we still have work to do
    if (residue > 0.0F) {
      // loop over all rows with the exception of the last row (since only
      // rotating above diagonal elements)
      for (ir = 0; ir < n - 1; ir++) {
        // loop over columns ic (where ic is always greater than ir since above
        // diagonal)
        for (ic = ir + 1; ic < n; ic++) {
          // only continue with this element if the element is non-zero
          if (fabsf(A[ir][ic]) > 0.0F) {
            // calculate cot(2*phi) where phi is the Jacobi rotation angle
            cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);

            // calculate tan(phi) correcting sign to ensure the smaller solution
            // is used
            tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
            if (cot2phi < 0.0F) {
              tanphi = -tanphi;
            }

            // calculate the sine and cosine of the Jacobi rotation angle phi
            cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
            sinphi = tanphi * cosphi;

            // calculate tan(phi/2)
            tanhalfphi = sinphi / (1.0F + cosphi);

            // set tmp = tan(phi) times current matrix element used in update of
            // leading diagonal elements
            ftmp = tanphi * A[ir][ic];

            // apply the jacobi rotation to diagonal elements [ir][ir] and
            // [ic][ic] stored in the eigenvalue array eigval[ir] = eigval[ir] -
            // tan(phi) *  A[ir][ic]
            eigval[ir] -= ftmp;
            // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
            eigval[ic] += ftmp;

            // by definition, applying the jacobi rotation on element ir, ic
            // results in 0.0
            A[ir][ic] = 0.0F;

            // apply the jacobi rotation to all elements of the eigenvector
            // matrix
            for (j = 0; j < n; j++) {
              // store eigvec[j][ir]
              ftmp = eigvec[j][ir];
              // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] +
              // tan(phi/2) * eigvec[j][ir])
              eigvec[j][ir] =
                  ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
              // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] -
              // tan(phi/2) * eigvec[j][ic])
              eigvec[j][ic] =
                  eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
            }

            // apply the jacobi rotation only to those elements of matrix m that
            // can change
            for (j = 0; j <= ir - 1; j++) {
              // store A[j][ir]
              ftmp = A[j][ir];
              // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) *
              // A[j][ir])
              A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
              // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) *
              // A[j][ic])
              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
            }
            for (j = ir + 1; j <= ic - 1; j++) {
              // store A[ir][j]
              ftmp = A[ir][j];
              // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) *
              // A[ir][j])
              A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
              // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) *
              // A[j][ic])
              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
            }
            for (j = ic + 1; j < n; j++) {
              // store A[ir][j]
              ftmp = A[ir][j];
              // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) *
              // A[ir][j])
              A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
              // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) *
              // A[ic][j])
              A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
            }
          } // end of test for matrix element already zero
        } // end of loop over columns
      } // end of loop over rows
    } // end of test for non-zero residue
  } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
}

// function uses Gauss-Jordan elimination to compute the inverse of matrix A in
// situ on exit, A is replaced with its inverse
void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
                    int8_t iPivot[], int8_t isize) {
  float largest;               // largest element used for pivoting
  float scaling;               // scaling factor in pivoting
  float recippiv;              // reciprocal of pivot element
  float ftmp;                  // temporary variable used in swaps
  int8_t i, j, k, l, m;        // index counters
  int8_t iPivotRow, iPivotCol; // row and column of pivot element

  // to avoid compiler warnings
  iPivotRow = iPivotCol = 0;

  // initialize the pivot array to 0
  for (j = 0; j < isize; j++) {
    iPivot[j] = 0;
  }

  // main loop i over the dimensions of the square matrix A
  for (i = 0; i < isize; i++) {
    // zero the largest element found for pivoting
    largest = 0.0F;
    // loop over candidate rows j
    for (j = 0; j < isize; j++) {
      // check if row j has been previously pivoted
      if (iPivot[j] != 1) {
        // loop over candidate columns k
        for (k = 0; k < isize; k++) {
          // check if column k has previously been pivoted
          if (iPivot[k] == 0) {
            // check if the pivot element is the largest found so far
            if (fabsf(A[j][k]) >= largest) {
              // and store this location as the current best candidate for
              // pivoting
              iPivotRow = j;
              iPivotCol = k;
              largest = (float)fabsf(A[iPivotRow][iPivotCol]);
            }
          } else if (iPivot[k] > 1) {
            // zero determinant situation: exit with identity matrix
            fmatrixAeqI(A, isize);
            return;
          }
        }
      }
    }
    // increment the entry in iPivot to denote it has been selected for pivoting
    iPivot[iPivotCol]++;

    // check the pivot rows iPivotRow and iPivotCol are not the same before
    // swapping
    if (iPivotRow != iPivotCol) {
      // loop over columns l
      for (l = 0; l < isize; l++) {
        // and swap all elements of rows iPivotRow and iPivotCol
        ftmp = A[iPivotRow][l];
        A[iPivotRow][l] = A[iPivotCol][l];
        A[iPivotCol][l] = ftmp;
      }
    }

    // record that on the i-th iteration rows iPivotRow and iPivotCol were
    // swapped
    iRowInd[i] = iPivotRow;
    iColInd[i] = iPivotCol;

    // check for zero on-diagonal element (singular matrix) and return with
    // identity matrix if detected
    if (A[iPivotCol][iPivotCol] == 0.0F) {
      // zero determinant situation: exit with identity matrix
      fmatrixAeqI(A, isize);
      return;
    }

    // calculate the reciprocal of the pivot element knowing it's non-zero
    recippiv = 1.0F / A[iPivotCol][iPivotCol];
    // by definition, the diagonal element normalizes to 1
    A[iPivotCol][iPivotCol] = 1.0F;
    // multiply all of row iPivotCol by the reciprocal of the pivot element
    // including the diagonal element the diagonal element
    // A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its
    // previous value
    for (l = 0; l < isize; l++) {
      A[iPivotCol][l] *= recippiv;
    }
    // loop over all rows m of A
    for (m = 0; m < isize; m++) {
      if (m != iPivotCol) {
        // scaling factor for this row m is in column iPivotCol
        scaling = A[m][iPivotCol];
        // zero this element
        A[m][iPivotCol] = 0.0F;
        // loop over all columns l of A and perform elimination
        for (l = 0; l < isize; l++) {
          A[m][l] -= A[iPivotCol][l] * scaling;
        }
      }
    }
  } // end of loop i over the matrix dimensions

  // finally, loop in inverse order to apply the missing column swaps
  for (l = isize - 1; l >= 0; l--) {
    // set i and j to the two columns to be swapped
    i = iRowInd[l];
    j = iColInd[l];

    // check that the two columns i and j to be swapped are not the same
    if (i != j) {
      // loop over all rows k to swap columns i and j of A
      for (k = 0; k < isize; k++) {
        ftmp = A[k][i];
        A[k][i] = A[k][j];
        A[k][j] = ftmp;
      }
    }
  }
}

// function re-orthonormalizes a 3x3 rotation matrix
void fmatrixAeqRenormRotA(float A[][3]) {
  float ftmp; // scratch variable

  // normalize the X column of the low pass filtered orientation matrix
  ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
  if (ftmp > CORRUPTMATRIX) {
    // normalize the x column vector
    ftmp = 1.0F / ftmp;
    A[X][X] *= ftmp;
    A[Y][X] *= ftmp;
    A[Z][X] *= ftmp;
  } else {
    // set x column vector to {1, 0, 0}
    A[X][X] = 1.0F;
    A[Y][X] = A[Z][X] = 0.0F;
  }

  // force the y column vector to be orthogonal to x using y = y-(x.y)x
  ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
  A[X][Y] -= ftmp * A[X][X];
  A[Y][Y] -= ftmp * A[Y][X];
  A[Z][Y] -= ftmp * A[Z][X];

  // normalize the y column vector
  ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
  if (ftmp > CORRUPTMATRIX) {
    // normalize the y column vector
    ftmp = 1.0F / ftmp;
    A[X][Y] *= ftmp;
    A[Y][Y] *= ftmp;
    A[Z][Y] *= ftmp;
  } else {
    // set y column vector to {0, 1, 0}
    A[Y][Y] = 1.0F;
    A[X][Y] = A[Z][Y] = 0.0F;
  }

  // finally set the z column vector to x vector cross y vector (automatically
  // normalized)
  A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
  A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
  A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
}
