/**
 *
 * @file core_zgetrf_incpiv.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.8.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup dplasma_cores_complex64
 *
 *  CORE_zgetrf_incpiv computes an LU factorization of a general M-by-N tile A
 *  using partial pivoting with row interchanges.
 *
 *  The factorization has the form
 *
 *    A = P * L * U
 *
 *  where P is a permutation matrix, L is lower triangular with unit
 *  diagonal elements (lower trapezoidal if m > n), and U is upper
 *  triangular (upper trapezoidal if m < n).
 *
 *  This is the right-looking Level 2.5 BLAS version of the algorithm.
 *
 *******************************************************************************
 *
 * @param[in] M
 *          The number of rows of the tile A.  M >= 0.
 *
 * @param[in] N
 *         The number of columns of the tile A.  N >= 0.
 *
 * @param[in] IB
 *         The inner-blocking size.  IB >= 0.
 *
 * @param[in,out] A
 *         On entry, the M-by-N tile to be factored.
 *         On exit, the factors L and U from the factorization
 *         A = P*L*U; the unit diagonal elements of L are not stored.
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,M).
 *
 * @param[out] IPIV
 *         The pivot indices; for 1 <= i <= min(M,N), row i of the
 *         tile was interchanged with row IPIV(i).
 *
 * @param[out] INFO
 *         See returned value.
 *
 *******************************************************************************
 *
 * @return
 *         \retval PLASMA_SUCCESS successful exit
 *         \retval <0 if INFO = -k, the k-th argument had an illegal value
 *         \retval >0 if INFO = k, U(k,k) is exactly zero. The factorization
 *              has been completed, but the factor U is exactly
 *              singular, and division by zero will occur if it is used
 *              to solve a system of equations.
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zgetrf_incpiv = PCORE_zgetrf_incpiv
#define CORE_zgetrf_incpiv PCORE_zgetrf_incpiv
#define CORE_zgessm PCORE_zgessm
int  CORE_zgessm(int M, int N, int K, int IB,
                 int *IPIV,
                 const PLASMA_Complex64_t *L, int LDL,
                 PLASMA_Complex64_t *A, int LDA);
#endif
int CORE_zgetrf_incpiv(int M, int N, int IB,
                       PLASMA_Complex64_t *A, int LDA,
                       int *IPIV, int *INFO)
{
    int i, j, k, sb;
    int iinfo;

    /* Check input arguments */
    *INFO = 0;
    if (M < 0) {
        coreblas_error(1, "Illegal value of M");
        return -1;
    }
    if (N < 0) {
        coreblas_error(2, "Illegal value of N");
        return -2;
    }
    if (IB < 0) {
        coreblas_error(3, "Illegal value of IB");
        return -3;
    }
    if ((LDA < max(1,M)) && (M > 0)) {
        coreblas_error(5, "Illegal value of LDA");
        return -5;
    }

    /* Quick return */
    if ((M == 0) || (N == 0) || (IB == 0))
        return PLASMA_SUCCESS;

    k = min(M, N);

    for(i =0 ; i < k; i += IB) {
        sb = min(IB, k-i);
        /*
         * Factor diagonal and subdiagonal blocks and test for exact singularity.
         */
        iinfo = LAPACKE_zgetf2_work(LAPACK_COL_MAJOR, M-i, sb, &A[LDA*i+i], LDA, &IPIV[i]);
        /*
         * Adjust INFO and the pivot indices.
         */
        if((*INFO == 0) && (iinfo > 0))
            *INFO = iinfo + i;

        if (i+sb < N) {
            CORE_zgessm(
                M-i, N-(i+sb), sb, sb,
                &IPIV[i],
                &A[LDA*i+i], LDA,
                &A[LDA*(i+sb)+i], LDA);
        }

        for(j = i; j < i+sb; j++) {
            IPIV[j] = i + IPIV[j];
        }
    }
    return PLASMA_SUCCESS;
}


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

void
setupInitialSeed(
		 int id,
		 gsl_rng *rgen );

void createInitialSample(
			 double *x,
			 long N,
			 gsl_rng *rgen,
			 int state_dim );


#include <stdio.h>

//#ifndef DARWIN
//#include <malloc.h>
//#endif

#include <stddef.h>
#include <Python.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <string.h>
#include <math.h>

/*
void inplace(double* npyArray3D, int npyLength1D, int npylength2D,
              int npylength2D, double* invec, int n)
{
  int i;
  for (i=0; i<n; i++) {
    invec[i] = npyArray3D[i];
  }
}
*/

int print_matrix(FILE *f, const gsl_matrix *m)
{
  int status, n = 0;

  for (size_t i = 0; i < m->size1; i++) {
    for (size_t j = 0; j < m->size2; j++) {
      if ((status = fprintf(f,"%g ", gsl_matrix_get(m, i, j))) < 0)
        return -1;
      n += status;
    }

    if ((status = fprintf(f, "\n")) < 0)
      return -1;
    n += status;
  }
  return n;
}

int print_vector(FILE *f, const gsl_vector *m)
{
  int status, n = 0;

  for (size_t i = 0; i < m->size; i++) {
    if ((status = fprintf(f, "%g ", gsl_vector_get(m, i))) < 0)
      return -1;
    n += status;
  }
  return n;
}

void print_mat(double* mat, int dim1, int dim2)
{
  int i, j;
  printf("\n");
  for (i=0; i<dim1; i++) {
    for (j=0; j<dim2; j++) {
      printf("%6.2f,", mat[i*dim1 + j]);
    }
    printf("\n");
  }
}
void print_vec(double* vec, int dim1)
{
  int i;
  printf("\n");
  for (i=0; i<dim1; i++) {
    printf("%6.2f,", vec[i]);
  }
  printf("\n");
}

int sum(int* npyArray3D, int npyLength1D, int npyLength2D, int npyLength3D)
{
  int i, j, k;
  int sum = 0;

  for (i=0; i<npyLength1D; i++)
    for (j=0; j<npyLength2D; j++)
      for (k=0; k<npyLength3D; k++)
        sum += npyArray3D[i*npyLength3D*npyLength2D + k*npyLength2D + j];

  return sum;
}

double get_det(PyObject *A)
{
  int MAT_DIM = 6;
  int i, signum;
  double det;
  int nInts = PyList_Size(A);

  gsl_matrix *m = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_permutation *p;

  p = gsl_permutation_alloc(m->size1);

  for (i=0; i<nInts; i++)
  {
    PyObject *oo = PyList_GetItem(A, i);
    //printf("%6.2f\n",PyFloat_AS_DOUBLE(oo));
    gsl_matrix_set (m, i%MAT_DIM, i/MAT_DIM, PyFloat_AS_DOUBLE(oo));
  }

  gsl_linalg_LU_decomp(m, p, &signum);
  det = gsl_linalg_LU_det(m, signum);
  //printf("%6.2f\n",det);
  return det;
}

double get_overlap(double* gr_icov, int gr_dim1, int gr_dim2,
                   double* gr_mn, int gr_mn_dim, double gr_icov_det,
                   double* st_icov, int st_dim1, int st_dim2,
                   double* st_mn, int st_mn_dim, double st_icov_det)
{
  int MAT_DIM = gr_dim1;
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

//for (l=0; l<1; l++){ //DELETE

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);
      gsl_matrix_set (B, i, j, st_icov[i*MAT_DIM + j]);
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    gsl_vector_set (a, i, gr_mn[i]);
    gsl_vector_set (b, i, st_mn[i]);
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));
//} //DELETE THIS

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

double get_overlap2(PyObject *gr_icov, PyObject *gr_mn, double gr_icov_det,
                    PyObject *st_icov, PyObject *st_mn, double st_icov_det)
{
  int MAT_DIM = 6;
  int i, j, signum;
  double ApB_det, d_temp, result;
  PyObject *o1, *o2;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      o1 = PyList_GetItem(gr_icov, i*MAT_DIM + j);
      gsl_matrix_set (A, i, j, PyFloat_AS_DOUBLE(o1));
      o2 = PyList_GetItem(st_icov, i*MAT_DIM + j);
      gsl_matrix_set (B, i, j, PyFloat_AS_DOUBLE(o2));
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    o1 = PyList_GetItem(gr_mn, i);
    gsl_vector_set (a, i, PyFloat_AS_DOUBLE(o1));
    o2 = PyList_GetItem(st_mn, i);
    gsl_vector_set (b, i, PyFloat_AS_DOUBLE(o2));
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, A, a,
                 1.0, AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, B, b,
                 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

/* Main function which performs fastest so far:
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 */
void get_overlaps(double* gr_icov, int gr_dim1, int gr_dim2,
                  double* gr_mn, int gr_mn_dim,
                  double gr_icov_det,
                  double* st_icovs, int st_dim1, int st_dim2, int st_dim3,
                  double* st_mns, int st_mn_dim1, int st_mn_dim2,
                  double* st_icov_dets, int st_icov_dets_dim,
                  double* rangevec, int n)
{
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  // INITIALISE GROUP MATRICES
  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);

  for (i=0; i<MAT_DIM; i++)
    gsl_vector_set (a, i, gr_mn[i]);

  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRICES
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        gsl_matrix_set(B,i,j, st_icovs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j]);

    for (i=0; i<MAT_DIM; i++)
      gsl_vector_set (b, i, st_mns[star_count*MAT_DIM + i]);

    // FIND OVERLAP
    // Adding A and B together and storing in ApB
    gsl_matrix_set_zero(ApB);
    gsl_matrix_add(ApB, A);
    gsl_matrix_add(ApB, B);

    // Storing the result A*a + B*b in AapBb
    gsl_vector_set_zero(AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

    // Getting determinant of ApB
    gsl_linalg_LU_decomp(ApB, p, &signum);
    //ApB_det = gsl_linalg_LU_det(ApB, signum);
    ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

    // Solve for c
    gsl_linalg_LU_solve(ApB, p, AapBb, c);

    // Compute the overlap formula
    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(a, v_temp);       //v_temp holds a
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
    gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (a-c)

    result = 0.0;
    gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

    result += d_temp;

    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(b, v_temp);       //v_temp holds b
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
    gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (b-c)

    gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
    result += d_temp;

    result = -0.5 * result;
    result = exp(result);

    result *= sqrt((gr_icov_det * st_icov_dets[star_count]/ApB_det)
                  / pow(2*M_PI, MAT_DIM));

    // STORE IN 'rangevec'
    rangevec[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);
}

/* New main function, speed not yet tested
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 *
 *      look up how to find inverse
 *      look up how to access math.pi
 */
void new_get_lnoverlaps(
  double* gr_cov, int gr_dim1, int gr_dim2,
  double* gr_mn, int gr_mn_dim,
  double* st_covs, int st_dim1, int st_dim2, int st_dim3,
  double* st_mns, int st_mn_dim1, int st_mn_dim2,
  double* rangevec, int n
  )
{
  //printf("Inside new_get_lnoverlaps function\n");
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double d_temp, result, ln_det_BpA;
  FILE* fout = stdout;
  gsl_permutation *p1;

  gsl_matrix *BpA      = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)
  //gsl_matrix *BpAi     = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)^-1
  gsl_vector *bma      = gsl_vector_alloc(MAT_DIM); //will hold b - a
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);

  p1 = gsl_permutation_alloc(BpA->size1);

  //printf("Memory allocated\n");
  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRICES
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        //perform B+A as part of the initialisation
        gsl_matrix_set(
          BpA,i,j,
          st_covs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j] +
          gr_cov[i*MAT_DIM+j]
        );
//    printf("Printing BpA\n");
//    print_matrix(fout, BpA);

    for (i=0; i<MAT_DIM; i++) {
      gsl_vector_set(
        bma, i,
        st_mns[star_count*MAT_DIM + i] -
        gr_mn[i]
      );
    }
    //printf("Printing bma\n");
    //print_vec(bma->data, 6);
    //printf("Matrices initialised\n\n");

    result = 6*log(2*M_PI);
    // To Do! put 6ln(2 pi) in here ^^

    // Get inverse of BpA, this line is wrong, fix when have internet
    gsl_linalg_LU_decomp(BpA, p1, &signum);
    ln_det_BpA = log(fabs(gsl_linalg_LU_det(BpA, signum)));
    result += ln_det_BpA;

    //printf("ln(det(BpA)) added\n");
    //printf("%6.2f\n\n",ln_det_BpA);
    //printf("result so far:\n%6.2f\n",result);
    //// Solve for c
    //gsl_linalg_LU_solve(ApB, p, AapBb, c);

    // Don't use invert, use solve like example above ^^
    //gsl_linalg_LU_invert(BpA, p1, BpAi);
    //
    gsl_vector_set_zero(v_temp);
    gsl_linalg_LU_solve(BpA, p1, bma, v_temp); //v_temp holds (B+A)^-1 (b-a)
    gsl_blas_ddot(v_temp, bma, &d_temp); //d_temp holds (b-a)^T (B+A)-1 (b-a)
    //printf("Printing bma_BpAi_bma\n");
    //printf("%6.2f\n\n", d_temp);

    result += d_temp;
    //printf("result after bma_BpAi_bma:\n%6.2f\n",result);

    result *= -0.5;
    //printf("Everything calculated\n");
    //printf("Final result:\n%6.2f\n", result);
    //
    // STORE IN 'rangevec'
    rangevec[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(BpA);
  //gsl_matrix_free(BpAi);
  gsl_vector_free(bma);
  gsl_vector_free(v_temp);

  gsl_permutation_free(p1);

  //printf("At end of new_get_lnoverlaps function\n");
}


/****************************************
 * MIT License
 *
 * Copyright (c) 2020 Miguel Ramos Pernas
 ****************************************/

#ifdef USE_CPU
#include "Python.h"
#include "math.h"
#include <gsl/gsl_integration.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_vegas.h>

#if NDIM == 1
/// Function proxy to integrate using 1-dimensional methods
double function_proxy(double x, void *vparams) {

  double *params = (double *)vparams;

  return FUNCTION(x, FWD_PARAMS(params)); // Definitions in "evaluators.c"
}
#endif

/// Function proxy to integrate using Monte Carlo methods
double monte_function_proxy(double *data, size_t, void *vparams) {

  double *params = (double *)vparams;

  return FUNCTION(DATA(data),
                  FWD_PARAMS(params)); // Definitions in "evaluators.c"
}

extern "C" {

#if NDIM == 1
/// Exposed function to integrate using the QNG method
PyObject *integrate_qng(double lb, double ub, PyObject *config,
                        double *params) {

  gsl_function func = {&function_proxy, params};

  double atol, rtol;
  PyArg_ParseTuple(config, "dd", &atol, &rtol);

  double res, err;
  size_t neval;
  gsl_integration_qng(&func, lb, ub, atol, rtol, &res, &err, &neval);

  return Py_BuildValue("(ddi)", res, err, neval);
}

/// Exposed function to integrate using the QAG method
PyObject *integrate_qag(double lb, double ub, PyObject *config,
                        double *params) {

  gsl_function func = {&function_proxy, params};

  double atol, rtol;
  int limit, key, workspace_size;
  PyArg_ParseTuple(config, "ddiii", &atol, &rtol, &limit, &key,
                   &workspace_size);

  gsl_integration_workspace *w =
      gsl_integration_workspace_alloc(workspace_size);

  double res, aerr;
  gsl_integration_qag(&func, lb, ub, atol, rtol, limit, key, w, &res, &aerr);

  gsl_integration_workspace_free(w);

  return Py_BuildValue("(dd)", res, aerr);
}

/// Exposed function to integrate using the CQUAD method
PyObject *integrate_cquad(double lb, double ub, PyObject *config,
                          double *params) {

  gsl_function func = {&function_proxy, params};

  double atol, rtol;
  int workspace_size;
  PyArg_ParseTuple(config, "ddi", &atol, &rtol, &workspace_size);

  gsl_integration_cquad_workspace *w =
      gsl_integration_cquad_workspace_alloc(workspace_size);

  double res, err;
  size_t neval;
  gsl_integration_cquad(&func, lb, ub, atol, rtol, w, &res, &err, &neval);

  gsl_integration_cquad_workspace_free(w);

  return Py_BuildValue("(ddi)", res, err, neval);
}
#endif

/// Exposed function to integrate using plain MonteCarlo
PyObject *integrate_plain(double *lb, double *ub, PyObject *config,
                          double *params) {

  gsl_monte_function func = {&monte_function_proxy, NDIM, params};

  double res, err;

  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);

  // Define the state
  gsl_monte_plain_state *s = gsl_monte_plain_alloc(NDIM);

  int calls;
  PyArg_ParseTuple(config, "i", &calls);

  // Calculate the integral
  gsl_monte_plain_integrate(&func, lb, ub, NDIM, calls, r, s, &res, &err);

  gsl_monte_plain_free(s);

  gsl_rng_free(r);

  return Py_BuildValue("(dd)", res, err);
}

/// Exposed function to integrate using the MISER method
PyObject *integrate_miser(double *lb, double *ub, PyObject *config,
                          double *params) {

  gsl_monte_function func = {&monte_function_proxy, NDIM, params};

  double res, err;

  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);

  // Define the state
  gsl_monte_miser_state *s = gsl_monte_miser_alloc(NDIM);

  int calls;
  double estimate_frac;
  int min_calls;
  int min_calls_per_bisection;
  double alpha;
  double dither;

  PyArg_ParseTuple(config, "idiidd", &calls, &estimate_frac, &min_calls,
                   &min_calls_per_bisection, &alpha, &dither);

  gsl_monte_miser_params p;
  gsl_monte_miser_params_get(s, &p);
  p.estimate_frac = estimate_frac;
  p.min_calls = min_calls;
  p.min_calls_per_bisection = min_calls_per_bisection;
  p.alpha = alpha;
  p.dither = dither;
  gsl_monte_miser_params_set(s, &p);

  // Calculate the integral
  gsl_monte_miser_integrate(&func, lb, ub, NDIM, calls, r, s, &res, &err);

  gsl_monte_miser_free(s);

  gsl_rng_free(r);

  return Py_BuildValue("(dd)", res, err);
}

/// Exposed function to integrate using the VEGAS method
PyObject *integrate_vegas(double *lb, double *ub, PyObject *config,
                          double *params) {

  gsl_monte_function func = {&monte_function_proxy, NDIM, params};

  double res, err;

  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);

  // Define the state
  gsl_monte_vegas_state *s = gsl_monte_vegas_alloc(NDIM);

  int calls;
  double alpha;
  int iterations;
  int mode;

  PyArg_ParseTuple(config, "idii", &calls, &alpha, &iterations, &mode);

  gsl_monte_vegas_params p;
  gsl_monte_vegas_params_get(s, &p);
  p.alpha = alpha;
  p.iterations = iterations;
  p.mode = mode;
  gsl_monte_vegas_params_set(s, &p);

  // Calculate the integral
  gsl_monte_vegas_integrate(&func, lb, ub, NDIM, calls, r, s, &res, &err);

  do {
    gsl_monte_vegas_integrate(&func, lb, ub, NDIM, calls, r, s, &res, &err);

  } while (fabs(gsl_monte_vegas_chisq(s) - 1.0) > 0.5);

  gsl_monte_vegas_free(s);

  gsl_rng_free(r);

  return Py_BuildValue("(dd)", res, err);
}
}

#endif // USE_CPU


/* linalg/luc.c
 *
 * Copyright (C) 2001, 2007, 2009 Brian Gough
 * Copyright (C) 2019 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_linalg.h>

#include "recurse.h"

static int LU_decomp_L2 (gsl_matrix_complex * A, gsl_vector_uint * ipiv);
static int LU_decomp_L3 (gsl_matrix_complex * A, gsl_vector_uint * ipiv);
static int singular (const gsl_matrix_complex * LU);
static int apply_pivots(gsl_matrix_complex * A, const gsl_vector_uint * ipiv);

/* Factorise a general N x N complex matrix A into,
 *
 *   P A = L U
 *
 * where P is a permutation matrix, L is unit lower triangular and U
 * is upper triangular.
 *
 * L is stored in the strict lower triangular part of the input
 * matrix. The diagonal elements of L are unity and are not stored.
 *
 * U is stored in the diagonal and upper triangular part of the
 * input matrix.
 *
 * P is stored in the permutation p. Column j of P is column k of the
 * identity matrix, where k = permutation->data[j]
 *
 * signum gives the sign of the permutation, (-1)^n, where n is the
 * number of interchanges in the permutation.
 *
 * See Golub & Van Loan, Matrix Computations, Algorithm 3.4.1 (Gauss
 * Elimination with Partial Pivoting).
 */

int
gsl_linalg_complex_LU_decomp (gsl_matrix_complex * A, gsl_permutation * p, int *signum)
{
  const size_t M = A->size1;

  if (p->size != M)
    {
      GSL_ERROR ("permutation length must match matrix size1", GSL_EBADLEN);
    }
  else
    {
      int status;
      const size_t N = A->size2;
      const size_t minMN = GSL_MIN(M, N);
      gsl_vector_uint * ipiv = gsl_vector_uint_alloc(minMN);
      gsl_matrix_complex_view AL = gsl_matrix_complex_submatrix(A, 0, 0, M, minMN);
      size_t i;

      status = LU_decomp_L3 (&AL.matrix, ipiv);

      /* process remaining right matrix */
      if (M < N)
        {
          gsl_matrix_complex_view AR = gsl_matrix_complex_submatrix(A, 0, M, M, N - M);

          /* apply pivots to AR */
          apply_pivots(&AR.matrix, ipiv);

          /* AR = AL^{-1} AR */
          gsl_blas_ztrsm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, GSL_COMPLEX_ONE, &AL.matrix, &AR.matrix);
        }

      /* convert ipiv array to permutation */

      gsl_permutation_init(p);
      *signum = 1;

      for (i = 0; i < minMN; ++i)
        {
          unsigned int pivi = gsl_vector_uint_get(ipiv, i);

          if (p->data[pivi] != p->data[i])
            {
              size_t tmp = p->data[pivi];
              p->data[pivi] = p->data[i];
              p->data[i] = tmp;
              *signum = -(*signum);
            }
        }

      gsl_vector_uint_free(ipiv);

      return status;
    }
}

/*
LU_decomp_L2
  LU decomposition with partial pivoting using Level 2 BLAS

Inputs: A    - on input, matrix to be factored; on output, L and U factors
        ipiv - (output) array containing row swaps

Notes:
1) Based on LAPACK ZGETF2
*/

static int
LU_decomp_L2 (gsl_matrix_complex * A, gsl_vector_uint * ipiv)
{
  const size_t M = A->size1;
  const size_t N = A->size2;
  const size_t minMN = GSL_MIN(M, N);

  if (ipiv->size != minMN)
    {
      GSL_ERROR ("ipiv length must equal MIN(M,N)", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;

      for (j = 0; j < minMN; ++j)
        {
          /* find maximum in the j-th column */
          gsl_vector_complex_view v = gsl_matrix_complex_subcolumn(A, j, j, M - j);
          size_t j_pivot = j + gsl_blas_izamax(&v.vector);
          gsl_vector_complex_view v1, v2;

          gsl_vector_uint_set(ipiv, j, j_pivot);

          if (j_pivot != j)
            {
              /* swap rows j and j_pivot */
              v1 = gsl_matrix_complex_row(A, j);
              v2 = gsl_matrix_complex_row(A, j_pivot);
              gsl_blas_zswap(&v1.vector, &v2.vector);
            }

          if (j < M - 1)
            {
              gsl_complex Ajj = gsl_matrix_complex_get(A, j, j);
              gsl_complex Ajjinv = gsl_complex_inverse(Ajj);

              if (gsl_complex_abs(Ajj) >= GSL_DBL_MIN)
                {
                  v1 = gsl_matrix_complex_subcolumn(A, j, j + 1, M - j - 1);
                  gsl_blas_zscal(Ajjinv, &v1.vector);
                }
              else
                {
                  for (i = 1; i < M - j; ++i)
                    {
                      gsl_complex * ptr = gsl_matrix_complex_ptr(A, j + i, j);
                      *ptr = gsl_complex_mul(*ptr, Ajjinv);
                    }
                }
            }

          if (j < minMN - 1)
            {
              gsl_matrix_complex_view A22 = gsl_matrix_complex_submatrix(A, j + 1, j + 1, M - j - 1, N - j - 1);
              v1 = gsl_matrix_complex_subcolumn(A, j, j + 1, M - j - 1);
              v2 = gsl_matrix_complex_subrow(A, j, j + 1, N - j - 1);

              gsl_blas_zgeru(GSL_COMPLEX_NEGONE, &v1.vector, &v2.vector, &A22.matrix);
            }
        }

      return GSL_SUCCESS;
    }
}

/*
LU_decomp_L3
  LU decomposition with partial pivoting using Level 3 BLAS

Inputs: A    - on input, matrix to be factored; on output, L and U factors
        ipiv - (output) array containing row swaps

Notes:
1) Based on ReLAPACK DGETRF
*/

static int
LU_decomp_L3 (gsl_matrix_complex * A, gsl_vector_uint * ipiv)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M < N)
    {
      GSL_ERROR ("matrix must have M >= N", GSL_EBADLEN);
    }
  else if (ipiv->size != GSL_MIN(M, N))
    {
      GSL_ERROR ("ipiv length must equal MIN(M,N)", GSL_EBADLEN);
    }
  else if (N <= CROSSOVER_LU)
    {
      /* use Level 2 algorithm */
      return LU_decomp_L2(A, ipiv);
    }
  else
    {
      /*
       * partition matrix:
       *
       *       N1  N2
       * N1  [ A11 A12 ]
       * M2  [ A21 A22 ]
       *
       * and
       *      N1  N2
       * M  [ AL  AR  ]
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      const size_t M2 = M - N1;
      gsl_matrix_complex_view A11 = gsl_matrix_complex_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_complex_view A12 = gsl_matrix_complex_submatrix(A, 0, N1, N1, N2);
      gsl_matrix_complex_view A21 = gsl_matrix_complex_submatrix(A, N1, 0, M2, N1);
      gsl_matrix_complex_view A22 = gsl_matrix_complex_submatrix(A, N1, N1, M2, N2);

      gsl_matrix_complex_view AL = gsl_matrix_complex_submatrix(A, 0, 0, M, N1);
      gsl_matrix_complex_view AR = gsl_matrix_complex_submatrix(A, 0, N1, M, N2);

      /*
       * partition ipiv = [ ipiv1 ] N1
       *                  [ ipiv2 ] N2
       */
      gsl_vector_uint_view ipiv1 = gsl_vector_uint_subvector(ipiv, 0, N1);
      gsl_vector_uint_view ipiv2 = gsl_vector_uint_subvector(ipiv, N1, N2);

      size_t i;

      /* recursion on (AL, ipiv1) */
      status = LU_decomp_L3(&AL.matrix, &ipiv1.vector);
      if (status)
        return status;

      /* apply ipiv1 to AR */
      apply_pivots(&AR.matrix, &ipiv1.vector);

      /* A12 = A11^{-1} A12 */
      gsl_blas_ztrsm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, GSL_COMPLEX_ONE, &A11.matrix, &A12.matrix);

      /* A22 = A22 - A21 * A12 */
      gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, GSL_COMPLEX_NEGONE, &A21.matrix, &A12.matrix, GSL_COMPLEX_ONE, &A22.matrix);

      /* recursion on (A22, ipiv2) */
      status = LU_decomp_L3(&A22.matrix, &ipiv2.vector);
      if (status)
        return status;

      /* apply pivots to A21 */
      apply_pivots(&A21.matrix, &ipiv2.vector);

      /* shift pivots */
      for (i = 0; i < N2; ++i)
        {
          unsigned int * ptr = gsl_vector_uint_ptr(&ipiv2.vector, i);
          *ptr += N1;
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_solve (const gsl_matrix_complex * LU, const gsl_permutation * p, const gsl_vector_complex * b, gsl_vector_complex * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (singular (LU))
    {
      GSL_ERROR ("matrix is singular", GSL_EDOM);
    }
  else
    {
      int status;

      /* copy x <- b */
      gsl_vector_complex_memcpy (x, b);

      /* solve for x */
      status = gsl_linalg_complex_LU_svx (LU, p, x);

      return status;
    }
}

int
gsl_linalg_complex_LU_svx (const gsl_matrix_complex * LU, const gsl_permutation * p, gsl_vector_complex * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution/rhs size", GSL_EBADLEN);
    }
  else if (singular (LU))
    {
      GSL_ERROR ("matrix is singular", GSL_EDOM);
    }
  else
    {
      /* apply permutation to RHS */
      gsl_permute_vector_complex (p, x);

      /* solve for c using forward-substitution, L c = P b */
      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasUnit, LU, x);

      /* perform back-substitution, U x = c */
      gsl_blas_ztrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LU, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_refine (const gsl_matrix_complex * A, const gsl_matrix_complex * LU, const gsl_permutation * p, const gsl_vector_complex * b, gsl_vector_complex * x, gsl_vector_complex * work)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("matrix a must be square", GSL_ENOTSQR);
    }
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (A->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be decomposition of a", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (LU->size1 != work->size)
    {
      GSL_ERROR ("matrix size must match workspace size", GSL_EBADLEN);
    }
  else if (singular (LU))
    {
      GSL_ERROR ("matrix is singular", GSL_EDOM);
    }
  else
    {
      int status;

      /* Compute residual = (A * x  - b) */

      gsl_vector_complex_memcpy (work, b);

      {
        gsl_complex one = GSL_COMPLEX_ONE;
        gsl_complex negone = GSL_COMPLEX_NEGONE;
        gsl_blas_zgemv (CblasNoTrans, one, A, x, negone, work);
      }

      /* Find correction, delta = - (A^-1) * residual, and apply it */

      status = gsl_linalg_complex_LU_svx (LU, p, work);

      {
        gsl_complex negone= GSL_COMPLEX_NEGONE;
        gsl_blas_zaxpy (negone, work, x);
      }

      return status;
    }
}

int
gsl_linalg_complex_LU_invert (const gsl_matrix_complex * LU, const gsl_permutation * p, gsl_matrix_complex * inverse)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (inverse->size1 != LU->size1 || inverse->size2 != LU->size2)
    {
      GSL_ERROR ("inverse matrix must match LU matrix dimensions", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_complex_memcpy(inverse, LU);
      return gsl_linalg_complex_LU_invx(inverse, p);
    }
}

int
gsl_linalg_complex_LU_invx (gsl_matrix_complex * LU, const gsl_permutation * p)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (singular (LU))
    {
      GSL_ERROR ("matrix is singular", GSL_EDOM);
    }
  else
    {
      int status;
      const size_t N = LU->size1;
      size_t i;

      /* compute U^{-1} */
      status = gsl_linalg_complex_tri_invert(CblasUpper, CblasNonUnit, LU);
      if (status)
        return status;

      /* compute L^{-1} */
      status = gsl_linalg_complex_tri_invert(CblasLower, CblasUnit, LU);
      if (status)
        return status;

      /* compute U^{-1} L^{-1} */
      status = gsl_linalg_complex_tri_UL(LU);
      if (status)
        return status;

      /* apply permutation to columns of A^{-1} */
      for (i = 0; i < N; ++i)
        {
          gsl_vector_complex_view v = gsl_matrix_complex_row(LU, i);
          gsl_permute_vector_complex_inverse(p, &v.vector);
        }

      return GSL_SUCCESS;
    }
}

gsl_complex
gsl_linalg_complex_LU_det (gsl_matrix_complex * LU, int signum)
{
  size_t i, n = LU->size1;

  gsl_complex det = gsl_complex_rect((double) signum, 0.0);

  for (i = 0; i < n; i++)
    {
      gsl_complex zi = gsl_matrix_complex_get (LU, i, i);
      det = gsl_complex_mul (det, zi);
    }

  return det;
}

double
gsl_linalg_complex_LU_lndet (gsl_matrix_complex * LU)
{
  size_t i, n = LU->size1;

  double lndet = 0.0;

  for (i = 0; i < n; i++)
    {
      gsl_complex z = gsl_matrix_complex_get (LU, i, i);
      lndet += log (gsl_complex_abs (z));
    }

  return lndet;
}

gsl_complex
gsl_linalg_complex_LU_sgndet (gsl_matrix_complex * LU, int signum)
{
  size_t i, n = LU->size1;

  gsl_complex phase = gsl_complex_rect((double) signum, 0.0);

  for (i = 0; i < n; i++)
    {
      gsl_complex z = gsl_matrix_complex_get (LU, i, i);

      double r = gsl_complex_abs(z);

      if (r == 0)
        {
          phase = gsl_complex_rect(0.0, 0.0);
          break;
        }
      else
        {
          z = gsl_complex_div_real(z, r);
          phase = gsl_complex_mul(phase, z);
        }
    }

  return phase;
}

static int
singular (const gsl_matrix_complex * LU)
{
  size_t i, n = LU->size1;

  for (i = 0; i < n; i++)
    {
      gsl_complex u = gsl_matrix_complex_get (LU, i, i);
      if (GSL_REAL(u) == 0 && GSL_IMAG(u) == 0) return 1;
    }

 return 0;
}

static int
apply_pivots(gsl_matrix_complex * A, const gsl_vector_uint * ipiv)
{
  if (0)
    {
    }
  else
    {
      size_t i;

      for (i = 0; i < ipiv->size; ++i)
        {
          size_t pi = gsl_vector_uint_get(ipiv, i);

          if (i != pi)
            {
              /* swap rows i and pi */
              gsl_vector_complex_view v1 = gsl_matrix_complex_row(A, i);
              gsl_vector_complex_view v2 = gsl_matrix_complex_row(A, pi);
              gsl_blas_zswap(&v1.vector, &v2.vector);
            }
        }

      return GSL_SUCCESS;
    }
}