text:
/**
@file
@brief Utilities to perform vectorized LU and LDU decomposition with complete
row and column pivoting, based on nonvectorized version of LU decomposition
with complete row and column pivoting written by Roger Wehage.
@author Kristopher Wehage
@date 2016
@version 1.1
@remark
THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 THE AUTHOR OR CONTRIBUTORS 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.
*/

#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#ifdef KSL_WITH_BLAS_
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif
#endif

#include "ksl/array.h"
#include "ksl/linalg.h"

/*!
@brief Function for performing modified double precision Gram-Schmidt
orthonormalization

This function implements a compact version of the Gram-Schmidt
algorithm to orthonormalize the columns of a matrix which has
full column rank. The matrix, whose columns are to be
orthonormalized, is passed in as A[m][n] where m and n are the
respective row and column dimensions of A[][]. The
orthonormalized columns are passed back in A[][].

@param m row dimension
@param n column dimension
@param A[m][n] a rectangular matrix to be orthonormalized, the orthonormalized
               matrix is returned in A
*/
void ksl_linalg_gramSchmidt(double* restrict A, const int m, const int n) {
  for(int k = 0; k < n; k++) {
#ifdef KSL_WITH_BLAS
    double R = 1.0 / cblas_dnrm2(m, A + k, n);
    cblas_dscal(m, R, A + k, n);
#else
    double R = 0.0;
    for(int i = 0; i < m; i++) {
      R += A[i * n + k] * A[i * n + k];
    }
    R = 1.0 / sqrt(R);
    for(int i = 0; i < m; i++) {
      A[i * n + k] *= R;
    }
#endif
    for(int j = k + 1; j < n; j++) {
      R = 0;
      for(int i = 0; i < m; i++) {
        R += A[i * n + k] * A[i * n + j];
      }
      for(int i = 0; i < m; i++) {
        A[i * n + j] -= A[i * n + k] * R;
      }
    }
  }
}

/*!
@brief Function for performing single precision modified Gram-Schmidt
orthonormalization

This function implements a compact version of the Gram-Schmidt
algorithm to orthonormalize the columns of a matrix which has
full column rank. The matrix, whose columns are to be
orthonormalized, is passed in as A[m][n] where m and n are the
respective row and column dimensions of A[][]. The
orthonormalized columns are passed back in A[][].

@param m row dimension
@param n column dimension
@param A[m][n] a rectangular matrix to be orthonormalized, the orthonormalized
               matrix is returned in A
*/
void ksl_linalg_gramSchmidtf(float* restrict A, const int m, const int n) {
  for(int k = 0; k < n; k++) {
#ifdef KSL_WITH_BLAS
    double R = 1.0 / cblas_snrm2(m, A + k, n);
    cblas_sscal(m, R, A + k, n);
#else
    double R = 0.0;
    for(int i = 0; i < m; i++) {
      R += A[i * n + k] * A[i * n + k];
    }
    R = 1.0 / sqrt(R);
    for(int i = 0; i < m; i++) {
      A[i * n + k] *= R;
    }
#endif
    for(int j = k + 1; j < n; j++) {
      R = 0;
      for(int i = 0; i < m; i++) {
        R += A[i * n + k] * A[i * n + j];
      }
      for(int i = 0; i < m; i++) {
        A[i * n + j] -= A[i * n + k] * R;
      }
    }
  }
}

/*!
@brief Row Major Order LU Decomposition with complete row and column pivoting

ksl_linalg_lu_full_rmo factors a double precision matrix, A[rowDim * colDim],
stored in Row Major Order using full row and column pivoting. Matrix A need
not have full row or column rank. The integer variable 'rank' represents the
matrix rank and internally is always one less than the true rank to be
consistent with C's indexing from zero. Upon return from the function,
rank will be set to the correct value.

The lower triangular (rank+1) by (rank+1) Lr matrix, except its unity
diagonal, is stored below the diagonal in A[0:rank][0:rank]. The upper
triangular (rank+1) by (rank+1) Ur matrix is stored on and above the
diagonal in A[0:rank][0:rank].

Following the first major matrix decomposition step, A[0:rank][rank+1:colDim]
stores the residual matrix UR and A[rank+1:rowDim][0:rank] stores the residual
matrix LR.

The following operations on this matrix are optionally performed after
ksl_linalg_lu_full_rmo() has factored A.

ksl_linalg_lu_setBMatrix_rmo() computes and stores B = inverse(Ur)*UR and stores
it back in A[0:rank][rank+1:colDim].
ksl_linalg_lu_setCMatrix_rmo() computes and stores C = LR*inverse(Lr) and stores
it back in A[rank+1:rowDim][0:rank].

The remaining A[rank+1:rowDim][rank+1:colDim] submatrix contains numbers whose
absolute values are all smaller than eps times the absolutely largest
entry in the original matrix. This part of the matrix is not used.

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param *A [in/out] matrix to be factored with dimensions
A[0:rowDim-1][0:colDim-1], LU factors are overwritten in original matrix
@param eps [in] input tolerance on the order of machine roundoff.
@param pr [out] output row permutation array with dimensions pr[0:rowDim-1]
@param pc [out] output column permutation array with dimensions pc[0:colDim-1]
@return rank of matrix A
*/
int ksl_linalg_lu_full_rmo(const int rowDim, const int colDim,
                           double* restrict A, const double eps,
                           int* restrict pr, int* restrict pc) {

  // printf("Jacobian matrix in fullFactor_LU\n");
  // for(int i = 0; i < rowDim; i++) {
  //   for(int j = 0; j < colDim; j++)
  //     printf("% 2.4f ", A[i * colDim + j]);
  //   printf("\n");
  // }

  int rank;     /* (rank+1) is the matrix rank */
  int pivotRow; /*  row with current pivotal element */
  int pivotCol; /* column with current pivotal element */
  double pivot = 0.0;
  /* current pivotal element, holds the current pivotal element
     value */
  double tol;                    /* tolerance for checking residual matrix
                                               infinity norm against */
  double save;                   /* variable for holding intermediate results*/
  double size = rowDim * colDim; /*overall size of matrix */

  /*
    Return failure if a bad row or column dimension was found.
  */
  if((rowDim <= 0) || (colDim <= 0)) {
    return (-1);
  }

  /*
    Consistent with C-indexing, rank always has a value of
    one less than the true value. rank+1 is returned.
  */
  rank = -1; /* initialize for indexing */

  /*
    Search through the entire A matrix to find the
    absolutely largest element for assigning to pivot.
    Save that row and column number in pivotRow and
    pivotCol. Also intialize the row permutation array pr.
  */
#ifdef KSL_WITH_BLAS_
  int index = cblas_idamax(size, A, 1);
#else
  int index = ksl_maxIndex(size, A);
#endif
  // div_t result = div(index, colDim);
  // pivotRow = result.quot;
  // pivotCol = result.rem;
  pivotRow = index / colDim;
  pivotCol = index - pivotRow * colDim;
  pivot = A[index];
  // printf("pivotRow: %d, pivotCol: %d\n", pivotRow, pivotCol);

  /* Initialize the column permutation array. */
  for(int col = 0; col < colDim; col++) {
    pc[col] = col;
  }
  for(int row = 0; row < rowDim; row++) {
    pr[row] = row;
  }

  /* Set tolerance to check all zero-elements against.
  pivot = infinity norm of A.
  */
  tol = fabs(eps * pivot);

  /*
    Major loop to permute and factor the matrix.
    Generate factors column by column, so outer loop
    sweeps over columns.
  */
  for(int col = 0; col < colDim; col++) {
    /*
      If the current pivotal element is <= tol, the
      remaining submatrix is zero and factorization
      is complete. The else statement for the following if test
      breaks out of this loop so not to waste time
      going through the remaining columns.
    */
    if(fabs(pivot) > tol) {
      /* Increase rank for the current column. */
      ++rank;

      /*
        pivotRow can never be less than rank.
        If pivotRow==rank, then no permutation is needed.
        If pivotRow>rank, then need to swap rows.
      */
      if(pivotRow > rank) {
        /*
          Swap entire rows A[rank][:] and A[pivotRow][:].
          This also swaps rows in the LR matrix to this point.

          swap rows:
            N = colDim
            stride = 1
          Start of replaced row is A + colDim * rank & stride is 1
          Start of pivot row is A + colDim * pivotRow & stride is 1
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(colDim, A + colDim * rank, 1, A + colDim * pivotRow, 1);
#else
        ksl_swapArray(colDim, A + colDim * rank, 1, A + colDim * pivotRow, 1);
#endif
        /*
          Swap row permutation entries pr[rank] and
          pr[pivotRow] to reflect row swaps in A.
        */
        ksl_swapi(pr + rank, pr + pivotRow);
      }

      /* pivotCol can never be less than rank.
      If pivotCol==rank, then no permutation is needed.
      If pivotCol>rank, then need to swap columns. */
      if(pivotCol > rank) {
        // printf("rank: %d, pivotCol: %d\n", rank, pivotCol);
        // printf("colDim %d\n", colDim);
        /*
        Swap entire columns A[:][rank] and A[:][pivotCol].
        This also swaps columns in the UR matrix to this point.
        swap columns:
          N = rowDim
          stride = colDim
          Start of replaced column is A + rank & stride is colDim
          Start of pivot column is A + pivotCol & stride is colDim
            */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(rowDim, A + rank, colDim, A + pivotCol, colDim);
#else
        ksl_swapArray(rowDim, A + rank, colDim, A + pivotCol, colDim);
#endif
        /*
          Swap column permutation entries pc[rank] and
          pr[pivotCol] to reflect column swaps.
        */
        ksl_swapi(pc + rank, pc + pivotCol);
      }

      /*
        rank cannot be > rowDim-1. If rank < rowDim-1
        there is still some processing left to do. If
        rank == rowDim-1, there is only a 1 on the
        diagonal of this column of Lr. This loop factors
        the matrix and searches for a new pivotal element
        for the next factorization step.
      */
      if(rank < rowDim - 1) {

        /*
          Copy the current pivotal element into save so new
          pivotal element can be stored back in pivot.
        */
        save = pivot;
        pivot = 0;

        /* Need only process the rows from rank+1 to rowDim. */
        for(int i = rank + 1; i < rowDim; i++) {

          /* Evaluate the current entry in the L matrix.*/
          A[i * colDim + rank] /= save;

          /*
            rank cannot be >= colDim. If rank < colDim
            there is still some processing left to do. If
            rank == colDim-1, only a pivot remains on the
            diagonal.
          */
          if(rank < colDim) {

            /*
              This next loop computes the remaining U matrix,
              searches for the largest pivotal element, and
              remembers the new pivotal element row and colum.
            */
            for(int j = rank + 1; j < colDim; j++) {
              A[i * colDim + j] -= A[i * colDim + rank] * A[rank * colDim + j];
              if(fabs(A[i * colDim + j]) > fabs(pivot)) {
                pivot = A[i * colDim + j];
                pivotRow = i;
                pivotCol = j;
              } /*End if*/
            }   /*End for j*/
          }     /*End if rank*/
        }       /*Endfor i = rank+1*/
      }         /*Endif (rank<rowDim-1)*/
      else {
        break; /*No more rows to process.*/
      }
    } /*End if*/
    else {
      break; /*Done if remainder of matrix is at noise level.*/
    }
  } /*End for col*/

  return (rank + 1);
}

/*!
@brief Row Major Order LU Decomposition with complete row and column pivoting
with one specified coordinate

ksl_linalg_lu_full_specified_rmo factors a double precision matrix, A[rowDim *
colDim], stored in Row Major Order using full row and column pivoting. Matrix A
need not have full row or column rank. The integer variable 'rank' represents
the matrix rank and internally is always one less than the true rank to be
consistent with C's indexing from zero. Upon return from the function,
rank will be set to the correct value.

The lower triangular (rank+1) by (rank+1) Lr matrix, except its unity
diagonal, is stored below the diagonal in A[0:rank][0:rank]. The upper
triangular (rank+1) by (rank+1) Ur matrix is stored on and above the
diagonal in A[0:rank][0:rank].

Following the first major matrix decomposition step, A[0:rank][rank+1:colDim]
stores the residual matrix UR and A[rank+1:rowDim][0:rank] stores the residual
matrix LR.

ksl_linalg_lu_setBMatrix_rmo() computes and stores B = inverse(Ur)*UR and stores
it back in A[0:rank][rank+1:colDim].
ksl_linalg_lu_setCMatrix_rmo() computes and stores C = LR*inverse(Lr) and stores
it back in A[rank+1:rowDim][0:rank].

The remaining A[rank+1:rowDim][rank+1:colDim-1] submatrix contains numbers whose
absolute values are all smaller than eps times the absolutely largest
entry in the original matrix. This part of the matrix is not used.
The remainig A[rank+1:rowDim][colDim] submatrix could contain numbers whose
absolute values are greater than eps times the absolutely largest
entry in the original matrix. Adding a check here would be one way to
assert that the specified variable has sufficient mobility.

@param rowDim input row dimension of matrix A.
@param colDim input column dimension of matrix A.
@param **A input/output matrix to be factored with dimensions
A[0:rowDim-1][0:colDim-1]:
@param eps input tolerance on the order of machine roundoff.
@param pr output row permutation array with dimensions pr[0:rowDim-1]
@param pc output column permutation array with dimensions pc[0:colDim-1]
@return rank of matrix A
*/
int ksl_linalg_lu_full_specified_rmo(const int rowDim, const int colDim,
                                     double* restrict A, double eps,
                                     int* restrict pr, int* restrict pc,
                                     const int specifiedIndex) {

  // printf("Jacobian matrix in fullFactor_LU\n");
  // for(int i = 0; i < rowDim; i++) {
  //   for(int j = 0; j < colDim; j++)
  //     printf("% 2.4f ", A[i * colDim + j]);
  //   printf("\n");
  // }

  int rank;     /* (rank+1) is the matrix rank */
  int pivotRow; /* row with current pivotal element */
  int pivotCol; /* column with current pivotal element */
  double pivot = 0.0;
  /* current pivotal element, holds the current pivotal element */
  double tol;  /* tolerance for checking residual matrix
                             infinity norm against */
  double save; /* variable for holding intermediate results*/
  // double size = rowDim * colDim; /*overall size of matrix */

  /*
    Return failure if a bad row or column dimension was found.
  */
  if((rowDim <= 0) || (colDim <= 1)) {
    return (-1);
  }

  /*
    Consistent with C-indexing, rank always has a value of
    one less than the true value. rank+1 is returned.
  */
  rank = -1; /* initialize for indexing */

  /*
    Initialize permutation arrays
  */
  for(int row = 0; row < rowDim; row++) {
    pr[row] = row;
  }
  for(int col = 0; col < colDim; col++) {
    pc[col] = col;
  }

  /*
    Swap the specified column to the last position in the input
    matrix
    Swap entire columns A[:][specifiedIndex] and A[:][colDim - 1].
    swap columns:
      N = rowDim
      stride = colDim
      Start of specefied column is A + specifiedIndex & stride is colDim
      Start of last column is A + colDim - 1 & stride is colDim

  */
  if(specifiedIndex != colDim - 1) {
#ifdef KSL_WITH_BLAS
    cblas_dswap(rowDim, A + specifiedIndex, colDim, A + colDim - 1, colDim);
#else
    ksl_swapArray(rowDim, A + specifiedIndex, colDim, A + colDim - 1, colDim);
#endif
    /*
      Swap column permutation entries pc[] and
      pc[pivotRow] to reflect row swaps in A.
    */
    ksl_swapi(pc + specifiedIndex, pc + colDim - 1);
  }

  /*
    Search through matrix A except the last column to find the
    absolutely largest element for assigning to pivot.
    Save that row and column number in pivotRow and
    pivotCol. This could cause numerical problems if the last
    column contains one or more entries substantially absolutely
    larger than all other entries in A.
  */
  pivot = A[0];
  pivotRow = 0;
  pivotCol = 0;
  for(int row = 0; row < rowDim; row++) {
    for(int col = 0; col < colDim - 1; col++) {
      if(fabs(A[row * colDim + col]) > fabs(pivot)) {
        pivot = A[row * colDim + col];
        pivotRow = row;
        pivotCol = col;
      }
    }
  }
  // printf("pivotRow: %d, pivotCol: %d\n", pivotRow, pivotCol);

  /* Set tolerance to check all zero-elements against.
  pivot = infinity norm of A.
 */
  tol = fabs(eps * pivot);

  /*
    Major loop to permute and factor the matrix.
    Generate factors column by column, so outer loop
    sweeps over columns.
  */
  for(int col = 0; col < colDim; col++) {
    /*
    If the current pivotal element is <= tol, the
    remaining submatrix is zero and factorization
    is complete. The else statement for the following if test
    breaks out of this loop so not to waste time
    going through the remaining columns.
    */
    if(fabs(pivot) > tol) {
      /* Increase rank for the current column. */
      ++rank;

      /*
        pivotRow can never be less than rank.
        If pivotRow==rank, then no permutation is needed.
        If pivotRow>rank, then need to swap rows.
      */
      if(pivotRow > rank) {
        /*
          Need to swap entire row A[rank][:] and A[pivotRow][:].
          This also swaps rows in the Lr matrix to this point.
          swap rows:
            N = colDim
            stride = 1
          Start of replaced row is A + colDim * rank & stride is 1
          Start of pivot row is A + colDim * pivotRow & stride is 1
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(colDim, A + colDim * rank, 1, A + colDim * pivotRow, 1);
#else
        ksl_swapArray(colDim, A + colDim * rank, 1, A + colDim * pivotRow, 1);
#endif
        /*
          Swap row permutation entries pr[rank] and
          pr[pivotRow] to reflect row swaps in A.
        */
        ksl_swapi(pr + rank, pr + pivotRow);
      }

      /*
        pivotCol can never be less than rank.
        If pivotCol==rank, then no permutation is needed.
        If pivotCol>rank, then need to swap columns.
      */
      if(pivotCol > rank) {
        // printf("rank: %d, pivotCol: %d\n", rank, pivotCol);
        // printf("colDim %d\n", colDim);
        /*
        Swap entire columns A[:][rank] and A[:][pivotCol].
        This also swaps columns in the UR matrix to this point.
        swap columns:
          N = rowDim
          stride = colDim
          Start of replaced column is A + rank & stride is colDim
          Start of pivot column is A + pivotCol & stride is colDim
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(rowDim, A + rank, colDim, A + pivotCol, colDim);
#else
        ksl_swapArray(rowDim, A + rank, colDim, A + pivotCol, colDim);
#endif
        /*
          Swap column permutation entries pc[rank] and
          pr[pivotCol] to reflect column swaps.
        */
        ksl_swapi(pc + rank, pc + pivotCol);
      }

      /*
        rank cannot be > rowDim-1. If rank < rowDim-1
        there is still some processing left to do. If
        rank == rowDim-1, there is only a 1 on the
        diagonal of this column of Lr. This loop factors
        the matrix and searches for a new pivotal element
        for the next factorization step.
      */
      if(rank < rowDim - 1) {

        /*
          Copy the current pivotal element into save so new
          pivotal element can be stored back in pivot.
        */
        save = pivot;
        pivot = 0;

        /* Need only process the rows from rank+1 to rowDim. */
        for(int i = rank + 1; i < rowDim; i++) {

          /* Evaluate the current entry in the L matrix.*/
          A[i * colDim + rank] /= save;

          /*
            rank cannot be >= colDim. If rank < colDim
            there is still some processing left to do. If
            rank == colDim-1, only a pivot remains on the
            diagonal.
          */
          if(rank < colDim) {

            /*
              This next loop computes the remaining U matrix,
              searches for the largest pivotal element, and
              remembers the new pivotal element row and colum.
            */
            for(int j = rank + 1; j < colDim; j++) {
              A[i * colDim + j] -= A[i * colDim + rank] * A[rank * colDim + j];
            }
            for(int j = rank + 1; j < colDim - 1; j++) {
              if(fabs(A[i * colDim + j]) > fabs(pivot)) {
                pivot = A[i * colDim + j];
                pivotRow = i;
                pivotCol = j;
              } /*End if*/
            }   /*End for j*/
          }     /*End if rank*/
        }       /*Endfor i = rank+1*/
      }         /*Endif (rank<rowDim-1)*/
      else {
        break; /*No more rows to process.*/
      }
    } /*End if*/
    else {
      break; /*Done if remainder of matrix is at noise level.*/
    }
  } /*End for col*/
  return (rank + 1);
}

/*!
@brief *Fast* Row Major Order LU Decomposition with no row or column pivoting
for a rectangular matrix

This function LU factors a double precision matrix, A[rank][colDim],
using no row or column pivoting. This function should be used only if the
left rank by rank submatrix is known to be nonsingular.

rank <= colDim or error.

The lower triangular rank by rank Lr matrix, except the unity
diagonal, is stored below the diagonal in A[0:rank-1][0:rank-1]. The upper
triangular rank by rank Ur matrix is stored on and above the
diagonal in A[0:rank-1][0:rank-1].

Note that this minimal algorithm variation does not compute the product
B = inverse(Ur)*UR

@param rank [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param *A [in/out] matrix to be factored with dimensions
A[0:rank-1][0:colDim-1]:
*/
inline void ksl_linalg_lu_rmo(const int rank, const int colDim,
                              double* restrict A) {

  if(rank > colDim) {
    // Error message & exit
  }

  /*
    Major loop to factor the matrix.
    Generate factors column by column
  */
  for(int row = 0; row < rank; row++) {

    /* i iterates over rows of A, up to rank-1 */
    for(int i = row + 1; i < rank; i++) {

      /* Evaluate the current entry in the L matrix.*/
      A[i * colDim + row] /= A[row * colDim + row];

      /* Compute U matrix */
      for(int j = row + 1; j < colDim; j++) {
        A[i * colDim + j] -= A[i * colDim + row] * A[row * colDim + j];
      }
    }
  }
}

/*!
@brief compute Row Major Order B matrix (inverse(Ur) * UR)

This block overwrites UR with inverse(Ur) * UR
If rank == 0 or rank == colDim nothing to process, so exit with message.
This loop computes A[0:rank-1][rank:colDim-1] = inverse(Ur)*UR
where UR is stored in A[0:rank-1][rank:colDim-1]
and Ur is stored in upper part of A[0:rank-1][0:rank-1].
i is the row number in Ur.
j is the column number in Ur & row number in UR.
k is the column number in UR.

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param rank [in] rank of matrix A.
@param *A [in/out] matrix with dimensions A[0:rowDim-1][0:colDim-1]:

*/
inline void ksl_linalg_lu_setBMatrix_rmo(const int rowDim, const int colDim,
                                         const int rank, double* restrict A) {

  if(rank > 0 && rank < colDim) {
    for(int i = rank - 1; i > -1; i--) {   /* rows of B */
      for(int k = rank; k < colDim; k++) { /* columns of B */
        double save = 0;
        for(int j = rank - 1; j > i; j--) { /* columns of Ur */
          save += A[i * colDim + j] * A[j * colDim + k];
        }
        A[i * colDim + k] = (A[i * colDim + k] - save) / A[i * colDim + i];
      }
    }
  } else {
    // Error & do something.
  }
}

/*!
@brief compute Row Major Order C matrix

This block overwrites LR with LR*inverse(Lr)
If rank==1, the Lr matrix is a 1 by 1 identity
matrix, so there is nothing to do here. This loop
computes A[rank:rowDim-1][0:rank-1]=LR*inverse(Lr)
where LR is stored in A[rank:rowDim-1][0:rank-1]
and Lr is stored in A[0:rank-1][0:rank-1].
j is the column number in LR. It ends at 1
because the diagonal entry in row 0 of Lr is 1.
i is the row number in LR.
k is the column number in Lr

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param rank [in] rank of matrix A.
@param *A [in/out] matrix with dimensions A[0:rowDim-1][0:colDim-1]:

*/
void ksl_linalg_lu_setCMatrix_rmo(int rowDim, int colDim, int rank, double* A) {

  if(rank > 1) {
    for(int j = rank - 1; j > 0; j--) {
      for(int i = rowDim - 1; i > rank - 1; i--) {
        for(int k = 0; k < j; k++)
          A[i * colDim + k] -= A[i * colDim + j] * A[j * colDim + k];
      }
    }
  } else {
    // Error & do something.
  }
}

/*!
@brief computes the Row Major Order L D L^T decomposition of a symmetric
positive definite matrix without pivoting

returns the matrix L D L^T in the original matrix A

@param *A [in/out] matrix with dimensions A[0:n-1][0:n-1]:
@param n row and column dimension of matrix A.

*/
int ksl_linalg_ldlt_rmo(double* restrict A, const int n) {
  for(int k = 0; k < n; k++) {
    double pivot_inv = A[k * n + k];
    if(pivot_inv > 0.0) {
      pivot_inv = 1.0 / A[k * n + k];
    } else {
      return k + 1;
    }
    for(int j = k + 1; j < n; j++) {
      A[k * n + j] = A[j * n + k];
      A[j * n + k] *= pivot_inv;
    }
    for(int j = k + 1; j < n; j++) {
      for(int i = k + 1; i < j + 1; i++) {
        A[j * n + i] -= A[j * n + k] * A[k * n + i];
      }
    }
  }
  for(int i = 0; i < n; i++) {
    for(int j = i + 1; j < n; j++) {
      A[i * n + j] = A[j * n + i];
    }
  }
  return 0;
}

/*!
@brief perform Row Major Order Cholesky (L * L^T) decomposition of a symmetric
matrix without pivoting

returns the matrix factor L in the lower triangular portion of matrix A

@param *A [in/out] matrix with dimensions A[0:n-1][0:n-1]:
@param n row and column dimension of matrix A.

*/
int ksl_linalg_cholesky_rmo(double* restrict A, const int n) {
  for(int k = 0; k < n; k++) {
    if(A[k * n + k] > 0.0) {
      A[k * n + k] = sqrt(A[k * n + k]);
    } else {
      return k + 1;
    }
    double pivot_inv = 1.0 / A[k * n + k];
    for(int j = k + 1; j < n; j++) {
      A[j * n + k] *= pivot_inv;
    }
    for(int j = k + 1; j < n; j++) {
      for(int i = k + 1; i < j + 1; i++) {
        A[j * n + i] -= A[j * n + k] * A[i * n + k];
      }
    }
  }
  for(int i = 0; i < n; i++) {
    for(int j = i + 1; j < n; j++) {
      A[i * n + j] = A[j * n + i];
    }
  }
  return 0;
}

/*!
@brief used to solve a system of equations

L * y = b

for y using forward elimination where L IS Row Major Order unit lower triangular

y = L^-1 * b

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *y [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_ldlt_forwardElimination_rmo(const double* restrict L,
                                                   const double* restrict b,
                                                   double* restrict y,
                                                   const int n) {
  for(int i = 0; i < n; i++) {
    y[i] = b[i];
    for(int j = 0; j < i; j++) {
      y[i] -= L[i * n + j] * y[j];
    }
  }
}

/*!
@brief used to solve a system of equations

L^T * x = y

using backward substitution where L IS Row Major Order unit lower triangular

x = L^-T * y

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *y [in] n by 1 column of right-hand side b[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], y[], x[]:

*/
inline void ksl_linalg_ldlt_backwardSubstitution_rmo(const double* restrict L,
                                                     const double* restrict y,
                                                     double* restrict x,
                                                     const int n) {
  x[n - 1] = y[n - 1];
  for(int i = n - 2; i > -1; i--) {
    x[i] = y[i];
    for(int j = i + 1; j < n; j++) {
      x[i] -= L[j * n + i] * x[j];
    }
  }
}

/*!
@brief used to solve a system of equations

L * y = b

for y using forward elimination where L is Row Major Order and IS NOT unit lower
triangular

y = L^-1 * b

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *y [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_cholesky_forwardElimination_rmo(const double* restrict L,
                                                       const double* restrict b,
                                                       double* restrict y,
                                                       const int n) {
  for(int i = 0; i < n; i++) {
    double t = b[i];
    for(int j = 0; j < i; j++) {
      t -= L[i * n + j] * y[j];
    }
    y[i] = t / L[i * n + i];
  }
}

/*!
@brief used to solve a system of equations

L^T * x = y

using backward substitution where L is Row Major Order and IS NOT unit lower
triangular

x = L^-T * y

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *y [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], y[], x[]:

*/
inline void
ksl_linalg_cholesky_backwardSubstitution_rmo(const double* restrict L,
                                             const double* restrict y,
                                             double* restrict x, const int n) {
  x[n - 1] = y[n - 1] / L[(n - 1) * n + (n - 1)];
  for(int i = n - 2; i > -1; i--) {
    double t = y[i];
    for(int j = i + 1; j < n; j++) {
      t -= L[j * n + i] * x[j];
    }
    x[i] = t / L[i * n + i];
  }
}

/*!
@brief solve the system of equations A * x = b where A is Row Major Order and
a symmetric positive definite matrix A

ksl_linalg_ldlt must be called on A prior to calling this function

@param *A [in] n by n matrix A[0:n-1][0:n-1] containing Choleski-factored
matrix:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_ldlt_solve_rmo(const double* restrict A,
                                      const double* restrict b,
                                      double* restrict x, const int n) {
  double y[n];
  ksl_linalg_ldlt_forwardElimination_rmo(A, b, y, n);
  for(int i = 0; i < n; i++) {
    y[i] /= A[i * n + i];
  }
  ksl_linalg_ldlt_backwardSubstitution_rmo(A, y, x, n);
}

/*!
@brief solve the system of equations A * x = b where A is Row Major Order and
a symmetric positive definite matrix A

ksl_linalg_cholesky must be called on A prior to calling this function

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *y [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_cholesky_solve_rmo(const double* restrict A,
                                          const double* restrict b,
                                          double* restrict x, const int n) {
  double y[n];
  ksl_linalg_cholesky_forwardElimination_rmo(A, b, y, n);
  ksl_linalg_cholesky_backwardSubstitution_rmo(A, y, x, n);
}

/*!
@brief compute inverse of a symmetric positive definite matrix A
where A and A_inverse are in Row Major Order.
*/

inline int ksl_linalg_symmetricMatrixInverse_rmo(double* restrict A,
                                                 const int n) {
  int status = ksl_linalg_ldlt_rmo(A, n);
  if(status > 0) {
    return status;
  }
  for(int i = 0; i < n - 1; i++) {
    for(int j = i + 1; j < n; j++) {
      A[i * n + j] = 0.0;
    }
  }
  for(int i = 0; i < n; i++) {
    A[i * n + i] = 1.0 / A[i * n + i];
  }
  for(int i = n - 2; i >= 0; i--) {  // i = n-1:-1:1
    for(int j = n - 1; j > i; j--) { // j = n:-1:i+1
      for(int k = j; k > i; k--) {   // k = j:-1:i+1
        A[i * n + j] -=
          A[k * n + i] * A[k * n + j]; // A(i,j) = A(i,j) - A(k,i)*A(k,j);
      }
    }
  }
  for(int i = n - 2; i >= 0; i--) {    // i = n-1:-1:1
    for(int j = 0; j <= i; j++) {      // j = 1:1:i
      for(int k = i + 1; k < n; k++) { // k = i+1:1:n
        A[j * n + i] -=
          A[j * n + k] * A[k * n + i]; // A(j,i) = A(j,i) - A(j,k)*A(k,i);
      }
    }
  }
  for(int i = 0; i < n - 1; i++) {
    for(int j = i + 1; j < n; j++) {
      A[j * n + i] = A[i * n + j];
    }
  }
  return 0;
}

// inline int ksl_linalg_symmetricMatrixInverse_rmo(double* restrict A, const
// int n) {
//   double A_inverse[n * n];
//   double a[n];
//
//   int status = ksl_linalg_ldlt_rmo(A, n);
//   if(status > 0) {
//     return status;
//   }
//   for(int i = 0; i < n; i++) {
//     memset(a, 0, n * sizeof(double));
//     a[i] = 1.0;
//     ksl_linalg_ldlt_solve_rmo(A, a, &A_inverse[i * n + 0], n);
//   }
//   memcpy(A, A_inverse, n * n * sizeof(double));
//   return 0;
// }

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*!
@brief Column Major Order LU Decomposition with complete row and column pivoting

ksl_linalg_lu_full_cmo factors a double precision matrix, A[rowDim * colDim],
stored in Column Major Order using full row and column pivoting. Matrix A need
not have full row or column rank. The integer variable 'rank' represents the
matrix rank and internally is always one less than the true rank to be
consistent with C's indexing from zero. Upon return from the function,
rank will be set to the correct value.

The lower triangular (rank+1) by (rank+1) Lr matrix, except its unity
diagonal, is stored below the diagonal in A[0:rank][0:rank]. The upper
triangular (rank+1) by (rank+1) Ur matrix is stored on and above the
diagonal in A[0:rank][0:rank].

Following the first major matrix decomposition step, A[0:rank][rank+1:colDim]
stores the residual matrix UR and A[rank+1:rowDim][0:rank] stores the residual
matrix LR.

The following operations on this matrix are optionally performed after
ksl_linalg_lu_full_cmo() has factored A.

ksl_linalg_lu_setBMatrix_cmo() computes and stores B = inverse(Ur)*UR and stores
it back in A[0:rank][rank+1:colDim].
ksl_linalg_lu_setCMatrix_cmo() computes and stores C = LR*inverse(Lr) and stores
it back in A[rank+1:rowDim][0:rank].

The remaining A[rank+1:rowDim][rank+1:colDim] submatrix contains numbers whose
absolute values are all smaller than eps times the absolutely largest
entry in the original matrix. This part of the matrix is not used.

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param *A [in/out] matrix to be factored with dimensions
A[0:rowDim-1][0:colDim-1], LU factors are overwritten in original matrix
@param eps [in] input tolerance on the order of machine roundoff.
@param pr [out] output row permutation array with dimensions pr[0:rowDim-1]
@param pc [out] output column permutation array with dimensions pc[0:colDim-1]
@return rank of matrix A
*/
int ksl_linalg_lu_full_cmo(const int rowDim, const int colDim,
                           double* restrict A, const double eps,
                           int* restrict pr, int* restrict pc) {

  // printf("Jacobian matrix in fullFactor_LU\n");
  // for(int i = 0; i < rowDim; i++) {
  //   for(int j = 0; j < colDim; j++)
  //     printf("% 2.4f ", A[j * rowDim + i]);
  //   printf("\n");
  // }

  int rank;     /* (rank+1) is the matrix rank */
  int pivotRow; /*  row with current pivotal element */
  int pivotCol; /* column with current pivotal element */
  double pivot = 0.0;
  /* current pivotal element, holds the current pivotal element
     value */
  double tol;                    /* tolerance for checking residual matrix
                                               infinity norm against */
  double save;                   /* variable for holding intermediate results*/
  double size = rowDim * colDim; /*overall size of matrix */

  /*
    Return failure if a bad row or column dimension was found.
  */
  if((rowDim <= 0) || (colDim <= 0)) {
    return (-1);
  }

  /*
    Consistent with C-indexing, rank always has a value of
    one less than the true value. rank+1 is returned.
  */
  rank = -1; /* initialize for indexing */

  /*
    Search through the entire A matrix to find the
    absolutely largest element for assigning to pivot.
    Save that row and column number in pivotRow and
    pivotCol. Also intialize the row permutation array pr.
  */
#ifdef KSL_WITH_BLAS_
  int index = cblas_idamax(size, A, 1);
#else
  int index = ksl_maxIndex(size, A);
#endif
  // div_t result = div(index, colDim);
  // pivotRow = result.quot;
  // pivotCol = result.rem;
  pivotCol = index / rowDim;
  pivotRow = index - pivotCol * rowDim;
  pivot = A[index];
  // printf("pivotRow: %d, pivotCol: %d\n", pivotRow, pivotCol);

  /* Initialize the column permutation array. */
  for(int col = 0; col < colDim; col++) {
    pc[col] = col;
  }
  for(int row = 0; row < rowDim; row++) {
    pr[row] = row;
  }

  /* Set tolerance to check all zero-elements against.
  pivot = infinity norm of A.
  */
  tol = fabs(eps * pivot);

  /*
    Major loop to permute and factor the matrix.
    Generate factors column by column, so outer loop
    sweeps over columns.
  */
  for(int col = 0; col < colDim; col++) {
    /*
      If the current pivotal element is <= tol, the
      remaining submatrix is zero and factorization
      is complete. The else statement for the following if test
      breaks out of this loop so not to waste time
      going through the remaining columns.
    */
    if(fabs(pivot) > tol) {
      /* Increase rank for the current column. */
      ++rank;

      /*
        pivotRow can never be less than rank.
        If pivotRow==rank, then no permutation is needed.
        If pivotRow>rank, then need to swap rows.
      */
      if(pivotRow > rank) {
        /*
          Swap entire rows A[rank][:] and A[pivotRow][:].
          This also swaps rows in the LR matrix to this point.

          swap rows:
            N = colDim
            stride = rowDim
          Start of replaced row is A + rank & stride is rowDim
          Start of pivot row is A + pivotRow & stride is rowDim
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(colDim, A + rank, rowDim, A + pivotRow, rowDim);
#else
        ksl_swapArray(colDim, A + rank, rowDim, A + pivotRow, rowDim);
#endif
        /*
          Swap row permutation entries pr[rank] and
          pr[pivotRow] to reflect row swaps in A.
        */
        ksl_swapi(pr + rank, pr + pivotRow);
      }

      /* pivotCol can never be less than rank.
      If pivotCol==rank, then no permutation is needed.
      If pivotCol>rank, then need to swap columns. */
      if(pivotCol > rank) {
        // printf("rank: %d, pivotCol: %d\n", rank, pivotCol);
        // printf("colDim %d\n", colDim);
        /*
        Swap entire columns A[:][rank] and A[:][pivotCol].
        This also swaps columns in the UR matrix to this point.
        swap columns:
          N = rowDim
          stride = 1
          Start of replaced column is A + rowDim * rank & stride is 1
          Start of pivot column is A + rowDim * pivotCol & stride is 1
            */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(rowDim, A + rowDim * rank, 1, A + rowDim * pivotCol, 1);
#else
        ksl_swapArray(rowDim, A + rowDim * rank, 1, A + rowDim * pivotCol, 1);
#endif
        /*
          Swap column permutation entries pc[rank] and
          pr[pivotCol] to reflect column swaps.
        */
        ksl_swapi(pc + rank, pc + pivotCol);
      }

      /*
        rank cannot be > rowDim-1. If rank < rowDim-1
        there is still some processing left to do. If
        rank == rowDim-1, there is only a 1 on the
        diagonal of this column of Lr. This loop factors
        the matrix and searches for a new pivotal element
        for the next factorization step.
      */
      if(rank < rowDim - 1) {

        /*
          Copy the current pivotal element into save so new
          pivotal element can be stored back in pivot.
        */
        save = pivot;
        pivot = 0;

        /* Need only process the rows from rank+1 to rowDim. */
        for(int i = rank + 1; i < rowDim; i++) {

          /*
            Evaluate the current entry in the L matrix.
            (rank is always one less than the true rank.)
            The first rank columns of the A matrix are complete.
            Now working in column rank of the A matrix, so the offset
            to the start of the working column is rank * rowDim.
          */
          A[rank * rowDim + i] /= save;

          /*
            rank cannot be >= colDim. If rank < colDim
            there is still some processing left to do. If
            rank == colDim-1, only a pivot remains on the
            diagonal.
          */
          if(rank < colDim) {

            /*
              This next loop computes the remaining U matrix,
              searches for the largest pivotal element, and
              remembers the new pivotal element row and colum.
            */
            for(int j = rank + 1; j < colDim; j++) {
              A[j * rowDim + i] -= A[rank * rowDim + i] * A[j * rowDim + rank];
              if(fabs(A[j * rowDim + i]) > fabs(pivot)) {
                pivot = A[j * rowDim + i];
                pivotRow = i;
                pivotCol = j;
              } /*End if*/
            }   /*End for j*/
          }     /*End if rank*/
        }       /*Endfor i = rank+1*/
      }         /*Endif (rank<rowDim-1)*/
      else {
        break; /*No more rows to process.*/
      }
    } /*End if*/
    else {
      break; /*Done if remainder of matrix is at noise level.*/
    }
  } /*End for col*/

  return (rank + 1);
}

/*!
@brief Column Major Order LU Decomposition with complete row and column pivoting
with one specified coordinate

ksl_linalg_lu_full_specified_cmo factors a double precision matrix, A[rowDim *
colDim], stored in Column Major Order using full row and column pivoting. Matrix
A need not have full row or column rank. The integer variable 'rank' represents
the matrix rank and internally is always one less than the true rank to be
consistent with C's indexing from zero. Upon return from the function,
rank will be set to the correct value.

The lower triangular (rank+1) by (rank+1) Lr matrix, except its unity
diagonal, is stored below the diagonal in A[0:rank][0:rank]. The upper
triangular (rank+1) by (rank+1) Ur matrix is stored on and above the
diagonal in A[0:rank][0:rank].

Following the first major matrix decomposition step, A[0:rank][rank+1:colDim]
stores the residual matrix UR and A[rank+1:rowDim][0:rank] stores the residual
matrix LR.

ksl_linalg_lu_setBMatrix_cmo() computes and stores B = inverse(Ur)*UR and stores
it back in A[0:rank][rank+1:colDim].
ksl_linalg_lu_setCMatrix_cmo() computes and stores C = LR*inverse(Lr) and stores
it back in A[rank+1:rowDim][0:rank].

The remaining A[rank+1:rowDim][rank+1:colDim-1] submatrix contains numbers whose
absolute values are all smaller than eps times the absolutely largest
entry in the original matrix. This part of the matrix is not used.
The remainig A[rank+1:rowDim][colDim] submatrix could contain numbers whose
absolute values are greater than eps times the absolutely largest
entry in the original matrix. Adding a check here would be one way to
assert that the specified variable has sufficient mobility.

@param rowDim input row dimension of matrix A.
@param colDim input column dimension of matrix A.
@param **A input/output matrix to be factored with dimensions
A[0:rowDim-1][0:colDim-1]:
@param eps input tolerance on the order of machine roundoff.
@param pr output row permutation array with dimensions pr[0:rowDim-1]
@param pc output column permutation array with dimensions pc[0:colDim-1]
@return rank of matrix A
*/
int ksl_linalg_lu_full_specified_cmo(const int rowDim, const int colDim,
                                     double* restrict A, double eps,
                                     int* restrict pr, int* restrict pc,
                                     const int specifiedIndex) {

  // printf("Jacobian matrix in fullFactor_LU\n");
  // for(int i = 0; i < rowDim; i++) {
  //   for(int j = 0; j < colDim; j++)
  //     printf("% 2.4f ", A[j * rowDim + i]);
  //   printf("\n");
  // }

  int rank;     /* (rank+1) is the matrix rank */
  int pivotRow; /* row with current pivotal element */
  int pivotCol; /* column with current pivotal element */
  double pivot = 0.0;
  /* current pivotal element, holds the current pivotal element */
  double tol;                    /* tolerance for checking residual matrix
                                               infinity norm against */
  double save;                   /* variable for holding intermediate results*/
  double size = rowDim * colDim; /*overall size of matrix */

  /*
    Return failure if a bad row or column dimension was found.
  */
  if((rowDim <= 0) || (colDim <= 1)) {
    return (-1);
  }

  /*
    Consistent with C-indexing, rank always has a value of
    one less than the true value. rank+1 is returned.
  */
  rank = -1; /* initialize for indexing */

  /*
    Initialize permutation arrays
  */
  for(int row = 0; row < rowDim; row++) {
    pr[row] = row;
  }
  for(int col = 0; col < colDim; col++) {
    pc[col] = col;
  }

  /*
    Swap the specified column to the last position in the input
    matrix
    Swap entire columns A[:][specifiedIndex] and A[:][colDim - 1].
    swap columns:
      N = rowDim
      stride = 1
      Start of specefied column is A + rowDim * specifiedIndex & stride 1
      Start of last column is A + rowDim * (colDim - 1) & stride is 1
  */
  if(specifiedIndex != colDim - 1) {
#ifdef KSL_WITH_BLAS
    cblas_dswap(rowDim, A + rowDim * specifiedIndex, 1,
                A + rowDim * (colDim - 1), 1);
#else
    ksl_swapArray(rowDim, A + rowDim * specifiedIndex, 1,
                  A + rowDim * (colDim - 1), 1);
#endif
    /*
      Swap column permutation entries pc[] and
      pc[pivotRow] to reflect row swaps in A.
    */
    ksl_swapi(pc + specifiedIndex, pc + colDim - 1);
  }

  /*
    Search through matrix A except the last column to find the
    absolutely largest element for assigning to pivot.
    Save that row and column number in pivotRow and
    pivotCol. This could cause numerical problems if the last
    column contains one or more entries substantially absolutely
    larger than all other entries in A.
  */
  pivot = A[0];
  pivotRow = 0;
  pivotCol = 0;
  for(int col = 0; col < colDim - 1; col++) {
    for(int row = 0; row < rowDim; row++) {
      if(fabs(A[col * rowDim + row]) > fabs(pivot)) {
        pivot = A[col * rowDim + row];
        pivotRow = row;
        pivotCol = col;
      }
    }
  }
  // printf("pivotRow: %d, pivotCol: %d\n", pivotRow, pivotCol);

  /* Set tolerance to check all zero-elements against.
  pivot = infinity norm of A.
 */
  tol = fabs(eps * pivot);

  /*
    Major loop to permute and factor the matrix.
    Generate factors column by column, so outer loop
    sweeps over columns.
  */
  for(int col = 0; col < colDim; col++) {
    /*
    If the current pivotal element is <= tol, the
    remaining submatrix is zero and factorization
    is complete. The else statement for the following if test
    breaks out of this loop so not to waste time
    going through the remaining columns.
    */
    if(fabs(pivot) > tol) {
      /* Increase rank for the current column. */
      ++rank;

      /*
        pivotRow can never be less than rank.
        If pivotRow==rank, then no permutation is needed.
        If pivotRow>rank, then need to swap rows.
      */
      if(pivotRow > rank) {
        /*
          Need to swap entire row A[rank][:] and A[pivotRow][:].
          This also swaps rows in the Lr matrix to this point.
          swap rows:
            N = colDim
            stride = rowDim
          Start of replaced row is A + rank & stride is rowDim
          Start of pivot row is A + pivotRow & stride is rowDim
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(colDim, A + rank, rowDim, A + pivotRow, rowDim);
#else
        ksl_swapArray(colDim, A + rank, rowDim, A + pivotRow, rowDim);
#endif
        /*
          Swap row permutation entries pr[rank] and
          pr[pivotRow] to reflect row swaps in A.
        */
        ksl_swapi(pr + rank, pr + pivotRow);
      }

      /*
        pivotCol can never be less than rank.
        If pivotCol==rank, then no permutation is needed.
        If pivotCol>rank, then need to swap columns.
      */
      if(pivotCol > rank) {
        // printf("rank: %d, pivotCol: %d\n", rank, pivotCol);
        // printf("colDim %d\n", colDim);
        /*
        Swap entire columns A[:][rank] and A[:][pivotCol].
        This also swaps columns in the UR matrix to this point.
        swap columns:
          N = rowDim
          stride = 1
          Start of replaced column is A + rowDim * rank & stride is 1
          Start of pivot column is A + rowDim * pivotCol & stride is 1
        */
#ifdef KSL_WITH_BLAS_
        cblas_dswap(rowDim, A + rowDim * rank, 1, A + rowDim * pivotCol, 1);
#else
        ksl_swapArray(rowDim, A + rowDim * rank, 1, A + rowDim * pivotCol, 1);
#endif
        /*
          Swap column permutation entries pc[rank] and
          pr[pivotCol] to reflect column swaps.
        */
        ksl_swapi(pc + rank, pc + pivotCol);
      }

      /*
        rank cannot be > rowDim-1. If rank < rowDim-1
        there is still some processing left to do. If
        rank == rowDim-1, there is only a 1 on the
        diagonal of this column of Lr. This loop factors
        the matrix and searches for a new pivotal element
        for the next factorization step.
      */
      if(rank < rowDim - 1) {

        /*
          Copy the current pivotal element into save so new
          pivotal element can be stored back in pivot.
        */
        save = pivot;
        pivot = 0;

        /* Need only process the rows from rank+1 to rowDim. */
        for(int i = rank + 1; i < rowDim; i++) {

          /* Evaluate the current entry in the L matrix.*/
          A[rank * rowDim + i] /= save;

          /*
            rank cannot be >= colDim. If rank < colDim
            there is still some processing left to do. If
            rank == colDim-1, only a pivot remains on the
            diagonal.
          */
          if(rank < colDim) {

            /*
              This next loop computes the remaining U matrix,
              searches for the largest pivotal element, and
              remembers the new pivotal element row and colum.
            */
            for(int j = rank + 1; j < colDim; j++) {
              A[j * rowDim + i] -= A[rank * rowDim + i] * A[j * rowDim + rank];
            }
            for(int j = rank + 1; j < colDim - 1; j++) {
              if(fabs(A[j * rowDim + i]) > fabs(pivot)) {
                pivot = A[j * rowDim + i];
                pivotRow = i;
                pivotCol = j;
              } /*End if*/
            }   /*End for j*/
          }     /*End if rank*/
        }       /*Endfor i = rank+1*/
      }         /*Endif (rank<rowDim-1)*/
      else {
        break; /*No more rows to process.*/
      }
    } /*End if*/
    else {
      break; /*Done if remainder of matrix is at noise level.*/
    }
  } /*End for col*/
  return (rank + 1);
}

/*!
@brief *Fast* Column Major Order LU Decomposition with no row or column pivoting
for a rectangular matrix

This function LU factors a double precision matrix, A[rank][colDim],
using no row or column pivoting. This function should be used only if the
left rank by rank submatrix is known to be nonsingular.

rank <= colDim or error.

The lower triangular rank by rank Lr matrix, except the unity
diagonal, is stored below the diagonal in A[0:rank-1][0:rank-1]. The upper
triangular rank by rank Ur matrix is stored on and above the
diagonal in A[0:rank-1][0:rank-1].

Note that this minimal algorithm variation does not compute the product
B = inverse(Ur)*UR

@param rank [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param *A [in/out] matrix to be factored with dimensions
A[0:rank-1][0:colDim-1]:
*/
inline void ksl_linalg_lu_cmo(const int rank, const int colDim,
                              double* restrict A) {

  if(rank > colDim) {
    // Error message & exit
  }

  /*
    Major loop to factor the matrix.
    Generate factors column by column
  */
  for(int row = 0; row < rank; row++) {

    /* i iterates over rows of A, up to rank-1 */
    for(int i = row + 1; i < rank; i++) {

      /* Evaluate the current entry in the L matrix.*/
      A[row * rank + i] /= A[row * rank + row];

      /* Compute U matrix */
      for(int j = row + 1; j < colDim; j++) {
        A[j * rank + i] -= A[row * rank + i] * A[j * rank + row];
      }
    }
  }
}

/*!
@brief compute Column Major Order B matrix (inverse(Ur) * UR)

This block overwrites UR with inverse(Ur) * UR
If rank == 0 or rank == colDim nothing to process, so exit with message.
This loop computes A[0:rank-1][rank:colDim-1] = inverse(Ur)*UR
where UR is stored in A[0:rank-1][rank:colDim-1]
and Ur is stored in upper part of A[0:rank-1][0:rank-1].
i is the row number in Ur.
j is the column number in Ur & row number in UR.
k is the column number in UR.

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param rank [in] rank of matrix A.
@param *A [in/out] matrix with dimensions A[0:rowDim-1][0:colDim-1]:

*/
inline void ksl_linalg_lu_setBMatrix_cmo(const int rowDim, const int colDim,
                                         const int rank, double* restrict A) {

  if(rank > 0 && rank < colDim) {
    for(int i = rank - 1; i > -1; i--) {   /* rows of B */
      for(int k = rank; k < colDim; k++) { /* columns of B */
        double save = 0;
        for(int j = rank - 1; j > i; j--) { /* columns of Ur */
          save += A[j * rowDim + i] * A[k * rowDim + j];
        }
        A[k * rowDim + i] = (A[k * rowDim + i] - save) / A[i * rowDim + i];
      }
    }
  } else {
    // Error & do something.
  }
}

/*!
@brief compute Column Major Order C matrix

This block overwrites LR with LR*inverse(Lr)
If rank==1, the Lr matrix is a 1 by 1 identity
matrix, so there is nothing to do here. This loop
computes A[rank:rowDim-1][0:rank-1]=LR*inverse(Lr)
where LR is stored in A[rank:rowDim-1][0:rank-1]
and Lr is stored in A[0:rank-1][0:rank-1].
j is the column number in LR. It ends at 1
because the diagonal entry in row 0 of Lr is 1.
i is the row number in LR.
k is the column number in Lr

@param rowDim [in] row dimension of matrix A.
@param colDim [in] column dimension of matrix A.
@param rank [in] rank of matrix A.
@param *A [in/out] matrix with dimensions A[0:rowDim-1][0:colDim-1]:
*/
void ksl_linalg_lu_setCMatrix_cmo(int rowDim, int colDim, int rank, double* A) {

  if(rank > 1) {
    for(int j = rank - 1; j > 0; j--) {
      for(int i = rowDim - 1; i > rank - 1; i--) {
        for(int k = 0; k < j; k++)
          A[k * rowDim + i] -= A[j * rowDim + i] * A[k * rowDim + j];
      }
    }
  } else {
    // Error & do something.
  }
}

/*!
@brief computes the Column Major Order L D L^T decomposition of a symmetric
matrix without pivoting

returns the matrix L D L^T in the original matrix A

@param *A [in/out] matrix with dimensions A[0:n-1][0:n-1]:
@param n row and column dimension of matrix A.

*/
int ksl_linalg_ldlt_cmo(double* restrict A, const int n) {
  for(int k = 0; k < n; k++) {
    double pivot_inv = A[k * n + k];
    if(pivot_inv > 0.0) {
      pivot_inv = 1.0 / A[k * n + k];
    } else {
      return k + 1;
    }
    for(int j = k + 1; j < n; j++) {
      A[j * n + k] = A[k * n + j];
      A[k * n + j] *= pivot_inv;
    }
    for(int j = k + 1; j < n; j++) {
      for(int i = k + 1; i < j + 1; i++) {
        A[i * n + j] -= A[k * n + j] * A[i * n + k];
      }
    }
  }
  for(int i = 0; i < n; i++) {
    for(int j = i + 1; j < n; j++) {
      A[j * n + i] = A[i * n + j];
    }
  }
  return 0;
}

/*!
@brief perform Column Major Order Cholesky (L * L^T) decomposition of a
symmetric matrix without pivoting

returns the matrix factor L in the lower triangular portion of matrix A

@param *A [in/out] matrix with dimensions A[0:n-1][0:n-1]:
@param n row and column dimension of matrix A.

*/
int ksl_linalg_cholesky_cmo(double* restrict A, const int n) {
  for(int k = 0; k < n; k++) {
    if(A[k * n + k] > 0.0) {
      A[k * n + k] = sqrt(A[k * n + k]);
    } else {
      return k + 1;
    }
    double pivot_inv = 1.0 / A[k * n + k];
    for(int j = k + 1; j < n; j++) {
      A[k * n + j] *= pivot_inv;
    }
    for(int j = k + 1; j < n; j++) {
      for(int i = k + 1; i < j + 1; i++) {
        A[i * n + j] -= A[k * n + j] * A[k * n + i];
      }
    }
  }
  for(int i = 0; i < n; i++) {
    for(int j = i + 1; j < n; j++) {
      A[j * n + i] = A[i * n + j];
    }
  }
  return 0;
}

/*!
@brief used to solve a system of equations

L * y = b

for y using forward elimination where L IS Column Major Order unit lower
triangular

y = L^-1 * b

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *y [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_ldlt_forwardElimination_cmo(const double* restrict L,
                                                   const double* restrict b,
                                                   double* restrict y,
                                                   const int n) {
  for(int i = 0; i < n; i++) {
    y[i] = b[i];
  }
  for(int i = 0; i < n; i++) {
    for(int j = i + 1; j < n; j++) {
      y[j] -= L[i * n + j] * y[i];
    }
  }
}

/*!
@brief used to solve a system of equations

L^T * x = y

using backward substitution where L IS Column Major Order unit lower triangular

x = L^-T * y

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *y [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], y[], x[]:

*/
inline void ksl_linalg_ldlt_backwardSubstitution_cmo(const double* restrict L,
                                                     const double* restrict y,
                                                     double* restrict x,
                                                     const int n) {
  for(int i = n - 1; i > -1; i--) {
    x[i] = y[i];
  }
  for(int i = n - 2; i > -1; i--) {
    for(int j = n - 1; j > i; j--) {
      x[i] -= L[i * n + j] * x[j];
    }
  }
}

/*!
@brief used to solve a system of equations

L * y = b

for y using forward elimination where L is Column Major Order and IS NOT unit
lower triangular

y = L^-1 * b

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *y [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_cholesky_forwardElimination_cmo(const double* restrict L,
                                                       const double* restrict b,
                                                       double* restrict y,
                                                       const int n) {
  for(int i = 0; i < n; i++) {
    y[i] = b[i];
  }
  for(int i = 0; i < n; i++) {
    y[i] /= L[i * n + i];
    for(int j = i + 1; j < n; j++) {
      y[j] -= L[j * n + i] * y[i];
    }
  }
}

/*!
@brief used to solve a system of equations

L^T * x = y

using backward substitution where L is Column Major Order and IS NOT unit lower
triangular

x = L^-T * y

@param *L [in] n by n matrix L[0:n-1][0:n-1]:
@param *y [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], y[], x[]:

*/
inline void
ksl_linalg_cholesky_backwardSubstitution_cmo(const double* restrict L,
                                             const double* restrict y,
                                             double* restrict x, const int n) {
  for(int i = 0; i < n; i++) {
    x[i] = y[i];
  }
  x[n - 1] /= L[(n - 1) * n + (n - 1)];
  for(int i = n - 2; i > -1; i--) {
    for(int j = i + 1; j < n; j++) {
      x[i] -= L[i * n + j] * x[j];
    }
    x[i] /= L[i * n + i];
  }
}

/*!
@brief solve the system of equations A * x = b where A is Column Major Order and
a symmetric positive definite matrix A

ksl_linalg_ldlt_cmo must be called on A prior to calling this function

@param *A [in] n by n matrix A[0:n-1][0:n-1] containing Choleski-factored
matrix:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

*/
inline void ksl_linalg_ldlt_solve_cmo(const double* restrict A,
                                      double* const restrict b,
                                      double* restrict x, const int n) {
  double y[n];
  ksl_linalg_ldlt_forwardElimination_cmo(A, b, y, n);
  for(int i = 0; i < n; i++) {
    y[i] /= A[i * n + i];
  }
  ksl_linalg_ldlt_backwardSubstitution_cmo(A, y, x, n);
}

/*!
@brief solve the system of equations A * x = b where A is Column Major Order and
a symmetric positive definite matrix A

@param *A [in] n by n matrix A[0:n-1][0:n-1] containing Choleski-factored
matrix:
@param *b [in] n by 1 column of right-hand side y[0:n-1].
@param *x [out] n by 1 column of unknowns.
@param n [in] dimension of L[][], b[], y[]:

ksl_linalg_cholesky must be called on A prior to calling this function
*/
inline void ksl_linalg_cholesky_solve_cmo(const double* restrict A,
                                          const double* restrict b,
                                          double* restrict x, const int n) {
  double y[n];
  ksl_linalg_cholesky_forwardElimination_cmo(A, b, y, n);
  ksl_linalg_cholesky_backwardSubstitution_cmo(A, y, x, n);
}

/*!
@brief compute inverse of a symmetric positive definite matrix A
where A and A_inverse are in Column Major Order.
*/

inline int ksl_linalg_symmetricMatrixInverse_cmo(double* restrict A,
                                                 const int n) {
  int status = ksl_linalg_ldlt_cmo(A, n);
  if(status > 0) {
    return status;
  }
  for(int i = 0; i < n - 1; i++) {
    for(int j = i + 1; j < n; j++) {
      A[j * n + i] = 0.0;
    }
  }
  for(int i = 0; i < n; i++) {
    A[i * n + i] = 1.0 / A[i * n + i];
  }
  for(int i = n - 2; i >= 0; i--) {  // i = n-1:-1:1
    for(int j = n - 1; j > i; j--) { // j = n:-1:i+1
      for(int k = j; k > i; k--) {   // k = j:-1:i+1
        A[j * n + i] -=
          A[i * n + k] * A[j * n + k]; // A(j,i) = A(j,i) - A(i,k)*A(j,k);
      }
    }
  }
  for(int i = n - 2; i >= 0; i--) {    // i = n-1:-1:1
    for(int j = 0; j <= i; j++) {      // j = 1:1:i
      for(int k = i + 1; k < n; k++) { // k = i+1:1:n
        A[i * n + j] -=
          A[k * n + j] * A[i * n + k]; // A(i,j) = A(i,j) - A(k,j)*A(k,i);
      }
    }
  }
  for(int i = 0; i < n - 1; i++) {
    for(int j = i + 1; j < n; j++) {
      A[i * n + j] = A[j * n + i];
    }
  }
  return 0;
}

// inline int ksl_linalg_symmetricMatrixInverse_cmo(double* restrict A, const
// int n) {
//   double A_inverse[n * n];
//   double a[n];
//
//   int status = ksl_linalg_ldlt_cmo(A, n);
//   if(status > 0) {
//     return status;
//   }
//   for(int i = 0; i < n; i++) {
//     memset(a, 0, n * sizeof(double));
//     a[i] = 1.0;
//     ksl_linalg_ldlt_solve_cmo(A, a, &A_inverse[i * n + 0], n);
//   }
//   memcpy(A, A_inverse, n * n * sizeof(double));
//   return 0;
// }

max_stars_repo_name: ideaplexus/ksl

max_issues_repo_name: ideaplexus/ksl

max_forks_repo_name: ideaplexus/ksl


text:
/* A simple example program to square a number using GSL. */

#include <gsl/gsl_math.h>
#include <stdio.h>
#include <stdlib.h>

int main(int argc, char **argv)
{
    double x;
    char *startptr, *endptr;

    if (argc != 2)
    {
        fputs("Expected exactly one command line argument\n", stderr);
        return EXIT_FAILURE;
    }

    startptr = argv[1];
    endptr = NULL;
    x = strtod(startptr, &endptr);

    if (startptr == endptr)
    {
        fputs("Expected command line argument to be a float\n", stderr);
        return EXIT_FAILURE;
    }

    x = gsl_pow_2(x);
    printf("%g\n", x);
    return EXIT_SUCCESS;
}

ext: c

max_stars_repo_name: luminartech/auditwheel

max_issues_repo_name: luminartech/auditwheel

max_forks_repo_name: luminartech/auditwheel


text:
#include <stdlib.h>
#include <stddef.h>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <cblas.h>
#include "LCP_Solver.h"
#include "debug.h"

void numericsError(char * functionName, char* message)
{
  char output[200] = "Numerics error - ";
  strcat(output, functionName);
  strcat(output, message);
  strcat(output, ".\n");
  fprintf(stderr, "%s", output);
  exit(EXIT_FAILURE);
}

void prodNumericsMatrix(int sizeX, int sizeY, double alpha, const NumericsMatrix* const A, const double* const x, double beta, double* y)
{
  assert(A);
  assert(x);
  assert(y);
  assert(A->size0 == sizeY);
  assert(A->size1 == sizeX);
  cblas_dgemv(CblasColMajor, CblasNoTrans, sizeY, sizeX, alpha, A->matrix0, sizeY, x, 1, beta, y, 1);
}

void deleteSolverOptions(SolverOptions* op)
{
  if(op)
  {
    if (op->iparam != NULL)
      free(op->iparam);
    op->iparam = NULL;
    if (op->dparam != NULL)
      free(op->dparam);
    op->dparam = NULL;
  }
}

void lcp_lexicolemke(LinearComplementarityProblem* problem, double *zlem , double *wlem , int *info , SolverOptions* options)
{
  /* matrix M of the lcp */
  double * M = problem->M->matrix0;
  assert(M);
  /* size of the LCP */
  int dim = problem->size;
  assert(dim>0);
  int dim2 = 2 * (dim + 1);

  int i, drive, block, Ifound;
  int ic, jc;
  int ITER;
  int nobasis;
  int itermax = options->iparam[0];

  i=0;
  int n = problem->size;
  double *q = problem->q;

  while ((i < (n - 1)) && (q[i] >= 0.))
    i++;

  if ((i == (n - 1)) && (q[n - 1] >= 0.))
  {
    /* TRIVIAL CASE : q >= 0
     * z = 0 and w = q is solution of LCP(q,M)
     */
    for (int j = 0 ; j < n; j++)
    {
      zlem[j] = 0.0;
      wlem[j] = q[j];
    }
    *info = 0;
    options->iparam[1] = 0;   /* Number of iterations done */
    options->dparam[1] = 0.0; /* Error */
    if (options->verboseMode > 0)
      printf("lcp_lexicolemke: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
    return ;
  }

  double z0, zb, dblock;
  double pivot, tovip;
  double tmp;
  int *basis;
  double** A;

  /*output*/
  options->iparam[1] = 0;

  /* Allocation */
  basis = (int *)malloc(dim * sizeof(int));
  A = (double **)malloc(dim * sizeof(double*));

  for (ic = 0 ; ic < dim; ++ic)
    A[ic] = (double *)malloc(dim2 * sizeof(double));

  /* construction of A matrix such that
   * A = [ q | Id | -d | -M ] with d = (1,...1)
   */
  /* We need to init only the part corresponding to Id */
  for (ic = 0 ; ic < dim; ++ic)
    for (jc = 1 ; jc <= dim; ++jc)
      A[ic][jc] = 0.0;

  for (ic = 0 ; ic < dim; ++ic)
    for (jc = 0 ; jc < dim; ++jc)
      A[ic][jc + dim + 2] = -M[dim * jc + ic];

  assert(problem->q);

  for (ic = 0 ; ic < dim; ++ic) A[ic][0] = problem->q[ic];

  for (ic = 0 ; ic < dim; ++ic) A[ic][ic + 1 ] =  1.0;
  for (ic = 0 ; ic < dim; ++ic) A[ic][dim + 1] = -1.0;

  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});
  /* End of construction of A */

  Ifound = 0;

  for (ic = 0 ; ic < dim  ; ++ic) basis[ic] = ic + 1;

  drive = dim + 1;
  block = 0;
  z0 = A[block][0];
  ITER = 0;

  /* Start research of argmin lexico */
  /* With this first step the covering vector enter in the basis */
  for (ic = 1 ; ic < dim ; ++ic)
  {
    zb = A[ic][0];
    if (zb < z0)
    {
      z0    = zb;
      block = ic;
    }
    else if (zb == z0)
    {
      for (jc = 0 ; jc < dim ; ++jc)
      {
        dblock = A[block][1 + jc] - A[ic][1 + jc];
        if (dblock < 0)
        {
          break;
        }
        else if (dblock > 0)
        {
          block = ic;
          break;
        }
      }
    }
  }

  /* Stop research of argmin lexico */
  DEBUG_PRINTF("Pivoting %i and %i\n", block, drive);

  pivot = A[block][drive];
  tovip = 1.0 / pivot;

  /* Pivot < block , drive > */
  A[block][drive] = 1;
  for (ic = 0       ; ic < drive ; ++ic) A[block][ic] = A[block][ic] * tovip;
  for (ic = drive + 1 ; ic < dim2  ; ++ic) A[block][ic] = A[block][ic] * tovip;

  /* */

  for (ic = 0 ; ic < block ; ++ic)
  {
    tmp = A[ic][drive];
    for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
  }
  for (ic = block + 1 ; ic < dim ; ++ic)
  {
    tmp = A[ic][drive];
    for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
  }

   nobasis = basis[block];
  basis[block] = drive;

  DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));
  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  while (ITER < itermax && !Ifound)
  {

    ++ITER;

    if (nobasis < dim + 1)      drive = nobasis + (dim + 1);
    else if (nobasis > dim + 1) drive = nobasis - (dim + 1);

    DEBUG_PRINTF("driving variable %i \n", drive);

    /* Start research of argmin lexico for minimum ratio test */
    pivot = 1e20;
    block = -1;

    for (ic = 0 ; ic < dim ; ++ic)
    {
      zb = A[ic][drive];
      if (zb > 0.0)
      {
        z0 = A[ic][0] / zb;
        if (z0 > pivot) continue;
        if (z0 < pivot)
        {
          pivot = z0;
          block = ic;
        }
        else
        {
          for (jc = 1 ; jc < dim + 1 ; ++jc)
          {
            assert(block >=0 && "lcp_lexicolemke: block <0");
            dblock = A[block][jc] / pivot - A[ic][jc] / zb;
            if (dblock < 0.0) break;
            else if (dblock > 0.0)
            {
              block = ic;
              break;
            }
          }
        }
      }
    }
    if (block == -1)
    {
      Ifound = 1;
      DEBUG_PRINT("The pivot column is nonpositive !\n"
          "It either means that the algorithm failed or that the LCP is infeasible\n"
          "Check the class of the M matrix to find out the meaning of this\n");
      break;
    }

    if (basis[block] == dim + 1) Ifound = 1;

    /* Pivot < block , drive > */
    pivot = A[block][drive];
    tovip = 1.0 / pivot;
    A[block][drive] = 1;

    for (ic = 0       ; ic < drive ; ++ic) A[block][ic] = A[block][ic] * tovip;
    for (ic = drive + 1 ; ic < dim2  ; ++ic) A[block][ic] = A[block][ic] * tovip;

    /* */

    for (ic = 0 ; ic < block ; ++ic)
    {
      tmp = A[ic][drive];
      for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
    }
    for (ic = block + 1 ; ic < dim ; ++ic)
    {
      tmp = A[ic][drive];
      for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
    }

    nobasis = basis[block];
    basis[block] = drive;

    DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));

    DEBUG_PRINT("total matrix\n");
    DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  } /* end while*/

  DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));

  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  for (ic = 0 ; ic < dim; ++ic)
  {
    drive = basis[ic];
    if (drive < dim + 1)
    {
      zlem[drive - 1] = 0.0;
      wlem[drive - 1] = A[ic][0];
    }
    else if (drive > dim + 1)
    {
      zlem[drive - dim - 2] = A[ic][0];
      wlem[drive - dim - 2] = 0.0;
    }
  }

  options->iparam[1] = ITER;

  if (Ifound) *info = 0;
  else *info = 1;

  free(basis);

  for (i = 0 ; i < dim ; ++i) free(A[i]);
  free(A);
}

int linearComplementarity_driver(LinearComplementarityProblem* problem, double *z , double *w, SolverOptions* options)
{
  /********************
   * 0 - Check inputs *
   ********************/

  if (options == NULL)
    numericsError("lcp_driver", "null input for solver options");
  if (problem == NULL || z == NULL || w == NULL)
    numericsError("lcp_driver", "null input for LinearComplementarityProblem and/or unknowns (z,w)");

  int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */

  if (NoDefaultOptions == 0)
  {
    numericsError("lcp_driver_DenseMatrix", "options for solver have not been set");
  }

  if (options->verboseMode > 0)
    printSolverOptions(options);

  /* Output info. : 0: ok -  >0: problem (depends on solver) */
  int info = -1;

  /******************************************
   *  1 - Check for trivial solution
   ******************************************/

  int i = 0;
  int n = problem->size;
  double *q = problem->q;
/*  if (!((options->solverId == SICONOS_LCP_ENUM) && (options->iparam[0] == 1 )))*/
    {
      while ((i < (n - 1)) && (q[i] >= 0.)) i++;
      if ((i == (n - 1)) && (q[n - 1] >= 0.))
      {
        /* TRIVIAL CASE : q >= 0
         * z = 0 and w = q is solution of LCP(q,M)
         */
        for (int j = 0 ; j < n; j++)
        {
          z[j] = 0.0;
          w[j] = q[j];
        }
        info = 0;
        options->dparam[1] = 0.0; /* Error */
        if (options->verboseMode > 0)
          printf("LCP_driver_DenseMatrix: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
        return info;
      }
    }

  /*************************************************
   *  2 - Call Lemke solver (if no trivial sol.)
   *************************************************/

  if (options->verboseMode == 1)
    printf(" ========================== Call Lemke solver for Linear Complementarity problem ==========================\n");

  /****** Lemke algorithm ******/
  /* IN: itermax
     OUT: iter */
   lcp_lexicolemke(problem, z , w , &info , options);

  /*************************************************
   *  3 - Computes w = Mz + q and checks validity
   *************************************************/
  if (options->filterOn > 0)
  {
    int info_ = lcp_compute_error(problem, z, w, options->dparam[0], &(options->dparam[1]),
				  options->verboseMode);
    if (info <= 0) /* info was not setor the solver was happy */
      info = info_;
  }

  return info;
}

void lcp_compute_error_only(unsigned int n, double *z , double *w, double * error)
{
  /* Checks complementarity */

  *error = 0.;
  double zi, wi;
  for (unsigned int i = 0 ; i < n ; i++)
  {
    zi = z[i];
    wi = w[i];
    if (zi < 0.0)
    {
      *error += -zi;
      if (wi < 0.0) *error += zi * wi;
    }
    if (wi < 0.0) *error += -wi;
    if ((zi > 0.0) && (wi > 0.0)) *error += zi * wi;
  }
}

int lcp_compute_error(LinearComplementarityProblem* problem, double *z , double *w, double tolerance,
		      double * error, int verbose)
{
  /* Checks inputs */
  if (problem == NULL || z == NULL || w == NULL)
    numericsError("lcp_compute_error", "null input for problem and/or z and/or w");

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  unsigned int n = problem->size;
  cblas_dcopy(n , problem->q , incx , w , incy);  // w <-q
  prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
  double normq = cblas_dnrm2(n , problem->q , incx);
  lcp_compute_error_only(n, z, w, error);
  *error = *error / (normq + 1.0); /* Need some comments on why this is needed */
  if (*error > tolerance)
  {
    if (verbose > 0)
	printf(" Numerics - lcp_compute_error : error = %g > tolerance = %g.\n", *error, tolerance);
    return 1;
  }
  else
    return 0;
}

int linearComplementarity_lexicolemke_setDefaultSolverOptions(SolverOptions* options)
{
  /* if (options->verboseMode > 0) */
  /* { */
  /*   printf("Set the Default SolverOptions for the Lemke Solver\n"); */
  /* } */

  options->isSet = 1;
  options->filterOn = 1;
  options->iSize = 5;
  options->dSize = 5;
  options->iparam = (int *)calloc(options->iSize, sizeof(int));
  options->dparam = (double *)calloc(options->dSize, sizeof(double));
  options->verboseMode = 0;
  options->dparam[0] = 1e-6;
  options->iparam[0] = 10000;
  return 0;
}

void printSolverOptions(SolverOptions* options)
{
  printf("\n ========== Numerics Non Smooth Solver parameters: \n");
  if (options->isSet == 0)
    printf("The solver parameters have not been set. \t options->isSet = %i \n", options->isSet);
  else
  {
    printf("The solver parameters below have  been set \t options->isSet = %i\n", options->isSet);
    printf("Name of the solver\t\t\t\t Lemke \n");
    if (options->iparam != NULL)
    {
      printf("int parameters \t\t\t\t\t options->iparam\n");
      printf("size of the int parameters\t\t\t options->iSize = %i\n", options->iSize);
      for (int i = 0; i < options->iSize; ++i)
        printf("\t\t\t\t\t\t options->iparam[%i] = %d\n", i, options->iparam[i]);
    }
    if (options->dparam != NULL)
    {
      printf("double parameters \t\t\t\t options->dparam\n");
      printf("size of the double parameters\t\t\t options->dSize = %i\n", options->dSize);
      for (int i = 0; i < options->iSize; ++i)
        printf("\t\t\t\t\t\t options->dparam[%i] = %.6le\n", i, options->dparam[i]);
    }
  }

  printf("See Lemke documentation for parameters definition)\n");
  printf("\n");
}


text:
/*
 *  rand.h
 *  Random number generation
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.0.0
 *  Last modified on 2015.09.17
 */

#ifndef DEF_RAND
#define DEF_RAND

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

void initRan();
void freeRan();
double ranInRange( double, double );
double enoise( double );
double interval( double );

#endif

//

ext: h

max_stars_repo_name: okamoto-kenji/varBayes-HMM

max_issues_repo_name: okamoto-kenji/varBayes-HMM

max_forks_repo_name: okamoto-kenji/varBayes-HMM


text:
/* gsl_histogram_copy.c
 * Copyright (C) 2000  Simone Piccardi
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
/***************************************************************
 *
 * File gsl_histogram_copy.c:
 * Routine to copy an histogram.
 * Need GSL library and headers.
 *
 * Author: S. Piccardi
 * Jan. 2000
 *
 ***************************************************************/
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

/*
 * gsl_histogram_copy:
 * copy the contents of an histogram into another
 */

int
gsl_histogram_memcpy (gsl_histogram * dest, const gsl_histogram * src)
{
  size_t n = src->n;
  size_t i;

  if (dest->n != src->n)
    {
      GSL_ERROR ("histograms have different sizes, cannot copy",
                 GSL_EINVAL);
    }

  for (i = 0; i <= n; i++)
    {
      dest->range[i] = src->range[i];
    }

  for (i = 0; i < n; i++)
    {
      dest->bin[i] = src->bin[i];
    }

  return GSL_SUCCESS;
}

/*
 * gsl_histogram_duplicate:
 * duplicate an histogram creating
 * an identical new one
 */

gsl_histogram *
gsl_histogram_clone (const gsl_histogram * src)
{
  size_t n = src->n;
  size_t i;
  gsl_histogram *h;

  h = gsl_histogram_calloc_range (n, src->range);

  if (h == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for histogram struct",
                        GSL_ENOMEM, 0);
    }

  for (i = 0; i < n; i++)
    {
      h->bin[i] = src->bin[i];
    }

  return h;
}

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: manggoguy/parsec-modified

max_forks_repo_name: manggoguy/parsec-modified


text:
#ifndef GBNET_RVNODE
#define GBNET_RVNODE

#include <string>
#include <vector>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_rstat.h>

extern gsl_rng *rng;

namespace gbn
{

    class RVNode
    {
        private:

        protected:

        public:

            unsigned int var_size, n_outcomes, n_stats;
            bool is_discrete;
            std::string name, uid, id;

            // // allow for several histograms in a single node.
            // // useful for multivariate distributions
            // gsl_histogram ** histogram;

            // online statistics
            gsl_rstat_workspace ** stats;

            RVNode ();
            virtual ~RVNode ();
            RVNode (std::string, std::string, bool, unsigned int, unsigned int);

            void print_id ();
            void burn_stats();

            virtual double get_own_likelihood ();
            double get_own_loglikelihood ();
            virtual double get_children_loglikelihood ();
            double get_blanket_loglikelihood ();

            virtual void sample (gsl_rng *, bool=false);

            double mean(unsigned int = 0);
            double variance(unsigned int = 0);
            double chain_length();
    };
}

#endif

max_stars_repo_name: umbibio/gbnet

max_issues_repo_name: umbibio/gbnet

max_forks_repo_name: umbibio/gbnet


text:
#pragma once
#include <thrust/host_vector.h>
#include <sstream>
#include "IsingLattice2D.h"
#include "class_mc_io.h"
#include "Matrix.h"
#include "MemTimeTester.h"
extern "C" {
#include "random.h"
}
#include <gsl/gsl_sf_zeta.h>
#include <cmath>
#include "obs_calc_fast.cuh"

double real_trigamma_cmplxarg(double x, double y, int m, int l);

class GeneralLRW{
	//This is a more lightweight version of the long range wolff class, designed to be
	//more general - hopefully to admit the use of user-defined "test_spins" functions
	//so that it can be used for many different MC simulations.  It does not hold the
	//lattice in order to keep the lattice easily available for GPU computations
	std::vector<std::vector<double>> interactions;//interaction matrix
	std::vector<std::vector<double>> interaction_sum;//values from interaction sum integral
	std::vector<double> site_sums;//absolute sum of interactions at each site
	double mag;//current magnetization
	double h; //applied field
	std::vector<spin> buffer;//holds the indices of spins to check
	std::vector<std::vector<int>> cluster;//0 at indices that are not in cluster, 1 at indices that are
	int cluster_size;//size of the cluster during a given MC step
	int cluster_mag;//total magnetization of the cluster
	void set_spin_boson_model_wc_exp(class_mc_params);//version for finite cutoff frequency with exponential tail
	void set_spin_boson_model_wc_hard(class_mc_params);//version for finite cutoff frequency with sharp cutoff
	void set_spin_boson_model(class_mc_params);//create interaction matrix for the spin boson model with infinite cutoff frequency
	void set_interaction_sum();
	void set_site_sums();
	MemTimeTester timer;

	inline double kernel_integrand(double alpha, int Nt, double rbv, double taub, double u){
		//evaluate the integrand of the kernel of the spin-boson effective interactions
		return 2.0 * alpha / Nt / Nt * u * cos(rbv*u)*(exp(u*(-taub)) + exp(u*(taub - 1.0)))/(1 - exp(-u));
	}

	double simpson_3_8(double alpha, int Nt, double bomc, double rbv, double taub, int N_points){
		//perform an integration of the spin-boson kernel with a hard, finite cutoff bomc.  Evaluated at
		//site distance rbv = r/(beta*v), taub = tau/beta, with number of time slices Nt and number of
		//integration slices N_points (this method will multiply N_points by 3 and use simpson's 3/8 rule)
		double du = (bomc)/(3.0*N_points);
		double result = 3.0*du*(4.0*alpha / Nt / Nt + kernel_integrand(alpha, Nt, rbv, taub, bomc))/8.0;
		for (int i = 1; i < 3*N_points; ++i){
			if(3*(i/3) == i){
				result += 0.75*du*kernel_integrand(alpha, Nt, rbv, taub, i*du);
			}
			else{
				result += 9.0*du*kernel_integrand(alpha, Nt, rbv, taub, i*du)/8.0;
			}
		}
		return result;
	}

public:
	GeneralLRW(class_mc_params);

	void print_timers(){ timer.print_timers(); }

	void step(IsingLattice2D& lat);

    bool step_one_site(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref);

	bool step_one_site_prealloc(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref,
			cufftHandle *full_forward_plan, cufftHandle *full_backward_plan, cufftHandle *onesite_forward_plan, cufftHandle *onesite_backward_plan,
			cudaError cuda_status, cufftDoubleReal *full_state_rs, cufftDoubleComplex *full_state_ft, cufftDoubleReal *onesite_state_rs, cufftDoubleComplex *onesite_state_ft);

	bool step_one_site_prealloc_lowmem(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref,
			thrust::host_vector<double>& ,thrust::host_vector<double>& ,thrust::host_vector<double>& ,thrust::host_vector<double>& ,
			thrust::host_vector<double>& ,thrust::host_vector<double>& ,
			cufftHandle *full_forward_plan, cufftHandle *full_backward_plan, cufftHandle *onesite_forward_plan, cufftHandle *onesite_backward_plan,
			cudaError cuda_status, cufftDoubleReal *full_state_rs, cufftDoubleComplex *full_state_ft, cufftDoubleReal *onesite_state_rs, cufftDoubleComplex *onesite_state_ft);

	bool metropolis_step(IsingLattice2D& lat, double *action);

	bool fast_metropolis_step(IsingLattice2D& lat, double *action);

	void test_spins(spin, IsingLattice2D& lat);

    void test_spins_one_site(spin, IsingLattice2D& lat);

	void set_mag(IsingLattice2D& lat){
		mag = 0;
		for (int i = 0; i < lat.get_Lx(); ++i){
			for (int j = 0; j < lat.get_Ly(); ++j){
				mag += lat.get_spin(i, j);
			}
		}
		mag = ((double) mag) / lat.get_N();
	}

	double get_mag(){
		return mag;
	}

	double get_cluster_size() { return (double)cluster_size; }

    void get_thrust_interactions(thrust::host_vector<double>& result){
		if(result.size() != interactions.size()*interactions[0].size()){
			result.resize(interactions.size()*interactions[0].size());
		}
		for (int i = 0; i < interactions.size(); ++i){
			for (int j = 0; j < interactions[i].size(); ++j){
				result[i*interactions[i].size() + j] = interactions[i][j];
			}
		}
    }

	std::vector<double> get_interaction_vector(){
		std::vector<double> result(interactions.size()*interactions[0].size());
		int Ly = interactions[0].size();
		for (int x = 0; x < interactions.size(); ++x){
			for (int y = 0; y < Ly; ++y){
				result[x*Ly + y] = interactions[x][y];
			}
		}
		return result;
	}

	std::vector<std::vector<double>> get_interactions(){
		return interactions;
	}

    double calc_mqt0_abs(IsingLattice2D&, double);

    double calc_mqt0_2_abs(IsingLattice2D&, double);

    double calc_mqt0_4_abs(IsingLattice2D&, double);

    double calc_mqt0_gpu(thrust::host_vector<double>&, double, int);

    double calc_locw1(IsingLattice2D&, double);

	double calc_sx(IsingLattice2D&);

	double calc_space_kinks(IsingLattice2D& lat);

	double calc_sz_stagger(IsingLattice2D&);

	double calc_xmag(IsingLattice2D&);

	double calc_xmag2(IsingLattice2D&);

	double calc_xmag4(IsingLattice2D&);

    double calc_mag(IsingLattice2D&);

	double calc_loc(IsingLattice2D&);

	double calc_loc2(IsingLattice2D&);

	double calc_loc4(IsingLattice2D&);

	double calc_s1s2(IsingLattice2D&);

	double calc_action_slow(IsingLattice2D&);

	double calc_point_action_slow(IsingLattice2D&,int, int);

	double calc_point_action_fast(IsingLattice2D&, int, int);

	void print_site_sums(){
		std::cout << "Site sums:\n" << vec2str(site_sums) << "\n";
	}

	void print_interactions() {
		std::stringstream outstring;
		outstring << "Interactions:\n";
		for (int i = 0; i < interactions.size(); ++i){
			outstring << vec2str(interactions[i]) << "\n";
		}
		std::cout << outstring.str() << "\n";
	}

	void print_interaction_sum() {
		std::stringstream outstring;
		outstring << "Interaction sum:\n";
		for (int i = 0; i < interaction_sum.size(); ++i){
			outstring << vec2str(interaction_sum[i]) << "\n";
		}
		std::cout << outstring.str() << "\n";
	}

	std::string get_int_string() {
		std::stringstream ss;
		ss << "Interactions:\n" ;
		for (int i = 0; i < interactions.size(); ++i){
			ss << vec2str(interactions[i]) << "\n";
		}
		return ss.str();
	}

};