/*
    * Copyright (C) 2012 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.algebra;
  
  /**
   * This class contains a series of helper statistics methods that can be used
   * to perform most typical operations in matrix algebra.
   * Note: Depending on the situation it might be more computationally efficient
   * to use methods implemented in Decomposer subclasses.
   */
  @SuppressWarnings("DuplicatedCode")
  public class Utils {
  
      /**
       * Constant defining the default threshold to compare the different content
       * of one matrix to check whether is symmetric.
       */
      public static final double DEFAULT_SYMMETRIC_THRESHOLD = 1e-12;
  
      /**
       * Constant defining the default threshold to determine whether a matrix is
       * orthonormal or not.
       */
      public static final double DEFAULT_ORTHOGONAL_THRESHOLD = 1e-12;
  
      /**
       * Constant defining machine precision
       */
      public static final double EPSILON = 1e-12;
  
      /**
       * Constant defining minimum allowed threshold
       */
      public static final double MIN_THRESHOLD = 0.0;
  
      /**
       * Constructor.
       */
      private Utils() {
      }
  
      /**
       * Computes trace of provided matrix.
       * Trace is the sum of the elements located on the diagonal of a given
       * matrix.
       *
       * @param m Input matrix.
       * @return Trace of provided matrix
       */
      public static double trace(final Matrix m) {
          final var minSize = Math.min(m.getRows(), m.getColumns());
          var trace = 0.0;
          for (var i = 0; i < minSize; i++) {
              trace += m.getElementAt(i, i);
          }
          return trace;
      }
  
      /**
       * Computes condition number of provided matrix.
       * Condition number determines how well-behaved a matrix is for solving
       * a linear system of equations
       *
       * @param m Input matrix.
       * @return Condition number of provided matrix.
       * @throws DecomposerException Exception thrown if decomposition fails for
       *                             any reason.
       * @see SingularValueDecomposer
       */
      public static double cond(final Matrix m) throws DecomposerException {
          final var decomposer = new SingularValueDecomposer(m);
          try {
              decomposer.decompose();
              return decomposer.getConditionNumber();
          } catch (final DecomposerException e) {
              throw e;
          } catch (final Exception e) {
              throw new DecomposerException(e);
          }
      }
  
      /**
       * Computes rank of provided matrix.
       * Rank indicates the number of linearly independent columns or rows on
       * a matrix.
       *
      * @param m Input matrix.
      * @return Rank of provided matrix.
      * @throws DecomposerException Exception thrown if decomposition fails for
      *                             any reason.
      */
     public static int rank(final Matrix m) throws DecomposerException {
         final var decomposer = new SingularValueDecomposer(m);
         try {
             decomposer.decompose();
             return decomposer.getRank();
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Computes determinant of provided matrix.
      * Determinant can be seen geometrically as a measure of hyper-volume
      * generated by the row/column vector contained within a given squared
      * matrix.
      *
      * @param m Input matrix.
      * @return Determinant of provided matrix.
      * @throws WrongSizeException  Exception thrown if provided matrix is not
      *                             squared.
      * @throws DecomposerException Exception thrown if decomposition fails for
      *                             any reason.
      */
     public static double det(final Matrix m) throws WrongSizeException, DecomposerException {
         if (m.getRows() != m.getColumns()) {
             throw new WrongSizeException();
         }
 
         final var decomposer = new LUDecomposer(m);
         try {
             decomposer.decompose();
             return decomposer.determinant();
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Solves a linear system of equations of the form: m * x = b.
      * Where x is the returned matrix containing the solution, m is the matrix
      * of the linear system of equations and b is the parameters matrix.
      *
      * @param m      Matrix of the linear system of equations.
      * @param b      Parameters matrix.
      * @param result Matrix where solution of the linear system of equations
      *               will be stored. Provided matrix will be resized if needed
      * @throws WrongSizeException           Exception thrown if provided matrix m has less
      *                                      rows than columns, or if provided matrix b has a number of rows different
      *                                      from the number of rows of m.
      * @throws RankDeficientMatrixException Exception thrown if provided matrix
      *                                      m is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition fails for
      *                                      any reason.
      */
     public static void solve(final Matrix m, final Matrix b, final Matrix result)
             throws WrongSizeException, RankDeficientMatrixException, DecomposerException {
         if (m.getRows() == m.getColumns()) {
             final var decomposer = new LUDecomposer(m);
             try {
                 decomposer.decompose();
                 decomposer.solve(b, result);
             } catch (final WrongSizeException | DecomposerException e) {
                 throw e;
             } catch (final SingularMatrixException e) {
                 throw new RankDeficientMatrixException(e);
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         } else {
             final var decomposer = new EconomyQRDecomposer(m);
             try {
                 decomposer.decompose();
                 decomposer.solve(b, result);
             } catch (final WrongSizeException | RankDeficientMatrixException e) {
                 throw e;
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         }
     }
 
     /**
      * Solves a linear system of equations of the form: m * x = b.
      * Where x is the returned matrix containing the solution, m is the matrix
      * of the linear system of equations and b is the parameter matrix.
      *
      * @param m Matrix of the linear system of equations.
      * @param b Parameters matrix.
      * @return Solution of the linear system of equations.
      * @throws WrongSizeException           Exception thrown if provided matrix m has fewer
      *                                      rows than columns, or if provided matrix b has a number of rows different
      *                                      from the number of rows of m.
      * @throws RankDeficientMatrixException Exception thrown if provided matrix
      *                                      m is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition fails for
      *                                      any reason.
      */
     public static Matrix solve(final Matrix m, final Matrix b) throws WrongSizeException, RankDeficientMatrixException,
             DecomposerException {
         if (m.getRows() == m.getColumns()) {
             final var decomposer = new LUDecomposer(m);
             try {
                 decomposer.decompose();
                 return decomposer.solve(b);
             } catch (final WrongSizeException | DecomposerException e) {
                 throw e;
             } catch (final SingularMatrixException e) {
                 throw new RankDeficientMatrixException(e);
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         } else {
             final var decomposer = new EconomyQRDecomposer(m);
             try {
                 decomposer.decompose();
                 return decomposer.solve(b);
             } catch (final WrongSizeException | RankDeficientMatrixException e) {
                 throw e;
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         }
     }
 
     /**
      * Solves a linear system of equations of the form m * x = b, where b is
      * assumed to be the parameters column vector, x is the returned matrix
      * containing the solution and m is the matrix of the linear system of
      * equations.
      *
      * @param m      Matrix of the linear system of equations.
      * @param b      Parameters column vector.
      * @param result Solution of the linear system of equations.
      *               m is rank deficient.
      * @throws DecomposerException Exception thrown if decomposition fails for
      *                             any reason.
      */
     public static void solve(final Matrix m, final double[] b, final double[] result) throws DecomposerException {
         if (m.getRows() == m.getColumns()) {
             final var decomposer = new LUDecomposer(m);
             try {
                 decomposer.decompose();
                 System.arraycopy(decomposer.solve(Matrix.newFromArray(b, true)).toArray(true),
                         0, result, 0, result.length);
             } catch (final DecomposerException e) {
                 throw e;
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         } else {
             final var decomposer = new EconomyQRDecomposer(m);
             try {
                 decomposer.decompose();
                 System.arraycopy(decomposer.solve(Matrix.newFromArray(b, true)).toArray(true),
                         0, result, 0, result.length);
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         }
     }
 
     /**
      * Solves a linear system of equations of the form: m * x = b.
      * Where x is the returned array containing the solution, m is the matrix
      * of the linear system of equations and b is the parameters array.
      *
      * @param m Matrix of the linear system of equations.
      * @param b Parameters array.
      * @return Solution of the linear system of equations.
      * m is rank deficient.
      * @throws DecomposerException Exception thrown if decomposition fails for
      *                             any reason.
      */
     public static double[] solve(final Matrix m, double[] b) throws DecomposerException {
         if (m.getRows() == m.getColumns()) {
             final var decomposer = new LUDecomposer(m);
             try {
                 decomposer.decompose();
                 return decomposer.solve(Matrix.newFromArray(b, true)).toArray(true);
             } catch (final DecomposerException e) {
                 throw e;
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         } else {
             final var decomposer = new EconomyQRDecomposer(m);
             try {
                 decomposer.decompose();
                 return decomposer.solve(Matrix.newFromArray(b, true)).toArray(true);
             } catch (final Exception e) {
                 throw new DecomposerException(e);
             }
         }
     }
 
     /**
      * Computes Frobenius norm of provided input matrix.
      *
      * @param m Input matrix.
      * @return Frobenius norm
      */
     public static double normF(final Matrix m) {
         return FrobeniusNormComputer.norm(m);
     }
 
     /**
      * Computes Frobenius norm of provided array.
      *
      * @param array Input array.
      * @return Frobenius norm.
      */
     public static double normF(final double[] array) {
         return FrobeniusNormComputer.norm(array);
     }
 
     /**
      * Computes infinity norm of provided input matrix.
      *
      * @param m Input matrix.
      * @return Infinity norm.
      */
     public static double normInf(final Matrix m) {
         return InfinityNormComputer.norm(m);
     }
 
     /**
      * Computes infinity norm of provided input matrix.
      *
      * @param array Input array.
      * @return Infinity norm.
      */
     public static double normInf(final double[] array) {
         return InfinityNormComputer.norm(array);
     }
 
     /**
      * Computes two norm of provided input matrix.
      *
      * @param m Input matrix.
      * @return Two norm.
      * @throws DecomposerException Exception thrown if decomposition fails
      *                             for any reason.
      */
     public static double norm2(final Matrix m) throws DecomposerException {
         final var decomposer = new SingularValueDecomposer(m);
         try {
             decomposer.decompose();
             return decomposer.getNorm2();
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Computes two norm of provided input array.
      *
      * @param array Input array.
      * @return Two norm
      * @throws DecomposerException Exception thrown if decomposition fails
      *                             for any reason.
      */
     public static double norm2(final double[] array) throws DecomposerException {
         final var decomposer = new SingularValueDecomposer(Matrix.newFromArray(array, true));
         try {
             decomposer.decompose();
             return decomposer.getNorm2();
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Computes one norm of provided input matrix.
      *
      * @param m Input matrix.
      * @return One norm.
      */
     public static double norm1(final Matrix m) {
         return OneNormComputer.norm(m);
     }
 
     /**
      * Computes one norm of provided input matrix.
      *
      * @param array Input array.
      * @return One norm.
      */
     public static double norm1(final double[] array) {
         return OneNormComputer.norm(array);
     }
 
     /**
      * Computes matrix inverse if provided matrix is squared, or pseudo-inverse
      * otherwise and stores the result in provided result matrix. Result matrix
      * will be resized if needed
      *
      * @param m      Matrix to be inverted.
      * @param result Matrix where matrix inverse is stored.
      * @throws WrongSizeException           Exception thrown if provided matrix m has less
      *                                      rows than columns.
      * @throws RankDeficientMatrixException Exception thrown if provided matrix
      *                                      m is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition to
      *                                      compute inverse fails for any other reason.
      */
     public static void inverse(final Matrix m, final Matrix result)
             throws WrongSizeException, RankDeficientMatrixException, DecomposerException {
         final int rows = m.getRows();
         final int columns = m.getColumns();
         if (result.getRows() != columns || result.getColumns() != rows) {
             // resize result matrix
             result.resize(columns, rows);
         }
         Utils.solve(m, Matrix.identity(rows, rows), result);
     }
 
     /**
      * Computes matrix inverse if provided matrix is squared, or pseudo-inverse
      * otherwise.
      *
      * @param m Matrix to be inverted.
      * @return Matrix inverse.
      * @throws WrongSizeException           Exception thrown if provided matrix m has less
      *                                      rows than columns.
      * @throws RankDeficientMatrixException Exception thrown if provided matrix
      *                                      m is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition to
      *                                      compute inverse fails for any other reason.
      */
     public static Matrix inverse(final Matrix m) throws WrongSizeException, RankDeficientMatrixException,
             DecomposerException {
         final var rows = m.getRows();
         return Utils.solve(m, Matrix.identity(rows, rows));
     }
 
     /**
      * Computes array pseudo-inverse considering it as a column matrix and
      * stores the result in provided result matrix. Result matrix will be
      * resized if needed
      *
      * @param array  array to be inverted
      * @param result Array pseudo inverse
      * @throws WrongSizeException           never thrown
      * @throws RankDeficientMatrixException Exception thrown if provided array
      *                                      is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition to compute
      *                                      inverse fails for any other reason.
      */
     public static void inverse(final double[] array, final Matrix result) throws WrongSizeException,
             RankDeficientMatrixException, DecomposerException {
         if (result.getRows() != array.length || result.getColumns() != array.length) {
             // resize result matrix
             result.resize(array.length, array.length);
         }
         Utils.solve(Matrix.newFromArray(array, true), Matrix.identity(array.length, array.length), result);
     }
 
     /**
      * Computes array pseudo-inverse considering it as a column matrix.
      *
      * @param array array to be inverted
      * @return Array pseudo inverse
      * @throws WrongSizeException           never thrown
      * @throws RankDeficientMatrixException Exception thrown if provided array
      *                                      is rank deficient.
      * @throws DecomposerException          Exception thrown if decomposition to compute
      *                                      inverse fails for any other reason.
      */
     public static Matrix inverse(final double[] array) throws WrongSizeException,
             RankDeficientMatrixException, DecomposerException {
         final var length = array.length;
         final var identity = Matrix.identity(length, length);
         return Utils.solve(Matrix.newFromArray(array, true), identity);
     }
 
     /**
      * Computes Moore-Penrose pseudo-inverse of provided matrix.
      * Moore-Penrose pseudo-inverse always exists for any matrix regardless of
      * its size, and can be computed as: pinv(A) = inv(A'*A)*A' in Matlab
      * notation.
      * However, for computation efficiency, Singular Value Decomposition is
      * used instead, obtaining the following expression: pinv(A) =
      * V * pinv(W) * U'. Where W is easily invertible since it is a diagonal
      * matrix taking into account that the inverse for singular values close to
      * zero (for a given tolerance) is zero, whereas for the rest is their
      * reciprocal.
      * In other words pinv(W)(i,i) = 1./W(i,i) for W(i,i) != 0 or zero
      * otherwise.
      * Notice that for invertible squared matrices, the pseudo-inverse is equal
      * to the inverse.
      * Also notice that pseudo-inverse can be used to solve overdetermined
      * systems of linear equations with least minimum square error (LMSE).
      * Finally, notice that Utils.inverse(Matrix) also can return a
      * pseudo-inverse, however, this method since it uses SVD decomposition is
      * numerically more stable at the expense of being computationally more
      * expensive.
      *
      * @param m Input matrix.
      * @return Moore-Penrose matrix pseudo-inverse.
      * @throws DecomposerException Exception thrown if matrix cannot be
      *                             inverted, usually because matrix contains numerically unstable values
      *                             such as NaN or inf.
      * @see SingularValueDecomposer#solve(double[]) to
      * solve systems of linear equations, as it can be more efficient specially
      * when several systems need to be solved using the same input system
      * matrix.
      */
     public static Matrix pseudoInverse(final Matrix m) throws DecomposerException {
         final var decomposer = new SingularValueDecomposer(m);
         try {
             decomposer.decompose();
             final var u = decomposer.getU();
             final var v = decomposer.getV();
             final var s = decomposer.getSingularValues();
             final var rows = m.getRows();
             final var cols = m.getColumns();
             final var threshold = EPSILON * Math.max(rows, cols) * s[0];
             final var invW = new Matrix(cols, cols);
             invW.initialize(0.0);
             double value;
             for (var n = 0; n < cols; n++) {
                 value = s[n];
                 if (value > threshold) {
                     invW.setElementAt(n, n, 1.0 / value);
                 }
             }
             // V * invW * U', where U' is transposed of U
             u.transpose();
             return v.multiplyAndReturnNew(invW.multiplyAndReturnNew(u));
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Computes Moore-Penrose pseudo-inverse of provided array considering it as
      * a column matrix.
      *
      * @param array Input array
      * @return More-Penrose pseudo-inverse
      * @throws DecomposerException Exception thrown if matrix cannot be
      *                             inverted, usually because matrix contains numerically unstable values
      *                             such as NaN or inf.
      * @see #pseudoInverse(Matrix)
      */
     public static Matrix pseudoInverse(final double[] array) throws DecomposerException {
         final var decomposer = new SingularValueDecomposer(Matrix.newFromArray(array, true));
         try {
             decomposer.decompose();
             final var u = decomposer.getU();
             final var v = decomposer.getV();
             final var s = decomposer.getSingularValues();
             final var rows = array.length;
             final var cols = 1;
             final var threshold = EPSILON * Math.max(rows, cols) * s[0];
             final var invW = new Matrix(cols, cols);
             invW.initialize(0.0);
             double value;
             for (var n = 0; n < cols; n++) {
                 value = s[n];
                 if (value > threshold) {
                     invW.setElementAt(n, n, 1.0 / value);
                 }
             }
             // V * invW * U', where U' is transposed of U
             u.transpose();
             return v.multiplyAndReturnNew(invW.multiplyAndReturnNew(u));
         } catch (final DecomposerException e) {
             throw e;
         } catch (final Exception e) {
             throw new DecomposerException(e);
         }
     }
 
     /**
      * Computes the skew-symmetric matrix of provided vector of length 3 and
      * stores the result in provided matrix.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      *
      * @param array  Array of length 3 that will be used to compute the skew-symmetric
      *               matrix.
      * @param result Matrix where skew matrix is stored. Provided matrix will be
      *               resized if needed
      * @throws WrongSizeException Exception thrown if provided array doesn't
      *                            have length equal to 3.
      */
     public static void skewMatrix(final double[] array, final Matrix result) throws WrongSizeException {
         if (array.length != 3) {
             throw new WrongSizeException("array length must be 3");
         }
 
         if (result.getRows() != 3 || result.getColumns() != 3) {
             result.resize(3, 3);
         }
         result.initialize(0.0);
 
         result.setElementAt(0, 1, -array[2]);
         result.setElementAt(0, 2, array[1]);
         result.setElementAt(1, 0, array[2]);
         result.setElementAt(1, 2, -array[0]);
         result.setElementAt(2, 0, -array[1]);
         result.setElementAt(2, 1, array[0]);
     }
 
     /**
      * Computes the skew-symmetric matrix of provided vector of length 3 and
      * stores the result in provided matrix.
      * If provided, jacobian is also set.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      *
      * @param array    Array of length 3 that will be used to compute the skew-symmetric
      *                 matrix.
      * @param result   Matrix where skew matrix is stored. Provided matrix will be
      *                 resized if needed
      * @param jacobian matrix where jacobian will be stored. Must be 9x3.
      * @throws WrongSizeException if provided array doesn't have length 3 or
      *                            if jacobian is not 9x3 (if provided).
      */
     public static void skewMatrix(final double[] array, final Matrix result, final Matrix jacobian)
             throws WrongSizeException {
         if (jacobian != null && (jacobian.getRows() != 9 || jacobian.getColumns() != 3)) {
             throw new WrongSizeException("jacobian must be 9x3");
         }
 
         skewMatrix(array, result);
 
         if (jacobian != null) {
             jacobian.initialize(0.0);
             jacobian.setElementAt(5, 0, 1.0);
             jacobian.setElementAt(7, 0, -1.0);
 
             jacobian.setElementAt(2, 1, -1.0);
             jacobian.setElementAt(6, 1, 1.0);
 
             jacobian.setElementAt(1, 2, 1.0);
             jacobian.setElementAt(3, 2, -1.0);
         }
     }
 
     /**
      * Computes the skew-symmetric matrix of provided vector of length 3.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      *
      * @param array Array of length 3 that will be used to compute the skew-symmetric
      *              matrix.
      * @return Skew matrix.
      * @throws WrongSizeException Exception thrown if provided array doesn't
      *                            have length equal to 3.
      */
     public static Matrix skewMatrix(final double[] array) throws WrongSizeException {
         final var sk = new Matrix(3, 3);
         skewMatrix(array, sk);
         return sk;
     }
 
     /**
      * Internal method to compute skew matrix
      *
      * @param m          Matrix of size 3x1 or 1x3 that will be used to compute the
      *                   skew-symmetric matrix.
      * @param result     Matrix where skew matrix is stored
      * @param columnwise boolean indicating whether m is column-wise
      * @throws WrongSizeException Exception thrown if provided m matrix doesn't
      *                            have proper size
      */
     private static void internalSkewMatrix(
             final Matrix m, final Matrix result, final boolean columnwise) throws WrongSizeException {
         if (result.getRows() != 3 || result.getColumns() != 3) {
             // resize result
             result.resize(3, 3);
         }
 
         result.initialize(0.0);
 
         result.setElementAt(0, 1, -m.getElementAtIndex(2, columnwise));
         result.setElementAt(0, 2, m.getElementAtIndex(1, columnwise));
         result.setElementAt(1, 0, m.getElementAtIndex(2, columnwise));
         result.setElementAt(1, 2, -m.getElementAtIndex(0, columnwise));
         result.setElementAt(2, 0, -m.getElementAtIndex(1, columnwise));
         result.setElementAt(2, 1, m.getElementAtIndex(0, columnwise));
     }
 
     /**
      * Computes the skew-symmetric matrix of provided matrix of size 3x1 or
      * 13.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      * Result is stored into provided result matrix, which will be resized if
      * needed
      *
      * @param m      Matrix of size 3x1 or 1x3 that will be used to compute the
      *               skew-symmetric matrix.
      * @param result Matrix where skew matrix will be stored.
      * @throws WrongSizeException Exception thrown if provided matrix is not 3x1
      *                            or 1x3.
      */
     public static void skewMatrix(final Matrix m, final Matrix result) throws WrongSizeException {
         boolean columnwise;
         if (m.getRows() == 3 && m.getColumns() == 1) {
             columnwise = false;
         } else if (m.getRows() == 1 && m.getColumns() == 3) {
             columnwise = true;
         } else {
             throw new WrongSizeException("m matrix must be 3x1 or 1x3");
         }
         internalSkewMatrix(m, result, columnwise);
     }
 
     /**
      * Computes the skew-symmetric matrix of provided matrix of size 3x1 or
      * 13.
      * If provided, jacobian is also set.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      * Result is stored into provided result matrix, which will be resized if
      * needed
      *
      * @param m        Matrix of size 3x1 or 1x3 that will be used to compute the
      *                 skew-symmetric matrix.
      * @param result   Matrix where skew matrix will be stored.
      * @param jacobian matrix where jacobian will be stored. Must be 9x3.
      * @throws WrongSizeException if provided matrix is not 3x1 or 1x3 or
      *                            if jacobian is not 9x3 (if provided).
      */
     public static void skewMatrix(final Matrix m, final Matrix result, final Matrix jacobian)
             throws WrongSizeException {
         if (jacobian != null && (jacobian.getRows() != 9 || jacobian.getColumns() != 3)) {
             throw new WrongSizeException("jacobian must be 9x3");
         }
 
         skewMatrix(m, result);
 
         if (jacobian != null) {
             jacobian.initialize(0.0);
             jacobian.setElementAt(5, 0, 1.0);
             jacobian.setElementAt(7, 0, -1.0);
 
             jacobian.setElementAt(2, 1, -1.0);
             jacobian.setElementAt(6, 1, 1.0);
 
             jacobian.setElementAt(1, 2, 1.0);
             jacobian.setElementAt(3, 2, -1.0);
         }
     }
 
     /**
      * Computes the skew-symmetric matrix of provided matrix of size 3x1 or
      * 13.
      * Skew-symmetric matrices are specially useful if several cross-products
      * need to be done for the same first vector over different vectors.
      * That is, having a vector a and vectors b1...bn of size 3x1:
      * c1 = a x b1
      * c2 = a x b2
      * ...
      * cn = a x bn
      * Instead of computing each product separately, the second factor can be
      * converted in a matrix where each row contains one of the vectors on the
      * second factor:
      * B = [b1, b2, ..., bn];
      * And all the cross products shown above can be computed as a single matrix
      * multiplication in the form:
      * C = skewMatrix(a) * B;
      *
      * @param m Matrix of size 3x1 or 1x3 that will be used to compute the
      *          skew-symmetric matrix.
      * @return Skew matrix.
      * @throws WrongSizeException Exception thrown if provided array doesn't
      *                            have length equal to 3.
      */
     public static Matrix skewMatrix(final Matrix m) throws WrongSizeException {
         boolean columnwise;
         if (m.getRows() == 3 && m.getColumns() == 1) {
             columnwise = false;
         } else if (m.getRows() == 1 && m.getColumns() == 3) {
             columnwise = true;
         } else {
             throw new WrongSizeException();
         }
 
         final var sk = new Matrix(3, 3);
         internalSkewMatrix(m, sk, columnwise);
         return sk;
     }
 
     /**
      * Computes the cross product of two vectors of length 3
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      *
      * @param v1        Left vector in the crossProduct operation (A in axb)
      * @param v2        Right vector in the crossProduct operation (b in axb)
      * @param result    array where the cross product of vectors v1 and b2 will be
      *                  stored.
      * @param jacobian1 jacobian of v1. Must be 3x3.
      * @param jacobian2 jacobian of v2. Must be 3x3.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3, or if jacobians are not 3x3.
      * @see #Utils  for more information.
      */
     public static void crossProduct(
             final double[] v1, final double[] v2, final double[] result,
             final Matrix jacobian1, final Matrix jacobian2) throws WrongSizeException {
 
         if (jacobian1 != null && (jacobian1.getRows() != 3 || jacobian1.getColumns() != 3)) {
             throw new WrongSizeException("if provided jacobian of v1 is not 3x3");
         }
         if (jacobian2 != null && (jacobian2.getRows() != 3 || jacobian2.getColumns() != 3)) {
             throw new WrongSizeException("if provided jacobian of v2 is not 3x3");
         }
 
         crossProduct(v1, v2, result);
 
         if (jacobian1 != null) {
             skewMatrix(v1, jacobian1);
             jacobian1.multiplyByScalar(-1.0);
         }
 
         if (jacobian2 != null) {
             skewMatrix(v2, jacobian2);
         }
     }
 
     /**
      * Computes the cross product of two vectors of length 3
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      *
      * @param v1     Left vector in the crossProduct operation (A in axb)
      * @param v2     Right vector in the crossProduct operation (b in axb)
      * @param result array where the cross product of vectors v1 and b2 will be
      *               stored.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3.
      * @see #Utils  for more information.
      */
     public static void crossProduct(final double[] v1, final double[] v2, final double[] result)
             throws WrongSizeException {
         if (v1.length != 3 || v2.length != 3 || result.length != 3) {
             throw new WrongSizeException("v1, v2 and result must have length 3");
         }
 
         final var skm = Utils.skewMatrix(v1);
         skm.multiply(Matrix.newFromArray(v2));
 
         final var buffer = skm.getBuffer();
         System.arraycopy(buffer, 0, result, 0, buffer.length);
     }
 
     /**
      * Computes the cross product of two vectors of length 3
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      *
      * @param v1 Left vector in the crossProduct operation (A in axb)
      * @param v2 Right vector in the crossProduct operation (b in axb)
      * @return Vector containing the cross product of vectors v1 and b2.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3.
      * @see #Utils  for more information.
      */
     public static double[] crossProduct(final double[] v1, final double[] v2) throws WrongSizeException {
         if (v1.length != 3 || v2.length != 3) {
             throw new WrongSizeException("v1, v2 must have length 3");
         }
 
         final var skm = Utils.skewMatrix(v1);
         skm.multiply(Matrix.newFromArray(v2));
 
         return skm.toArray();
     }
 
     /**
      * Computes the cross product of two vectors of length 3
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      *
      * @param v1        Left vector in the crossProduct operation (A in axb)
      * @param v2        Right vector in the crossProduct operation (b in axb)
      * @param jacobian1 jacobian of v1. Must be 3x3.
      * @param jacobian2 jacobian of v2. Must be 3x3.
      * @return Vector containing the cross product of vectors v1 and b2.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3, or if jacobians are not 3x3.
      */
     public static double[] crossProduct(
             final double[] v1, final double[] v2, final Matrix jacobian1, final Matrix jacobian2)
             throws WrongSizeException {
 
         if (jacobian1 != null && (jacobian1.getRows() != 3 || jacobian1.getColumns() != 3)) {
             throw new WrongSizeException("if provided jacobian of v1 is not 3x3");
         }
         if (jacobian2 != null && (jacobian2.getRows() != 3 || jacobian2.getColumns() != 3)) {
             throw new WrongSizeException("if provided jacobian of v2 is not 3x3");
         }
 
         if (jacobian1 != null) {
             skewMatrix(v1, jacobian1);
             jacobian1.multiplyByScalar(-1.0);
         }
 
         if (jacobian2 != null) {
             skewMatrix(v2, jacobian2);
         }
 
         return crossProduct(v1, v2);
     }
 
     /**
      * Computes the cross product of one vector of length 3 and N vectors of
      * length 3.
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      * The result is stored into provided result matrix, which will be resized
      * if needed
      *
      * @param v      Left operand in crossProduct operation
      * @param m      Right operand in crossProduct operation, matrix containing a
      *               vector in every row.
      * @param result Matrix where the cross products of vector v and the vectors
      *               contained in matrix m will be stored.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3 or if number of rows of provided matrix isn't 3.
      * @see Utils#skewMatrix(Matrix)  for more information.
      */
     public static void crossProduct(final double[] v, final Matrix m, final Matrix result) throws WrongSizeException {
         if (v.length != 3 || m.getColumns() != 3) {
             throw new WrongSizeException();
         }
         Utils.skewMatrix(v).multiply(m, result);
     }
 
     /**
      * Computes the cross product of one vector of length 3 and N vectors of
      * length 3.
      * The cross product of two vectors a and b is denoted as 'axb' or 'a^b',
      * resulting in a perpendicular vector to both a and b vectors. This implies
      * the following relation:
      * c·a = 0
      * c·b = 0
      * A cross product can be calculated as the determinant of this matrix
      * |i   j   k |
      * |a0  a1  a2|
      * |b0  b1  b2|
      * Resulting:
      * a x b = (a1b2 - a2b1) i + (a2b0 - a0b2) j + (a0b1 - a1b0) k =
      * = (a1b2 - a2b1, a2b0 - a0b2, a0b1 - a1b0)
      * This implementation does not use the determinant to compute the cross
      * product. A symmetric-skew matrix will be computed and used instead.
      *
      * @param v Left operand in crossProduct operation
      * @param m Right operand in crossProduct operation, matrix containing a
      *          vector in every row.
      * @return Matrix containing the cross products of vector v and the vectors
      * contained in matrix m.
      * @throws WrongSizeException Exception thrown if provided vectors don't
      *                            have length 3 or if number of rows of provided matrix isn't 3.
      * @see Utils#skewMatrix(Matrix)  for more information.
      */
     public static Matrix crossProduct(final double[] v, final Matrix m) throws WrongSizeException {
         if (v.length != 3 || m.getColumns() != 3) {
             throw new WrongSizeException();
         }
 
         final var skm = Utils.skewMatrix(v);
         skm.multiply(m);
 
         return skm;
     }
 
     /**
      * Check if the matrix is symmetric.
      *
      * @param m         Input matrix.
      * @param threshold Threshold value to determine if a matrix is symmetric.
      *                  This value is typically zero or close to zero.
      * @return Boolean relation whether the matrix is symmetric or not.
      * @throws IllegalArgumentException Raised if provided threshold is negative
      */
     public static boolean isSymmetric(final Matrix m, final double threshold) {
 
         if (threshold < MIN_THRESHOLD) {
             throw new IllegalArgumentException();
         }
 
         final var length = m.getRows();
         if (length != m.getColumns()) {
             return false;
         }
 
         for (var j = 0; j < length; j++) {
             for (var i = j + 1; i < length; i++) {
                 if (Math.abs(m.getElementAt(j, i) - m.getElementAt(i, j)) > threshold) {
                     return false;
                 }
             }
         }
         return true;
     }
 
     /**
      * Check if the matrix is symmetric. DEFAULT_SYMMETRIC_THRESHOLD is used
      * as threshold.
      *
      * @param m Input matrix.
      * @return Boolean relation whether the matrix is symmetric or not.
      */
     public static boolean isSymmetric(final Matrix m) {
         return isSymmetric(m, DEFAULT_SYMMETRIC_THRESHOLD);
     }
 
     /**
      * Checks if the matrix is orthogonal (its transpose is its inverse).
      * Orthogonal matrices must be square and non-singular
      *
      * @param m         Input matrix
      * @param threshold Threshold value to determine if a matrix is orthogonal.
      *                  This value is typically zero or close to zero.
      * @return True if matrix is orthogonal, false otherwise.
      * @throws IllegalArgumentException Raised if provided threshold is negative
      */
     public static boolean isOrthogonal(final Matrix m, final double threshold) {
 
         if (threshold < MIN_THRESHOLD) {
             throw new IllegalArgumentException();
         }
 
         final var length = m.getRows();
         if (length != m.getColumns()) {
             return false;
         }
 
         try {
             // to get faster computation it is better to try m' * m
             final var tmp = m.transposeAndReturnNew();
             tmp.multiply(m);
 
             for (var j = 0; j < length; j++) {
                 for (var i = j + 1; i < length; i++) {
                     if (Math.abs(tmp.getElementAt(i, j)) > threshold) {
                         return false;
                     }
                 }
             }
             return true;
 
         } catch (final WrongSizeException e) {
             return false;
         }
     }
 
     /**
      * Checks if the matrix is orthogonal (its transpose is its inverse).
      * DEFAULT_ORTHOGONAL_THRESHOLD is used as threshold. Orthogonal matrices
      * must be square and non-singular
      *
      * @param m Input matrix.
      * @return True if matrix is orthogonal, false otherwise.
      */
     public static boolean isOrthogonal(final Matrix m) {
         return isOrthogonal(m, DEFAULT_ORTHOGONAL_THRESHOLD);
     }
 
     /**
      * Checks if the matrix is orthonormal (it is orthogonal and its Frobenius
      * norm is one)
      *
      * @param m         Input matrix
      * @param threshold Threshold value to determine if a matrix is orthonormal.
      *                  This value is typically zero or close to zero.
      * @return True if matrix is orthonormal, false otherwise.
      * @throws IllegalArgumentException Raised if provided threshold is negative
      */
     public static boolean isOrthonormal(final Matrix m, final double threshold) {
         return isOrthogonal(m, threshold) && (Math.abs(Utils.normF(m) - 1.0) < threshold);
     }
 
     /**
      * Checks if the matrix is orthonormal up to DEFAULT_ORTHOGONAL_THRESHOLD
      * (it is orthogonal and its Frobenius norm is one)
      *
      * @param m Input matrix
      * @return True if matrix is orthonormal, false otherwise.
      */
     public static boolean isOrthonormal(final Matrix m) {
         return isOrthonormal(m, DEFAULT_ORTHOGONAL_THRESHOLD);
     }
 
     /**
      * Computes the dot product of provided arrays as the sum of the product of
      * the elements of both arrays.
      *
      * @param firstOperand  first operand.
      * @param secondOperand second operand.
      * @return dot product.
      * @throws IllegalArgumentException if first and second operands arrays
      *                                  don't have the same length.
      */
     public static double dotProduct(final double[] firstOperand, final double[] secondOperand) {
         return ArrayUtils.dotProduct(firstOperand, secondOperand);
     }
 
     /**
      * Computes the dot product of provided arrays as the sum of the product of
      * the elements of both arrays.
      *
      * @param firstOperand   first operand.
      * @param secondOperand  second operand.
      * @param jacobianFirst  matrix where jacobian of first operand will be
      *                       stored. Must be a column matrix having the same number of rows as the
      *                       first operand length.
      * @param jacobianSecond matrix where jacobian of second operand will be
      *                       stored. Must be a column matrix having the same number of rows as the
      *                       second operand length.
      * @return dot product.
      * @throws IllegalArgumentException if first and second operands don't have
      *                                  the same length or if jacobian matrices are not column vectors having the
      *                                  same length as their respective operands.
      */
     public static double dotProduct(
             final double[] firstOperand, final double[] secondOperand, final Matrix jacobianFirst,
             final Matrix jacobianSecond) {
         return ArrayUtils.dotProduct(firstOperand, secondOperand, jacobianFirst, jacobianSecond);
     }
 
     /**
      * Computes the dot product of provided matrices, as the sum of the product
      * of the elements on both matrices, assuming that both represent column
      * vectors.
      *
      * @param firstOperand  first operand.
      * @param secondOperand second operand.
      * @return dot product.
      * @throws WrongSizeException if first operand columns are not equal to
      *                            second operand rows, or if first operand rows is not one, or if second
      *                            operand columns is not one.
      */
     public static double dotProduct(final Matrix firstOperand, final Matrix secondOperand) throws WrongSizeException {
         if (firstOperand.getColumns() != secondOperand.getRows() || firstOperand.getRows() != 1
                 || secondOperand.getColumns() != 1) {
             throw new WrongSizeException("first operand must be 1xN, and second operand must be Nx1");
         }
 
         return dotProduct(firstOperand.getBuffer(), secondOperand.getBuffer());
     }
 
     /**
      * Computes the dot product of provided matrices, as the sum of the product
      * of the elements on both matrices, assuming that both represent column
      * vectors.
      *
      * @param firstOperand   first operand. Must be 1xN.
      * @param secondOperand  second operand. Must be Nx1.
      * @param jacobianFirst  matrix where jacobian of first operand will be
      *                       stored. Must be a column matrix having the same number of rows as the
      *                       first operand length. Must be Nx1.
      * @param jacobianSecond matrix where jacobian of second operand will be
      *                       stored. Must be a column matrix having the same number of rows as the
      *                       second operand length. Must be Nx1.
      * @return dot product.
      * @throws WrongSizeException       if first operand columns are not equal to
      *                                  second operand rows, or if first operand rows is not one, or if second
      *                                  operand columns is not one.
      * @throws IllegalArgumentException if jacobian matrices are not column
      *                                  vectors having the same length as their respective operands.
      */
     public static double dotProduct(
             final Matrix firstOperand, final Matrix secondOperand, final Matrix jacobianFirst,
             final Matrix jacobianSecond) throws WrongSizeException {
         if (firstOperand.getColumns() != secondOperand.getRows() || firstOperand.getRows() != 1
                 || secondOperand.getColumns() != 1) {
             throw new WrongSizeException("first operand must be 1xN, and second operand must be Nx1");
         }
 
         return dotProduct(firstOperand.getBuffer(), secondOperand.getBuffer(), jacobianFirst, jacobianSecond);
     }
 
 
     /**
      * Computes the Schur complement of a symmetric matrix.
      * For a matrix M of size sxs having the typical partition
      * M = [A B ; B' C]
      * where A is posxpos, B is posx(s - pos) and C is (s-pos)x(s-pos)
      * Then pos indicates the position that delimits the separation between
      * these sub-matrices in the diagonal.
      * If fromStart is true, then Schur complement of A is computed and result
      * will have size (s-pos)x(s-pos).
      * If fromStart is false, then Schur complement of C is computed and result
      * will have size posxpos.
      * <p>
      * Definitions and rationale:
      * If M is a symmetrical matrix partitioned as:
      * M = [M_11 M_12 ; M_12' M_22]
      * then the Schur complement of M_11 is
      * S = M_22 - M_12 ¡ * M_11^-1 * M_12
      * The optionally returned inverse block is just iA = M_11^-1
      * whose name comes from 'inverse of A', A given by the popular partition
      * of M = [A B ; B' C], therefore A = M_11.
      * If M is symmetrical and positive, then using the Cholesky decomposition
      * M = [R_11' 0 ; R_12' R_22] * [R_11 R_12 ; 0 R_22]
      * leads to
      * S = R_22' * R_22
      * iA = (R_11' * R_11)^-1 = R_11^-1 * R_11^-T
      * which constitutes and efficient and stable way to compute the Schur
      * complement. The square root factors R_22 and R_11^_1 can be obtained by
      * setting the optional flag sqrt.
      * The Schur complement has applications for solving linear equations, and
      * applications to probability theory to compute conditional covariances.
      *
      * @param m         matrix to compute the Schur complement from
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param sqrt      true to return the square root of the Schur complement, which
      *                  is an upper triangular matrix, false to return the full Schur complement,
      *                  which is S'*S.
      * @param result    instance where the Schur complement will be stored. If
      *                  needed this instance will be resized.
      * @param iA        instance where the inverse block will be stored, if provided.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(final Matrix m, int pos, final boolean fromStart, final boolean sqrt, final Matrix result,
                               final Matrix iA) throws DecomposerException, RankDeficientMatrixException {
         final var rows = m.getRows();
         final var cols = m.getColumns();
         if (rows != cols) {
             throw new IllegalArgumentException("matrix must be square");
         }
         if (pos >= rows) {
             throw new IllegalArgumentException("pos must be lower than rows");
         }
         if (pos == 0) {
             throw new IllegalArgumentException("pos must be greater than 0");
         }
 
         try {
             final Matrix m2;
             if (fromStart) {
                 // if position is taken from origin, no reordering is needed
                 m2 = m;
             } else {
                 // if position is taken from pos to end, then reordering is needed
 
                 // create index positions to reorder matrix to decompose using
                 // Cholesky
                 final var index = new int[rows];
                 final var complSize = rows - pos;
                 for (int i = 0, value = pos; i < complSize; i++, value++) {
                     index[i] = value;
                 }
                 for (int i = complSize, j = 0; i < rows; i++, j++) {
                     index[i] = j;
                 }
 
                 m2 = new Matrix(rows, rows);
                 for (var j = 0; j < rows; j++) {
                     for (var i = 0; i < rows; i++) {
                         m2.setElementAt(i, j, m.getElementAt(index[i],
                                 index[j]));
                     }
                 }
 
                 pos = rows - pos;
             }
 
             final var decomposer = new CholeskyDecomposer(m2);
             decomposer.decompose();
 
             // pick the upper right matrix of cholesky
             final var r = decomposer.getR();
 
             // s, which is the square root of Schur complement and is the result,
             // is an upper right matrix because it is a sub-matrix of r
             final var sizeMinus1 = rows - 1;
             r.getSubmatrix(pos, pos, sizeMinus1, sizeMinus1, result);
 
             if (!sqrt) {
                 // if the full version of the Schur complement is required
                 // we multiply by its transpose S = S'*S
                 final var transS = result.transposeAndReturnNew();
                 transS.multiply(result); //transS is now S'*S
                 result.copyFrom(transS);
             }
 
             if (iA != null) {
                 // minor of r is the square root of inverse block
                 final var posMinus1 = pos - 1;
                 r.getSubmatrix(0, 0, posMinus1, posMinus1, iA);
                 Utils.inverse(iA, iA);
                 // to obtain full inverse block iA we need to multiply it by its
                 // transpose iA = iA * iA'
                 final var transiA = iA.transposeAndReturnNew();
                 iA.multiply(transiA);
             }
         } catch (final DecomposerException | RankDeficientMatrixException e) {
             // if matrix is numerically unstable or singular
             throw e;
         } catch (final AlgebraException ignore) {
             // Cholesky will not raise any other exception
         }
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix, returning always the
      * full Schur complement.
      *
      * @param m         matrix to compute the Schur complement from
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param result    instance where the Schur complement will be stored. If
      *                  needed this instance will be resized.
      * @param iA        instance where the inverse block will be stored, if provided.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(
             final Matrix m, final int pos, final boolean fromStart, final Matrix result, final Matrix iA)
             throws DecomposerException, RankDeficientMatrixException {
         schurc(m, pos, fromStart, false, result, iA);
     }
 
     /**
      * Computes the Schur complement of the sub-matrix A within a symmetric
      * matrix, returning always the full Schur complement.
      *
      * @param m      matrix to compute the Schur complement from
      * @param pos    position to delimit the Schur complement.
      * @param result instance where the Schur complement will be stored. If
      *               needed this instance will be resized.
      * @param iA     instance where the inverse block will be stored, if provided.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(final Matrix m, final int pos, final Matrix result, final Matrix iA)
             throws DecomposerException, RankDeficientMatrixException {
         schurc(m, pos, true, result, iA);
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix.
      *
      * @param m         matrix to compute the Schur complement from.
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param sqrt      true to return the square root of the Schur complement, which
      *                  is an upper triangular matrix, false to return the full Schur complement,
      *                  which is S'*S.
      * @param iA        instance where the inverse block will be stored, if provided.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(
             final Matrix m, final int pos, final boolean fromStart, final boolean sqrt, final Matrix iA)
             throws DecomposerException, RankDeficientMatrixException {
         final var rows = m.getRows();
         final var cols = m.getColumns();
         if (rows != cols) {
             throw new IllegalArgumentException("matrix must be square");
         }
         if (pos >= rows) {
             throw new IllegalArgumentException("pos must be lower than rows");
         }
         if (pos == 0) {
             throw new IllegalArgumentException("pos must be greater than 0");
         }
 
         final var size = fromStart ? pos : rows - pos;
         Matrix result = null;
         try {
             result = new Matrix(size, size);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
 
         schurc(m, pos, fromStart, sqrt, result, iA);
         return result;
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix, returning always the
      * full Schur complement.
      *
      * @param m         matrix to compute the Schur complement from
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param iA        instance where the inverse block will be stored, if provided.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrice">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(
             final Matrix m, final int pos, final boolean fromStart, final Matrix iA)
             throws DecomposerException, RankDeficientMatrixException {
         return schurcAndReturnNew(m, pos, fromStart, false, iA);
     }
 
     /**
      * Computes the Schur complement of the sub-matrix A within a symmetric
      * matrix, returning always the full Schur complement.
      *
      * @param m   matrix to compute the Schur complement from
      * @param pos position to delimit the Schur complement.
      * @param iA  instance where the inverse block will be stored, if provided.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException     if m is not square, pos is greater or
      *                                      equal than matrix size, or pos is zero.
      * @throws DecomposerException          if m is numerically unstable.
      * @throws RankDeficientMatrixException if iA matrix is singular or
      *                                      numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(final Matrix m, final int pos, final Matrix iA)
             throws DecomposerException, RankDeficientMatrixException {
         return schurcAndReturnNew(m, pos, true, iA);
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix.
      *
      * @param m         matrix to compute the Schur complement from.
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param sqrt      true to return the square root of the Schur complement, which
      *                  is an upper triangular matrix, false to return the full Schur complement,
      *                  which is S'*S.
      * @param result    instance where the Schur complement will be stored. If
      *                  needed this instance will be resized.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(final Matrix m, final int pos, final boolean fromStart, final boolean sqrt,
                               final Matrix result) throws DecomposerException {
         try {
             schurc(m, pos, fromStart, sqrt, result, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix, returning always the
      * full Schur complement.
      *
      * @param m         matrix to compute the Schur complement from
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param result    instance where the Schur complement will be stored. If
      *                  needed this instance will be resized.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(
             final Matrix m, final int pos, final boolean fromStart, final Matrix result) throws DecomposerException {
         try {
             schurc(m, pos, fromStart, result, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
     }
 
     /**
      * Computes the Schur complement of the sub-matrix A within a symmetric
      * matrix, returning always the full Schur complement.
      *
      * @param m      matrix to compute the Schur complement from
      * @param pos    position to delimit the Schur complement.
      * @param result instance where the Schur complement will be stored. If
      *               needed this instance will be resized.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static void schurc(final Matrix m, final int pos, final Matrix result) throws DecomposerException {
         try {
             schurc(m, pos, result, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix.
      *
      * @param m         matrix to compute the Schur complement from.
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @param sqrt      true to return the square root of the Schur complement, which
      *                  is an upper triangular matrix, false to return the full Schur complement,
      *                  which is S'*S.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(
             final Matrix m, final int pos, final boolean fromStart, final boolean sqrt) throws DecomposerException {
         Matrix result = null;
         try {
             result = schurcAndReturnNew(m, pos, fromStart, sqrt, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
 
         return result;
     }
 
     /**
      * Computes the Schur complement of a symmetric matrix, returning always the
      * full Schur complement.
      *
      * @param m         matrix to compute the Schur complement from
      * @param pos       position to delimit the Schur complement.
      * @param fromStart true to compute Schur complement of A, false to compute
      *                  Schur complement of C.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(
             final Matrix m, final int pos, final boolean fromStart) throws DecomposerException {
         Matrix result = null;
         try {
             result = schurcAndReturnNew(m, pos, fromStart, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
 
         return result;
     }
 
     /**
      * Computes the Schur complement of the sub-matrix A within a symmetric
      * matrix, returning always the full Schur complement.
      *
      * @param m   matrix to compute the Schur complement from
      * @param pos position to delimit the Schur complement.
      * @return a new matrix containing the Schur complement.
      * @throws IllegalArgumentException if m is not square, pos is greater or
      *                                  equal than matrix size, or pos is zero.
      * @throws DecomposerException      if m is numerically unstable.
      * @see #schurc(com.irurueta.algebra.Matrix, int, boolean, boolean, com.irurueta.algebra.Matrix, com.irurueta.algebra.Matrix)
      * @see <a href="http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices">http://scicomp.stackexchange.com/questions/5050/cholesky-factorization-of-block-matrices</a>
      */
     public static Matrix schurcAndReturnNew(final Matrix m, final int pos) throws DecomposerException {
         Matrix result = null;
         try {
             result = schurcAndReturnNew(m, pos, null);
         } catch (final RankDeficientMatrixException ignore) {
             // if no iA is provided, this exception is never raised.
         }
 
         return result;
     }
 }