#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>

/*
 *  A simple helper function to display the contents of a 2-D gsl matrix
 */
int print_matrix(FILE *f, const gsl_matrix *m)
{
  int status, n = 0;
  size_t i, j;

  for (i = 0; i < m->size1; i++) {
    for (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;
}

/*
 *  A simple helper function to display the contennts of a 1-D gsl vector
 */
int print_vector(FILE *f, const gsl_vector *m)
{
  int status, n = 0;
  size_t i;

  for (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");
}

/* Function: get_lnoverlaps
 * ------------------------
 *   Calculates the log overlap (convolution) with a set of 6D Gaussians with
 *   a single 6D Gaussian, each in turn.
 *
 *   In Chronostar, this is used to see how well the kinematic properties
 *   of stars overlap with a proposed Gaussian distribution.
 *
 * Paramaters
 *  name      type            description
 * ----------
 *  gr_cov        (6*6 npArray)   group's covariance matrix
 *  gr_mn         (1*6 npArray)   group's central estimate (mean)
 *  st_covs       (n*6*6 npArray) array of each star's cov matrix
 *  st_mns:       (n*6 npArray)   array of each star's central estimate
 *  lnols_output: (n npArray)     used to store and return calculated overlaps
 *  nstars:       (int)           number of stars, used for array dimensions
 *
 * Returns
 * -------
 *  (nstars) array of calculated log overlaps of every star with component
 *      thanks to Swig magic the result is stored in `lnols_output` which is
 *      returned as output to the python call as a numpy array
 *
 * Notes
 * -----
 *   For each star calculates:
 *     log(1/sqrt( (2*PI)^6*det(C) ) * exp( -0.5*(b-a)^T*(C)^-1*(b-a) )
 *   where
 *     C = st_cov + gr_cov
 *   and
 *     a = gr_mn, b = st_mn
 *   Expanding and simplifying this becomes:
 *     -0.5[ 6*ln(2*PI) + ln(|C|) + (b-a)^T*(C^-1)*(b-a) ]
 *
 *   Stark improvement on previous implementations. Doesn't require input as
 *   inverse covariance matrices. Never performs a matrix inversion.
 */
void 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* lnols_output, int 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;
  gsl_permutation *p1;

  gsl_matrix *BpA      = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //will hold (B+A)
  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);

  // Go through each star, calculating and storing overlap
  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRIX
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        //performing st_cov+gr_cov 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]
        );

    // INITIALISE CENTRAL ESTIMATES
    // performing st_mn - gr_mn as part of the initialisation
    for (i=0; i<MAT_DIM; i++) {
      gsl_vector_set(
        bma, i,
        st_mns[star_count*MAT_DIM + i] - gr_mn[i]
      );
    }

    // CALCULATE OVERLAPS
    // Performed in 4 stages
    // Calc and sum up the inner terms:
    // 1) 6 ln(2pi)
    // 2) ln(|C|)
    // 3) (b-a)^T(C^-1)(b-a)
    // Then apply -0.5 coefficient

    // 1) Calc 6 ln(2pi)
    result = 6*log(2*M_PI);

    // 2) Get log determiant of C
    gsl_linalg_LU_decomp(BpA, p1, &signum);
    ln_det_BpA = log(fabs(gsl_linalg_LU_det(BpA, signum)));
    result += ln_det_BpA;

    // 3) Calc (b-a)^T(C^-1)(b-a)
    gsl_vector_set_zero(v_temp);
    gsl_linalg_LU_solve(BpA, p1, bma, v_temp); /* v_temp holds (B+A)^-1 (b-a) *
                                                * utilises `p1` as calculated *
                                                * above                       */
    gsl_blas_ddot(v_temp, bma, &d_temp); //d_temp holds (b-a)^T (B+A)-1 (b-a)
    result += d_temp;

    // 4) Apply coefficient
    result *= -0.5;

    // STORE RESULT 'lnols_output'
    lnols_output[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(BpA);
  gsl_vector_free(bma);
  gsl_vector_free(v_temp);
  gsl_permutation_free(p1);
}

/* NOTE:
 * Everything below this line is left simply for correctness comparisons
 */
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
 * mostly here for debugging reasons
 * --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
 */
double new_get_lnoverlap(
  double* gr_cov, int gr_dim1, int gr_dim2,
  double* gr_mn, int gr_mn_dim,
  double* st_cov, int st_dim1, int st_dim2,
  double* st_mn, int st_mn_dim
  )
{
  //printf("Inside new_get_lnoverlaps function\n");
  printf("-----------------------------------------------------------\n");
  printf("new_get_lnoverlap(): In c implemntation of Tim's derivation\n");
  printf("-----------------------------------------------------------\n");
  //printf("Inputs are:\n");
  //printf("  A\n");
  //print_mat(gr_cov, gr_dim1, gr_dim2);
  //printf("  a\n");
  //print_vec(gr_mn, gr_mn_dim);
  //printf("  B\n");
  //print_mat(st_cov, st_dim1, st_dim2);
  //printf("  b\n");
  //print_vec(st_mn, st_mn_dim);

  // ALLOCATE MEMORY
  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");
  // 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_cov[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_mn[i] -
      gr_mn[i]
    );
  }
  printf("Printing bma\n");
  print_vec(bma->data, 6);
  printf("Matrices initialised\n\n");

  result = 6*log(2*M_PI);
  printf("Added 6log(2pi):\n%6.2f\n", result);

  // 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)));
  printf("Log of det(ApB): %6.2f\n", ln_det_BpA);
  result += ln_det_BpA;

  printf("result so far:\n%6.2f\n",result);
  //
  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.8f\n", 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);

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


/* interpolation/interp_poly.c
 *
 * Copyright (C) 2001 DAN, HO-JIN
 *
 * 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 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 program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_interp.h>

typedef struct
{
  double *d;
  double *coeff;
  double *work;
}
polynomial_state_t;

static void *
polynomial_alloc (size_t size)
{
  polynomial_state_t *state =
    (polynomial_state_t *) malloc (sizeof (polynomial_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for polynomial state",
                      GSL_ENOMEM);
    }

  state->d = (double *) malloc (sizeof (double) * size);

  if (state->d == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->coeff = (double *) malloc (sizeof (double) * size);

  if (state->coeff == 0)
    {
      free (state->d);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->work = (double *) malloc (sizeof (double) * size);

  if (state->work == 0)
    {
      free (state->coeff);
      free (state->d);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  return state;
}

static int
polynomial_init (void *vstate,
                 const double xa[], const double ya[], size_t size)
{
  polynomial_state_t *state = (polynomial_state_t *) vstate;

  int status = gsl_poly_dd_init (state->d, xa, ya, size);

  return status;
}

static int
polynomial_eval (const void *vstate,
                 const double xa[], const double ya[], size_t size, double x,
                 gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  *y = gsl_poly_dd_eval (state->d, xa, size, x);

  return GSL_SUCCESS;
}

static int
polynomial_deriv (const void *vstate,
                  const double xa[], const double ya[], size_t size, double x,
                  gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  gsl_poly_dd_taylor (state->coeff, x, state->d, xa, size, state->work);

  *y = state->coeff[1];

  return GSL_SUCCESS;
}

static int
polynomial_deriv2 (const void *vstate,
                   const double xa[], const double ya[], size_t size,
                   double x, gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  gsl_poly_dd_taylor (state->coeff, x, state->d, xa, size, state->work);

  *y = 2.0 * state->coeff[2];

  return GSL_SUCCESS;
}

static int
polynomial_integ (const void *vstate, const double xa[], const double ya[],
                  size_t size, gsl_interp_accel * acc, double a, double b,
                  double *result)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;
  size_t i;
  double sum;

  gsl_poly_dd_taylor (state->coeff, 0.0, state->d, xa, size, state->work);

  sum = state->coeff[0] * (b - a);

  for (i = 1; i < size; i++)
    {
      sum += state->coeff[i] * (pow (b, i + 1) - pow (a, i + 1)) / (i + 1.0);
    }

  *result = sum;

  return GSL_SUCCESS;
}

static void
polynomial_free (void *vstate)
{
  polynomial_state_t *state = (polynomial_state_t *) vstate;
  free (state->d);
  free (state->coeff);
  free (state->work);
  free (state);
}

static const gsl_interp_type polynomial_type = {
  "polynomial",
  3,
  &polynomial_alloc,
  &polynomial_init,
  &polynomial_eval,
  &polynomial_deriv,
  &polynomial_deriv2,
  &polynomial_integ,
  &polynomial_free,
};

const gsl_interp_type *gsl_interp_polynomial = &polynomial_type;


#ifdef USE_MKL
#include <mkl_lapacke.h>
#else
#include <lapacke.h>
#endif

// contains all calls to C-interface LAPACK functions (LAPACKE)
// https://www.netlib.org/lapack/lapacke.html

// returns the solution, x, to a real system of linear equations
//   A * x = b,
// solution is returned in b, i.e. b --> x, for return value 0
int specex_posv(int n, const double *A, const double *b){
  return LAPACKE_dposv(LAPACK_COL_MAJOR, 'L', n, 1, A, n, b, n);
}

// invert matrix A in place; A := inv(A)
// http://www.netlib.org/lapack/explore-html/d8/d63/dpotri_8f_source.html
int specex_potri(int n, const double *A){
  return LAPACKE_dpotri(LAPACK_COL_MAJOR, 'L', n, A, n);
}


/*
 * kjg_fpca.c
 *
 *  Created on: Apr 28, 2014
 *      Author: Kevin
 */

#include <stdio.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_blas.h>

#include "kjg_fpca.h"
#include "kjg_gsl.h"

#include "admutils.h"
#include "gval.h"

size_t KJG_FPCA_ROWS = 256;

void
kjg_fpca (size_t K, size_t L, size_t I, double *eval, double *evec)
{
  if (K >= L)
    exit (1);
  if (I == 0)
    exit (1);

  size_t m = get_ncols ();
  size_t n = get_nrows ();

  // PART A - compute Q such that X ~ Q * (Q^T) * X
  gsl_matrix *G1 = gsl_matrix_alloc (n, L);
  gsl_matrix *G2 = gsl_matrix_alloc (n, L);
  gsl_matrix *Q = gsl_matrix_alloc (m, (I + 1) * L);
  gsl_matrix *Gswap;

  gsl_rng *r = kjg_gsl_rng_init ();
  kjg_gsl_ran_ugaussian_matrix (r, G1);
  gsl_rng_free (r);

  size_t i;
  for (i = 0; i < I; i++)
    {
      gsl_matrix_view Qi = gsl_matrix_submatrix (Q, 0, i * L, m, L);

      // do the multiplication
      kjg_fpca_XTXA (G1, &Qi.matrix, G2);

      // scale to prevent G2 from blowing up
      gsl_matrix_scale (G2, 1.0 / m);

      Gswap = G2;
      G2 = G1;
      G1 = Gswap;
    }

  gsl_matrix_view Qi = gsl_matrix_submatrix (Q, 0, I * L, m, L);
  kjg_fpca_XA (G1, &Qi.matrix);

  {
    gsl_matrix *V = gsl_matrix_alloc (Q->size2, Q->size2);
    gsl_vector *S = gsl_vector_alloc (Q->size2);

    kjg_gsl_SVD (Q, V, S);

    gsl_matrix_free (V);
    gsl_vector_free (S);
  }

  // kjg_gsl_matrix_QR(Q); // QR decomposition is less accurate than SVD

  gsl_matrix_free (G1);
  gsl_matrix_free (G2);

  // PART B - compute B matrix, take SVD and return
  gsl_matrix *B = gsl_matrix_alloc (n, (I + 1) * L);
  kjg_fpca_XTB (Q, B);

  gsl_matrix *Utilda = gsl_matrix_alloc ((I + 1) * L, (I + 1) * L);
  gsl_vector *Stilda = gsl_vector_alloc ((I + 1) * L);

  kjg_gsl_SVD (B, Utilda, Stilda);

  gsl_matrix_view Vk = gsl_matrix_submatrix (B, 0, 0, n, K);
  gsl_matrix_view evec_view = gsl_matrix_view_array (evec, n, K);
  gsl_matrix_memcpy (&evec_view.matrix, &Vk.matrix);

  gsl_vector_view Sk = gsl_vector_subvector (Stilda, 0, K);
  gsl_vector_view eval_view = gsl_vector_view_array (eval, K);
  gsl_vector_mul (&Sk.vector, &Sk.vector);
  gsl_vector_scale (&Sk.vector, 1.0 / m);
  gsl_vector_memcpy (&eval_view.vector, &Sk.vector);

  gsl_matrix_free (Q);
  gsl_matrix_free (B);
  gsl_matrix_free (Utilda);
  gsl_vector_free (Stilda);
}

void
kjg_fpca_XTXA (const gsl_matrix * A1, gsl_matrix * B, gsl_matrix * A2)
{
  size_t m = get_ncols ();
  size_t n = get_nrows ();

  size_t i, r;			// row index
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);	// normalized

  gsl_matrix_view Bi, Xi;

  gsl_matrix_set_zero (A2);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xi = gsl_matrix_view_array (Y, r, n);
      Bi = gsl_matrix_submatrix (B, i, 0, r, B->size2);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1, &Xi.matrix, A1, 0,
		      &Bi.matrix);
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1, &Xi.matrix, &Bi.matrix,
		      1, A2);
    }

  free (Y);
}

void
kjg_fpca_XA (const gsl_matrix * A, gsl_matrix * B)
{
  size_t n = get_nrows ();
  size_t m = get_ncols ();

  size_t i, r;
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);

  gsl_matrix_view Hmat, Xmat;

  gsl_matrix_set_zero (B);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xmat = gsl_matrix_view_array (Y, r, n);
      Hmat = gsl_matrix_submatrix (B, i, 0, r, B->size2);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1, &Xmat.matrix, A, 0,
		      &Hmat.matrix);
    }

  free (Y);
}

void
kjg_fpca_XTB (const gsl_matrix * B, gsl_matrix * A)
{
  size_t n = get_nrows ();
  size_t m = get_ncols ();

  size_t i, r;
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);
  gsl_matrix_view Xmat;

  gsl_matrix_set_zero (A);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xmat = gsl_matrix_view_array (Y, r, n);
      gsl_matrix_const_view Hmat = gsl_matrix_const_submatrix (B, i, 0, r,
							       B->size2);
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1, &Xmat.matrix, &Hmat.matrix,
		      1, A);
    }

  free (Y);
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
 *
 */
/*
 *
 *
 *       Filename:  total_current.c
 *
 *    Description:  Total current ( AC + DC )
 *
 *        Version:  1.0
 *        Created:  26/06/2014 14:59:12
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include "funcs.h"
#include <gsl/gsl_vector.h>

/*
 *      FUNCTION
 *         Name:  tot_current
 *  Description:
 *
 */
double tot_current ( const gsl_vector* rho, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double Omega = pars->Omega ;
	double o_1   = pars->omega_1 ;
	double D     = sqrt(o_1*o_1 - Omega*Omega) ;
	double curr = -((VECTOR(rho, 2)*D + VECTOR(rho, 3)*Omega)/o_1) ;
	return (curr) ;
}		/* -----  end of function tot_current  ----- */


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

#include <math.h>
#include <options/options.h>

#include "../csm/csm_all.h"

struct {
	double interval;
	int seed;
}p ;

LDP ld_resample(LDP ld);

gsl_rng * rng;

int main(int argc, const char ** argv) {
	sm_set_program_name(argv[0]);

	struct option* ops = options_allocate(3);
	options_double(ops, "interval", &p.interval, sqrt(2.0), " 1 = no resampling");

	if(!options_parse_args(ops, argc, argv)) {
		options_print_help(ops, stderr);
		return -1;
	}

	int count = -1;
	LDP ld;
	while( (ld = ld_read_smart(stdin))) {
		count++;

		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}

/*		if(count & 1) {*/
			LDP ld2 = ld_resample(ld);
			ld_write_as_json(ld2, stdout);
			ld_free(ld2);
			ld_free(ld);
/*		} else {
			ld_write_as_json(ld, stdout);
			ld_free(ld);
		}*/

		count++;
	}

	return 0;
}

LDP ld_resample(LDP ld) {
	/* FIXME magic number */
	int n = (int) (floor(ld->nrays / p.interval));

	LDP ld2 = ld_alloc_new(n);
	int k;
	for(k=0;k<n;k++) {
		double index = k * p.interval;
		int i = (int) floor(index);
		int j = i + 1;
		double a = 1 - (index - i);

		if(  (j>= ld->nrays) || !ld->valid[i] || !ld->valid[j]) {
			ld2->valid[k] = 0;
			ld2->readings[k] = NAN;
			ld2->alpha_valid[k] = 0;
			ld2->alpha[k] = NAN;
			ld2->theta[k] = ld->theta[i];
		} else {
			ld2->theta[k] = a * ld->theta[i] + (1-a) * ld->theta[j];

			if(is_nan(ld2->theta[k])) {
				sm_debug("Hey, k=%d theta[%d]=%f theta[%d]=%f\n",
					k,i,ld->theta[i],j,ld->theta[j]);
			}

			ld2->readings[k] = a * ld->readings[i] + (1-a) * ld->readings[j];
			ld2->valid[k] = 1;
		}

/*		sm_debug("k=%d index=%f i=%d a=%f valid %d reading %f\n", k,index,i,a,ld2->valid[k],ld2->readings[k]);*/
	}

	ld2->min_theta = ld2->theta[0];
	ld2->max_theta = ld2->theta[n-1];
	ld2->tv = ld->tv;

	copy_d(ld->odometry, 3, ld2->odometry);
	copy_d(ld->estimate, 3, ld2->estimate);

	return ld2;
}


//im_clam.c --
// A. Kern 3/20/15
//
// Composite Likelihood estimation of canonical IM models via AFS

//going to use NLopt for optimization

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <nlopt.h>
#include "AFS.h"
#include "adkGSL.h"
#include "cs.h"
#include "time.h"
#include <unistd.h>
#include "adkCSparse.h"
#include "AFS_ctmc.h"
#include <slepcmfn.h>
#include "AFS_ctmc_petsc.h"
#include "im_clam.h"

void import2DSFSData(const char *fileName, gsl_matrix *obsData);

PetscInt n1, n2;

afsStateSpace *stateSpace, *reducedStateSpace;

clam_lik_params *currentParams, *nextParams;

gsl_matrix *transMat;
double *res;

double lowerBounds[5] = {0.01,0.01,0.0,0.0,0.001};
double upperBounds[5] = {10.0,10.0,20.0,20.0,10.0};
PetscBool vbse = PETSC_FALSE;

static char help[] = "im_clam\n\
	Example: mpiexec -n <np> ./im_clam -s <stateSpace file> -m <mats file> -d <data file> \n\n\toptions:\n\
	\t-exp expected value mode (requires -x flag too)\n\
	\t-GIM uncertainty estimation mode (requires -x flag too)\n\
	\t-mo multiple optimizations from different start points\n\
	\t-global multi-level optimization (MLSL algo.)\n\
	\t-x <theta_2, theta_A, mig12, mig21, t_div> parameter starting values\n\
	\t-obs (prints out observed AFS as well as that expected from MLE params)\n\
	\t-u mutation rate per base pair per generation (only used to unscale parameters; default 1e-8)\n\
	\t-g generation time (gens/year; default 20)\n\
	\t-seqLen sequence length scanned for polymorphisms (used to unscale parameter)\n\
	\t-put upper bound for optimization of thetas\n\
	\t-pum upper bound for optimization of migration rates\n\
	\t-pudt upper bound for optimization of divergence time\n\
	\t-r randomSeed\n\
	\t-v verbose output\n";

int main(int argc, char **argv){
	PetscInt i,j, N,runMode;
	clock_t time1, time2;
	PetscInt nnz;
	PetscInt seed;
	char  filename[PETSC_MAX_PATH_LEN], filename2[PETSC_MAX_PATH_LEN], filename3[PETSC_MAX_PATH_LEN] ;
	double lik = 0.0;
	double mle[5] = {0.1,2,1,1,5};
 	const gsl_rng_type * T;
  	gsl_rng * r;
	PetscErrorCode ierr;
	PetscMPIInt    rank,size;
	PetscBool      flg,obsFlag;
//	Vec tmpVec;
	PetscInt dim=5;
	double snpNumber, pi_est, p, N0;
	double u=1e-8;
	double genPerYear=20;
	double put = 10.0;
	double pum = 20.0;
	double pudt=10.0;
	double propSnp;
	PetscInt seqLen;
	gsl_matrix *fi,*gi;
	FILE *infile;
	PetscInt testInt=0;
	PetscScalar one = 1.0;
	cs *ident;

	///////////////////
	/////	PETSC / Slepc library version of this code
	SlepcInitialize(&argc,&argv,(char*)0,help);
	ierr = MPI_Comm_rank(PETSC_COMM_WORLD, &rank);CHKERRQ(ierr);
	ierr = MPI_Comm_size(PETSC_COMM_WORLD, &size);CHKERRQ(ierr);
	if(rank==0){

		printf("\n.___   _____             .__\n|   | /     \\       ____ |  | _____    _____\n|   |/  \\ /  \\    _/ ___\\|  | \\__  \\  /     \\\n|   /    Y    \\   \\  \\___|  |__/ __ \\|  Y Y  \\\n|___\\____|__  /____\\___  >____(____  /__|_|  /\n            \\/_____/   \\/          \\/      \\/\n\n\n");

		printf("im_clam -- Isolation with Migration Composite Likelihood Analysis using Markov chains\n");
		//printf("A.D. Kern 2015\n///////////////////\n");
		printf("\n\n");
	}
	if(argc<2){
		printf("%s",help);
		exit(666);
	}
	time1=clock();

	ierr = PetscOptionsGetString(NULL,"-s",filename,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetString(NULL,"-m",filename2,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetString(NULL,"-d",filename3,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetInt(NULL,"-r",&seed,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-u",&u,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-g",&genPerYear,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetInt(NULL,"-seqLen",&seqLen,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-put",&put,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-pum",&pum,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-pudt",&pudt,&flg);CHKERRQ(ierr);
	//set upper bounds
	upperBounds[0]=put;upperBounds[1]=put;upperBounds[2]=pum;upperBounds[3]=pum;upperBounds[4]=pudt;

	if(!flg) seed=time(NULL);

	//printf("rank:%d %d\n",rank,seed);
	MPI_Bcast(&seed,1,MPI_INT,0,PETSC_COMM_WORLD);
	//printf("rank:%d %d\n",rank,seed);

	runMode = 1;
	obsFlag=PETSC_FALSE;

	//setup RNG for starting point for optimization
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);
	gsl_rng_set(r,seed);

	for(i=0;i<dim;i++){
		mle[i] = gsl_ran_flat(r,lowerBounds[i], upperBounds[i]);
	}

	PetscOptionsHasName(NULL,"-v",&flg); if(flg) vbse=PETSC_TRUE;
	PetscOptionsHasName(NULL,"-obs",&flg); if(flg) obsFlag=PETSC_TRUE;

	PetscOptionsHasName(NULL,"-exp",&flg); if(flg) runMode=2;
	ierr = PetscOptionsGetRealArray(NULL,"-x",mle,&dim,&flg);CHKERRQ(ierr);

	//double check we are set for exp mode
	if(runMode == 2 && !flg){
		printf("for exp runmode need to set parameter values using -x flag\n");
		exit(1);
	}

	PetscOptionsHasName(NULL,"-mo",&flg); if(flg) runMode=3;
	PetscOptionsHasName(NULL,"-global",&flg); if(flg) runMode=4;
	PetscOptionsHasName(NULL,"-GIM",&flg); if(flg) runMode=5;

	//import state space
	stateSpace = afsStateSpaceImportFromFile(filename);
	N = stateSpace->nstates;

	//quick peak at the mats file to set nnz
	//open file
	infile = fopen(filename2, "r");
	if (infile == NULL){
		fprintf(stderr,"Error opening mats file! ARRRRR!!!!\n");
		exit(1);
	}
	testInt=fscanf(infile,"nnz: %d", &nnz);
	fclose(infile);
	//got nnz, time for allocs

	//setup currentParams struct; alloc all arrays
	currentParams = malloc(sizeof(clam_lik_params));
	currentParams->rng = r;
	currentParams->n1 = n1 = stateSpace->states[0]->popMats[0]->size1 - 1;
	currentParams->n2 = n2 = stateSpace->states[0]->popMats[1]->size2 - 1;
	currentParams->stateSpace = stateSpace;
	currentParams->rank = rank;
//	currentParams->snpNumber = snpNumber;
	currentParams->expAFS = gsl_matrix_alloc(n1+1,n2+1);
	currentParams->expAFS2 = gsl_matrix_alloc(n1+1,n2+1);
	currentParams->map = malloc(sizeof(PetscInt)*N);
	currentParams->reverseMap = malloc(sizeof(PetscInt)*N);
	currentParams->rates = gsl_vector_alloc(stateSpace->nstates);
	currentParams->stateVec = gsl_vector_alloc(stateSpace->nstates);
	currentParams->resVec = gsl_vector_alloc(stateSpace->nstates);
	gsl_vector_set_zero(currentParams->resVec);
//	currentParams->paramVector = gsl_vector_alloc(5);
//	currentParams->paramCIUpper = gsl_vector_alloc(5);
//	currentParams->paramCILower = gsl_vector_alloc(5);
//	currentParams->mlParams = gsl_vector_alloc(5);

	currentParams->top = malloc(sizeof(double) * nnz);
	currentParams->topA = malloc(sizeof(double) * nnz);

	currentParams->move = malloc(sizeof(PetscInt) * nnz);
	currentParams->moveA = malloc(sizeof(PetscInt) * nnz);
//	printf("allocated moveType...\n");
	for(i=0;i< (nnz);i++){
		currentParams->move[i]=0;
		currentParams->top[i]=0;
		currentParams->moveA[i]=0;
		currentParams->topA[i]=0;
	}
	currentParams->dim1 = malloc(sizeof(PetscInt) * nnz);
	currentParams->dim2 = malloc(sizeof(PetscInt) * nnz);
	currentParams->dim1A = malloc(sizeof(PetscInt) * N * 10);
	currentParams->dim2A = malloc(sizeof(PetscInt) * N * 10);
	currentParams->b = malloc(N*sizeof(double));
	currentParams->expoArray = malloc(N*sizeof(double));
	currentParams->st = malloc(N*sizeof(double));
	currentParams->paramVector = gsl_vector_alloc(5);
	gsl_vector_set_all(currentParams->paramVector,1.0);
	currentParams->fEvals=0;

	currentParams->triplet = cs_spalloc(N, N, nnz + N , 1, 1); //alloc sparse mat with extra space for nonzero identity mats
	ident = cs_spalloc(N,N,N,1,1);
	for(i=0;i<N;i++) cs_entry(ident,i,i,1);
	currentParams->eye = cs_compress(ident);
	cs_spfree(ident);

	//set up some petsc matrices//////////////////////////////////////////////////////////////
	//
	//

	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->C);CHKERRQ(ierr);
//	MatSetType(C,MATMPIAIJ);
	ierr = MatSetSizes(currentParams->C, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->C);CHKERRQ(ierr);
//	MatSetOption(currentParams->C, MAT_NEW_NONZERO_ALLOCATION_ERR, PETSC_FALSE);
	ierr = MatSetUp(currentParams->C);CHKERRQ(ierr);

	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, N, N, NULL, &currentParams->denseMat1);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->denseMat1);CHKERRQ(ierr);
	MatAssemblyBegin(currentParams->denseMat1,MAT_FINAL_ASSEMBLY);
	MatAssemblyEnd(currentParams->denseMat1,MAT_FINAL_ASSEMBLY);

	///////// Set up MFN ////////////////////////////////////////////////////////////////////////////////////////
	ierr = MFNCreate(PETSC_COMM_WORLD,&currentParams->mfn);CHKERRQ(ierr);
	ierr = MFNSetFunction(currentParams->mfn,SLEPC_FUNCTION_EXP);CHKERRQ(ierr);
	ierr = MFNSetTolerances(currentParams->mfn,1e-07,PETSC_DEFAULT);CHKERRQ(ierr);

	//mapping stateSpace to reducedSpace
	currentParams->reducedStateSpace = afsStateSpaceNew();
	afsStateSpaceMapAndReducePopn(currentParams->stateSpace, currentParams->map, currentParams->reducedStateSpace, currentParams->reverseMap);
	currentParams->Na=currentParams->reducedStateSpace->nstates;

	VecCreate(PETSC_COMM_WORLD,&currentParams->ancStateVec);
	VecSetSizes(currentParams->ancStateVec,PETSC_DECIDE,currentParams->Na);
	VecSetFromOptions(currentParams->ancStateVec);
	VecDuplicate(currentParams->ancStateVec,&currentParams->ancResVec);

	//set up more matrices
	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->C2);CHKERRQ(ierr);
	MatSetType(currentParams->C2,MATMPIAIJ);
	ierr = MatSetSizes(currentParams->C2, PETSC_DECIDE, PETSC_DECIDE,currentParams->Na,currentParams->Na);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->C2);CHKERRQ(ierr);
	ierr = MatSetUp(currentParams->C2);CHKERRQ(ierr);

	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, currentParams->Na, currentParams->Na, NULL, &currentParams->denseMat2);CHKERRQ(ierr);

	ident = cs_spalloc(currentParams->Na,currentParams->Na,currentParams->Na,1,1);
	for(i=0;i<currentParams->Na;i++) cs_entry(ident,i,i,1);
	currentParams->eyeAnc = cs_compress(ident);
	cs_spfree(ident);

	//DTMC with PETSC  /////////////
//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->D);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->D, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->D);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->D);CHKERRQ(ierr);

//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->D_copy);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->D_copy, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->D_copy);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->D_copy);CHKERRQ(ierr);

	//sparse identity mat
//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->ident);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->ident, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->ident);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->ident);CHKERRQ(ierr);

  //	MatAssemblyBegin(currentParams->ident,MAT_FINAL_ASSEMBLY);
  //	MatAssemblyEnd(currentParams->ident,MAT_FINAL_ASSEMBLY);
//	MatShift(currentParams->ident,one);

	//dense identity mat
//	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, N, N, NULL, &currentParams->denseIdent);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->denseIdent);CHKERRQ(ierr);

//	MatAssemblyBegin(currentParams->denseIdent,MAT_FINAL_ASSEMBLY);
  //	MatAssemblyEnd(currentParams->denseIdent,MAT_FINAL_ASSEMBLY);
//	MatShift(currentParams->denseIdent,one);

//	VecCreate(PETSC_COMM_WORLD,&currentParams->xInv);
//	VecSetSizes(currentParams->xInv,PETSC_DECIDE,N);
//	VecSetFromOptions(currentParams->xInv);
//	VecDuplicate(currentParams->xInv,&currentParams->bInv);

	///////////////////////////////////////

	//////////////////////////////

	// setup done! ///////////////
	////////////////////////////////////

	//import mats
	mcMatsImportFromFile(filename2, &nnz, currentParams->top, currentParams->move, currentParams->dim1, currentParams->dim2);
	currentParams->nnz = nnz;

	//init snpNumber to avoid compiler warning
	snpNumber=0;
	if(runMode != 2){
		//alloc data matrix and import data; estimate pi and N1
		currentParams->obsData = gsl_matrix_alloc(n1+1,n2+1);
		import2DSFSData(filename3, currentParams->obsData);
		snpNumber = gsl_matrix_sum(currentParams->obsData);
		propSnp = (float) snpNumber / (float) seqLen;
	//	pi_est = 0.0;
	//	for(i=1;i<n1;i++){
	//		for(j=0;j<n2+1;j++){
	//			p =  (float) i / n1;
	//			pi_est += (2 * p * (1.0 - p)) * gsl_matrix_get(currentParams->obsData,i,j);
	//		}
	//	}
	//	pi_est *= n1 / (n1 - 1) / snpNumber;
	//	N0=pi_est / u / 4;
	}

	////////////////////////////
	time2=clock();
	if(rank==0)printf("setup time:%f secs\n\n",(double) (time2-time1)/CLOCKS_PER_SEC);

	time1=clock();
	//expected AFS
	//printf("runMode = %d\n",runMode);
	switch(runMode){

		case 1:
		if(rank==0){
			printf("\nParameter estimation run mode\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		maximizeLikNLOpt(&lik, currentParams, mle);
		fi = getGodambeInfoMatrix(mle, lik, currentParams);
		//get N0 estimate
		N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;
		if(rank == 0){
			printf("for scaling:\nu: %e gen: %lf N0:%lf meanTreeLength:%lf seqLen:%d\n",u,genPerYear,N0,currentParams->meanTreeLength,seqLen);
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");

			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float)mle[i]*N0);
			for(i=2;i<4;i++)printf("%f\t",(float)mle[i]/N0/4);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))*N0);
			for(i=2;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))/N0/4);
			printf("%f\t",(float)sqrt(gsl_matrix_get(fi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");

			printf("\n\nlikelihood: %lf\n",-lik);
			currentParams->nnz = nnz;
			printf("\nExpected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
		}

		break;
		case 2:
		if(rank == 0)printf("expected value run mode\n");
		for(i=0;i<5;i++){
			gsl_vector_set(currentParams->paramVector,i,mle[i]);
		}
		calcLogAFS_IM(currentParams);
		currentParams->nnz = nnz;
		if(rank == 0){
			printf("Expected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
			printf("parameter values used:\n");
			for(i=0;i<dim;i++)printf("%f\t",gsl_vector_get(currentParams->paramVector,i));
			printf("\n\n");
		}
		time2=clock();
		//lik = calcLikNLOpt(5,mle,NULL,currentParams);
		//

		//printf("Likelihood: %g\n",lik);
		//if(rank==0)printf("w/ CSPARSE time:%f secs\n Liklihood Func. Evals: %d\n",(double) (time2-time1)/CLOCKS_PER_SEC,currentParams->fEvals);
		time1=clock();
		break;
		case 3:
		if(rank==0){
			printf("\nParameter estimation run mode with multiple optimizations\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		for(j = 0; j < 3; j++){

			for(i=0;i<5;i++)mle[i] = gsl_ran_flat(r,lowerBounds[i], upperBounds[i]);
			printf("optimization %d initial parameter guess:\n",j);
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			maximizeLikNLOpt(&lik, currentParams, mle);
			if(rank == 0){
				printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
				printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
				for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
				printf("\n\nlikelihood: %lf\n",-lik);
				currentParams->nnz = nnz;
				printf("\nExpected AFS:\n");
				gsl_matrix_prettyPrint(currentParams->expAFS);
			}
		}

		break;
		case 4:
		if(rank==0){
			printf("\nParameter estimation run mode\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		maximizeLikNLOpt_MLSL(&lik, currentParams, mle);
		fi = getGodambeInfoMatrix(mle, lik, currentParams);
		if(rank == 0){
			N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");

			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float)mle[i]*N0);
			for(i=2;i<4;i++)printf("%f\t",(float)mle[i]/N0/4);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))*N0);
			for(i=2;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))/N0/4);
			printf("%f\t",(float)sqrt(gsl_matrix_get(fi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");

			printf("\n\nlikelihood: %lf\n",-lik);
			currentParams->nnz = nnz;
			printf("\nExpected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
		}

		break;
		case 5:
		for(i=0;i<5;i++){
			gsl_vector_set(currentParams->paramVector,i,mle[i]);
		}
		if(rank==0){
			printf("\nUncertainty estimation (via Godambe Information) run mode\n\n");
			printf("MLE parameters:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n");
		}
		lik = calcLikNLOpt(5,mle,NULL,currentParams);
		gi = getGodambeInfoMatrix(mle, lik, currentParams);
		N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;

		// if(rank==0){
		// 	printf("FIM:\n");
		// 	gsl_matrix_prettyPrint(fi);
		// 	printf("\nGIM:\n");
		// 	gsl_matrix_prettyPrint(gi);
		// 	printf("\n");
		// }
		if(rank==0){
			printf("\nlikelihood: %lf\n",-lik);
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<4;i++)printf("%f\t",(float)mle[i]*N0);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(gi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(gi,i,i))*N0);
			printf("%f\t",(float)sqrt(gsl_matrix_get(gi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");
		}
		break;
	}

	if(obsFlag && rank==0){
		gsl_matrix_scale(currentParams->obsData,1.0/snpNumber);
		printf("%f SNPs in dataset\n",snpNumber);
		printf("Observed AFS:\n");
		gsl_matrix_prettyPrint(currentParams->obsData);
		gsl_matrix_scale(currentParams->obsData,snpNumber);
	}

	MatDestroy(&currentParams->denseMat1);
	MatDestroy(&currentParams->denseMat2);
	MatDestroy(&currentParams->C);
	MatDestroy(&currentParams->C2);
	MatDestroy(&currentParams->D);
	MatDestroy(&currentParams->D_copy);
	MatDestroy(&currentParams->denseIdent);
	MatDestroy(&currentParams->ident);
	MFNDestroy(&currentParams->mfn);
	VecDestroy(&currentParams->ancStateVec);
	VecDestroy(&currentParams->ancResVec);
	VecDestroy(&currentParams->xInv);
	VecDestroy(&currentParams->bInv);
	gsl_rng_free (r);

	afsStateSpaceFree(currentParams->reducedStateSpace);
	time2=clock();
	if(rank==0)printf("total run time:%f secs\n Liklihood Func. Evals: %d\n",(double) (time2-time1)/CLOCKS_PER_SEC,currentParams->fEvals);
	ierr = PetscFinalize();

	return(0);
}

//import2DSFSData -- imports a matrix from fileName and stuffs it in prealloc'd obsData
void import2DSFSData(const char *fileName, gsl_matrix *obsData){
	FILE *infile;

	//open file
	infile = fopen(fileName, "r");
	if (infile == NULL){
		fprintf(stderr,"Error opening data file! ARRRRR!!!!\n");
		exit(1);
	}
	gsl_matrix_fscanf(infile,obsData);
	fclose(infile);
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
 *
 */

/*
 *
 *
 *       Filename:  red_gen.c
 *
 *    Description:  Redfield-type Generator
 *
 *        Version:  1.0
 *        Created:  25/04/2014 22:28:19
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_math.h>
#include <math.h>
#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  red_mat
 *  Description:  Creating the Redfield generator matrix in Bloch form, taking as
 *  		  arguments all the integrals, the physical parameters
 *  		  and the pointer to the matrix
 *
 */
int red_mat ( gsl_matrix* red_mx , void* params )
{
	double integrals[12] ;
	if ( integration(params,integrals) != 0 )
		return -1;

	/* Setting the integrals */
	double rcc, icc, rss, iss, rsc, isc, rcs, ics, rc0, ic0, rs0, is0 ;
	rcc = integrals[0] ;
	icc = integrals[1] ;
	rss = integrals[2] ;
	iss = integrals[3] ;
	rsc = integrals[4] ;
	isc = integrals[5] ;
	rcs = integrals[6] ;
	ics = integrals[7] ;
	rc0 = integrals[8] ;
	ic0 = integrals[9] ;
	rs0 = integrals[10] ;
	is0 = integrals[11] ;

	/* Copying the parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	double D = sqrt(POW_2(o_1)-POW_2(O)) ;

	/* Ensuring that the matrix is zeroed */
	gsl_matrix_set_zero ( red_mx ) ;

	/* Building the matrix */
	gsl_matrix_set ( red_mx, 1, 0, (D/o_1)*(-(O/o_1)*ic0+iss+(O/o_1)*icc) ) ;
	gsl_matrix_set ( red_mx, 1, 1, POW_2(D/o_1)*rc0+(O/o_1)*rss+POW_2(O/o_1)*rcc ) ;
	gsl_matrix_set ( red_mx, 1, 2, o_1/4+(O/o_1)*rsc-POW_2(O/o_1)*rcs ) ;
	gsl_matrix_set ( red_mx, 1, 3, (D/o_1)*(rsc-(O/o_1)*rcs) ) ;
	gsl_matrix_set ( red_mx, 2, 0, (D/o_1)*(is0+isc-(O/o_1)*ics) ) ;
	gsl_matrix_set ( red_mx, 2, 1, -(o_1/4)-(O/o_1)*rsc+rcs ) ;
	gsl_matrix_set ( red_mx, 2, 2, POW_2(D/o_1)*rc0+rcc+(O/o_1)*rss ) ;
	gsl_matrix_set ( red_mx, 2, 3, -(D/o_1)*((O/o_1)*rcc+rss) ) ;
	gsl_matrix_set ( red_mx, 3, 0, (1+POW_2(O/o_1))*ics-2*(O/o_1)*isc ) ;
	gsl_matrix_set ( red_mx, 3, 1, -(D/o_1)*rs0 ) ;
	gsl_matrix_set ( red_mx, 3, 2, -(O*D/(o_1*o_1))*rc0 ) ;
	gsl_matrix_set ( red_mx, 3, 3, (1+POW_2(O/o_1))*rcc+2*(O/o_1)*rss ) ;

	/* Multiplies times -4 to obtain the Bloch matrix */
	gsl_matrix_scale ( red_mx, -4 ) ;

	return 0;
}		/* -----  end of function red_mat  ----- */


#ifndef __DREAM_H__
#define __DREAM_H__

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <map>
#include <vector>
#include <algorithm>
using namespace std;

#include <gsl/gsl_statistics.h>
#include <gsl/gsl_math.h>

#include <rapidjson/document.h>
#include <rng/RngStream.h>
#include "array.h"

typedef double (*LikFun)(int chain_id, int gen, const double* state,
                         const void* pars, bool recalc);
typedef void (*ReportFun)(int gen, int chain_id, int n,
                          const double* state, double lik,
                          int burnIn, const void* pars);

typedef struct t_dream_pars {
  int vflag;                 /* vebose flag */
  int maxEvals;              /* max number of function evaluations */
  int optimAlg;              /* lock variables */
  int numChains;
  string fn;                 /* input filename */
  string out_fn;             /* output filename */
  int appendFile;            /* continue from previous state */
  int report_interval;       /* report interval for state */
  int diagnostics;           /* report diagnostics at the end of the run */
  int burnIn;                /* number of steps for which to run an adaptive proposal size */
  int recalcLik;             /* recalculate likelihood of previously evaluated states */

  // DREAM variables
  int collapseOutliers;
  int gelmanEvals;
  int loopSteps;
  int deltaMax;
  int pCR_update;
  int nCR;

  double noise;
  double bstar_zero;
  double scaleReductionCrit;
  double reenterBurnin;

  int nfree;
  size_t nvar;
  double* varLo;
  double* varHi;
  double* varInit;
  int* varLock;
  string* varName;
  char* scale;

  LikFun fun;
  void* funPars;
  ReportFun rfun;
} dream_pars;

void dream_pars_default(dream_pars* p);
void dream_pars_init_vars(dream_pars* p, size_t n);
int  dream_pars_free_vars(dream_pars* p);

size_t dream_par_by_name(const dream_pars* p, string name);

void dream_pars_read_json(dream_pars* p, rapidjson::Value& jpars);
void dream_pars_from_json(dream_pars* p, rapidjson::Value& jpars);
void dream_set_init(dream_pars* p, int n,
                    const double* init, const string* name, const int* lock,
                    const double* lo, const double* hi, const char* scale);

int dream_restore_state(const dream_pars* p, Array3D<double>& state, Array2D<double>& lik, vector<double>& pCR, int& inBurnIn);
void dream_initialize(const dream_pars* p, rng::RngStream* rng, Array2DView<double>& state, ArrayView<double>& lik);
int dream(const dream_pars* p, rng::RngStream* rng);

void check_outliers(int t, Array2D<double>& lik, vector<double>& meanlik,
                    vector<bool>& outliers);
void gen_CR(rng::RngStream* rng, const vector<double>& pCR,
            Array2D<int>& CRm, vector<unsigned>& L);
void gelman_rubin(Array3D<double>& state, vector<double>& scaleReduction,
                  const int* lockVar, int first = 0, int numIter = -1,
                  int adjustDF = 0);

#endif


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include "mathfunctions.h"
#include "globals.h"
#include "constitutive_equations.h"

/*****************************************************************************/
/***************************************************************************/
/************ Compute the spatial discretization of the equation ***********/
/***************************************************************************/

extern const double g;
extern void Compute_InnerProducts(double IP1[], double IP2[], double IP3[], double IP4[], double IP5[], double IP6[], double IP7[], double IP8[], double IP9[], double IP10[]);
extern void RoeFlux( double *Fhat, double A_L, double A_R, double Q_L, double Q_R, double b, double m1val, double m2val, double g);
extern void LF(double *Fhat, double A_L, double A_R, double Q_L, double Q_R, double b, double g);

extern void* xcalloc(int items, int size);
extern void GetLGLWeights(int N, double *w);

void computeL()
{
	double* Fhat1L = xcalloc(NumNodes, sizeof(double));
	double* Fhat2L = xcalloc(NumNodes, sizeof(double));
	double* Fhat1R = xcalloc(NumNodes, sizeof(double));
	double* Fhat2R = xcalloc(NumNodes, sizeof(double));

#ifdef WDON
/**** Compute the mass over an element to use later for ensuring ********/
/**** positive mass with the flux in a wetting and drying treatment *************/
	double mass[NumEl];
	double LGLWeight[Np];
	GetLGLWeights(P, LGLWeight);

	for (int i = 0; i < NumEl; ++i)
	{
		double avgArea = 0;
		int begNode = i*Np + 1;
		for (int j =0; j < Np; j++)
		{
			avgArea += LGLWeight[j]*A[begNode+1];
		}
		mass[i] = avgArea;
	}
#endif

/************** Compute the numerical flux at the faces *****************/
	for (int i=0; i < NumEdges; ++i)
	{
		double A_L, Q_L, A_R, Q_R, m1val, m2val;

		int leftNode = i*Np;
		int rightNode = leftNode+1;

		m1val = m1[i];
		m2val = m2[i];

		double tmpF[2];

		#ifdef WDON
		// Check to see if the elements separated by this boundary are both dry
		if (i > 0 && i < NumNodes-1)
		{
			if (WD[i-1] == 0 && WD[i] == 0)
			{
				// Reflection flux for the left element
				A_L = A[leftNode];
				A_R = A_L;
				Q_L = Q[leftNode];
				Q_R = -Q_L;
				//LF(tmpF, A_L, A_R, Q_L, Q_R,b[i], m1val, m2val, 0);
				RoeFlux(tmpF, A_L, A_R, Q_L, Q_R,b[i], m1val, m2val,0);
				Fhat1L[i] = tmpF[0];
				Fhat2L[i] = tmpF[1];

				// Reflection flux for the right element
				A_R = A[rightNode];
				A_L = A_R;
				Q_R = Q[rightNode];
				Q_L = -Q_R;
				//LF(tmpF, A_L, A_R, Q_L, Q_R, b[i], m1val, m2val, 0);
				RoeFlux(tmpF, A_L, A_R, Q_L, Q_R, b[i], m1val, m2val, 0);
				Fhat1R[i] = tmpF[0];
				Fhat2R[i] = tmpF[1];

				if (isnan(tmpF[0]) || isnan(tmpF[1]))
				{
					printf("both elements dry flux not a number, edge %d \n",i);
					exit(EXIT_FAILURE);
				}

			}

		}

		// if the elements are not both dry
		if ((i == 0) || (i == NumEl) || WD[i-1] == 1 || WD[i] == 1)
		{
			A_L = A[leftNode];
			A_R = A[rightNode];

			Q_L = Q[leftNode];
			Q_R = Q[rightNode];

			RoeFlux(tmpF, A_L, A_R, Q_L,Q_R,b[i], m1val, m2val, g);
			//LF(tmpF, A_L, A_R, Q_L,Q_R,b[i], m1val, m2val, g);

			if (isnan(tmpF[0]) || isnan(tmpF[1]))
			{
				printf(" both elements wet flux not a number, edge %d \n",i);
				exit(EXIT_FAILURE);
			}

			if (i==0 || WD[i-1] == 1)
			{
				Fhat1L[i] = tmpF[0];
				Fhat2L[i] = tmpF[1];

			}
			else
			{
				double newtmpF[2];
				//LF(newtmpF, A_L, A_R, Q_L, Q_R, b[i], m1val, m2val, 0);
				RoeFlux(newtmpF, A_L, A_R, Q_L, Q_R, b[i], m1val, m2val, 0);
				Fhat1L[i] = newtmpF[0];
				Fhat2L[i] = newtmpF[1];
				if (isnan(newtmpF[0]) || isnan(newtmpF[1]))
				{
					printf("left element dry flux not a number, edge %d \n",i);
					exit(EXIT_FAILURE);
				}

			}
			if (i == NumEl || WD[i]==1)
			{
				Fhat1R[i] = tmpF[0];
				Fhat2R[i] = tmpF[1];
			}
			else
			{
				//LF(tmpF, A_L,A_R,Q_L,Q_R,b[i], m1val, m2val, 0);
				RoeFlux(tmpF, A_L,A_R,Q_L,Q_R,b[i], m1val, m2val, 0);
				Fhat1R[i] = tmpF[0];
				Fhat2R[i] = tmpF[1];
				if (isnan(tmpF[0]) || isnan(tmpF[1]))
				{
					printf("right element dry flux not a number, edge %d \n",i);
					exit(EXIT_FAILURE);
				}

			}
		}

		#else
		A_L = A[leftNode];
		A_R = A[rightNode];

		Q_L = Q[leftNode];
		Q_R = Q[rightNode];

		RoeFlux(tmpF, A_L, A_R, Q_L,Q_R,b[i],m1val,m2val,g);
		//LF(tmpF, A_L, A_R, Q_L,Q_R,b[i],m1val,m2val,g);

		if (isnan(tmpF[0]) || isnan(tmpF[1]))
		{
			printf("flux not a number, edge %d \n",i);
			printf("A_L = %lf A_R = %lf Q_L = %lf Q_R = %lf, b = %lf, m1 = %lf, m2 = %lf \n", A_L, A_R, Q_L, Q_R, b[i], m1val, m2val);
			exit(EXIT_FAILURE);
		}

		Fhat1L[i] = tmpF[0];
		Fhat2L[i] = tmpF[1];

		Fhat1R[i] = tmpF[0];
		Fhat2R[i] = tmpF[1];

		#endif

		/**************************************************************************************************/
		double u_L = Q_L/A_L;
		double u_R = Q_R/A_R;
		double c_L = sqrt(g*A_L/b[i]);
		double c_R = sqrt(g*A_R/b[i]);

		if (i==0)
			max_lambda = fmax((fabs(u_L)+c_L), (fabs(u_R)+c_R));
		// Compute maximum eigenvalue for the next time step
		double current_max =fmax((fabs(u_L) + c_L), (fabs(u_R) + c_R));
		max_lambda = max((max_lambda),(current_max));
	}

	printf("max_lambda = %lf\n", max_lambda);
	#ifdef WDON
	for (int i =1; i < NumNodes-1; ++i)
	{
		int leftNode = i*Np;
		int rightNode = leftNode+1;
		double maxBetaOverAlpha = 2;
		// Check to see if this flux might possibly result in negative mass
		// If the mass will be negative on the left side
	      	if (Fhat1L[i]*maxBetaOverAlpha*dt > mass[i-1])
		{
	    double A_L = A[leftNode];
			double A_R = A_L;
			double Q_L = Q[leftNode];
	    double Q_R = -Q_L;
	    double tmpF[2];
			double localG = g;

			if (WD[i] == 0)
				localG = 0;

			//LF(tmpF, A_L, A_R, Q_L, Q_R, b[i],m1val,m2val,localG);
			RoeFlux(tmpF, A_L, A_R, Q_L, Q_R, b[i],m1val,m2val,localG);
	      		Fhat1L[i] = tmpF[0];
	      		Fhat2L[i] = tmpF[1];
			printf("element %d will be dry\n",i);
	    	}

		// If the mass will be negative on the right side
		if (-Fhat1R[i]*maxBetaOverAlpha*dt > mass[i])
		{
			double A_R = A[rightNode];
			double A_L = A_R;
			double Q_R = Q[rightNode];
			double Q_L = -Q_R;
			double tmpF[2];
			double localG = g;
			if (WD[i] == 0)
				localG = 0;
			//LF(tmpF, A_L, A_R, Q_L, Q_R, b[i],m1val,m2val,localG);
			RoeFlux(tmpF, A_L, A_R, Q_L, Q_R, b[i],m1val,m2val,localG);
			Fhat1R[i] = tmpF[0];
			Fhat2R[i] = tmpF[1];
			printf("element %d will be dry \n",i);

		}
	}
	#endif

	// Compute the right hand side
//	double IP1[NumEl], IP2[NumEl], IP3[NumEl], IP4[NumEl], IP5[NumEl], IP6[NumEl], IP7[NumEl], IP8[NumEl], IP9[NumEl], IP10[NumEl];
//	Compute_InnerProducts(IP1, IP2, IP3, IP4, IP5, IP6, IP7, IP8, IP9, IP10);

	int k;
	for (k=0; k < NumEl; ++k)
	{

		double h = dh[k];
		gsl_vector *F1 = gsl_vector_alloc(Np);
		gsl_vector *F2 = gsl_vector_alloc(Np);
		gsl_vector *ST21 = gsl_vector_alloc(Np);
		gsl_vector *ST22 = gsl_vector_alloc(Np);
		gsl_vector *ST23 = gsl_vector_alloc(Np);

		int begNode = k*Np;
		for (int i = 0; i < Np; i++)
		{
			double Aval = A[begNode+i+1];
			double Qval = Q[begNode+i+1];
			double bval = NodalB[begNode+i];
			double S0 = dz[begNode+i];
			double m1val = Nodalm1[begNode+i];
			double m2val = Nodalm2[begNode+i];
			double dm1val = dm1[begNode+i];
			double dm2val = dm2[begNode+i];
			double hval = getH(Aval, bval, m1val, m2val);
			double I1val = getI1(Aval, bval, m1val, m2val);
			double I2val = getI2(Aval, bval, db[k*Np+i], m1val, dm1val, m2val, dm2val);
			double Sfval = getS_f(Aval, Qval, bval, m1val, m2val, NodalnFriction[begNode+i]);
			double beta = 1.0;
			double localG;
			#ifdef WDON
			if(WD[k]==1)
				localG = g;
			else
				localG = 0;
			#else
				localG = g;
			#endif

			gsl_vector_set(F1, i, Qval);
			gsl_vector_set(F2, i, beta*Qval*Qval/Aval + g*I1val);
			gsl_vector_set(ST21, i, g*I2val);
			//gsl_vector_set(ST22, i, g*hval*bval*S0);
			gsl_vector_set(ST22, i, g*Aval*S0);
			gsl_vector_set(ST23, i, g*Aval*Sfval);

		}

		gsl_vector *localFhat1 = gsl_vector_alloc(2);
		gsl_vector_set(localFhat1, 0, Fhat1R[k]);
		gsl_vector_set(localFhat1, 1, Fhat1L[k+1]);

		gsl_vector *localFhat2 = gsl_vector_alloc(2);
		gsl_vector_set(localFhat2, 0, Fhat2R[k]);
		gsl_vector_set(localFhat2, 1, Fhat2L[k+1]);

		// cacluate the volume integral of the flux
		gsl_vector *localRHS1 = gsl_vector_calloc(Np);
		gsl_vector *localRHS2 = gsl_vector_calloc(Np);
		gsl_blas_dgemv(CblasNoTrans, 2.0/h, VolMat, F1, 1.0, localRHS1);
		gsl_blas_dgemv(CblasNoTrans, 2.0/h, VolMat, F2, 1.0, localRHS2);

		// calculate the surface integral of the flux
		gsl_vector *SurfPart1 = gsl_vector_calloc(Np);
		gsl_vector *SurfPart2 = gsl_vector_calloc(Np);
		gsl_blas_dgemv(CblasNoTrans, 2.0/h, LIFT, localFhat1, 1.0, SurfPart1);
		gsl_blas_dgemv(CblasNoTrans, 2.0/h, LIFT, localFhat2, 1.0, SurfPart2);

		// calculate the RHS
		gsl_vector_add(localRHS1, SurfPart1);
		gsl_vector_add(localRHS2, SurfPart2);
		gsl_vector_add(localRHS2, ST21);
		gsl_vector_add(localRHS2, ST22);
		gsl_vector_sub(localRHS2, ST23);

		gsl_vector_free(F1);
		gsl_vector_free(F2);
		gsl_vector_free(localFhat1);
		gsl_vector_free(localFhat2);
		gsl_vector_free(SurfPart1);
		gsl_vector_free(SurfPart2);
		gsl_vector_free(ST21);
		gsl_vector_free(ST22);
		gsl_vector_free(ST23);

		for(int i = 0; i < Np; i++)
		{
			RHSA[begNode+i+1] = gsl_vector_get(localRHS1, i);
			RHSQ[begNode+i+1] = gsl_vector_get(localRHS2, i);
		}

		gsl_vector_free(localRHS1);
		gsl_vector_free(localRHS2);

	/*
		double F_el1[2] = {Fhat1R[k], Fhat1L[k+1]};
		double F_el2[2] = {Fhat2R[k], Fhat2L[k+1]};

		double invMrow1[2] = {4/h, -2/h};
		double invMrow2[2] = {-2/h, 4/h};
		//double invM[2][2] = {{4/(b-a), 2/(a-b)},{2/(a-b), 4/(b-a)}};

		double S1 = IP1[k] + F_el1[0];
		double S2 = IP2[k] - F_el1[1];
		double S3 = IP3[k] + F_el2[0]  + IP4[k] + IP5[k] - IP6[k];
		double S4 = IP7[k] - F_el2[1] + IP8[k] + IP9[k] - IP10[k];

		// Compute RHSA = invM*[S1;S2] and RHSQ = invM*[S3;S4]
		RHSA[2*k + 1] = invMrow1[0]*S1 + invMrow1[1]*S2;
		RHSA[2*k + 2] = invMrow2[0]*S1 + invMrow2[1]*S2;
		RHSQ[2*k + 1] = invMrow1[0]*S3 + invMrow1[1]*S4;
		RHSQ[2*k + 2] = invMrow2[0]*S3 + invMrow2[1]*S4;
*/
	}

	// The value at the ghost nodes will be the same as the values at the other side of the face

	RHSA[0] = RHSA[1];
	RHSA[NumNodes - 1] = RHSA[NumNodes - 2];

	RHSQ[0] = RHSQ[1];
	RHSQ[NumNodes - 1] = RHSQ[NumNodes - 2];

	free(Fhat1L);
	free(Fhat1R);
	free(Fhat2L);
	free(Fhat2R);

}


/* linalg/qr_ud.c
 *
 * Copyright (C) 2020 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_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

/*
 * this module contains routines for the QR factorization of a matrix
 * using the recursive Level 3 BLAS algorithm of Elmroth and Gustavson with
 * additional modifications courtesy of Julien Langou.
 */

static int aux_QR_TRD_decomp (gsl_matrix * U, gsl_matrix * A, const gsl_vector * D, gsl_matrix * T);
static double qrtd_householder_transform (double *v0, double *v1);
static double qrtrd_householder_transform (double *v0, gsl_vector * v, double *d);

/*
gsl_linalg_QR_UD_decomp()
  Compute the QR decomposition of the "triangle on top of diagonal" matrix

  [ U ] = Q [ R ]
  [ D ]     [ 0 ]

where U is N-by-N upper triangular, D is N-by-N diagonal

Inputs: U - on input, upper triangular N-by-N matrix
            on output, R factor in upper triangle
        D - diagonal N-by-N matrix
        Y - (output) upper triangular N-by-N matrix (lower part of V)
        T - (output) block reflector matrix, N-by-N

Notes:
1) Based on the Elmroth/Gustavson algorithm, taking into account the
sparse structure of the U,S matrices

2) The Householder matrix V has the special form:

      N
V = [ I ] N
    [ Y ] N

with Y upper triangular

3) The orthogonal matrix is

Q = I - V T V^T
*/

int
gsl_linalg_QR_UD_decomp (gsl_matrix * U, const gsl_vector * D, gsl_matrix * Y, gsl_matrix * T)
{
  const size_t N = U->size1;

  if (N != U->size2)
    {
      GSL_ERROR ("U matrix must be square", GSL_ENOTSQR);
    }
  else if (D->size != N)
    {
      GSL_ERROR ("D matrix does not match dimensions of U", GSL_EBADLEN);
    }
  else if (Y->size1 != Y->size2)
    {
      GSL_ERROR ("Y matrix must be square", GSL_ENOTSQR);
    }
  else if (Y->size1 != N)
    {
      GSL_ERROR ("Y matrix does not match dimensions of U", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (N == 1)
    {
      /* base case, compute Householder transform for single column matrix */
      double * T00 = gsl_matrix_ptr(T, 0, 0);
      double * U00 = gsl_matrix_ptr(U, 0, 0);
      double * Y00 = gsl_matrix_ptr(Y, 0, 0);
      *Y00 = gsl_vector_get(D, 0);
      *T00 = qrtd_householder_transform(U00, Y00);
      return GSL_SUCCESS;
    }
  else
    {
      /*
       * partition matrices:
       *
       *       N1  N2              N1  N2
       * N1 [ U11 U12 ] and  N1 [ T11 T12 ]
       * N2 [  0  U22 ]      N2 [  0  T22 ]
       * N1 [ D11 D12 ]
       * N2 [  0  D22 ]
       */
      int status;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_view U11 = gsl_matrix_submatrix(U, 0, 0, N1, N1);
      gsl_matrix_view U12 = gsl_matrix_submatrix(U, 0, N1, N1, N2);
      gsl_matrix_view U22 = gsl_matrix_submatrix(U, N1, N1, N2, N2);

      gsl_vector_const_view D1 = gsl_vector_const_subvector(D, 0, N1);
      gsl_vector_const_view D2 = gsl_vector_const_subvector(D, N1, N2);

      gsl_matrix_view Y11 = gsl_matrix_submatrix(Y, 0, 0, N1, N1);
      gsl_matrix_view Y12 = gsl_matrix_submatrix(Y, 0, N1, N1, N2);

      gsl_matrix_view T11 = gsl_matrix_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_view T12 = gsl_matrix_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_view T22 = gsl_matrix_submatrix(T, N1, N1, N2, N2);

      gsl_matrix_view m;

      /*
       * Eq. 2: recursively factor
       *
       *       N1           N1
       * N1 [ U11 ] = Q1 [ R11 ] N1
       * N2 [  0  ]      [  0  ] N2
       * N1 [ D11 ]      [  0  ] N1
       * N2 [  0  ]      [  0  ] N2
       */
      status = gsl_linalg_QR_UD_decomp(&U11.matrix, &D1.vector, &Y11.matrix, &T11.matrix);
      if (status)
        return status;

      /*
       * Eq. 3:
       *
       *      N2              N2            N2
       * N1 [ R12  ] = Q1^T [ U12 ] = [   U12 - W   ] N1
       * N2 [ U22~ ]        [ U22 ]   [     U22     ] N2
       * N1 [ D12~ ]        [  0  ]   [  - V31 W    ] N1
       * N2 [ D22~ ]        [ D22 ]   [     D22     ] N2
       *
       * where W = T11^T U12, using T12 as temporary storage, and
       *
       *        N1
       * V1 = [  I  ] N1
       *      [  0  ] N2
       *      [ V31 ] N1
       *      [  0  ] N2
       */
      gsl_matrix_memcpy(&T12.matrix, &U12.matrix);                                                    /* W := U12 */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &T11.matrix, &T12.matrix); /* W := T11^T W */
      gsl_matrix_sub(&U12.matrix, &T12.matrix);                                                       /* R12 := U12 - W */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &Y11.matrix, &T12.matrix); /* W := -V31 W */
      gsl_matrix_memcpy(&Y12.matrix, &T12.matrix);                                                    /* Y12 := -V31 W */

      /*
       * Eq. 4: factor the "triangle on top of rectangle on top of diagonal" matrix
       *
       * N2 [ U22 ] = Q2~ [ R22 ] N2
       * N1 [ Y12 ]       [  0  ] N1
       * N2 [ D22 ]       [  0  ] N2
       */
      m = gsl_matrix_submatrix(Y, 0, N1, N, N2);
      status = aux_QR_TRD_decomp(&U22.matrix, &m.matrix, &D2.vector, &T22.matrix);
      if (status)
        return status;

      /*
       * Eq. 13: update T12 := -T11 * V1^T * V2 * T22
       *
       * where:
       *
       *        N1                N2
       * V1 = [  I   ] N1   V2 = [  0   ] N1
       *      [  0   ] N2        [  I   ] N2
       *      [ V31~ ] N1        [ V32~ ] N1
       *      [  0   ] N2        [ V42~ ] N2
       *
       * Note: V1^T V2 = V31~^T V32~
       */

      gsl_matrix_memcpy(&T12.matrix, &Y12.matrix);                                                    /* T12 := V32~ */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &Y11.matrix, &T12.matrix); /* T12 := V31~^T V32~ */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &T11.matrix, &T12.matrix); /* T12 := -T11 * T12 */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &T22.matrix, &T12.matrix); /* T12 := T12 * T22 */

      return GSL_SUCCESS;
    }
}

/* Find the least squares solution to the overdetermined system
 *
 *   [ U ] x = b
 *   [ D ]
 *
 * using the QR factorization [ U; D ] = Q R.
 *
 * Inputs: R    - upper triangular R matrix, N-by-N
 *         Y    - upper triangular Y matrix, N-by-N
 *         T    - upper triangular block reflector, N-by-N
 *         b    - right hand side, size 2*N
 *         x    - (output) solution, size 2*N
 *                x(1:N) = least squares solution vector
 *                x(N+1:2*N) = vector whose norm equals ||b - Ax||
 *         work - workspace, size N
 */

int
gsl_linalg_QR_UD_lssolve (const gsl_matrix * R, const gsl_matrix * Y, const gsl_matrix * T,
                          const gsl_vector * b, gsl_vector * x, gsl_vector * work)
{
  const size_t N = R->size1;
  const size_t M = 2 * N;

  if (R->size2 != N)
    {
      GSL_ERROR ("R matrix must be square", GSL_ENOTSQR);
    }
  else if (Y->size1 != Y->size2)
    {
      GSL_ERROR ("Y matrix must be square", GSL_ENOTSQR);
    }
  else if (Y->size1 != N)
    {
      GSL_ERROR ("Y and R must have same dimensions", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (M != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (N != work->size)
    {
      GSL_ERROR ("workspace must be length N", GSL_EBADLEN);
    }
  else
    {
      return gsl_linalg_QR_UU_lssolve(R, Y, T, b, x, work);
    }
}

/*
aux_QR_TRD_decomp()
  Compute the QR decomposition of the "triangle on top of rectangle on top of diagonal" matrix

  [ U ] = Q [ R ]
  [ A ]     [ 0 ]
  [ D ]     [ 0 ]

where U is N-by-N upper triangular, A is M-by-N dense, D is N-by-N diagonal

Inputs: U - on input, upper triangular N-by-N matrix
            on output, R factor in upper triangle
        A - on input, A is stored in A(1:M,1:N)
            on output, Y is stored in A(1:M+N,1:N)
            size (M+N)-by-N
        D - diagonal N-by-N matrix
        T - (output) block reflector matrix, N-by-N

Notes:
1) Based on the Elmroth/Gustavson algorithm, taking into account the
sparse structure of the U,S matrices

2) The Householder matrix V has the special form:

      N
V = [ I ] N
    [ Y ] M+N

with Y upper trapezoidal

3) The orthogonal matrix is

Q = I - V T V^T
*/

static int
aux_QR_TRD_decomp (gsl_matrix * U, gsl_matrix * A, const gsl_vector * D, gsl_matrix * T)
{
  const size_t N = U->size1;

  if (N != U->size2)
    {
      GSL_ERROR ("U matrix must be square", GSL_ENOTSQR);
    }
  else if (A->size1 <= N)
    {
      GSL_ERROR ("A matrix must have > N rows", GSL_EBADLEN);
    }
  else if (D->size != N)
    {
      GSL_ERROR ("D matrix does not match dimensions of U", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (N == 1)
    {
      /* base case, compute Householder transform for single column matrix */
      const size_t M = A->size1 - N;
      double * T00 = gsl_matrix_ptr(T, 0, 0);
      double * U00 = gsl_matrix_ptr(U, 0, 0);
      gsl_vector_view v = gsl_matrix_subcolumn(A, 0, 0, M);
      double * D00 = gsl_matrix_ptr(A, M, 0);
      *D00 = gsl_vector_get(D, 0);
      *T00 = qrtrd_householder_transform(U00, &v.vector, D00);
      return GSL_SUCCESS;
    }
  else
    {
      /*
       * partition matrices:
       *
       *       N1  N2              N1  N2
       * N1 [ U11 U12 ] and  N1 [ T11 T12 ]
       * N2 [  0  U22 ]      N2 [  0  T22 ]
       * N1 [ D11 D12 ]
       * N2 [  0  D22 ]
       */
      int status;
      const size_t M = A->size1 - N;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_view U11 = gsl_matrix_submatrix(U, 0, 0, N1, N1);
      gsl_matrix_view U12 = gsl_matrix_submatrix(U, 0, N1, N1, N2);
      gsl_matrix_view U22 = gsl_matrix_submatrix(U, N1, N1, N2, N2);

      gsl_matrix_view A1 = gsl_matrix_submatrix(A, 0, 0, M, N1);
      gsl_matrix_view A2 = gsl_matrix_submatrix(A, 0, N1, M, N2);

      gsl_vector_const_view D1 = gsl_vector_const_subvector(D, 0, N1);
      gsl_vector_const_view D2 = gsl_vector_const_subvector(D, N1, N2);

      gsl_matrix_view T11 = gsl_matrix_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_view T12 = gsl_matrix_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_view T22 = gsl_matrix_submatrix(T, N1, N1, N2, N2);

      gsl_matrix_view Y41 = gsl_matrix_submatrix(A, M, 0, N1, N1);
      gsl_matrix_view Y42 = gsl_matrix_submatrix(A, M, N1, N1, N2);

      gsl_matrix_view m;

      /*
       * Eq. 2: recursively factor
       *
       *       N1           N1
       * N1 [ U11 ] = Q1 [ R11 ] N1
       * N2 [  0  ]      [  0  ] N2
       * M  [ A1  ]      [  0  ] M
       * N1 [ D11 ]      [  0  ] N1
       * N2 [  0  ]      [  0  ] N2
       */
      m = gsl_matrix_submatrix(A, 0, 0, M + N1, N1);
      status = aux_QR_TRD_decomp(&U11.matrix, &m.matrix, &D1.vector, &T11.matrix);
      if (status)
        return status;

      /*
       * Eq. 3:
       *
       *      N2              N2            N2
       * N1 [ R12  ] = Q1^T [ U12 ] = [   U12 - W   ] N1
       * N2 [ U22~ ]        [ U22 ]   [     U22     ] N2
       * M  [ A2~  ]        [ A2  ]   [ A2 - V31 W  ] M
       * N1 [ D12~ ]        [  0  ]   [  - V41 W    ] N1
       * N2 [ D22~ ]        [ D22 ]   [     D22     ] N2
       *
       * where W = T11^T ( U12 + V31^T A2 ), using T12 as temporary storage, and
       *
       *        N1
       * V1 = [  I  ] N1
       *      [  0  ] N2
       *      [ V31 ] M
       *      [ V41 ] N1
       *      [  0  ] N2
       */
      gsl_matrix_memcpy(&T12.matrix, &U12.matrix);                                                    /* W := U12 */
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &A1.matrix, &A2.matrix, 1.0, &T12.matrix);        /* W := U12 + V31^T A2 */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &T11.matrix, &T12.matrix); /* W := T11^T W */
      gsl_matrix_sub(&U12.matrix, &T12.matrix);                                                       /* R12 := U12 - W */
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &A1.matrix, &T12.matrix, 1.0, &A2.matrix);     /* A2 := A2 - V31 W */

      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &Y41.matrix, &T12.matrix); /* W := -V41 W */
      gsl_matrix_memcpy(&Y42.matrix, &T12.matrix);                                                       /* V42 := -431 W */

      /*
       * Eq. 4: factor the "triangle on top of rectangle on top of diagonal" matrix
       *
       * N2 [ U22 ] = Q2~ [ R22 ] N2
       * N1 [ Y12 ]       [  0  ] N1
       * N2 [ D22 ]       [  0  ] N2
       */
      m = gsl_matrix_submatrix(A, 0, N1, M + N, N2);
      status = aux_QR_TRD_decomp(&U22.matrix, &m.matrix, &D2.vector, &T22.matrix);
      if (status)
        return status;

      /*
       * Eq. 13: update T12 := -T11 * V1^T * V2 * T22
       *
       * where:
       *
       *        N1                N2
       * V1 = [  I   ] N1   V2 = [  0   ] N1
       *      [  0   ] N2        [  I   ] N2
       *      [ V31~ ] M         [ V32~ ] M
       *      [ V41~ ] N1        [ V42~ ] N1
       *      [  0   ] N2        [ V52~ ] N2
       *
       * Note: V1^T V2 = V31~^T V32~ + V41~^T V42~
       */

      gsl_matrix_memcpy(&T12.matrix, &Y42.matrix);                                                    /* T12 := V42~ */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &Y41.matrix, &T12.matrix); /* T12 := V41~^T V42~ */
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &A1.matrix, &A2.matrix, 1.0, &T12.matrix);        /* T12 := V31~^T V32~^T + V41~^T V42~ */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &T11.matrix, &T12.matrix); /* T12 := -T11 * T12 */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &T22.matrix, &T12.matrix); /* T12 := T12 * T22 */

      return GSL_SUCCESS;
    }
}

/*
qrtd_householder_transform()
  This routine is an optimized version of
gsl_linalg_householder_transform(), designed for the QR
decomposition of M-by-N matrices of the form:

B = [ U ]
    [ S ]

where U,S are N-by-N upper triangular.
This routine computes a householder transformation (tau,v) of a
x so that P x = [ I - tau*v*v' ] x annihilates x(1:n-1). x will
be a subcolumn of the matrix B, and so its structure will be:

x = [ x0 ] <- 1 nonzero value for the diagonal element of U
    [ y0 ] <- 1 nonzero value for the diagonal element of S

Inputs: v0 - pointer to diagonal element of U
             on input, v0 = x0;
        v1 - on input, pointer to diagonal element of S
             on output, householder vector v
*/

static double
qrtd_householder_transform (double *v0, double *v1)
{
  /* replace v[0:M-1] with a householder vector (v[0:M-1]) and
     coefficient tau that annihilate v[1:M-1] */

  double alpha, beta, tau ;

  /* compute xnorm = || [ 0 ; v ] ||, ignoring zero part of vector */
  double xnorm = fabs(*v1);

  if (xnorm == 0)
    {
      return 0.0; /* tau = 0 */
    }

  alpha = *v0;
  beta = - GSL_SIGN(alpha) * hypot(alpha, xnorm) ;
  tau = (beta - alpha) / beta ;

  {
    double s = (alpha - beta);

    if (fabs(s) > GSL_DBL_MIN)
      {
        *v1 /= s;
        *v0 = beta;
      }
    else
      {
        *v1 *= GSL_DBL_EPSILON / s;
        *v1 /= GSL_DBL_EPSILON;
        *v0 = beta;
      }
  }

  return tau;
}

/*
qrtrd_householder_transform()
  This routine is an optimized version of
gsl_linalg_householder_transform(), designed for the QR
decomposition of M-by-N matrices of the form:

B = [ U ]
    [ A ]
    [ D ]

where U is N-by-N upper triangular A is M-by-N dense and D is N-by-N diagonal.
This routine computes a householder transformation (tau,v) of a
x so that P x = [ I - tau*v*v' ] x annihilates x(1:n-1). x will
be a subcolumn of the matrix B, and so its structure will be:

x = [ x0 ] <- 1 nonzero value for the diagonal element of S
    [ 0  ] <- N - j - 1 zeros, where j is column of matrix in [0,N-1]
    [ x  ] <- M nonzero values for the dense part A

Inputs: v0 - pointer to diagonal element of S
             on input, v0 = x0;
        v  - on input, x vector
             on output, householder vector v
        d  - on input, diagonal element of D
             on output, householder element
*/

static double
qrtrd_householder_transform (double *v0, gsl_vector * v, double *d)
{
  /* replace v[0:M-1] with a householder vector (v[0:M-1]) and
     coefficient tau that annihilate v[1:M-1] */

  double alpha, beta, tau ;

  /* compute xnorm = || [ 0 ; v ; 0 ; d] ||, ignoring zero part of vector */
  double xnorm;

  xnorm = gsl_blas_dnrm2(v);
  xnorm = gsl_hypot(xnorm, *d);

  if (xnorm == 0)
    {
      return 0.0; /* tau = 0 */
    }

  alpha = *v0;
  beta = - GSL_SIGN(alpha) * hypot(alpha, xnorm) ;
  tau = (beta - alpha) / beta ;

  {
    double s = (alpha - beta);

    if (fabs(s) > GSL_DBL_MIN)
      {
        gsl_blas_dscal (1.0 / s, v);
        *d /= s;
        *v0 = beta;
      }
    else
      {
        gsl_blas_dscal (GSL_DBL_EPSILON / s, v);
        gsl_blas_dscal (1.0 / GSL_DBL_EPSILON, v);
        *d *= GSL_DBL_EPSILON / s;
        *d /= GSL_DBL_EPSILON;
        *v0 = beta;
      }
  }

  return tau;
}


#include <viaio/VImage.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <viaio/Vlib.h>
#include "gsl_utils.h"

void whitecov(VImage rho_vol, gsl_matrix_float *Y,
	      gsl_matrix_float* invM, gsl_matrix_float* pinvX,
	      gsl_matrix_float* X, int numlags, int slice)
{

  /* set up variables */
  int n = Y->size1;
  /* these values represent the matlab interval 2:n */
  int k1 = 1;
  int k2 = n-1;
  /* loop counter */
  int i,j;

  /* matrix buffers */
  gsl_matrix_float *result,*buffer;

  /* : X^T */
  gsl_matrix_float* transX = gsl_matrix_float_alloc(X->size2,X->size1);
  gsl_matrix_float_transpose_memcpy(transX,X);

  /* : betahat_ls = pinvX * Y; */
  gsl_matrix_float* betahat_ls = fmat_x_mat(pinvX, Y, NULL);

  /* gsl_blas_sgemm(CblasNoTrans,CblasNoTrans,1.0,pinvX,Y,0.0,betahat_ls); */

  /* : resid = Y - X' * betahat_ls; */
  result = fmat_x_mat(transX, betahat_ls, NULL);
  gsl_matrix_float_sub(Y,result);
  gsl_matrix_float_free(result);

  /* rename */
  gsl_matrix_float* resid = Y;

  if (numlags == 1) {

    /* : Cov0=sum(resid.*resid,1); */
    result = gsl_matrix_float_alloc(resid->size1, resid->size2);
    gsl_matrix_float_memcpy (result, resid);
    gsl_matrix_float_mul_elements (result, result);
    gsl_vector_float* Cov0 = fsum(result, 1,NULL);
    gsl_matrix_float_free(result);

    /* : Cov1=sum(resid(k1,:).*resid(k1-1,:),1); */
    gsl_matrix_float_view sub1 =
      gsl_matrix_float_submatrix(resid, k1,0,k2,resid->size2);
    gsl_matrix_float_view sub2 =
      gsl_matrix_float_submatrix(resid, k1-1, 0,k2,resid->size2);
    result = gsl_matrix_float_alloc(k2, resid->size2);
    gsl_matrix_float_memcpy (result, &sub1.matrix);
    gsl_matrix_float_mul_elements (result, &sub2.matrix);
    gsl_vector_float* Cov1 = fsum(result,1,NULL);
    gsl_matrix_float_free(result);

    /* : Covadj=invM*[Cov0; Cov1]; */
    buffer = gsl_matrix_float_alloc(2,Cov0->size);
    float* pBuffer = buffer->data;
    float* pCov0 = Cov0->data;
    float* pCov1 = Cov1->data;
    for(i=0;i<Cov0->size;i++) {
      *pBuffer++ = *pCov0++;
    }
    for(i=0;i<Cov1->size;i++) {
      *pBuffer++ = *pCov1++;
    }
    gsl_matrix_float* Covadj = fmat_x_mat(invM, buffer, NULL);
    gsl_matrix_float_free(buffer);

    /* :  rho_vol(:,slice,1)=(Covadj(2,:)./ ...
     * (Covadj(1,:)+(Covadj(1,:)<=0)).*(Covadj(1,:)>0))'; */
    int cols = Covadj->size2;
    float* p = Covadj->data;
    float val;
    for(i=0;i<cols;i++) {
      val = *(p+cols) / ((*p <= 0) ? *p +1 : *p) * (*p > 0);
      VPixel(rho_vol, i, slice, 0, VFloat) = val;
      p++;
    }

    gsl_matrix_float_free(Covadj);
    gsl_vector_float_free(Cov0);
    gsl_vector_float_free(Cov1);
  }

  else {
    /* :
     * for lag=0:numlags
     *    Cov(lag+1,:)=sum(resid(1:(n-lag),:).*resid((lag+1):n,:));
     * end
     */
    int lag;
    gsl_matrix_float* Cov = gsl_matrix_float_alloc(numlags+1,resid->size2);
    gsl_vector_float* v;
    for(lag=0;lag<=numlags;lag++) {
      gsl_matrix_float_view sub1 =
	gsl_matrix_float_submatrix(resid,0,0,n-lag,resid->size2);
      gsl_matrix_float_view sub2 =
	gsl_matrix_float_submatrix(resid,lag,0,n-lag,resid->size2 );
      result = gsl_matrix_float_alloc(n-lag, resid->size2);
      gsl_matrix_float_memcpy (result, &sub1.matrix);
      gsl_matrix_float_mul_elements (result, &sub2.matrix);
      v = fsum(result, 1,NULL);
      gsl_matrix_float_free(result);

      memcpy((float*)(Cov->data+(lag*Cov->size2)),
	     (float*)v->data,
	     sizeof(float)*v->size);

      gsl_vector_float_free(v);
    }

    /* :  Covadj=invM*Cov; */
    gsl_matrix_float* Covadj = fmat_x_mat(invM, Cov, NULL);

    /* : rho_vol(:,slice,:)= ( Covadj(0:(numlags+1),:) ...
     *  .*( ones(numlags,1)*((Covadj(1,:)>0)./ ...
     * (Covadj(1,:)+(Covadj(1,:)<=0)))) )'; */
    gsl_matrix_float* line = gsl_matrix_float_alloc(1,Covadj->size2);
    float* pC = Covadj->data;
    float* pB = line->data;
    for(i=0;i<line->size2;i++) {
      *pB = (float)(*pC > 0) / (float)(*pC+(*pC<=0));
      pB++;
      pC++;
    }

    buffer = gsl_matrix_float_alloc(numlags,1);
    gsl_matrix_float_set_all(buffer,1);
    result = fmat_x_mat(buffer, line,NULL);

    gsl_matrix_float_view sub =
      gsl_matrix_float_submatrix(Covadj,1,0,numlags,Covadj->size2);
    gsl_matrix_float_free(buffer);
    buffer = gsl_matrix_float_alloc(result->size1, result->size2);
    gsl_matrix_float_memcpy (buffer, &sub.matrix);
    gsl_matrix_float_mul_elements (buffer, result);

    float* p = buffer->data;
    for(i=0;i<buffer->size1;i++) {
      for(j=0;j<buffer->size2;j++) {
	VPixel(rho_vol,j,slice,i, VFloat) = (float)*p++;
      }
    }

    gsl_matrix_float_free(buffer);
    gsl_matrix_float_free(result);
    gsl_matrix_float_free(line);
    gsl_matrix_float_free(Covadj);
    gsl_matrix_float_free(Cov);
  }

  gsl_matrix_float_free(betahat_ls);
  gsl_matrix_float_free(transX);
}