/* linalg/test_cod.c
 *
 * Copyright (C) 2017 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 <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

static int test_COD_decomp_eps(const gsl_matrix * m, const double eps, const char *desc);
static int test_COD_decomp(gsl_rng *r);
static int test_COD_lssolve_eps(const gsl_matrix * m, const double * actual, const double eps, const char *desc);
static int test_COD_lssolve(void);
static int test_COD_lssolve2_eps(const double lambda, const gsl_matrix * A, const gsl_vector * b, const double eps, const char *desc);
static int test_COD_lssolve2(gsl_rng * r);

/* create a matrix of a given rank */
static int
create_rank_matrix(const size_t rank, gsl_matrix * m, gsl_rng * r)
{
  const size_t M = m->size1;
  const size_t N = m->size2;
  size_t i;
  gsl_vector *u = gsl_vector_alloc(M);
  gsl_vector *v = gsl_vector_alloc(N);

  gsl_matrix_set_zero(m);

  /* add several rank-1 matrices together */
  for (i = 0; i < rank; ++i)
    {
      create_random_vector(u, r);
      create_random_vector(v, r);
      gsl_blas_dger(1.0, u, v, m);
    }

  gsl_vector_free(u);
  gsl_vector_free(v);

  return GSL_SUCCESS;
}

static int
test_COD_decomp_eps(const gsl_matrix * m, const double eps, const char *desc)
{
  int s = 0;
  size_t i, j, M = m->size1, N = m->size2;
  size_t rank;

  gsl_matrix * QRZT = gsl_matrix_alloc(M, N);
  gsl_matrix * Q = gsl_matrix_alloc(M, M);
  gsl_matrix * R = gsl_matrix_alloc(M, N);
  gsl_matrix * QR = gsl_matrix_alloc(M, N);
  gsl_matrix * Z = gsl_matrix_alloc(N, N);
  gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * work = gsl_vector_alloc(N);
  gsl_matrix * lhs = gsl_matrix_alloc(M, N);
  gsl_matrix * rhs = gsl_matrix_alloc(M, N);

  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_matrix_memcpy(QRZT, m);

  s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
  s += gsl_linalg_COD_unpack(QRZT, tau_Q, tau_Z, rank, Q, R, Z);

  /* compute lhs = m P */
  gsl_matrix_memcpy(lhs, m);
  gsl_permute_matrix(perm, lhs);

  /* compute rhs = Q R Z^T */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Q, R, 0.0, QR);
  gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, QR, Z, 0.0, rhs);

  for (i = 0; i < M; i++)
    {
      for (j = 0; j < N; j++)
        {
          double aij = gsl_matrix_get(rhs, i, j);
          double bij = gsl_matrix_get(lhs, i, j);

          gsl_test_rel(aij, bij, eps, "%s (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, j, aij, bij);
        }
    }

  gsl_permutation_free (perm);
  gsl_vector_free(work);
  gsl_vector_free(tau_Q);
  gsl_vector_free(tau_Z);
  gsl_matrix_free(QRZT);
  gsl_matrix_free(lhs);
  gsl_matrix_free(rhs);
  gsl_matrix_free(QR);
  gsl_matrix_free(Q);
  gsl_matrix_free(R);
  gsl_matrix_free(Z);

  return s;
}

static int
test_COD_decomp(gsl_rng *r)
{
  int s = 0;
  size_t N;
  gsl_matrix *m;

  /* test COD decomposition on Hilbert matrices */
  for (N = 1; N <= 12; ++N)
    {
      m = gsl_matrix_alloc(N, N);
      create_hilbert_matrix2(m);

      test_COD_decomp_eps(m, 256.0 * N * GSL_DBL_EPSILON, "COD_decomp hilbert");

      gsl_matrix_free(m);
    }

  /* build some matrices of a given rank and test */

  m = gsl_matrix_alloc(100, 50);
  create_rank_matrix(26, m, r);
  test_COD_decomp_eps(m, 1.0e2 * GSL_DBL_EPSILON, "COD_decomp rank 26");
  gsl_matrix_free(m);

  m = gsl_matrix_alloc(550, 200);
  create_rank_matrix(176, m, r);
  test_COD_decomp_eps(m, 1.0e3 * GSL_DBL_EPSILON, "COD_decomp rank 176");
  gsl_matrix_free(m);

  test_COD_decomp_eps(m35, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp m(3,5)");
  test_COD_decomp_eps(m53, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp m(5,3)");

  test_COD_decomp_eps(s35, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp s(3,5)");
  test_COD_decomp_eps(s53, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp s(5,3)");

  test_COD_decomp_eps(vander2, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp vander(2)");
  test_COD_decomp_eps(vander3, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp vander(3)");
  test_COD_decomp_eps(vander4, 1.0e1 * GSL_DBL_EPSILON, "COD_decomp vander(4)");
  test_COD_decomp_eps(vander12, 1e-3, "COD_decomp vander(12)"); /* FIXME: large tolerance needed */

  return s;
}

static int
test_COD_lssolve_eps(const gsl_matrix * m, const double * actual, const double eps, const char *desc)
{
  int s = 0;
  size_t i, M = m->size1, N = m->size2;

  gsl_vector * lhs = gsl_vector_alloc(M);
  gsl_vector * rhs = gsl_vector_alloc(M);
  gsl_matrix * QRZT  = gsl_matrix_alloc(M, N);
  gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * work = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  size_t rank;

  gsl_matrix_memcpy(QRZT, m);

  for (i = 0; i < M; i++)
    gsl_vector_set(rhs, i, i + 1.0);

  s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
  s += gsl_linalg_COD_lssolve(QRZT, tau_Q, tau_Z, perm, rank, rhs, x, res);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      gsl_test_rel(xi, actual[i], eps,
                   "%s (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, actual[i]);
    }

  /* compute residual r = b - m x */
  if (M == N)
    {
      gsl_vector_set_zero(r);
    }
  else
    {
      gsl_vector_memcpy(r, rhs);
      gsl_blas_dgemv(CblasNoTrans, -1.0, m, x, 1.0, r);
    }

  for (i = 0; i < N; i++)
    {
      double r1 = gsl_vector_get(res, i);
      double r2 = gsl_vector_get(r, i);

      if (fabs(r2) < 1.0e3 * GSL_DBL_EPSILON)
        {
          gsl_test_abs(r1, r2, 10.0 * eps,
                       "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, r1, r2);
        }
      else
        {
          gsl_test_rel(r1, r2, eps,
                       "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, r1, r2);
        }
    }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(tau_Q);
  gsl_vector_free(tau_Z);
  gsl_matrix_free(QRZT);
  gsl_vector_free(rhs);
  gsl_vector_free(lhs);
  gsl_vector_free(work);
  gsl_permutation_free(perm);

  return s;
}

static int
test_COD_lssolve(void)
{
  int s = 0;

  test_COD_lssolve_eps(m53, m53_lssolution, 1.0e4 * GSL_DBL_EPSILON, "COD_lssolve m(5,3)");

  test_COD_lssolve_eps(hilb2, hilb2_solution, 1.0e2 * GSL_DBL_EPSILON, "COD_lssolve hilbert(2)");
  test_COD_lssolve_eps(hilb3, hilb3_solution, 1.0e2 * GSL_DBL_EPSILON, "COD_lssolve hilbert(3)");
  test_COD_lssolve_eps(hilb4, hilb4_solution, 1.0e4 * GSL_DBL_EPSILON, "COD_lssolve hilbert(4)");

  test_COD_lssolve_eps(vander2, vander2_solution, 1.0e1 * GSL_DBL_EPSILON, "COD_lssolve vander(2)");
  test_COD_lssolve_eps(vander3, vander3_solution, 1.0e1 * GSL_DBL_EPSILON, "COD_lssolve vander(3)");
  test_COD_lssolve_eps(vander4, vander4_solution, 1.0e2 * GSL_DBL_EPSILON, "COD_lssolve vander(4)");

  /* rank-1 least squares problem from the 'lin2' test dataset for multifit_nlinear */
  {
    const size_t M = 20;
    const size_t N = 5;

    /* unique minimum norm solution from "Factorize" matlab package */
    const double x_ls[] = { 1.818181818181817e-02, 3.636363636363636e-02, 5.454545454545454e-02,
                            7.272727272727272e-02, 9.090909090909088e-02 };

    gsl_matrix *m = gsl_matrix_alloc(M, N);
    size_t i, j;

    for (i = 0; i < M; ++i)
      {
        for (j = 0; j < N; ++j)
          {
            gsl_matrix_set(m, i, j, (i + 1.0) * (j + 1.0));
          }
      }

    test_COD_lssolve_eps(m, x_ls, 1.0e2 * GSL_DBL_EPSILON, "COD_lssolve lin2");

    gsl_matrix_free(m);
  }

  return s;
}

/* solve: min ||b - A x||^2 + lambda^2 ||x||^2 */
static int
test_COD_lssolve2_eps(const double lambda, const gsl_matrix * A, const gsl_vector * b, const double eps, const char *desc)
{
  int s = 0;
  size_t i, M = A->size1, N = A->size2;

  gsl_vector * lhs = gsl_vector_alloc(M);
  gsl_matrix * QRZT  = gsl_matrix_alloc(M, N);
  gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * work = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * x_aug = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  size_t rank;

  /* form full rank augmented system B = [ A ; lambda*I_N ], f = [ rhs ; 0 ] and solve with QRPT */
  {
    gsl_vector_view v;
    gsl_matrix_view m;
    gsl_permutation *p = gsl_permutation_alloc(N);
    gsl_matrix * B = gsl_matrix_calloc(M + N, N);
    gsl_vector * f = gsl_vector_calloc(M + N);
    gsl_vector * tau = gsl_vector_alloc(N);
    gsl_vector * residual = gsl_vector_alloc(M + N);
    int signum;

    m = gsl_matrix_submatrix(B, 0, 0, M, N);
    gsl_matrix_memcpy(&m.matrix, A);

    m = gsl_matrix_submatrix(B, M, 0, N, N);
    v = gsl_matrix_diagonal(&m.matrix);
    gsl_vector_set_all(&v.vector, lambda);

    v = gsl_vector_subvector(f, 0, M);
    gsl_vector_memcpy(&v.vector, b);

    /* solve: [ A ; lambda*I ] x_aug = [ b ; 0 ] */
    gsl_linalg_QRPT_decomp(B, tau, p, &signum, work);
    gsl_linalg_QRPT_lssolve(B, tau, p, f, x_aug, residual);

    gsl_permutation_free(p);
    gsl_matrix_free(B);
    gsl_vector_free(f);
    gsl_vector_free(tau);
    gsl_vector_free(residual);
  }

  gsl_matrix_memcpy(QRZT, A);

  s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);

  {
    gsl_matrix *S = gsl_matrix_alloc(rank, rank);
    gsl_vector *workr = gsl_vector_alloc(rank);

    s += gsl_linalg_COD_lssolve2(lambda, QRZT, tau_Q, tau_Z, perm, rank, b, x, res, S, workr);

    gsl_matrix_free(S);
    gsl_vector_free(workr);
  }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(x_aug, i);
      gsl_test_rel(xi, yi, eps,
                   "%s (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, yi);
    }

  /* compute residual r = b - A x */
  if (M == N)
    {
      gsl_vector_set_zero(r);
    }
  else
    {
      gsl_vector_memcpy(r, b);
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
    }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(res, i);
      double yi = gsl_vector_get(r, i);

      gsl_test_rel(xi, yi, sqrt(eps),
                   "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, yi);
    }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(x_aug);
  gsl_vector_free(tau_Q);
  gsl_vector_free(tau_Z);
  gsl_matrix_free(QRZT);
  gsl_vector_free(lhs);
  gsl_vector_free(work);
  gsl_permutation_free(perm);

  return s;
}

static int
test_COD_lssolve2(gsl_rng * r)
{
  int s = 0;
  gsl_matrix *m;
  gsl_vector *b;
  size_t M, N;
  const double lambda = 2.3;

  M = 100;
  N = 50;
  m = gsl_matrix_alloc(M, N);
  b = gsl_vector_alloc(M);
  create_rank_matrix(26, m, r);
  create_random_vector(b, r);
  test_COD_lssolve2_eps(lambda, m, b, 1.0e4 * M * GSL_DBL_EPSILON, "COD_lssolve2 rank 26");
  gsl_matrix_free(m);
  gsl_vector_free(b);

  M = 500;
  N = 450;
  m = gsl_matrix_alloc(M, N);
  b = gsl_vector_alloc(M);
  create_rank_matrix(278, m, r);
  create_random_vector(b, r);
  test_COD_lssolve2_eps(lambda, m, b, 1.0e6 * M * GSL_DBL_EPSILON, "COD_lssolve2 rank 278");
  gsl_matrix_free(m);
  gsl_vector_free(b);

  return s;
}


#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_vegas.h>
#include "timer.c"
#include "timer.h"

double g (double *t, size_t dim, void *params);

void timer_start(void);
double timer_stop(void);

int main (void)
{
  double tvegas, tmonte;
  double res, err;
  double distmin = 1.001;
  double distmax = 4.;
  double dist = distmin;
  int np = 20;
  double nt = (distmax - distmin) / ((double) np - 1.);
  size_t dim = 6;
  double x1[] = { 0., 0., 0., 0., 0., 0. };
  double xu[] = { 1., 1., 1., 1., 1., 1. };

  double vegas[20], dipole[20], distance[20], monte[20];
  int f = 0;
  gsl_rng *r = gsl_rng_alloc (gsl_rng_taus2);
  unsigned long seed = 1UL;

  gsl_rng_set (r, seed);

  size_t calls = 1500000;

  //Vegas Integration

  gsl_monte_function G = { &g, dim, &dist };

  gsl_monte_vegas_state *sv = gsl_monte_vegas_alloc (dim);

  gsl_monte_vegas_init (sv);

  timer_start();

  do
    {

      gsl_monte_vegas_integrate (&G, x1, xu, dim, calls / 5, r, sv, &res,
				 &err);
      do
	{
	  gsl_monte_vegas_integrate (&G, x1, xu, dim, calls, r, sv, &res,
				     &err);
	  fflush (stdout);
	}
      while (fabs (gsl_monte_vegas_chisq (sv) - 1.) > .2);
      vegas[f] = res;
      f++;
      dist = dist + nt;
    }
  while (dist <= distmax);

  tvegas = timer_stop();

  gsl_monte_vegas_free (sv);

  // Montecarlo Integration

  double sum = 0.;
  double x[6];
  long i, j, l;

  l = 1E6;
  dist = distmin;

  timer_start();

  for (j = 0; j < np; j++)
    {
      sum = 0.;
      for (i = 0; i < l; i++)
	{
	  for (int k = 0; k < (int) dim; k++)
	    {
	      x[k] = gsl_rng_uniform (r);
	    }
	  sum += g (x, dim, &dist);
	}

      res = sum / (double) l;
      monte[j] = res;
      dist += nt;
    }

  tmonte =  timer_stop();

  gsl_rng_free (r);
  dist = distmin;

  for (int y = 0; y < np; y++)
    {
      dipole[y] = 2. / pow (dist, 3);
      distance[y] = dist;
      dist = dist + nt;
    }

  printf
    ("#    Dist               Vegas           Monte       Dipolapprox\n");
  for (int q = 0; q < np; q++)
    {
      double dd = distance[q];
      double vv = fabs (vegas[q]);
      double hh = fabs (monte[q]);
      double di = fabs (dipole[q]);
      printf ("   %.6f         %.6f       %.6f      %.6f\n", dd, vv, hh, di);
    }

  printf("\n\n");
  printf("Time for Vegas  = %f\n", tvegas);
  printf("Time for Monte  = %f\n", tmonte);
  printf("Speed up Factor = %f\n", tvegas/tmonte);

  return 0;
}


// mimetic_bayes program
// Alessandro Casalino, University of Trento
// For licence informations, see the Github repository license
//
// Compile with: gcc-8 -O2 -DHAVE_INLINE -DGSL_RANGE_CHECK_OFF Ligo_abc_gsl.c -o Ligo_abc_gsl.o -lgsl -lgslcblas -fopenmp
// Run with: ./Ligo_abc_gsl.o
// Test running time: time ./Ligo_abc_gsl.o

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <sys/time.h>
#include <omp.h>

// PROGRAM VARIABLES

// Variables for parallelization with OpenMP
// Info on OpenMP: https://bisqwit.iki.fi/story/howto/openmp/#Syntax
// Enable OpenMP
int OPENMP = 1;
// Number of threads used by OpenMP (can't be bigger than the number of threads available on your computer)
// If 0, the number of threads available on the computer is used
int OMP_THREAD_NUM = 0;

// Number of Chains (for higher efficiency, consider N_CHAIN as a multiple of OMP_THREAD_NUM)
int N_CHAINS = 8;

// Make the Lag test (can make the computation very long!)
int LAG_TEST = 0;
// Ratio of lag function computed with respect to the total number of points in every chain
// Smaller values make the computation faster
double LAG_TEST_STOP_RATIO = 0.001;

// Write (in the terminal) debug informations and informations that might be considered in order to improve results
int DEBUG = 1;

// Write .csv files
#define CSV 1
// Write covariant matrix in .csv file
int CSV_COV = 1;
// Write correlation matrix (Pearson coefficients) in .csv file
int CSV_CORR = 1;

// NUMERICAL VARIABLES
// Chain evaluation dimension (every step found from proposal distribution is multiplied by this quantity)
// This is also used to define the covariance matrix for the Gaussian proposal
double SPEED[] = {1e-1,5e-16,4e-16};
// Mean value for Gaussian proposal (PROPOSAL=1)
double MEAN_P1[] = {0.,0.,0.};
// Number of points in every chain
int N = 1e6;

// PHYSICAL VARIABLES
// Choose to use the constraint on Ct2 in the prior
int CT2_CONSTR = 1;
// Choose the proposal distribution
int PROPOSAL = 1;

// Note that the constraints are defined in rho(pos)

// PROGRAM

typedef struct chain {
  gsl_vector * mean;
  gsl_matrix * covariance;
  double acceptance_ratio;
} chain_res;

inline double cT2 (gsl_vector * pos) {

  double a=gsl_vector_get(pos,0), b=gsl_vector_get(pos,1), c=gsl_vector_get(pos,2);

  return (2.0-2.0*a)/(2.0-2.0*a+b);

}

inline double cS2 (gsl_vector * pos) {

  double a=gsl_vector_get(pos,0), b=gsl_vector_get(pos,1), c=gsl_vector_get(pos,2);

  return (b-c)*(2.0*a-2.0)/(2.0*a-b-2.0)/(4.0-4.0*a-b+3.0*c);

}

inline double MPl2 (gsl_vector * pos) {

  double a=gsl_vector_get(pos,0), b=gsl_vector_get(pos,1), c=gsl_vector_get(pos,2);

  return 4.0-4.0*a-b+3.0*c;

}

inline double rho(gsl_vector * pos){

  double a=gsl_vector_get(pos,0), b=gsl_vector_get(pos,1), c=gsl_vector_get(pos,2);

  // Value of the sound speed squared
  double cs2=cS2(pos);
  // Value of the tensor speed squared
  double ct2=cT2(pos);
  // Value of the Planck mass rescaling factor
  double Mpl2=MPl2(pos);

  // Value of the argument of the Gaussian in the Likelihood
  double delta=fabs(sqrt(ct2)-1.0);

  // This is the experimental sigma on cT2
  double sigma = 4.5e-16;

  double result = 0.;

  // With these ifs I'm introducing the constraints on cT2, cS2 and on the parameters a and c
  // To eliminate these constraints, put CT2_CONSTR=0 at the beginning of the file
  //
  // OLD constraint: CT2_CONSTR==1 && a>=-1. && a<=1. && b>=0. && b<=1. && ct2>=0.0 && ct2<= 1.0 && cs2>=0.0 && cs2<=1.0
  //
  // NEW (correct) constraint: CT2_CONSTR==1 && a>=-1. && a<=1. && Mpl2>=0. && c<=1. && c>=0 && ct2>=0.0 && ct2<= 1.0 && cs2<=0.0
  //
  if (CT2_CONSTR==1 && a>=-1. && a<=1. && c>=0. && c<=1. && ct2>=0. && ct2<=1. && cs2>=0. && cs2<=1.){
    result=exp(-0.5 * pow(delta/sigma,2.0) );
  }
  else if(CT2_CONSTR==1){
    result=exp(-1e90);
  }
  else{
    result=exp(-0.5 * pow(delta/sigma,2.0) );
  }

  return result;
}

chain_res chain_analysis(double (* chain)[3]){

  chain_res result;

  gsl_vector * mean = gsl_vector_alloc(3);

  int i,k,s;
  for(k=0;k<3;k++){
    double mean_temp = 0.;
    for(i=0;i<N;i++){
      mean_temp = mean_temp + chain[i][k];
    }
    gsl_vector_set(mean, k, mean_temp);
  }
  gsl_vector_scale(mean,1./N);

  gsl_matrix * cm = gsl_matrix_alloc(3,3);

  for(k=0;k<3;k++){
    double mean_row = gsl_vector_get(mean,k);
    for(s=0;s<3;s++){
      double mean_column = gsl_vector_get(mean,s);
      double cm_temp = 0.;
      for(i=0;i<N;i++){
        cm_temp = cm_temp + (chain[i][k]-mean_row)*(chain[i][s]-mean_column);
      }
      gsl_matrix_set(cm, k, s, cm_temp);
    }
  }
  gsl_matrix_scale(cm,1./(N-1.));

  result.mean = gsl_vector_alloc(3);
  gsl_vector_memcpy(result.mean,mean);
  result.covariance = gsl_matrix_alloc(3,3);
  gsl_matrix_memcpy(result.covariance,cm);

  gsl_matrix_free(cm);
  gsl_vector_free(mean);

  return result;

}

inline double Gaussian_proposal(double mean, double sigma, double u1, double u2) {

  // Box Muller transform from flat distribution to Guassian distribution
  return mean + sigma * sqrt(-2.*log(u1)) * cos(2.*M_PI*u2);

}

void C_Delta(double (* chain)[3], chain_res results, double f_stop, int chain_number){

  int id_thread = omp_get_thread_num();

  double mean_X = gsl_vector_get(results.mean,0);
  double mean_Y = gsl_vector_get(results.mean,1);
  double mean_Z = gsl_vector_get(results.mean,2);

  double cm_XX = gsl_matrix_get(results.covariance,0,0);
  double cm_YY = gsl_matrix_get(results.covariance,1,1);
  double cm_ZZ = gsl_matrix_get(results.covariance,2,2);

  int stop = N*f_stop;

  double * C_Delta_X; double * C_Delta_Y; double * C_Delta_Z;
  C_Delta_X = (double *) malloc(sizeof(double) * stop);
  C_Delta_Y = (double *) malloc(sizeof(double) * stop);
  C_Delta_Z = (double *) malloc(sizeof(double) * stop);

  int i,j;
  for(i=0;i<stop;i++){
    for(j=0;j<N-i;j++){
      C_Delta_X[i]=C_Delta_X[i]+(chain[j][0]-mean_X)*(chain[j+i][0]-mean_X);
      C_Delta_Y[i]=C_Delta_Y[i]+(chain[j][1]-mean_Y)*(chain[j+i][1]-mean_Y);
      C_Delta_Z[i]=C_Delta_Z[i]+(chain[j][2]-mean_Z)*(chain[j+i][2]-mean_Z);
    }
    C_Delta_X[i]=C_Delta_X[i]/(N-i)/cm_XX;
    C_Delta_Y[i]=C_Delta_Y[i]/(N-i)/cm_YY;
    C_Delta_Z[i]=C_Delta_Z[i]/(N-i)/cm_ZZ;
  }

  FILE *fp;
  char filename[25];

  sprintf(filename,"Ligo_abc_DeltaC_t%d.csv",chain_number);
  fp = fopen (filename, "w+");

  for(i=0;i<stop;i++){
    fprintf(fp, "%d,%.8e,%.8e,%.8e\n", i, C_Delta_X[i], C_Delta_Y[i], C_Delta_Z[i]);
  }

  fclose(fp);

  free(C_Delta_X);
  free(C_Delta_Y);
  free(C_Delta_Z);

}

chain_res LHSampling(int proposal, int chain_number){

  int k;

  int id_thread=omp_get_thread_num();

  const gsl_rng_type * T;
  gsl_rng * r;

  gsl_rng_env_setup();

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  unsigned long seed = time(NULL) + chain_number;

  gsl_rng_set(r, seed);

  gsl_vector * pos_ini = gsl_vector_alloc(3);
  for(k=0;k<3;k++){
    if(k==0){
      double random_number = 2. * gsl_rng_uniform (r) - 1.;
      gsl_vector_set(pos_ini,k,SPEED[k]*random_number);
    }
    else if (k==1){
      double random_number = gsl_rng_uniform (r);
      gsl_vector_set(pos_ini,k,SPEED[k]*random_number);
    }
    else {
      double random_number = gsl_rng_uniform (r);
      gsl_vector_set(pos_ini,k,SPEED[k]*random_number);
    }
  }

  // Here definitions only for the Gaussian proposal distribution
  gsl_vector * pos_mean_p1 = gsl_vector_alloc(3);
  for(k=0;k<3;k++){
    gsl_vector_set(pos_mean_p1, k, MEAN_P1[k]);
  }

  gsl_matrix * S = gsl_matrix_calloc(3,3);
  gsl_matrix * L = gsl_matrix_alloc(3,3);
  gsl_vector * pos_temp2 = gsl_vector_alloc(3);

  gsl_matrix_set(S,0,0,pow(SPEED[0],2.));
  gsl_matrix_set(S,1,1,pow(SPEED[1],2.));
  gsl_matrix_set(S,2,2,pow(SPEED[2],2.));
  gsl_matrix_memcpy(L,S);
  gsl_linalg_cholesky_decomp1(L);
  // End of Gaussian proposal definitions

  gsl_vector * pos = gsl_vector_alloc(3);
  gsl_vector_memcpy(pos,pos_ini);

  #if CSV==1
    FILE *fp;
    char filename[25];
    sprintf(filename,"Ligo_abc_%d.txt",chain_number);
    fp = fopen (filename, "w+");
  #endif

  double (* chain)[3] = malloc(sizeof(* chain) * N);

  int i = 0; int j = 0; int rc = 1;

  while (i<N) {

    double f0 = rho(pos);

    gsl_vector * pos_temp = gsl_vector_calloc(3);

    // NOTE: Introducing the proposal distributions here

    if(proposal==0){

      for(k=0;k<3;k++){

        double random_number = 2. * gsl_rng_uniform (r) - 1.;
        gsl_vector_set(pos_temp, k, SPEED[k]*random_number);

      }

      gsl_vector_add(pos_temp,pos);

    }

    else if(proposal=!0){

      for(k=0;k<3;k++){

        gsl_vector_set(pos_temp2, k, Gaussian_proposal(0., 1., gsl_rng_uniform (r), gsl_rng_uniform (r)));

      }

      gsl_blas_dgemv (CblasNoTrans,
                  1., L, pos_temp2,
                  0., pos_temp);
      gsl_vector_add(pos_temp,pos_mean_p1);
      gsl_vector_add(pos_temp,pos);

    }

    double f1 = rho(pos_temp);

    // NOTE: here we also print data in .csv file (if CSV==1) in Getdist ready format
    // Getdist output with (no commas): repetitions count log(rho) {parameters} {(optional) other outputs - not considered by Getdist}

    if (f1>f0) {

      gsl_vector_memcpy(pos,pos_temp);
      j++;

      #if CSV==1
      fprintf(fp, "%d %.8e %.8e %.8e %.8e\n", rc, -log(rho(pos)), gsl_vector_get(pos,0), gsl_vector_get(pos,1), gsl_vector_get(pos,2));
      #endif

      rc = 1;

    }
    else {

      double ra = gsl_rng_uniform (r);

      if (ra<f1/f0) {
        gsl_vector_memcpy(pos,pos_temp);
        j++;

        #if CSV==1
        fprintf(fp, "%d %.8e %.8e %.8e %.8e\n", rc, -log(rho(pos)), gsl_vector_get(pos,0), gsl_vector_get(pos,1), gsl_vector_get(pos,2));
        #endif

        rc = 1;
      }
      else{
        rc++;
      }

    }

    for(k=0;k<3;k++){
      chain[i][k]=gsl_vector_get(pos,k);
    }

    gsl_vector_free(pos_temp);
    i++;

  }

  chain_res result;

  result = chain_analysis(chain);

  result.acceptance_ratio = (double) j/i;

  if(DEBUG==1 && result.acceptance_ratio<0.05) printf("\n WARNING (chain %d): Acceptance ratio at %e. The chain seems stuck at initial point. \n", chain_number, result.acceptance_ratio);
  if(DEBUG==1 && result.acceptance_ratio>0.8) printf("\n WARNING (chain %d): Acceptance ratio at %e. The chain seems evolving too slowly. \n", chain_number, result.acceptance_ratio);

  // Check of correlation (with lag)
  if(LAG_TEST==1){

    printf(" Computing Lag function..\n");
    if(DEBUG==1) printf(" Note: This computation can take a lot of computational time.\n");
    if(DEBUG==1) printf("       Lower LAG_TEST_STOP_RATIO for faster results.\n");
    C_Delta(chain,result,LAG_TEST_STOP_RATIO, chain_number);

  }

  #if CSV==1
    if(CSV_COV==1){
      FILE *fp2;
      sprintf(filename,"Ligo_abc_%d.covmat",chain_number);
      fp2 = fopen (filename, "w+");
      fprintf(fp2, "%.8e %.8e %.8e\n", gsl_matrix_get(result.covariance,0,0),gsl_matrix_get(result.covariance,0,1),gsl_matrix_get(result.covariance,0,2));
      fprintf(fp2, "%.8e %.8e %.8e\n", gsl_matrix_get(result.covariance,1,0),gsl_matrix_get(result.covariance,1,1),gsl_matrix_get(result.covariance,1,2));
      fprintf(fp2, "%.8e %.8e %.8e\n", gsl_matrix_get(result.covariance,2,0),gsl_matrix_get(result.covariance,2,1),gsl_matrix_get(result.covariance,2,2));
      fclose(fp2);
    }
  #endif

  #if CSV==1
    if(CSV_CORR==1){
      FILE *fp3;
      sprintf(filename,"Ligo_abc_%d.corr",chain_number);
      fp3 = fopen (filename, "w+");
      fprintf(fp3, "%.8e %.8e %.8e\n", 1.,gsl_matrix_get(result.covariance,0,1)/sqrt(fabs(gsl_matrix_get(result.covariance,0,0)))/sqrt(fabs(gsl_matrix_get(result.covariance,1,1))),gsl_matrix_get(result.covariance,0,2)/sqrt(fabs(gsl_matrix_get(result.covariance,0,0)))/sqrt(fabs(gsl_matrix_get(result.covariance,2,2))));
      fprintf(fp3, "%.8e %.8e %.8e\n", gsl_matrix_get(result.covariance,1,0)/sqrt(fabs(gsl_matrix_get(result.covariance,0,0)))/sqrt(fabs(gsl_matrix_get(result.covariance,1,1))),1.,gsl_matrix_get(result.covariance,1,2)/sqrt(fabs(gsl_matrix_get(result.covariance,1,1)))/sqrt(fabs(gsl_matrix_get(result.covariance,2,2))));
      fprintf(fp3, "%.8e %.8e %.8e\n", gsl_matrix_get(result.covariance,2,0)/sqrt(fabs(gsl_matrix_get(result.covariance,0,0)))/sqrt(fabs(gsl_matrix_get(result.covariance,2,2))),gsl_matrix_get(result.covariance,2,1)/sqrt(fabs(gsl_matrix_get(result.covariance,1,1)))/sqrt(fabs(gsl_matrix_get(result.covariance,2,2))),1.);
      fclose(fp3);
    }
  #endif

  // Free all the variables
  free(chain);

  gsl_vector_free(pos_ini);
  gsl_vector_free(pos_mean_p1);
  gsl_vector_free(pos);
  gsl_rng_free(r);

  gsl_vector_free(pos_temp2);
  gsl_matrix_free(S);
  gsl_matrix_free(L);

  #if CSV==1
    fclose(fp);
  #endif

  return result;

}

inline double R(double sigma2_chain, double sigma2_mean){

  int noc = N_CHAINS;

  return sqrt(((N-1.)/N*sigma2_chain + (noc+1.)/N/noc*sigma2_mean)/sigma2_chain);

}

gsl_vector * GR_Test(chain_res * chain_results) {

  int i, k;
  int noc = N_CHAINS;

  gsl_vector * mean = gsl_vector_alloc(3);
  gsl_vector * sigma2_chain = gsl_vector_alloc(3);
  gsl_vector * sigma2_mean = gsl_vector_alloc(3);

  for(k=0;k<3;k++){
    double mean_temp = 0.;
    double sigma2_temp = 0.;
    for(i=0;i<noc;i++){
      mean_temp = mean_temp + gsl_vector_get(chain_results[i].mean,k)/noc;
      sigma2_temp = sigma2_temp + fabs(gsl_matrix_get(chain_results[i].covariance,k,k))/noc;
    }
    gsl_vector_set(mean, k, mean_temp);
    gsl_vector_set(sigma2_chain, k, sigma2_temp);
  }

  for(k=0;k<3;k++){
    double mean_temp = 0.;
    double sigma2_temp = 0.;
    for(i=0;i<noc;i++){
      sigma2_temp = sigma2_temp + pow(gsl_vector_get(mean,k)-gsl_vector_get(chain_results[i].mean,k),2.)*N/(noc-1.);
    }
    gsl_vector_set(sigma2_mean, k, sigma2_temp);
  }

  double R_a = R(gsl_vector_get(sigma2_chain,0), gsl_vector_get(sigma2_mean,0));
  double R_b = R(gsl_vector_get(sigma2_chain,1), gsl_vector_get(sigma2_mean,1));
  double R_c = R(gsl_vector_get(sigma2_chain,2), gsl_vector_get(sigma2_mean,2));

  printf("\n Gelman-Rubin test.\n");
  printf(" \t- R_a : %f\n", R_a);
  printf(" \t- R_b : %f\n", R_b);
  printf(" \t- R_c : %f\n", R_c);

  if(DEBUG==1) printf(" Converge if (approximately) R < 1.2. For better results R < 1.01.\n");

  printf("\n ------------------------------------------------------------------------ \n");

  gsl_vector_free(mean);
  gsl_vector_free(sigma2_chain);
  gsl_vector_free(sigma2_mean);

  gsl_vector * result = gsl_vector_alloc(3);
  gsl_vector_set(result,0,R_a);gsl_vector_set(result,1,R_b);gsl_vector_set(result,2,R_c);

  return result;

}

void init_msg(int proposal){

  printf(" Starting the MCMC computation with Metropolis-Hastings algorithm.\n");
  printf(" Properties of the chains:\n");
  if(proposal==0){
    printf(" \t- Proposal distribution: flat.\n");
  }
  if(proposal!=0){
    printf(" \t- Proposal distribution: Gaussian.\n");
  }
  if(OPENMP==0){
    printf(" \t- The chain will be %d points long.\n", N);
  }
  if(OPENMP==1){
    printf(" \t- Each of the %d chains will be %d points long.\n", N_CHAINS, N);
  }
  printf(" ------------------------------------------------------------------------ \n");

}

int main(){

  int proposal=PROPOSAL;

  printf(" ------------------------------------------------------------------------ \n");
  printf("\t       mimetic_bayes program ");
  if(DEBUG==1) printf("(with DEBUG mode on)");
  printf("\n");
  printf(" ------------------------------------------------------------------------ \n");

  if(OPENMP==1){

    int i;

    chain_res * chain_results;
    chain_results = (chain_res *) malloc(sizeof(chain_res) * N_CHAINS);

    if(OMP_THREAD_NUM==0) OMP_THREAD_NUM = omp_get_max_threads();
    if(DEBUG==1) {
      printf(" Info on OpenMP on this computer. \n");
      printf(" \t- Number of processors available = %d\n", omp_get_num_procs ( ) );
      printf(" \t- Number of threads              = %d\n", omp_get_max_threads ( ) );
      printf(" \t- Number of working threads      = %d\n", OMP_THREAD_NUM );
      printf(" Specify the number of working threads in .c file.\n");
      printf(" ------------------------------------------------------------------------ \n");
    }

    printf(" Number of chains requested = %d. \n", N_CHAINS);

    printf(" ------------------------------------------------------------------------ \n");

    if(OMP_THREAD_NUM>omp_get_max_threads()){
      printf(" Fatal Error. Can't set number of threads (OMP_THREAD_NUM = %d) bigger than the available number of threads (%d). \n",OMP_THREAD_NUM, omp_get_max_threads());
      printf(" \t Change OMP_THREAD_NUM and try again. \n");
      printf(" The program will be terminated. \n");
      exit(0);
    }

    omp_set_num_threads(OMP_THREAD_NUM);

    init_msg(proposal);

    printf("\n Doing parallelized workflow. Please wait. \n\n");
    if(DEBUG==1) printf(" NOTE: Some WARNING might be shown. \n");
    if(DEBUG==1) printf("       Consider to stop the program and check the parameters if WARNINGs appear. \n\n");

    #pragma omp parallel for
    for(i=1;i<=N_CHAINS;i++)
    {
      int current_thread = omp_get_thread_num();
      if(DEBUG==1) printf(" Thread %d working on chain %d .\n", current_thread, i);
      chain_results[i-1]=LHSampling(proposal, i);
      if(DEBUG==1) printf(" Thread %d finished working on chain %d. \n", current_thread, i);
    }

    printf(" ------------------------------------------------------------------------ \n");

    for(i=0;i<N_CHAINS;i++){

      printf(" CHAIN %d RESULTS. \n", i+1);

      printf("\n Acceptance ratio: %f \n\n", chain_results[i].acceptance_ratio);
      printf(" Results: Mean value pm sqrt(covariant_ii).\n");
      printf(" \ta: %e pm %e\n", gsl_vector_get(chain_results[i].mean,0), sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,0,0))));
      printf(" \tb: %e pm %e\n", gsl_vector_get(chain_results[i].mean,1), sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,1,1))));
      printf(" \tc: %e pm %e\n", gsl_vector_get(chain_results[i].mean,2), sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,2,2))));

      printf("\n Results: Correlation coefficients.\n");
      printf(" \tab: %e \n", gsl_matrix_get(chain_results[i].covariance,0,1)/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,0,0)))/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,1,1))));
      printf(" \tac: %e \n", gsl_matrix_get(chain_results[i].covariance,0,2)/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,0,0)))/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,2,2))));
      printf(" \tbc: %e \n", gsl_matrix_get(chain_results[i].covariance,1,2)/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,1,1)))/sqrt(fabs(gsl_matrix_get(chain_results[i].covariance,2,2))));
      printf(" These are the Pearson coefficients : covariant_ij / sigma_i sigma_j\n\n");

      printf(" ------------------------------------------------------------------------ \n");

    }

    gsl_vector * GR_test_result = gsl_vector_alloc(3);
    GR_test_result = GR_Test(chain_results);
    double R_a = gsl_vector_get(GR_test_result,0);
    double R_b = gsl_vector_get(GR_test_result,1);
    double R_c = gsl_vector_get(GR_test_result,2);

    if(DEBUG==1){

      int k = 0;

      double C00 = sqrt(gsl_matrix_get(chain_results[0].covariance,0,0));
      double C11 = sqrt(gsl_matrix_get(chain_results[0].covariance,1,1));
      double C22 = sqrt(gsl_matrix_get(chain_results[0].covariance,2,2));

      printf("\n INFORMATIONS for better convergence. \n\n");
      printf(" WARNING: These are hints based on the results and thumb rules. \n");
      printf("          Follow these hints at your own risk. \n\n");

      if(R_a>1.2||R_b>1.2||R_c>1.2) {
        printf(" - An R_i>1.2 : Chains are sampling different parameter spaces.\n");
        printf("                There might be some troubles in your SPEED vector definition.\n");
        k++;
      }
      if((R_a<1.2&&R_a>1.01)||(R_b<1.2&&R_b>1.01)||(R_c<1.2&&R_c>1.01)) {
        printf(" - An R_i>1.01 : The results might be good, but the results can be improved.\n");
        printf("                 Try different values for the SPEED vector to achieve better results.\n");
        k++;
      }

      double p_value = 0.5;
      if(fabs(SPEED[0]-C00/4.)/fabs(SPEED[0]+C00/4.)>p_value){
        printf(" - From covariance matrix: Try with a SPEED[0] = %e .\n",C00/4.);
        k++;
      }
      if(fabs(SPEED[1]-C11/4.)/fabs(SPEED[1]+C11/4.)>p_value){
        printf(" - From covariance matrix: Try with a SPEED[1] = %e .\n",C11/4.);
        k++;
      }
      if(fabs(SPEED[2]-C22/4.)/fabs(SPEED[2]+C22/4.)>p_value){
        printf(" - From covariance matrix: Try with a SPEED[2] = %e .\n",C22/4.);
        k++;
      }

      int j=0, r=0;
      for(i=0;i<N_CHAINS;i++){
        if(chain_results[i].acceptance_ratio>0.5) j++;
        if(chain_results[i].acceptance_ratio<0.1) r++;
      }
      if(j>0){
        printf(" - From acceptance rations: Maybe some acceptance ratios are too big.\n");
        printf("                            The configuration space might be explored slowly.\n");
        printf("                            This is not necessarily a problem,\n");
        printf("                            but if the results are not good:\n");
        printf("                            try with different SPEED vector values.\n");
        k++;
      }
      if(r>0){
        printf(" - From acceptance rations: Maybe some acceptance ratios are too small.\n");
        printf("                            The configuration space might be explored very inefficiently.\n");
        printf("                            This is not necessarily a problem,\n");
        printf("                            but if the results are not good:\n");
        printf("                            try with different SPEED vector values.\n");
        k++;
      }

      double mean_a = gsl_vector_get(chain_results[0].mean,0);
      double sigma_a = sqrt(fabs(gsl_matrix_get(chain_results[0].covariance,0,0)));
      if(PROPOSAL==1 && (fabs(mean_a-MEAN_P1[0])/fabs(mean_a+MEAN_P1[0])>3.*sigma_a)) {
        printf(" - A burn-in removal might be needed: Try with MEAN_P1[0] = %e.\n",mean_a);
        printf("   Note that this can improve but also worsen the results.\n");
        printf("   Revert the result and make a burn-in removal instead if the results get worse.\n");
      }
      double mean_b = gsl_vector_get(chain_results[0].mean,1);
      double sigma_b = sqrt(fabs(gsl_matrix_get(chain_results[0].covariance,1,1)));
      if(PROPOSAL==1 && (fabs(mean_b-MEAN_P1[1])/fabs(mean_b+MEAN_P1[1])>3.*sigma_b)) {
        printf(" - A burn-in removal might be needed: Try with MEAN_P1[1] = %e.\n",mean_b);
        printf("   Note that this can improve but also worsen the results.\n");
        printf("   Revert the result and make a burn-in removal instead if the results get worse.\n");
      }
      double mean_c = gsl_vector_get(chain_results[0].mean,2);
      double sigma_c = sqrt(fabs(gsl_matrix_get(chain_results[0].covariance,2,2)));
      if(PROPOSAL==1 && (fabs(mean_a-MEAN_P1[2])/fabs(mean_a+MEAN_P1[2])>3.*sigma_c)) {
        printf(" - A burn-in removal might be needed: Try with MEAN_P1[2] = %e .\n",mean_c);
        printf("   Note that this can improve but also worsen the results.\n");
        printf("   Revert the result and make a burn-in removal instead if the results get worse.\n");
      }

      if(k==0){
        printf(" Congratulations! The convercence seems good. \n");
      }

      printf("\n ------------------------------------------------------------------------ \n");

    }

    free(chain_results);

  }
  else{

    init_msg(proposal);

    LHSampling(proposal, 0);
    printf(" ------------------------------------------------------------------------ \n");

  }

  return 0;

}


/* cdf/tdistinv.c
 *
 * Copyright (C) 2007, 2010 Brian Gough
 * Copyright (C) 2002 Jason H. Stover.
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

#include <stdio.h>

static double
inv_cornish_fisher (double z, double nu)
{
  double a = 1 / (nu - 0.5);
  double b = 48.0 / (a * a);

  double cf1 = z * (3 + z * z);
  double cf2 = z * (945 + z * z * (360 + z * z * (63 + z * z * 4)));

  double y = z - cf1 / b + cf2 / (10 * b * b);

  double t = GSL_SIGN (z) * sqrt (nu * expm1 (a * y * y));

  return t;
}

double
gsl_cdf_tdist_Pinv (const double P, const double nu)
{
  double x, ptail;

  if (P == 1.0)
    {
      return GSL_POSINF;
    }
  else if (P == 0.0)
    {
      return GSL_NEGINF;
    }

  if (nu == 1.0)
    {
      x = tan (M_PI * (P - 0.5));
      return x;
    }
  else if (nu == 2.0)
    {
      x = (2 * P - 1) / sqrt (2 * P * (1 - P));
      return x;
    }

  ptail = (P < 0.5) ? P : 1 - P;

  if (sqrt (M_PI * nu / 2) * ptail > pow (0.05, nu / 2))
    {
      double xg = gsl_cdf_ugaussian_Pinv (P);
      x = inv_cornish_fisher (xg, nu);
    }
  else
    {
      /* Use an asymptotic expansion of the tail of integral */

      double beta = gsl_sf_beta (0.5, nu / 2);

      if (P < 0.5)
        {
          x = -sqrt (nu) * pow (beta * nu * P, -1.0 / nu);
        }
      else
        {
          x = sqrt (nu) * pow (beta * nu * (1 - P), -1.0 / nu);
        }

      /* Correct nu -> nu/(1+nu/x^2) in the leading term to account
         for higher order terms. This avoids overestimating x, which
         makes the iteration unstable due to the rapidly decreasing
         tails of the distribution. */

      x /= sqrt (1 + nu / (x * x));
    }

  {
    double dP, phi;
    unsigned int n = 0;

  start:
    dP = P - gsl_cdf_tdist_P (x, nu);
    phi = gsl_ran_tdist_pdf (x, nu);

    if (dP == 0.0 || n++ > 32)
      goto end;

    {
      double lambda = dP / phi;
      double step0 = lambda;
      double step1 = ((nu + 1) * x / (x * x + nu)) * (lambda * lambda / 4.0);

      double step = step0;

      if (fabs (step1) < fabs (step0))
        {
          step += step1;
        }

      if (P > 0.5 && x + step < 0)
        x /= 2;
      else if (P < 0.5 && x + step > 0)
        x /= 2;
      else
        x += step;

      if (fabs (step) > 1e-10 * fabs (x))
        goto start;
    }

  end:
    if (fabs(dP) > GSL_SQRT_DBL_EPSILON * P)
      {
        GSL_ERROR_VAL("inverse failed to converge", GSL_EFAILED, GSL_NAN);
      }

    return x;
  }
}

double
gsl_cdf_tdist_Qinv (const double Q, const double nu)
{
  double x, qtail;

  if (Q == 0.0)
    {
      return GSL_POSINF;
    }
  else if (Q == 1.0)
    {
      return GSL_NEGINF;
    }

  if (nu == 1.0)
    {
      x = tan (M_PI * (0.5 - Q));
      return x;
    }
  else if (nu == 2.0)
    {
      x = (1 - 2 * Q) / sqrt (2 * Q * (1 - Q));
      return x;
    }

  qtail = (Q < 0.5) ? Q : 1 - Q;

  if (sqrt (M_PI * nu / 2) * qtail > pow (0.05, nu / 2))
    {
      double xg = gsl_cdf_ugaussian_Qinv (Q);
      x = inv_cornish_fisher (xg, nu);
    }
  else
    {
      /* Use an asymptotic expansion of the tail of integral */

      double beta = gsl_sf_beta (0.5, nu / 2);

      if (Q < 0.5)
        {
          x = sqrt (nu) * pow (beta * nu * Q, -1.0 / nu);
        }
      else
        {
          x = -sqrt (nu) * pow (beta * nu * (1 - Q), -1.0 / nu);
        }

      /* Correct nu -> nu/(1+nu/x^2) in the leading term to account
         for higher order terms. This avoids overestimating x, which
         makes the iteration unstable due to the rapidly decreasing
         tails of the distribution. */

      x /= sqrt (1 + nu / (x * x));
    }

  {
    double dQ, phi;
    unsigned int n = 0;

  start:
    dQ = Q - gsl_cdf_tdist_Q (x, nu);
    phi = gsl_ran_tdist_pdf (x, nu);

    if (dQ == 0.0 || n++ > 32)
      goto end;

    {
      double lambda = - dQ / phi;
      double step0 = lambda;
      double step1 = ((nu + 1) * x / (x * x + nu)) * (lambda * lambda / 4.0);

      double step = step0;

      if (fabs (step1) < fabs (step0))
        {
          step += step1;
        }

      if (Q < 0.5 && x + step < 0)
        x /= 2;
      else if (Q > 0.5 && x + step > 0)
        x /= 2;
      else
        x += step;

      if (fabs (step) > 1e-10 * fabs (x))
        goto start;
    }
  }

end:

  return x;
}


#ifndef _GP_H_
#define _GP_H_

#include "armadillo"
using namespace arma;
#include "myTypes.h"

#include "KernelFunction.h"
#include "MeanFunction.h"

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

double f_eval_mean(const gsl_vector *x, void *param);
void df_eval_mean(const gsl_vector *x, void *param, gsl_vector *g);
void fdf_eval_mean(const gsl_vector *x, void *param, double *f, gsl_vector *g);

double f_eval_kernel(const gsl_vector *x, void *param);
void df_eval_kernel(const gsl_vector *x, void *param, gsl_vector *g);
void fdf_eval_kernel(const gsl_vector *x, void *param, double *f, gsl_vector *g);

double f_eval_noise(const gsl_vector *x, void *param);
void df_eval_noise(const gsl_vector *x, void *param, gsl_vector *g);
void fdf_eval_noise(const gsl_vector *x, void *param, double *f, gsl_vector *g);

class GP {
public:
  GP(REAL s2_n, KernelFunction *kernel, MeanFunction *mean);

  void SetTraining(const Mat<REAL>& X, const Row<REAL> &y);
  void AddTraining(const Mat<REAL>& X, const Row<REAL> &y);
  void Predict(const Mat<REAL> &Xs, Row<REAL> &mu);
  void Predict(const Mat<REAL> &Xs, Row<REAL> &mu, Row<REAL> &var);
  void Predict(const Col<REAL> &Xs, REAL &mu);
  void Predict(const Col<REAL> &Xs, REAL &mu, REAL &var);

  void PredictGradient(const Col<REAL> &Xs, Col<REAL> &grad);
  void PredictGradient(const Col<REAL> &Xs, Col<REAL> &grad, Col<REAL> &vargrad);

  void ComputeAlpha();
  void ComputeChol();
  void ComputeW();
  REAL ComputeLikelihood();

  void SetKernelFuncParams(const Col<REAL>& param);
  void SetMeanFuncParams(const Col<REAL>& param);
  void SetNoise(const REAL &s2_n);

  void GradLikelihoodMeanParams(Col<REAL> &grad);
  void GradLikelihoodKernelParams(Col<REAL> &grad);
  void GradLikelihoodNoise(Col<REAL> &grad);

  void HessianLikelihoodMeanParams(Mat<REAL> &hessian);

  void OptimizeNoiseParam(REAL &noise_param, int max_iterations=10);
  void OptimizeMeanParam(Col<REAL> &mean_param, int max_iterations=10);
  void OptimizeKernelParam(Col<REAL> &kernel_param, int max_iterations=10);

  inline Mat<REAL> &GetTrainingData() { return X; }
  inline KernelFunction *GetKernelFunction() { return this->kernel; }
  inline MeanFunction *GetMeanFunction() { return this->mean; }

protected:
  KernelFunction *kernel;
  MeanFunction *mean;
  Mat<REAL> K;
  Mat<REAL> W;
  Mat<REAL> X;
  Mat<REAL> L;
  Row<REAL> y;
  Row<REAL> meanvals;
  Col<REAL> alpha;
  REAL s2_n;
  REAL loglikelihood;

  bool need_to_compute_alpha;
  bool need_to_compute_chol;
  bool need_to_compute_w;

  void MatrixMap(Mat<REAL> &matrix, const Mat<REAL> &a, const Mat<REAL> &b);
};

#endif


#include <cerrno>
#include <cstdio>
#include <cstdlib>
#include <sys/stat.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <time.h>

#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_math.h>
#include <math.h>       /* pow */
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>

// functions particular to bh_clean.cpp

/* Time as a function of scale factor in seconds */
// integrand
double timeofa_int(double ap, void * params) {
  double integrand = 1./(ap*hubble(om,orad,ap));
  return integrand;
}

double timeofa(double a){
	gsl_integration_workspace * w  = gsl_integration_workspace_alloc (1000);
	double result, error;
	gsl_function F;
	F.function = &timeofa_int;
	gsl_integration_qags (&F, ai, a, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
	return result;
}

/* scale factor as a function of time */
// fill array to then spline
void aoftime(arrays_T xxyy){
		for(int i=0; i<1001; i++){
			double a = ai*exp(i*log(0.9999/ai)/(1000.));
			(*xxyy).xx[i] = timeofa(a);
			(*xxyy).yy[i] = a;
		}
		// set final time
		(*xxyy).xx[1001] = 1./hubble(om,orad,1.);
		(*xxyy).yy[1001] = 1.;
}

/* Log10[Mass] as a function of scale factor  --  not a step function*/
// M = 10^lgm ,   a is the scale factor , rem= false for no planck mass remnants
double bhmasslg(double lgm, double a, bool rem){
	// If M_i^3 <= 3 K M_p^4 t  then the black hole should have  0 mass ---> Log10[M_i] <= 1/3 (Log10[ 3 K M_p^4] + Log10[t]) = 1/3 ( 17.803 + Log10[t])
	double logdect = 1./3. * (17.803 + log10(timeofa(a)));
	if (lgm>logdect) {
		return log10(pow(pow(10., lgm*3.) - decfac*timeofa(a), 1.0/3.));
	}
	else{
		if (!rem) {
			return -100.;
		}
		else{
			return lgmp;
		}
	}
}

// Initial mass from final mass. Any bh that has decayed completely earlier than a will be mapped to decfac*timeofa(a)^(1/3)
// (There's no way of knowing with certainty what its initial mass is if its final mass is 0)
double bhmasslgi(double lgm, double a, bool rem){
  double logdect = 1./3. * (17.803 + log10(timeofa(a)));
  double mymasslg = log10(pow(10., lgm*3.) + decfac*timeofa(a))/3.;
  if(lgm<logdect ){
    return -100;
  }
  else if(mymasslg>lgmax){
		return -100;
    }
  else{
    return mymasslg;
  }
}

/* Calculate normalisation for psibroad */
// normalisation integrand --- see equation 3.4 for example
// Params are peak mass (as proxy for M_s)
// Normalisation = 1/ Integrate[ 10^psilg dM ] =   1/ Integrate[ 10^(psilg + lgm) x Ln[10] dlgm ]

double normlg_int(double lgm, void * params) {
	myparam_type pars = *(myparam_type *)(params);
	double peakm = pars.peakm;
	double expt = lgm + psibroadlg(lgm,peakm);
	double integrand =pow(10.,expt);
  return integrand;
}

// Normalisation = 10^normlg
double normlg(double peakm){
	struct myparam_type pars = {peakm, 1., ai, false};

	gsl_integration_workspace * w  = gsl_integration_workspace_alloc (1000);
	double result, error;
	gsl_function F;
	F.function = &normlg_int;
	F.params = &pars;
	gsl_integration_qags (&F, lgmp, lgmax, 0., 1e-5, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
	return  log10(1./(log(10.)*result));
}

/* Calculate normalisation for psi_mono */
// normalisation integrand --- see equation 3.4 for example
// Params are peak mass
// Normalisation = 1/ Integrate[ 10^psilg dM ] =   1/ Integrate[ 10^(psilg + lgm) x Ln[10] dlgm ]

double normlg_int_mono(double lgm, void * params) {
	myparam_type pars = *(myparam_type *)(params);
	double peakm = pars.peakm;
	double expt = lgm + psilowlg(lgm,peakm);
	double integrand =pow(10.,expt);
  return integrand;
}

// Normalisation = 10^normlg
double normlg_mono(double peakm){
	struct myparam_type pars = {peakm, 1., ai, false};

	gsl_integration_workspace * w  = gsl_integration_workspace_alloc (1000);
	double result, error;
	gsl_function F;
	F.function = &normlg_int_mono;
	F.params = &pars;
	gsl_integration_qags (&F, lgmp, lgmax, 0., 1e-5, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
	return  log10(1./(log(10.)*result));
}


// Copyright 2018, Kevin Lang, Oath Research

/*
gcc -Wall -pedantic -Iinclude -Llib -o multi_reading_linsolve multi_reading_linsolve.c -lgsl -lgslcblas
*/

#include <stdio.h>
#include <assert.h>
#include <gsl/gsl_linalg.h>

/*****************************************************************************/

#ifdef EXAMPLE_FOR_INTUITION

#define NUMEQNS 5
#define NUMVARS 4
double a_data[] =
    { 0.18, 0.60, 0.57, 0.96,
      0.41, 0.24, 0.99, 0.58,
      0.14, 0.30, 0.97, 0.66,
      0.51, 0.13, 0.19, 0.85,
      0.65, 0.43, 1.16, 1.51
    };

double b_data[] =
  { 1.0, 2.0, 3.0, 4.0,
    7.0
 };

#endif

/*****************************************************************************/

void read_a_data (int num_eqns, int num_vars, double * a_data) {
  int i, j, nxt;
  double tmp;
  nxt = 0;
  for (i = 0; i < num_eqns; i++) {
    for (j = 0; j < num_vars; j++) {
      int got = scanf ("%lf", &tmp);
      assert (got == 1);
      a_data[nxt++] = tmp;
    }
  }
  assert (nxt == num_eqns * num_vars);
}

/*****************************************************************************/

void read_b_data (int num_eqns, double * b_data) {
  int i, nxt;
  double tmp;
  nxt = 0;
  for (i = 0; i < num_eqns; i++) {
    int got = scanf ("%lf", &tmp);
    assert (got == 1);
    b_data[nxt++] = tmp;
  }
  assert (nxt == num_eqns);
}

/*****************************************************************************/

int main (int argc, char ** argv)
{
  if (argc != 3) {
    fprintf (stderr, "Usage: %s num_eqns num_vars < data_file\n", argv[0]);
    fprintf (stderr, "    The data_file contains a matrix followed by a target vector.\n");
    fprintf (stderr, "    The matrix should be in equation major order.\n");
    fprintf (stderr, "    This version solves multiple problems with matching matrix dimensions.\n");
    fprintf (stderr, "    Each problem should be preceding by a '1'.\n");
    fprintf (stderr, "    The last problem should be followed by a '0'.\n");
    return (-1);
  }

  int num_eqns  = atoi(argv[1]);
  int num_vars  = atoi(argv[2]);

  double * a_data = (double *) malloc (num_eqns * num_vars * sizeof(double));
  assert (a_data != NULL);

  double * b_data = (double *) malloc (num_eqns * sizeof(double));
  assert (b_data != NULL);

  while (1) {
    int keep_going = 0;
    int got = scanf ("%d", &keep_going);
    assert (got == 1);
    if (keep_going == 0) return 0;

    read_a_data (num_eqns, num_vars, a_data);
    read_b_data (num_eqns, b_data);

    gsl_matrix_view m = gsl_matrix_view_array (a_data, num_eqns, num_vars);
    gsl_vector *x     = gsl_vector_alloc (num_vars);
    gsl_vector *tau   = gsl_vector_alloc (num_vars);
    gsl_vector *residual = gsl_vector_alloc (num_eqns);
    gsl_vector_view b = gsl_vector_view_array (b_data, num_eqns);
    int s;

    s = gsl_linalg_QR_decomp (&m.matrix, tau);
    s = gsl_linalg_QR_lssolve (&m.matrix, tau, &b.vector, x, residual);
    gsl_vector_fprintf (stdout, x, "%.15g"); fflush (stdout);
    gsl_vector_free (x);
    gsl_vector_free (tau);
    gsl_vector_free (residual);
  }

  return 0;
}

    //  printf ("residual = \n"); gsl_vector_fprintf (stdout, residual, "%g");
    //      printf("solver got A: %d %d %.19g\n", i, j, tmp);
    //    printf("solver got B: %d %.19g\n", i, tmp);


#ifndef SOURCE_LINGAUSS_H_
#define SOURCE_LINGAUSS_H_

// *** source/lin_gauss.h ***
// Author: Kevin Wolz, date: 06/2018
//
// This program defines the class "LinGauss" describing linear Gaussian models.

#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>
#include <stdlib.h>
#include <sys/stat.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_integration.h>
#include "utils.h"

class LinGauss {
 public:
	unsigned int datdim_;
	unsigned int pardim_;
	// Ensures that g_matrix_, covar_matrix_, prior_matrix_, prior_param_values_,
	// data_mean_values_ are initialized and != 0.
	bool initialized_;
	// Ensures that cover_matrix_ is inverted and stored in inv_cover_matrix_.
	bool inverted_;
	// Ensures that this instance of LinGauss is initialized and fisher_matrix_
	// is calculated.
	bool fisherized_;
	gsl_matrix* g_matrix_; 						// design matrix of the model
	gsl_matrix* covar_matrix_;				// data covariance matrix
	gsl_matrix* inv_covar_matrix_; 		// inverse data covariance matrix
	gsl_matrix* fisher_matrix_; 			// Fisher matrix
	gsl_matrix* prior_matrix_; 			 	// prior precision matrix
	gsl_vector* prior_param_values_; 	// prior fiducial parameter values
	gsl_vector* param_values_; 				// ML estimator values
	gsl_vector* data_mean_values_; 		// mean of the data vector

	LinGauss(unsigned int num_data, unsigned int num_params);
	void DisallocateMembers();
	void CalculateFisherMatrix();
	void CalculateMlEstimator(gsl_vector* data);
	double CalculateChisquared(gsl_vector* data, gsl_vector* params);
};
#endif // SOURCE_LINGAUSS_H_


/* cheb/deriv.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_chebyshev.h>

int gsl_cheb_calc_deriv(gsl_cheb_series * deriv, const gsl_cheb_series * f)
{
  const size_t n = f->order + 1;
  const double con = 2.0 / (f->b - f->a);
  size_t i;

  if(deriv->order != f->order)
    {
      GSL_ERROR ("order of chebyshev series must be equal", GSL_ENOMEM);
    }

  /* set the other parameters in the chebyshev struct */

  deriv->a = f->a;
  deriv->b = f->b;

  /* error in derivative is n^2 c_n */
  /* deriv->err = n * n * f->c[n-1];*/

  /* FIXME:  should probably set deriv->f[] as well */

  deriv->c[n-1] = 0.0;

  if(n > 1) {
    deriv->c[n-2] = 2.0 *(n-1.0) * f->c[n-1];

    for(i = n; i>=3; i--)
      deriv->c[i-3] = deriv->c[i-1] + 2.0 *(i-2.0) * f->c[i-2];

    for(i = 0  ; i<n ; i++)
      deriv->c[i] *= con;
  }

  return GSL_SUCCESS;
}


/* bspline/greville.c
 *
 * Copyright (C) 2006, 2007, 2008, 2009 Patrick Alken
 * Copyright (C) 2008, 2011 Rhys Ulerich
 *
 * 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 <gsl/gsl_bspline.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_vector.h>

/* Return the location of the i-th Greville abscissa */
double
gsl_bspline_greville_abscissa (size_t i, gsl_bspline_workspace *w)
{
  const size_t stride = w->knots->stride;
  size_t km1 = w->km1;
  double * data = w->knots->data + (i+1)*stride;
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= gsl_bspline_ncoeffs(w)))
    {
      GSL_ERROR_VAL ("Greville abscissa index out of range", GSL_EINVAL, 0);
    }
#endif

  if (km1 == 0)
    {
      /* Return interval midpoints in degenerate k = 1 case*/
      km1   = 2;
      data -= stride;
    }

  return gsl_stats_mean(data, stride, km1);
}

int
gsl_bspline_knots_greville (const gsl_vector *abscissae,
                            gsl_bspline_workspace *w,
                            double *abserr)
{
  /* Limited function: see https://savannah.gnu.org/bugs/index.php?34361 */

  int s;

  /* Check incoming arguments satisfy mandatory algorithmic assumptions */
  if (w->k < 2)
    GSL_ERROR ("w->k must be at least 2", GSL_EINVAL);
  else if (abscissae->size < 2)
    GSL_ERROR ("abscissae->size must be at least 2", GSL_EINVAL);
  else if (w->nbreak != abscissae->size - w->k + 2)
    GSL_ERROR ("w->nbreak must equal abscissae->size - w->k + 2", GSL_EINVAL);

  if (w->nbreak == 2)
    {
      /* No flexibility in abscissae values possible in this degenerate case */
      s = gsl_bspline_knots_uniform (
              gsl_vector_get (abscissae, 0),
              gsl_vector_get (abscissae, abscissae->size - 1), w);
    }
  else
    {
      double * storage;
      gsl_matrix_view A;
      gsl_vector_view tau, b, x, r;
      size_t i, j;

      /* Constants derived from the B-spline workspace and abscissae details */
      const size_t km2    = w->k - 2;
      const size_t M      = abscissae->size - 2;
      const size_t N      = w->nbreak - 2;
      const double invkm1 = 1.0 / w->km1;

      /* Allocate working storage and prepare multiple, zero-filled views */
      storage = (double *) calloc (M*N + 2*N + 2*M, sizeof (double));
      if (storage == 0)
        GSL_ERROR ("failed to allocate working storage", GSL_ENOMEM);
      A   = gsl_matrix_view_array (storage, M, N);
      tau = gsl_vector_view_array (storage + M*N,             N);
      b   = gsl_vector_view_array (storage + M*N + N,         M);
      x   = gsl_vector_view_array (storage + M*N + N + M,     N);
      r   = gsl_vector_view_array (storage + M*N + N + M + N, M);

      /* Build matrix from interior breakpoints to interior Greville abscissae.
       * For example, when w->k = 4 and w->nbreak = 7 the matrix is
       *   [   1,      0,      0,      0,      0;
       *     2/3,    1/3,      0,      0,      0;
       *     1/3,    1/3,    1/3,      0,      0;
       *       0,    1/3,    1/3,    1/3,      0;
       *       0,      0,    1/3,    1/3,    1/3;
       *       0,      0,      0,    1/3,    2/3;
       *       0,      0,      0,      0,      1  ]
       * but only center formed as first/last breakpoint is known.
       */
      for (j = 0; j < N; ++j)
        for (i = 0; i <= km2; ++i)
          gsl_matrix_set (&A.matrix, i+j, j, invkm1);

      /* Copy interior collocation points from abscissae into b */
      for (i = 0; i < M; ++i)
        gsl_vector_set (&b.vector, i, gsl_vector_get (abscissae, i+1));

      /* Adjust b to account for constraint columns not stored in A */
      for (i = 0; i < km2; ++i)
        {
          double * const v = gsl_vector_ptr (&b.vector, i);
          *v -= (1 - (i+1)*invkm1) * gsl_vector_get (abscissae, 0);
        }
      for (i = 0; i < km2; ++i)
        {
          double * const v = gsl_vector_ptr (&b.vector, M - km2 + i);
          *v -= (i+1)*invkm1 * gsl_vector_get (abscissae, abscissae->size - 1);
        }

      /* Perform linear least squares to determine interior breakpoints */
      s =  gsl_linalg_QR_decomp (&A.matrix, &tau.vector)
        || gsl_linalg_QR_lssolve (&A.matrix, &tau.vector,
                                  &b.vector, &x.vector, &r.vector);
      if (s)
        {
          free (storage);
          return s;
        }

      /* "Expand" solution x by adding known first and last breakpoints. */
      x = gsl_vector_view_array_with_stride (
          gsl_vector_ptr (&x.vector, 0) - x.vector.stride,
          x.vector.stride, x.vector.size + 2);
      gsl_vector_set (&x.vector, 0, gsl_vector_get (abscissae, 0));
      gsl_vector_set (&x.vector, x.vector.size - 1,
                      gsl_vector_get (abscissae, abscissae->size - 1));

      /* Finally, initialize workspace knots using the now-known breakpoints */
      s = gsl_bspline_knots (&x.vector, w);
      free (storage);
    }

  /* Sum absolute errors in the resulting vs requested interior abscissae */
  /* Provided as a fit quality metric which may be monitored by callers */
  if (!s && abserr)
    {
      size_t i;
      *abserr = 0;
      for (i = 1; i < abscissae->size - 1; ++i)
        *abserr += fabs (   gsl_bspline_greville_abscissa (i, w)
                          - gsl_vector_get (abscissae, i) );
    }

  return s;
}


static char help[] = "1D reaction-diffusion problem with DMDA and SNES.  Option prefix -rct_.\n\n";

#include <petsc.h>

typedef struct {
    PetscReal  rho, M, alpha, beta;
    PetscBool  noRinJ;
} AppCtx;

extern PetscReal f_source(PetscReal);
extern PetscErrorCode InitialAndExact(DMDALocalInfo*, PetscReal*, PetscReal*, AppCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, PetscReal*, PetscReal*, AppCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, PetscReal*, Mat, Mat, AppCtx*);

//STARTMAIN
int main(int argc,char **args) {
  PetscErrorCode ierr;
  DM            da;
  SNES          snes;
  AppCtx        user;
  Vec           u, uexact;
  PetscReal     errnorm, *au, *auex;
  DMDALocalInfo info;

  ierr = PetscInitialize(&argc,&args,NULL,help); if (ierr) return ierr;
  user.rho   = 10.0;
  user.M     = PetscSqr(user.rho / 12.0);
  user.alpha = user.M;
  user.beta  = 16.0 * user.M;
  user.noRinJ = PETSC_FALSE;

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"rct_","options for reaction",""); CHKERRQ(ierr);
  ierr = PetscOptionsBool("-noRinJ","do not include R(u) term in Jacobian",
      "reaction.c",user.noRinJ,&(user.noRinJ),NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = DMDACreate1d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,9,1,1,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(da,&u); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&uexact); CHKERRQ(ierr);
  ierr = DMDAVecGetArray(da,u,&au); CHKERRQ(ierr);

  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  ierr = DMDAVecGetArray(da,uexact,&auex); CHKERRQ(ierr);
  ierr = InitialAndExact(&info,au,auex,&user); CHKERRQ(ierr);
  ierr = DMDAVecRestoreArray(da,u,&au); CHKERRQ(ierr);
  ierr = DMDAVecRestoreArray(da,uexact,&auex); CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)FormJacobianLocal,&user); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

  ierr = SNESSolve(snes,NULL,u); CHKERRQ(ierr);

  ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
  ierr = VecNorm(u,NORM_INFINITY,&errnorm); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
      "on %d point grid:  |u-u_exact|_inf = %g\n",info.mx,errnorm); CHKERRQ(ierr);

  VecDestroy(&u);  VecDestroy(&uexact);
  SNESDestroy(&snes);  DMDestroy(&da);
  return PetscFinalize();
}
//ENDMAIN

PetscReal f_source(PetscReal x) {
    return 0.0;
}

PetscErrorCode InitialAndExact(DMDALocalInfo *info, PetscReal *u0,
                               PetscReal *uex, AppCtx *user) {
    PetscInt   i;
    PetscReal  h = 1.0 / (info->mx-1), x;
    for (i=info->xs; i<info->xs+info->xm; i++) {
        x = h * i;
        u0[i]  = user->alpha * (1.0 - x) + user->beta * x;
        uex[i] = user->M * PetscPowReal(x + 1.0,4.0);
    }
    return 0;
}

//STARTFUNCTIONS
PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal *u,
                                 PetscReal *FF, AppCtx *user) {
    PetscInt   i;
    PetscReal  h = 1.0 / (info->mx-1), x, R;
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if (i == 0) {
            FF[i] = u[i] - user->alpha;
        } else if (i == info->mx-1) {
            FF[i] = u[i] - user->beta;
        } else {  // interior location
            if (i == 1) {
                FF[i] = - u[i+1] + 2.0 * u[i] - user->alpha;
            } else if (i == info->mx-2) {
                FF[i] = - user->beta + 2.0 * u[i] - u[i-1];
            } else {
                FF[i] = - u[i+1] + 2.0 * u[i] - u[i-1];
            }
            R = - user->rho * PetscSqrtReal(u[i]);
            x = i * h;
            FF[i] -= h*h * (R + f_source(x));
        }
    }
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, PetscReal *u,
                                 Mat J, Mat P, AppCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, col[3];
    PetscReal  h = 1.0 / (info->mx-1), dRdu, v[3];
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if ((i == 0) | (i == info->mx-1)) {
            v[0] = 1.0;
            ierr = MatSetValues(P,1,&i,1,&i,v,INSERT_VALUES); CHKERRQ(ierr);
        } else {
            col[0] = i;
            v[0] = 2.0;
            if (!user->noRinJ) {
                dRdu = - (user->rho / 2.0) / PetscSqrtReal(u[i]);
                v[0] -= h*h * dRdu;
            }
            col[1] = i-1;   v[1] = (i > 1) ? - 1.0 : 0.0;
            col[2] = i+1;   v[2] = (i < info->mx-2) ? - 1.0 : 0.0;
            ierr = MatSetValues(P,1,&i,3,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}
//ENDFUNCTIONS


//This computes "the" DCT (discrete cosine transformation) along dim of matrix X.
//This uses a CBLAS matrix multiplication by the DCT-II matrix.

//For complex input X, the output Y is complex, and the DCT is just the DCT of the
//real and imaginary parts separately (following Octave convention).

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int dct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int dct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int dct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int dct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);

int dct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in dct_cblas_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in dct_cblas_s: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float s = sc ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f;
        const float dcsc = sc ? 1.0f/sqrtf((float)ndct) : 2.0f;

        //Initialize DCT-II matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in dct_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l, ++DCT) { *DCT = dcsc; }
        for (size_t n=1u; n<ndct; ++n)
        {
            for (size_t l=0u; l<Lx; ++l, ++DCT)
            {
                *DCT = s * cosf(P_N*(0.5f+(float)l)*(float)n);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,1,0.0f,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                cblas_sgemm(CblasColMajor,CblasTrans,CblasNoTrans,(int)ndct,(int)V,(int)Lx,1.0f,DCT,(int)Lx,X,(int)Lx,0.0f,Y,(int)ndct);
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, ++Y)
                    {
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,(int)K,0.0f,Y,(int)K);
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int dct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in dct_cblas_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in dct_cblas_d: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const double s = sc ? 2.0/sqrt((double)(2u*ndct)) : 2.0;
        const double dcsc = sc ? 1.0/sqrt((double)ndct) : 2.0;

        //Initialize DCT-II matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in dct_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l, ++DCT) { *DCT = dcsc; }
        for (size_t n=1u; n<ndct; ++n)
        {
            for (size_t l=0u; l<Lx; ++l, ++DCT)
            {
                *DCT = s * cos(P_N*(0.5+(double)l)*(double)n);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,1,0.0,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                cblas_dgemm(CblasColMajor,CblasTrans,CblasNoTrans,(int)ndct,(int)V,(int)Lx,1.0,DCT,(int)Lx,X,(int)Lx,0.0,Y,(int)ndct);
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, ++Y)
                    {
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,(int)K,0.0,Y,(int)K);
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int dct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in dct_cblas_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in dct_cblas_c: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float s = sc ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f;
        const float dcsc = sc ? 1.0f/sqrtf((float)ndct) : 2.0f;

        //Initialize DCT-II matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in dct_cblas_c: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l, ++DCT) { *DCT = dcsc; }
        for (size_t n=1u; n<ndct; ++n)
        {
            for (size_t l=0u; l<Lx; ++l, ++DCT)
            {
                *DCT = s * cosf(P_N*(0.5f+(float)l)*(float)n);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx, Y+=2u*ndct)
                {
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, ++Y)
                    {
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2*(int)K,0.0f,Y,2*(int)K);
                        ++X; ++Y;
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2*(int)K,0.0f,Y,2*(int)K);
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int dct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in dct_cblas_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in dct_cblas_z: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const double s = sc ? 2.0/sqrt((double)(2u*ndct)) : 2.0;
        const double dcsc = sc ? 1.0/sqrt((double)ndct) : 2.0;

        //Initialize DCT-II matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in dct_cblas_z: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l, ++DCT) { *DCT = dcsc; }
        for (size_t n=1u; n<ndct; ++n)
        {
            for (size_t l=0u; l<Lx; ++l, ++DCT)
            {
                *DCT = s * cos(P_N*(0.5+(double)l)*(double)n);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx, Y+=2u*ndct)
                {
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, ++Y)
                    {
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2*(int)K,0.0,Y,2*(int)K);
                        ++X; ++Y;
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2*(int)K,0.0,Y,2*(int)K);
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


// Computational routines for one-dimensional orthogonal polynomial transforms.

#ifndef FASTTRANSFORMS_H
#define FASTTRANSFORMS_H

#include <stdlib.h>
#include <math.h>
#include <immintrin.h>
#include <cblas.h>

#ifdef _OPENMP
    #include <omp.h>
#else
    #define omp_get_thread_num() 0
    #define omp_get_num_threads() 1
#endif

#define M_SQRT_PI    1.772453850905516027   /* sqrt(pi)       */
#define M_1_SQRT_PI  0.564189583547756287   /* 1/sqrt(pi)     */
#define M_SQRT_PI_2  0.886226925452758014   /* sqrt(pi)/2     */

#define MAX(a,b) ((a) > (b) ? a : b)
#define MIN(a,b) ((a) < (b) ? a : b)

#if __SSE2__
    #define VECTOR_SIZE_2 2
    typedef double double2 __attribute__ ((vector_size (VECTOR_SIZE_2*8)));
    #define vall2(x) ((double2) _mm_set1_pd(x))
    #define vload2(v) ((double2) _mm_loadu_pd(v))
    #define vstore2(u, v) (_mm_storeu_pd(u, v))
#endif
#if __AVX__
    #define VECTOR_SIZE_4 4
    typedef double double4 __attribute__ ((vector_size (VECTOR_SIZE_4*8)));
    #define vall4(x) ((double4) _mm256_set1_pd(x))
    #define vload4(v) ((double4) _mm256_loadu_pd(v))
    #define vstore4(u, v) (_mm256_storeu_pd(u, v))
#endif
#if __AVX512F__
    #define VECTOR_SIZE_8 8
    typedef double double8 __attribute__ ((vector_size (VECTOR_SIZE_8*8)));
    #define vall8(x) ((double8) _mm512_set1_pd(x))
    #define vload8(v) ((double8) _mm512_loadu_pd(v))
    #define vstore8(u, v) (_mm512_storeu_pd(u, v))
#endif

static inline double stirlingseries(const double z);
static inline double Aratio(const int n, const double alpha, const double beta);
static inline double Analphabeta(const int n, const double alpha, const double beta);
static inline double lambda(const double x);
static inline double lambda2(const double x, const double l1, const double l2);

double * plan_leg2cheb(const int normleg, const int normcheb, const int n);
double * plan_cheb2leg(const int normcheb, const int normleg, const int n);
double * plan_ultra2ultra(const int normultra1, const int normultra2, const int n, const double lambda1, const double lambda2);
double * plan_jac2jac(const int normjac1, const int normjac2, const int n, const double alpha, const double beta, const double gamma);

typedef struct {
    double * s;
    double * c;
    int n;
} RotationPlan;

void freeRotationPlan(RotationPlan * RP);

RotationPlan * plan_rotsphere(const int n);

void kernel_sph_hi2lo(const RotationPlan * RP, const int m, double * A);
void kernel_sph_lo2hi(const RotationPlan * RP, const int m, double * A);

void kernel_sph_hi2lo_SSE(const RotationPlan * RP, const int m, double * A);
void kernel_sph_lo2hi_SSE(const RotationPlan * RP, const int m, double * A);

void kernel_sph_hi2lo_AVX(const RotationPlan * RP, const int m, double * A);
void kernel_sph_lo2hi_AVX(const RotationPlan * RP, const int m, double * A);

void kernel_sph_hi2lo_AVX512(const RotationPlan * RP, const int m, double * A);
void kernel_sph_lo2hi_AVX512(const RotationPlan * RP, const int m, double * A);

RotationPlan * plan_rottriangle(const int n, const double alpha, const double beta, const double gamma);

void kernel_tri_hi2lo(const RotationPlan * RP, const int m, double * A);
void kernel_tri_lo2hi(const RotationPlan * RP, const int m, double * A);

void kernel_tri_hi2lo_SSE(const RotationPlan * RP, const int m, double * A);
void kernel_tri_lo2hi_SSE(const RotationPlan * RP, const int m, double * A);

void kernel_tri_hi2lo_AVX(const RotationPlan * RP, const int m, double * A);
void kernel_tri_lo2hi_AVX(const RotationPlan * RP, const int m, double * A);

void kernel_tri_hi2lo_AVX512(const RotationPlan * RP, const int m, double * A);
void kernel_tri_lo2hi_AVX512(const RotationPlan * RP, const int m, double * A);

RotationPlan * plan_rotdisk(const int n);

void kernel_disk_hi2lo(const RotationPlan * RP, const int m, double * A);
void kernel_disk_lo2hi(const RotationPlan * RP, const int m, double * A);

void kernel_disk_hi2lo_SSE(const RotationPlan * RP, const int m, double * A);
void kernel_disk_lo2hi_SSE(const RotationPlan * RP, const int m, double * A);

void kernel_disk_hi2lo_AVX(const RotationPlan * RP, const int m, double * A);
void kernel_disk_lo2hi_AVX(const RotationPlan * RP, const int m, double * A);

void kernel_disk_hi2lo_AVX512(const RotationPlan * RP, const int m, double * A);
void kernel_disk_lo2hi_AVX512(const RotationPlan * RP, const int m, double * A);

typedef struct {
    double * s1;
    double * c1;
    double * s2;
    double * c2;
    double * s3;
    double * c3;
    int n;
    int s;
} SpinRotationPlan;

void freeSpinRotationPlan(SpinRotationPlan * SRP);

SpinRotationPlan * plan_rotspinsphere(const int n, const int s);

void kernel_spinsph_hi2lo(const SpinRotationPlan * SRP, const int m, double * A);
void kernel_spinsph_lo2hi(const SpinRotationPlan * SRP, const int m, double * A);

void kernel_spinsph_hi2lo_SSE(const SpinRotationPlan * SRP, const int m, double * A);
void kernel_spinsph_lo2hi_SSE(const SpinRotationPlan * SRP, const int m, double * A);

void kernel_spinsph_hi2lo_AVX(const SpinRotationPlan * SRP, const int m, double * A);
void kernel_spinsph_lo2hi_AVX(const SpinRotationPlan * SRP, const int m, double * A);

void kernel_spinsph_hi2lo_AVX512(const SpinRotationPlan * SRP, const int m, double * A);
void kernel_spinsph_lo2hi_AVX512(const SpinRotationPlan * SRP, const int m, double * A);

static inline void apply_givens(const double S, const double C, double * X, double * Y);
static inline void apply_givens_t(const double S, const double C, double * X, double * Y);

static inline void apply_givens_SSE(const double S, const double C, double * X, double * Y);
static inline void apply_givens_t_SSE(const double S, const double C, double * X, double * Y);

static inline void apply_givens_AVX(const double S, const double C, double * X, double * Y);
static inline void apply_givens_t_AVX(const double S, const double C, double * X, double * Y);

static inline void apply_givens_AVX512(const double S, const double C, double * X, double * Y);
static inline void apply_givens_t_AVX512(const double S, const double C, double * X, double * Y);

void execute_sph_hi2lo(const RotationPlan * RP, double * A, const int M);
void execute_sph_lo2hi(const RotationPlan * RP, double * A, const int M);

void execute_sph_hi2lo_SSE(const RotationPlan * RP, double * A, double * B, const int M);
void execute_sph_lo2hi_SSE(const RotationPlan * RP, double * A, double * B, const int M);

void execute_sph_hi2lo_AVX(const RotationPlan * RP, double * A, double * B, const int M);
void execute_sph_lo2hi_AVX(const RotationPlan * RP, double * A, double * B, const int M);

void execute_sph_hi2lo_AVX512(const RotationPlan * RP, double * A, double * B, const int M);
void execute_sph_lo2hi_AVX512(const RotationPlan * RP, double * A, double * B, const int M);

void execute_tri_hi2lo(const RotationPlan * RP, double * A, const int M);
void execute_tri_lo2hi(const RotationPlan * RP, double * A, const int M);

void execute_tri_hi2lo_SSE(const RotationPlan * RP, double * A, double * B, const int M);
void execute_tri_lo2hi_SSE(const RotationPlan * RP, double * A, double * B, const int M);

void execute_tri_hi2lo_AVX(const RotationPlan * RP, double * A, double * B, const int M);
void execute_tri_lo2hi_AVX(const RotationPlan * RP, double * A, double * B, const int M);

void execute_tri_hi2lo_AVX512(const RotationPlan * RP, double * A, double * B, const int M);
void execute_tri_lo2hi_AVX512(const RotationPlan * RP, double * A, double * B, const int M);

void execute_disk_hi2lo(const RotationPlan * RP, double * A, const int M);
void execute_disk_lo2hi(const RotationPlan * RP, double * A, const int M);

void execute_disk_hi2lo_SSE(const RotationPlan * RP, double * A, double * B, const int M);
void execute_disk_lo2hi_SSE(const RotationPlan * RP, double * A, double * B, const int M);

void execute_disk_hi2lo_AVX(const RotationPlan * RP, double * A, double * B, const int M);
void execute_disk_lo2hi_AVX(const RotationPlan * RP, double * A, double * B, const int M);

void execute_disk_hi2lo_AVX512(const RotationPlan * RP, double * A, double * B, const int M);
void execute_disk_lo2hi_AVX512(const RotationPlan * RP, double * A, double * B, const int M);

void execute_spinsph_hi2lo(const SpinRotationPlan * SRP, double * A, const int M);
void execute_spinsph_lo2hi(const SpinRotationPlan * SRP, double * A, const int M);

void execute_spinsph_hi2lo_SSE(const SpinRotationPlan * SRP, double * A, double * B, const int M);
void execute_spinsph_lo2hi_SSE(const SpinRotationPlan * SRP, double * A, double * B, const int M);

void execute_spinsph_hi2lo_AVX(const SpinRotationPlan * SRP, double * A, double * B, const int M);
void execute_spinsph_lo2hi_AVX(const SpinRotationPlan * SRP, double * A, double * B, const int M);

void execute_spinsph_hi2lo_AVX512(const SpinRotationPlan * SRP, double * A, double * B, const int M);
void execute_spinsph_lo2hi_AVX512(const SpinRotationPlan * SRP, double * A, double * B, const int M);

typedef struct {
    RotationPlan * RP;
    double * B;
    double * P1;
    double * P2;
    double * P1inv;
    double * P2inv;
} SphericalHarmonicPlan;

void freeSphericalHarmonicPlan(SphericalHarmonicPlan * P);

SphericalHarmonicPlan * plan_sph2fourier(const int n);

void execute_sph2fourier(const SphericalHarmonicPlan * P, double * A, const int N, const int M);
void execute_fourier2sph(const SphericalHarmonicPlan * P, double * A, const int N, const int M);

typedef struct {
    RotationPlan * RP;
    double * B;
    double * P1;
    double * P2;
    double * P3;
    double * P4;
    double * P1inv;
    double * P2inv;
    double * P3inv;
    double * P4inv;
    double alpha;
    double beta;
    double gamma;
} TriangularHarmonicPlan;

void freeTriangularHarmonicPlan(TriangularHarmonicPlan * P);

TriangularHarmonicPlan * plan_tri2cheb(const int n, const double alpha, const double beta, const double gamma);

void execute_tri2cheb(const TriangularHarmonicPlan * P, double * A, const int N, const int M);
void execute_cheb2tri(const TriangularHarmonicPlan * P, double * A, const int N, const int M);

static void alternate_sign(double * A, const int N);

static void chebyshev_normalization(double * A, const int N, const int M);
static void chebyshev_normalization_t(double * A, const int N, const int M);

void permute(const double * A, double * B, const int N, const int M, const int L);
void permute_t(double * A, const double * B, const int N, const int M, const int L);

void permute_sph(const double * A, double * B, const int N, const int M, const int L);
void permute_t_sph(double * A, const double * B, const int N, const int M, const int L);

void permute_tri(const double * A, double * B, const int N, const int M, const int L);
void permute_t_tri(double * A, const double * B, const int N, const int M, const int L);

#define permute_disk(A, B, N, M, L) permute_sph(A, B, N, M, L)
#define permute_t_disk(A, B, N, M, L) permute_t_sph(A, B, N, M, L)

#define permute_spinsph(A, B, N, M, L) permute_sph(A, B, N, M, L)
#define permute_t_spinsph(A, B, N, M, L) permute_t_sph(A, B, N, M, L)

void swap(double * A, double * B, const int N);
void warp(double * A, const int N, const int M, const int L);
void warp_t(double * A, const int N, const int M, const int L);

#endif //FASTTRANSFORMS_H


/* filter/test_rmedian.c
 *
 * Copyright (C) 2018 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>

/* slow/dumb rmedian which constructs actual window for each sample, sorts
 * it and finds median */
static int
slow_rmedian(const gsl_filter_end_t endtype, const gsl_vector * x, gsl_vector * y, const int K)
{
  const int n = (int) x->size;
  const int H = K / 2;
  double *window = malloc(K * sizeof(double));
  int i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(endtype, x, i, H, H, window);
      double yi;
      int j;

      /* fill first half of window with previous filter output values */
      for (j = i - H; j < i; ++j)
        {
          if (j < 0)
            {
              if (endtype == GSL_FILTER_END_PADVALUE)
                window[j - i + H] = gsl_vector_get(x, 0);
              else if (endtype == GSL_FILTER_END_PADZERO)
                window[j - i + H] = 0.0;
            }
          else
            {
              window[j - i + H] = gsl_vector_get(y, j);
            }
        }

      yi = gsl_stats_median(window, 1, wsize);
      gsl_vector_set(y, i, yi);
    }

  free(window);

  return GSL_SUCCESS;
}

/* test square wave input (root signal) */
static void
test_rmedian_root(const gsl_filter_end_t etype, const size_t n, const size_t k)
{
  const double tol = 1.0e-12;
  gsl_filter_rmedian_workspace *w = gsl_filter_rmedian_alloc(k);
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  char buf[2048];
  size_t i;

  /* test a root sequence (square input): x = [zero one zero] */
  gsl_vector_set_all(x, 0.0);

  for (i = n / 3; i <= n / 2; ++i)
    gsl_vector_set(x, i, 1.0);

  /* compute y = rmedian(x) and test y = x */
  gsl_filter_rmedian(etype, x, y, w);

  sprintf(buf, "n=%zu k=%zu RMF square wave root sequence", n, k);
  compare_vectors(tol, y, x, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_filter_rmedian_free(w);
}

/* test random input and in-place */
static void
test_rmedian_random(const gsl_filter_end_t etype, const size_t n, const int K, gsl_rng * r)
{
  const double tol = 1.0e-12;
  gsl_filter_rmedian_workspace *w = gsl_filter_rmedian_alloc(K);
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  gsl_vector *z = gsl_vector_alloc(n);
  char buf[2048];

  /* test filter with random input against slow algorithm */
  random_vector(x, r);

  /* y = rmedian(x) */
  gsl_filter_rmedian(etype, x, y, w);

  /* y = rmedian(x) with slow algorithm */
  slow_rmedian(etype, x, z, K);

  /* test y = z */
  sprintf(buf, "n=%zu K=%d RMF symmetric random slow test", n, K);
  compare_vectors(tol, y, z, buf);

  /* test in-place filter */

  /* z = rmedian(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_filter_rmedian(etype, z, z, w);

  sprintf(buf, "n=%zu K=%d RMF symmetric random in-place", n, K);
  compare_vectors(tol, z, y, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
  gsl_filter_rmedian_free(w);
}

void
test_rmedian(gsl_rng * rng_p)
{
  /* test root sequences */

  test_rmedian_root(GSL_FILTER_END_PADZERO, 1000, 3);
  test_rmedian_root(GSL_FILTER_END_PADZERO, 2000, 101);

  test_rmedian_root(GSL_FILTER_END_PADVALUE, 1000, 3);
  test_rmedian_root(GSL_FILTER_END_PADVALUE, 2000, 101);

  /* test random input */

  test_rmedian_random(GSL_FILTER_END_PADZERO, 10, 1, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADZERO, 100, 3, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADZERO, 1000, 3, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADZERO, 100, 1001, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADZERO, 5, 7, rng_p);

  test_rmedian_random(GSL_FILTER_END_PADVALUE, 10, 1, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADVALUE, 100, 3, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADVALUE, 1000, 3, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADVALUE, 100, 1001, rng_p);
  test_rmedian_random(GSL_FILTER_END_PADVALUE, 5, 7, rng_p);
}