/* blas/gsl_blas.h
 *
 * 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*
 * Author:  G. Jungman
 */
#ifndef __GSL_BLAS_H__
#define __GSL_BLAS_H__

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

#include <gsl/gsl_blas_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/* ========================================================================
 * Level 1
 * ========================================================================
 */

int gsl_blas_sdsdot (float alpha,
                     const gsl_vector_float * X,
                     const gsl_vector_float * Y,
                     float * result
                     );

int gsl_blas_dsdot (const gsl_vector_float * X,
                    const gsl_vector_float * Y,
                    double * result
                    );

int gsl_blas_sdot (const gsl_vector_float * X,
                   const gsl_vector_float * Y,
                   float * result
                   );

int gsl_blas_ddot (const gsl_vector * X,
                   const gsl_vector * Y,
                   double * result
                   );

int  gsl_blas_cdotu (const gsl_vector_complex_float * X,
                     const gsl_vector_complex_float * Y,
                     gsl_complex_float * dotu);

int  gsl_blas_cdotc (const gsl_vector_complex_float * X,
                     const gsl_vector_complex_float * Y,
                     gsl_complex_float * dotc);

int  gsl_blas_zdotu (const gsl_vector_complex * X,
                     const gsl_vector_complex * Y,
                     gsl_complex * dotu);

int  gsl_blas_zdotc (const gsl_vector_complex * X,
                     const gsl_vector_complex * Y,
                     gsl_complex * dotc);

float  gsl_blas_snrm2  (const gsl_vector_float * X);
float  gsl_blas_sasum  (const gsl_vector_float * X);
double gsl_blas_dnrm2  (const gsl_vector * X);
double gsl_blas_dasum  (const gsl_vector * X);
float  gsl_blas_scnrm2 (const gsl_vector_complex_float * X);
float  gsl_blas_scasum (const gsl_vector_complex_float * X);
double gsl_blas_dznrm2 (const gsl_vector_complex * X);
double gsl_blas_dzasum (const gsl_vector_complex * X);

CBLAS_INDEX_t gsl_blas_isamax (const gsl_vector_float * X);
CBLAS_INDEX_t gsl_blas_idamax (const gsl_vector * X);
CBLAS_INDEX_t gsl_blas_icamax (const gsl_vector_complex_float * X);
CBLAS_INDEX_t gsl_blas_izamax (const gsl_vector_complex * X);

int  gsl_blas_sswap (gsl_vector_float * X,
                     gsl_vector_float * Y);

int  gsl_blas_scopy (const gsl_vector_float * X,
                     gsl_vector_float * Y);

int  gsl_blas_saxpy (float alpha,
                     const gsl_vector_float * X,
                     gsl_vector_float * Y);

int  gsl_blas_dswap (gsl_vector * X,
                     gsl_vector * Y);

int  gsl_blas_dcopy (const gsl_vector * X,
                     gsl_vector * Y);

int  gsl_blas_daxpy (double alpha,
                     const gsl_vector * X,
                     gsl_vector * Y);

int  gsl_blas_cswap (gsl_vector_complex_float * X,
                     gsl_vector_complex_float * Y);

int  gsl_blas_ccopy (const gsl_vector_complex_float * X,
                     gsl_vector_complex_float * Y);

int  gsl_blas_caxpy (const gsl_complex_float alpha,
                     const gsl_vector_complex_float * X,
                     gsl_vector_complex_float * Y);

int  gsl_blas_zswap (gsl_vector_complex * X,
                     gsl_vector_complex * Y);

int  gsl_blas_zcopy (const gsl_vector_complex * X,
                     gsl_vector_complex * Y);

int  gsl_blas_zaxpy (const gsl_complex alpha,
                     const gsl_vector_complex * X,
                     gsl_vector_complex * Y);

int  gsl_blas_srotg (float a[], float b[], float c[], float s[]);

int  gsl_blas_srotmg (float d1[], float d2[], float b1[], float b2, float P[]);

int  gsl_blas_srot (gsl_vector_float * X,
                    gsl_vector_float * Y,
                    float c, float s);

int  gsl_blas_srotm (gsl_vector_float * X,
                     gsl_vector_float * Y,
                     const float P[]);

int  gsl_blas_drotg (double a[], double b[], double c[], double s[]);

int  gsl_blas_drotmg (double d1[], double d2[], double b1[],
                      double b2, double P[]);

int  gsl_blas_drot (gsl_vector * X,
                    gsl_vector * Y,
                    const double c, const double s);

int  gsl_blas_drotm (gsl_vector * X,
                     gsl_vector * Y,
                     const double P[]);

void gsl_blas_sscal  (float  alpha, gsl_vector_float * X);
void gsl_blas_dscal  (double alpha, gsl_vector * X);
void gsl_blas_cscal  (const gsl_complex_float alpha, gsl_vector_complex_float * X);
void gsl_blas_zscal  (const gsl_complex alpha, gsl_vector_complex * X);
void gsl_blas_csscal (float  alpha, gsl_vector_complex_float * X);
void gsl_blas_zdscal (double alpha, gsl_vector_complex * X);

/* ===========================================================================
 * Level 2
 * ===========================================================================
 */

/*
 * Routines with standard 4 prefixes (S, D, C, Z)
 */
int  gsl_blas_sgemv (CBLAS_TRANSPOSE_t TransA,
                     float alpha,
                     const gsl_matrix_float * A,
                     const gsl_vector_float * X,
                     float beta,
                     gsl_vector_float * Y);

int  gsl_blas_strmv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_float * A,
                     gsl_vector_float * X);

int  gsl_blas_strsv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_float * A,
                     gsl_vector_float * X);

int  gsl_blas_dgemv (CBLAS_TRANSPOSE_t TransA,
                     double alpha,
                     const gsl_matrix * A,
                     const gsl_vector * X,
                     double beta,
                     gsl_vector * Y);

int  gsl_blas_dtrmv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix * A,
                     gsl_vector * X);

int  gsl_blas_dtrsv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix * A,
                     gsl_vector * X);

int  gsl_blas_cgemv (CBLAS_TRANSPOSE_t TransA,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_vector_complex_float * X,
                     const gsl_complex_float beta,
                     gsl_vector_complex_float * Y);

int  gsl_blas_ctrmv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_complex_float * A,
                     gsl_vector_complex_float * X);

int  gsl_blas_ctrsv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_complex_float * A,
                     gsl_vector_complex_float * X);

int  gsl_blas_zgemv (CBLAS_TRANSPOSE_t TransA,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_vector_complex * X,
                     const gsl_complex beta,
                     gsl_vector_complex * Y);

int  gsl_blas_ztrmv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_complex * A,
                     gsl_vector_complex * X);

int  gsl_blas_ztrsv (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag,
                     const gsl_matrix_complex * A,
                     gsl_vector_complex *X);

/*
 * Routines with S and D prefixes only
 */
int  gsl_blas_ssymv (CBLAS_UPLO_t Uplo,
                     float alpha,
                     const gsl_matrix_float * A,
                     const gsl_vector_float * X,
                     float beta,
                     gsl_vector_float * Y);

int  gsl_blas_sger (float alpha,
                    const gsl_vector_float * X,
                    const gsl_vector_float * Y,
                    gsl_matrix_float * A);

int  gsl_blas_ssyr (CBLAS_UPLO_t Uplo,
                    float alpha,
                    const gsl_vector_float * X,
                    gsl_matrix_float * A);

int  gsl_blas_ssyr2 (CBLAS_UPLO_t Uplo,
                     float alpha,
                     const gsl_vector_float * X,
                     const gsl_vector_float * Y,
                     gsl_matrix_float * A);

int  gsl_blas_dsymv (CBLAS_UPLO_t Uplo,
                     double alpha,
                     const gsl_matrix * A,
                     const gsl_vector * X,
                     double beta,
                     gsl_vector * Y);
int  gsl_blas_dger (double alpha,
                    const gsl_vector * X,
                    const gsl_vector * Y,
                    gsl_matrix * A);

int  gsl_blas_dsyr (CBLAS_UPLO_t Uplo,
                    double alpha,
                    const gsl_vector * X,
                    gsl_matrix * A);

int  gsl_blas_dsyr2 (CBLAS_UPLO_t Uplo,
                     double alpha,
                     const gsl_vector * X,
                     const gsl_vector * Y,
                     gsl_matrix * A);

/*
 * Routines with C and Z prefixes only
 */

int  gsl_blas_chemv (CBLAS_UPLO_t Uplo,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_vector_complex_float * X,
                     const gsl_complex_float beta,
                     gsl_vector_complex_float * Y);

int  gsl_blas_cgeru (const gsl_complex_float alpha,
                     const gsl_vector_complex_float * X,
                     const gsl_vector_complex_float * Y,
                     gsl_matrix_complex_float * A);

int  gsl_blas_cgerc (const gsl_complex_float alpha,
                     const gsl_vector_complex_float * X,
                     const gsl_vector_complex_float * Y,
                     gsl_matrix_complex_float * A);

int  gsl_blas_cher (CBLAS_UPLO_t Uplo,
                    float alpha,
                    const gsl_vector_complex_float * X,
                    gsl_matrix_complex_float * A);

int  gsl_blas_cher2 (CBLAS_UPLO_t Uplo,
                     const gsl_complex_float alpha,
                     const gsl_vector_complex_float * X,
                     const gsl_vector_complex_float * Y,
                     gsl_matrix_complex_float * A);

int  gsl_blas_zhemv (CBLAS_UPLO_t Uplo,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_vector_complex * X,
                     const gsl_complex beta,
                     gsl_vector_complex * Y);

int  gsl_blas_zgeru (const gsl_complex alpha,
                     const gsl_vector_complex * X,
                     const gsl_vector_complex * Y,
                     gsl_matrix_complex * A);

int  gsl_blas_zgerc (const gsl_complex alpha,
                     const gsl_vector_complex * X,
                     const gsl_vector_complex * Y,
                     gsl_matrix_complex * A);

int  gsl_blas_zher (CBLAS_UPLO_t Uplo,
                    double alpha,
                    const gsl_vector_complex * X,
                    gsl_matrix_complex * A);

int  gsl_blas_zher2 (CBLAS_UPLO_t Uplo,
                     const gsl_complex alpha,
                     const gsl_vector_complex * X,
                     const gsl_vector_complex * Y,
                     gsl_matrix_complex * A);

/*
 * ===========================================================================
 * Prototypes for level 3 BLAS
 * ===========================================================================
 */

/*
 * Routines with standard 4 prefixes (S, D, C, Z)
 */
int  gsl_blas_sgemm (CBLAS_TRANSPOSE_t TransA,
                     CBLAS_TRANSPOSE_t TransB,
                     float alpha,
                     const gsl_matrix_float * A,
                     const gsl_matrix_float * B,
                     float beta,
                     gsl_matrix_float * C);

int  gsl_blas_ssymm (CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo,
                     float alpha,
                     const gsl_matrix_float * A,
                     const gsl_matrix_float * B,
                     float beta,
                     gsl_matrix_float * C);

int  gsl_blas_ssyrk (CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t Trans,
                     float alpha,
                     const gsl_matrix_float * A,
                     float beta,
                     gsl_matrix_float * C);

int  gsl_blas_ssyr2k (CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t Trans,
                      float alpha,
                      const gsl_matrix_float * A,
                      const gsl_matrix_float * B,
                      float beta,
                      gsl_matrix_float * C);

int  gsl_blas_strmm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     float alpha,
                     const gsl_matrix_float * A,
                     gsl_matrix_float * B);

int  gsl_blas_strsm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     float alpha,
                     const gsl_matrix_float * A,
                     gsl_matrix_float * B);

int  gsl_blas_dgemm (CBLAS_TRANSPOSE_t TransA,
                     CBLAS_TRANSPOSE_t TransB,
                     double alpha,
                     const gsl_matrix * A,
                     const gsl_matrix * B,
                     double beta,
                     gsl_matrix * C);

int  gsl_blas_dsymm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo,
                     double alpha,
                     const gsl_matrix * A,
                     const gsl_matrix * B,
                     double beta,
                     gsl_matrix * C);

int  gsl_blas_dsyrk (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t Trans,
                     double alpha,
                     const gsl_matrix * A,
                     double beta,
                     gsl_matrix * C);

int  gsl_blas_dsyr2k (CBLAS_UPLO_t Uplo,
                      CBLAS_TRANSPOSE_t Trans,
                      double alpha,
                      const  gsl_matrix * A,
                      const  gsl_matrix * B,
                      double beta,
                      gsl_matrix * C);

int  gsl_blas_dtrmm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     double alpha,
                     const gsl_matrix * A,
                     gsl_matrix * B);

int  gsl_blas_dtrsm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     double alpha,
                     const gsl_matrix * A,
                     gsl_matrix * B);

int  gsl_blas_cgemm (CBLAS_TRANSPOSE_t TransA,
                     CBLAS_TRANSPOSE_t TransB,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_matrix_complex_float * B,
                     const gsl_complex_float beta,
                     gsl_matrix_complex_float * C);

int  gsl_blas_csymm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_matrix_complex_float * B,
                     const gsl_complex_float beta,
                     gsl_matrix_complex_float * C);

int  gsl_blas_csyrk (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t Trans,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_complex_float beta,
                     gsl_matrix_complex_float * C);

int  gsl_blas_csyr2k (CBLAS_UPLO_t Uplo,
                      CBLAS_TRANSPOSE_t Trans,
                      const gsl_complex_float alpha,
                      const gsl_matrix_complex_float * A,
                      const gsl_matrix_complex_float * B,
                      const gsl_complex_float beta,
                      gsl_matrix_complex_float * C);

int  gsl_blas_ctrmm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     gsl_matrix_complex_float * B);

int  gsl_blas_ctrsm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     gsl_matrix_complex_float * B);

int  gsl_blas_zgemm (CBLAS_TRANSPOSE_t TransA,
                     CBLAS_TRANSPOSE_t TransB,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_matrix_complex * B,
                     const gsl_complex beta,
                     gsl_matrix_complex * C);

int  gsl_blas_zsymm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_matrix_complex * B,
                     const gsl_complex beta,
                     gsl_matrix_complex * C);

int  gsl_blas_zsyrk (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t Trans,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_complex beta,
                     gsl_matrix_complex * C);

int  gsl_blas_zsyr2k (CBLAS_UPLO_t Uplo,
                      CBLAS_TRANSPOSE_t Trans,
                      const gsl_complex alpha,
                      const gsl_matrix_complex * A,
                      const gsl_matrix_complex * B,
                      const gsl_complex beta,
                      gsl_matrix_complex *C);

int  gsl_blas_ztrmm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     gsl_matrix_complex * B);

int  gsl_blas_ztrsm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
                     CBLAS_DIAG_t Diag,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     gsl_matrix_complex * B);

/*
 * Routines with prefixes C and Z only
 */
int  gsl_blas_chemm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo,
                     const gsl_complex_float alpha,
                     const gsl_matrix_complex_float * A,
                     const gsl_matrix_complex_float * B,
                     const gsl_complex_float beta,
                     gsl_matrix_complex_float * C);

int  gsl_blas_cherk (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t Trans,
                     float alpha,
                     const gsl_matrix_complex_float * A,
                     float beta,
                     gsl_matrix_complex_float * C);

int  gsl_blas_cher2k (CBLAS_UPLO_t Uplo,
                      CBLAS_TRANSPOSE_t Trans,
                      const gsl_complex_float alpha,
                      const gsl_matrix_complex_float * A,
                      const gsl_matrix_complex_float * B,
                      float beta,
                      gsl_matrix_complex_float * C);

int  gsl_blas_zhemm (CBLAS_SIDE_t Side,
                     CBLAS_UPLO_t Uplo,
                     const gsl_complex alpha,
                     const gsl_matrix_complex * A,
                     const gsl_matrix_complex * B,
                     const gsl_complex beta,
                     gsl_matrix_complex * C);

int  gsl_blas_zherk (CBLAS_UPLO_t Uplo,
                     CBLAS_TRANSPOSE_t Trans,
                     double alpha,
                     const gsl_matrix_complex * A,
                     double beta,
                     gsl_matrix_complex * C);

int  gsl_blas_zher2k (CBLAS_UPLO_t Uplo,
                      CBLAS_TRANSPOSE_t Trans,
                      const gsl_complex alpha,
                      const gsl_matrix_complex * A,
                      const gsl_matrix_complex * B,
                      double beta,
                      gsl_matrix_complex * C);

__END_DECLS

#endif /* __GSL_BLAS_H__ */


/* monte/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Michael Booth
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>

#define CONSTANT
#define PRODUCT
#define GAUSSIAN
#define DBLGAUSSIAN
#define TSUDA

#define PLAIN
#define MISER
#define VEGAS

double xl[11]  = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
double xu[11]  = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };
double xu2[11] = { 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 };
double xu3[2]  = { GSL_DBL_MAX, GSL_DBL_MAX };

double fconst (double x[], size_t d, void *params);
double f0 (double x[], size_t d, void *params);
double f1 (double x[], size_t d, void *params);
double f2 (double x[], size_t d, void *params);
double f3 (double x[], size_t d, void *params);

void my_error_handler (const char *reason, const char *file,
		       int line, int err);

struct problem {
  gsl_monte_function * f;
  double * xl;
  double * xu;
  size_t dim;
  size_t calls;
  double expected_result;
  double expected_error;
  char * description;
} ;

gsl_monte_function
make_function (double (*f)(double *, size_t, void *), size_t d, void * p);

gsl_monte_function
make_function (double (*f)(double *, size_t, void *), size_t d, void * p)
{
  gsl_monte_function f_new;

  f_new.f = f;
  f_new.dim = d;
  f_new.params = p;

  return f_new;
}

void
add (struct problem * problems, int * n,
     gsl_monte_function * f, double xl[], double xu[], size_t dim, size_t calls,
     double result, double err, char * description);

void
add (struct problem * problems, int * n,
     gsl_monte_function * f, double xl[], double xu[], size_t dim, size_t calls,
     double result, double err, char * description)
{
  int i = *n;
  problems[i].f = f;
  problems[i].xl = xl;
  problems[i].xu = xu;
  problems[i].dim = dim;
  problems[i].calls = calls;
  problems[i].expected_result = result;
  problems[i].expected_error = err;
  problems[i].description = description;
  (*n)++;
}

#define TRIALS 10

int
main (void)
{
  double result[TRIALS], error[TRIALS];
  double a = 0.1;
  double c = (1.0 + sqrt (10.0)) / 9.0;

  gsl_monte_function Fc = make_function(&fconst, 0, 0);
  gsl_monte_function F0 = make_function(&f0, 0, &a);
  gsl_monte_function F1 = make_function(&f1, 0, &a);
  gsl_monte_function F2 = make_function(&f2, 0, &a);
  gsl_monte_function F3 = make_function(&f3, 0, &c);

  /* The relationship between the variance of the function itself, the
     error on the integral and the number of calls is,

     sigma = sqrt(variance/N)

     where the variance is the <(f - <f>)^2> where <.> denotes the
     volume average (integral over the integration region divided by
     the volume) */

  int n = 0;
  struct problem * I;
  struct problem problems[256];

#ifdef CONSTANT
    /* variance(Fc) = 0 */

    add(problems,&n, &Fc, xl, xu,  1, 1000, 1.0, 0.0, "constant, 1d");
    add(problems,&n, &Fc, xl, xu,  2, 1000, 1.0, 0.0, "constant, 2d");
    add(problems,&n, &Fc, xl, xu,  3, 1000, 1.0, 0.0, "constant, 3d");
    add(problems,&n, &Fc, xl, xu,  4, 1000, 1.0, 0.0, "constant, 4d");
    add(problems,&n, &Fc, xl, xu,  5, 1000, 1.0, 0.0, "constant, 5d");
    add(problems,&n, &Fc, xl, xu,  6, 1000, 1.0, 0.0, "constant, 6d");
    add(problems,&n, &Fc, xl, xu,  7, 1000, 1.0, 0.0, "constant, 7d");
    add(problems,&n, &Fc, xl, xu,  8, 1000, 1.0, 0.0, "constant, 8d");
    add(problems,&n, &Fc, xl, xu,  9, 1000, 1.0, 0.0, "constant, 9d");
    add(problems,&n, &Fc, xl, xu, 10, 1000, 1.0, 0.0, "constant, 10d");
#endif

#ifdef PRODUCT
    /* variance(F0) = (4/3)^d - 1 */

    add(problems,&n, &F0, xl, xu,  1, 3333,   1.0, 0.01, "product, 1d" );
    add(problems,&n, &F0, xl, xu,  2, 7777,   1.0, 0.01, "product, 2d" );
    add(problems,&n, &F0, xl, xu,  3, 13703,  1.0, 0.01, "product, 3d" );
    add(problems,&n, &F0, xl, xu,  4, 21604,  1.0, 0.01, "product, 4d" );
    add(problems,&n, &F0, xl, xu,  5, 32139,  1.0, 0.01, "product, 5d" );
    add(problems,&n, &F0, xl, xu,  6, 46186,  1.0, 0.01, "product, 6d" );
    add(problems,&n, &F0, xl, xu,  7, 64915,  1.0, 0.01, "product, 7d" );
    add(problems,&n, &F0, xl, xu,  8, 89887,  1.0, 0.01, "product, 8d" );
    add(problems,&n, &F0, xl, xu,  9, 123182, 1.0, 0.01, "product, 9d" );
    add(problems,&n, &F0, xl, xu, 10, 167577, 1.0, 0.01, "product, 10d" );
#endif

#ifdef GAUSSIAN
    /* variance(F1) = (1/(a sqrt(2 pi)))^d - 1 */

    add(problems,&n, &F1, xl, xu,  1, 298,      1.0, 0.1, "gaussian, 1d" );
    add(problems,&n, &F1, xl, xu,  2, 1492,     1.0, 0.1, "gaussian, 2d" );
    add(problems,&n, &F1, xl, xu,  3, 6249,     1.0, 0.1, "gaussian, 3d" );
    add(problems,&n, &F1, xl, xu,  4, 25230,    1.0, 0.1, "gaussian, 4d" );
    add(problems,&n, &F1, xl, xu,  5, 100953,   1.0, 0.1, "gaussian, 5d" );
    add(problems,&n, &F1, xl, xu,  6, 44782,    1.0, 0.3, "gaussian, 6d" );
    add(problems,&n, &F1, xl, xu,  7, 178690,   1.0, 0.3, "gaussian, 7d" );
    add(problems,&n, &F1, xl, xu,  8, 712904,   1.0, 0.3, "gaussian, 8d" );
    add(problems,&n, &F1, xl, xu,  9, 2844109,  1.0, 0.3, "gaussian, 9d" );
    add(problems,&n, &F1, xl, xu, 10, 11346390, 1.0, 0.3, "gaussian, 10d" );
#endif

#ifdef DBLGAUSSIAN
    /* variance(F2) = 0.5 * (1/(a sqrt(2 pi)))^d - 1 */

    add(problems,&n, &F2, xl, xu,  1, 9947,  1.0, 0.01, "double gaussian, 1d" );
    add(problems,&n, &F2, xl, xu,  2, 695,   1.0, 0.1, "double gaussian, 2d" );
    add(problems,&n, &F2, xl, xu,  3, 3074,  1.0, 0.1, "double gaussian, 3d" );
    add(problems,&n, &F2, xl, xu,  4, 12565, 1.0, 0.1, "double gaussian, 4d" );
    add(problems,&n, &F2, xl, xu,  5, 50426, 1.0, 0.1, "double gaussian, 5d" );
    add(problems,&n, &F2, xl, xu,  6, 201472, 1.0, 0.1, "double gaussian, 6d" );
    add(problems,&n, &F2, xl, xu,  7, 804056, 1.0, 0.1, "double gaussian, 7d" );
    add(problems,&n, &F2, xl, xu,  8, 356446, 1.0, 0.3, "double gaussian, 8d" );
    add(problems,&n, &F2, xl, xu,  9, 1422049, 1.0, 0.3, "double gaussian, 9d" );
    add(problems,&n, &F2, xl, xu, 10, 5673189, 1.0, 0.3, "double gaussian, 10d" );
#endif

#ifdef TSUDA
    /* variance(F3) = ((c^2 + c + 1/3)/(c(c+1)))^d - 1 */

    add(problems,&n, &F3, xl, xu,  1, 4928,   1.0, 0.01, "tsuda function, 1d" );
    add(problems,&n, &F3, xl, xu,  2, 12285,  1.0, 0.01, "tsuda function, 2d" );
    add(problems,&n, &F3, xl, xu,  3, 23268,  1.0, 0.01, "tsuda function, 3d" );
    add(problems,&n, &F3, xl, xu,  4, 39664,  1.0, 0.01, "tsuda function, 4d" );
    add(problems,&n, &F3, xl, xu,  5, 64141,  1.0, 0.01, "tsuda function, 5d" );
    add(problems,&n, &F3, xl, xu,  6, 100680, 1.0, 0.01, "tsuda function, 6d" );
    add(problems,&n, &F3, xl, xu,  7, 155227, 1.0, 0.01, "tsuda function, 7d" );
    add(problems,&n, &F3, xl, xu,  8, 236657, 1.0, 0.01, "tsuda function, 8d" );
    add(problems,&n, &F3, xl, xu,  9, 358219, 1.0, 0.01, "tsuda function, 9d" );
    add(problems,&n, &F3, xl, xu, 10, 539690, 1.0, 0.01, "tsuda function, 10d" );
#endif

    add(problems,&n,   0,  0,  0, 0,    0,   0,   0, 0  );

  /* gsl_set_error_handler (&my_error_handler); */
  gsl_ieee_env_setup ();
  gsl_rng_env_setup ();

#ifdef A
  printf ("testing allocation/input checks\n");

  status = gsl_monte_plain_validate (s, xl, xu3, 1, 1);
  gsl_test (status != 0, "error if limits too large");
  status = gsl_monte_plain_validate (s, xl, xu, 0, 10);
  gsl_test (status != 0, "error if num_dim = 0");
  status = gsl_monte_plain_validate (s, xl, xu, 1, 0);
  gsl_test (status != 0, "error if calls = 0");
  status = gsl_monte_plain_validate (s, xu, xl, 1, 10);
  gsl_test (status != 0, "error if xu < xl");
#endif

#ifdef PLAIN
#define NAME "plain"
#define MONTE_STATE gsl_monte_plain_state
#define MONTE_ALLOC gsl_monte_plain_alloc
#define MONTE_INTEGRATE gsl_monte_plain_integrate
#define MONTE_FREE gsl_monte_plain_free
#define MONTE_SPEEDUP 1
#define MONTE_ERROR_TEST(err,expected) gsl_test_factor(err,expected, 5.0, NAME ", %s, abserr[%d]", I->description, i)
#include "test_main.c"
#undef NAME
#undef MONTE_STATE
#undef MONTE_ALLOC
#undef MONTE_INTEGRATE
#undef MONTE_FREE
#undef MONTE_ERROR_TEST
#undef MONTE_SPEEDUP
#endif

#ifdef MISER
#define NAME "miser"
#define MONTE_STATE gsl_monte_miser_state
#define MONTE_ALLOC gsl_monte_miser_alloc
#define MONTE_INTEGRATE gsl_monte_miser_integrate
#define MONTE_FREE gsl_monte_miser_free
#define MONTE_SPEEDUP 2
#define MONTE_ERROR_TEST(err,expected) gsl_test(err > 5.0 * expected, NAME ", %s, abserr[%d] (obs %g vs plain %g)", I->description, i, err, expected)
#include "test_main.c"
#undef NAME
#undef MONTE_STATE
#undef MONTE_ALLOC
#undef MONTE_INTEGRATE
#undef MONTE_FREE
#undef MONTE_ERROR_TEST
#undef MONTE_SPEEDUP
#endif

#ifdef VEGAS
#define NAME "vegas"
#define MONTE_STATE gsl_monte_vegas_state
#define MONTE_ALLOC gsl_monte_vegas_alloc
#define MONTE_INTEGRATE(f,xl,xu,dim,calls,r,s,res,err) { gsl_monte_vegas_integrate(f,xl,xu,dim,calls,r,s,res,err) ; if (s->chisq < 0.5 || s->chisq > 2) gsl_monte_vegas_integrate(f,xl,xu,dim,calls,r,s,res,err); }
#define MONTE_FREE gsl_monte_vegas_free
#define MONTE_SPEEDUP 3
#define MONTE_ERROR_TEST(err,expected) gsl_test(err > 3.0 * (expected == 0 ? 1.0/(I->calls/MONTE_SPEEDUP) : expected), NAME ", %s, abserr[%d] (obs %g vs exp %g)", I->description, i, err, expected)
#include "test_main.c"
#undef NAME
#undef MONTE_STATE
#undef MONTE_ALLOC
#undef MONTE_INTEGRATE
#undef MONTE_FREE
#undef MONTE_ERROR_TEST
#undef MONTE_SPEEDUP
#endif

  exit (gsl_test_summary ());
}

/* Simple constant function */
double
fconst (double x[], size_t num_dim, void *params)
{
  return 1;
}

/* Simple product function */
double
f0 (double x[], size_t num_dim, void *params)
{
  double prod = 1.0;
  unsigned int i;

  for (i = 0; i < num_dim; ++i)
    {
      prod *= 2.0 * x[i];
    }

  return prod;
}

/* Gaussian centered at 1/2. */

double
f1 (double x[], size_t num_dim, void *params)
{
  double a = *(double *)params;
  double sum = 0.;

  unsigned int i;
  for (i = 0; i < num_dim; i++)
    {
      double dx = x[i] - 0.5;
      sum += dx * dx;
    }
  return (pow (M_2_SQRTPI / (2. * a), (double) num_dim) *
	  exp (-sum / (a * a)));
}

/* double gaussian */
double
f2 (double x[], size_t num_dim, void *params)
{
  double a = *(double *)params;
  double sum1 = 0.;
  double sum2 = 0.;

  unsigned int i;
  for (i = 0; i < num_dim; i++)
    {
      double dx1 = x[i] - 1. / 3.;
      double dx2 = x[i] - 2. / 3.;
      sum1 += dx1 * dx1;
      sum2 += dx2 * dx2;
    }
  return 0.5 * pow (M_2_SQRTPI / (2. * a), num_dim)
    * (exp (-sum1 / (a * a)) + exp (-sum2 / (a * a)));
}

/* Tsuda's example */
double
f3 (double x[], size_t num_dim, void *params)
{
  double c = *(double *)params;

  double prod = 1.;

  unsigned int i;

  for (i = 0; i < num_dim; i++)
    {
      prod *= c / (c + 1) * pow((c + 1) / (c + x[i]), 2.0);
    }

  return prod;
}

void
my_error_handler (const char *reason, const char *file, int line, int err)
{
  if (0)
    printf ("(caught [%s:%d: %s (%d)])\n", file, line, reason, err);
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <assert.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#include "core_allvars.h"
#include "model_dust.h"
#include "model_misc.h"

double integrate_arr(const double arr1[MAX_STRING_LEN], const double arr2[MAX_STRING_LEN], const int npts, const double lower_limit, const double upper_limit);
double interpolate_arr(const double arr1[MAX_STRING_LEN], const double arr2[MAX_STRING_LEN], const int npts, const int xi);
double compute_imf (const double m);
double compute_taum (const double m);

void produce_metals_dust(const double metallicity, const double dt, const int p, const int centralgal, const int step, struct GALAXY *galaxies)
{
  double A = run_params.BinaryFraction;

  double yield;
  double yCr_snia, yFe_snia, yNi_snia;
  double yC_agb, yN_agb, yO_agb;
  double yC_sn, yO_sn, yMg_sn, ySi_sn, yS_sn, yCa_sn, yFe_sn;
  double Cr_snia, Fe_snia, Ni_snia;
  double C_agb, N_agb, O_agb;
  double C_sn, O_sn, Mg_sn, Si_sn, S_sn, Ca_sn, Fe_sn;
  double yCagb[run_params.countagb], yNagb[run_params.countagb], yOagb[run_params.countagb];
  double m_agb[run_params.countagb];
  double yCsn[run_params.countsn], yOsn[run_params.countsn], yMgsn[run_params.countsn], ySisn[run_params.countsn], ySsn[run_params.countsn], yCasn[run_params.countsn], yFesn[run_params.countsn];
  double m_sn[run_params.countsn];

  int i, j;
  double phi, taum, time, sfr;
  double gamma = 2.0; //to compute binary mass function for SN Ia
  double low_agb = 1; //lower limit for stars that ends up as AGB, Msun
  double low_binary = 3; //lower limit for binary star, Msun
  double up_agb = 6; //upper limit for stars that ends up as AGB, Msun
  double low_sn = run_params.msn[0]; //lower limit for stars that ends up as SN II, Msun
  double up_binary = 16; //upper limit for binary stars, Msun
  double up_sn = 40; //upper limit for stars that ends up as SN II, Msun
  double age[SNAPLEN], sfh[SNAPLEN];

  for (i=0; i<SNAPLEN; i++)
  {
        age[i] = run_params.lbtime[i]; //change the variable name to represent the age of universe instead of lbtime
        sfh[i] = galaxies[p].Sfr[i];
  }

/* the following computation is from eq. 9 Arrigoni et al. 2010 */

  // SN Ia //
  int count = 20; //any nymber will do
  double mu[count]; // M2/Mbinary
  double mbin[count]; //total mass of binary
  double fmu, yCrmu[count], yFemu[count], yNimu[count]; //to integrate over mu
  double yCrphi[count], yFephi[count], yNiphi[count];
  double max_mu = 0.5; //maximum value of mu

  for (i=0; i<count; i++){
        mbin[i] = low_binary + ((up_binary - low_binary) / (count-i));
        mu[i] = max_mu/(count-i);
        fmu = pow(2, 1+gamma) * (1+gamma) * pow(mu[i], gamma); //eq. 10 Arrigoni et al. 2010
        taum = compute_taum(mu[i] * mbin[i]);
        time = age[galaxies[p].SnapNum] - taum;
        if(time < run_params.lbtime[0]){
                sfr=0;
        }
        else {
                sfr = interpolate_arr(age, sfh, SNAPLEN, time);
        }
        yCrmu[i] = fmu * sfr * run_params.qCrsnia;
        yFemu[i] = fmu * sfr * run_params.qFesnia;
        yNimu[i] = fmu * sfr * run_params.qNisnia;
  }

  double yCr = integrate_arr(mu, yCrmu, count, max_mu/count, max_mu);
  double yFe = integrate_arr(mu, yFemu, count, max_mu/count, max_mu);
  double yNi = integrate_arr(mu, yNimu, count, max_mu/count, max_mu);
  for (i=0; i<count; i++){
        yCrphi[i] = yCr * compute_imf(mbin[i]);
        yFephi[i] = yFe * compute_imf(mbin[i]);
        yNiphi[i] = yNi * compute_imf(mbin[i]);
  //printf("SNIa done \n");
  }

  // AGB //
    if(run_params.AGBYields == 0)
    {
	i = j = 0;
	double Z_std[METALGRID] = {0.0, 1e-4, 4e-4, 4e-3, 8e-3, 0.02, 0.05};
	double sfrz = metallicity;
	if (sfrz < Z_std[0]){
		sfrz = Z_std[0];}
	else if (sfrz > Z_std[6]){
		sfrz = Z_std[6];}

	for (i=0; i<METALGRID; i++) {
		if(Z_std[i] <= sfrz && Z_std[i] > sfrz) {
			j = i;
		}
	}

	//double yCagb[run_params.countagb], yNagb[run_params.countagb], yOagb[run_params.countagb];
	//double m_agb[run_params.countagb];
	for(i=0; i<run_params.countagb; i++){
		if(run_params.magb[i] != 0) {
			m_agb[i] = run_params.magb[i];
			phi = compute_imf(run_params.magb[i]);
			taum = compute_taum(run_params.magb[i]);
			time = age[galaxies[p].SnapNum] - taum;
			if(time < run_params.lbtime[0]){
				sfr=0;
			}
			else {
				 sfr = interpolate_arr(age, sfh, SNAPLEN, time);
			}
			yCagb[i] = run_params.qCagb[i][j] * phi * sfr;
        		yNagb[i] = run_params.qNagb[i][j] * phi * sfr;
        		yOagb[i] = run_params.qOagb[i][j] * phi * sfr;
		}
		else {
        		yCagb[i] = 0;
        		yNagb[i] = 0;
        		yOagb[i] = 0;
		}
	}
    //printf("AGB done \n");
    }

  // SN II //
    if(run_params.SNIIYields == 0) //Woosley & Weaver 1995
    {
	i = j = 0;
	double Z_std[METALGRID] = {0.0, 1e-4, 4e-4, 4e-3, 8e-3, 0.02, 0.05};
	double sfrz = metallicity;
        if (sfrz < Z_std[0]){
                sfrz = Z_std[0];}
        else if (sfrz > Z_std[6]){
                sfrz = Z_std[6];}

        for (i=0; i<METALGRID; i++) {
                if(Z_std[i] <= sfrz && Z_std[i] > sfrz) {
                        j = i;
                }
        }

      	for(i=0; i<run_params.countsn; i++){
        	if(run_params.msn[i] != 0) {
         		m_sn[i] = run_params.msn[i];
         		phi = compute_imf(run_params.msn[i]);
          		taum = compute_taum(run_params.msn[i]);
          		time = age[galaxies[p].SnapNum] - taum;
          		if(time < run_params.lbtime[0]){
                		sfr=0;
          		}
         		 else {
                	sfr = interpolate_arr(age, sfh, SNAPLEN, time);
	  		}
          		yCsn[i] = run_params.qCsn[i][j] * phi * sfr;
          		yOsn[i] = run_params.qOsn[i][j] * phi * sfr;
          		yMgsn[i] = run_params.qMgsn[i][j] * phi * sfr;
          		ySisn[i] = run_params.qSisn[i][j] * phi * sfr;
          		ySsn[i] = run_params.qSsn[i][j] * phi * sfr;
          		yCasn[i] = run_params.qCasn[i][j] * phi * sfr;
          		yFesn[i] = run_params.qFesn[i][j] * phi * sfr;
        	}
        	else {
        	yCsn[i] = 0;
        	yOsn[i] = 0;
        	yMgsn[i] = 0;
        	ySisn[i] = 0;
        	ySsn[i] = 0;
        	yCasn[i] = 0;
        	yFesn[i] = 0;
        	}
    	}
     //printf("SNII done \n");
     }

    else if(run_params.SNIIYields == 1) //Nomoto et al. 2006
    {
        i = j = 0;
        double Z_std[METALGRID] = {0.0, 0.001, 0004, 0.02};
        double sfrz = metallicity;
        if (sfrz < Z_std[0]){
                sfrz = Z_std[0];}
        else if (sfrz > Z_std[3]){
                sfrz = Z_std[3];}

        for (i=0; i<METALGRID; i++) {
                if(Z_std[i] <= sfrz && Z_std[i] > sfrz) {
                        j = i;
                }
        }

        for(i=0; i<run_params.countsn; i++){
                if(run_params.msn[i] != 0) {
                        m_sn[i] = run_params.msn[i];
                        phi = compute_imf(run_params.msn[i]);
                        taum = compute_taum(run_params.msn[i]);
                        time = age[galaxies[p].SnapNum] - taum;
                        if(time < run_params.lbtime[0]){
                                sfr=0;
                        }
                         else {
                        sfr = interpolate_arr(age, sfh, SNAPLEN, time);
                        }
                        yCsn[i] = run_params.qCsn[i][j] * phi * sfr;
                        yOsn[i] = run_params.qOsn[i][j] * phi * sfr;
                        yMgsn[i] = run_params.qMgsn[i][j] * phi * sfr;
                        ySisn[i] = run_params.qSisn[i][j] * phi * sfr;
                        ySsn[i] = run_params.qSsn[i][j] * phi * sfr;
                        yCasn[i] = run_params.qCasn[i][j] * phi * sfr;
                        yFesn[i] = run_params.qFesn[i][j] * phi * sfr;
                }
                else {
                yCsn[i] = 0;
                yOsn[i] = 0;
                yMgsn[i] = 0;
                ySisn[i] = 0;
                ySsn[i] = 0;
                yCasn[i] = 0;
                yFesn[i] = 0;
                }
        }
     //printf("SNII done \n");
     }

    //printf("start integration");
    yCr_snia = A * integrate_arr(mbin, yCrphi, count, mbin[0], up_binary);
    yNi_snia = A * integrate_arr(mbin, yNiphi, count, mbin[0], up_binary);
    yFe_snia = A * integrate_arr(mbin, yFephi, count, mbin[0], up_binary);
    yC_agb = (1-A) * integrate_arr(m_agb, yCagb, run_params.countagb, low_binary, up_agb) + integrate_arr(m_agb, yCagb, run_params.countagb, low_agb, low_binary);
    yN_agb = (1-A) * integrate_arr(m_agb, yNagb, run_params.countagb, low_binary, up_agb) + integrate_arr(m_agb, yNagb, run_params.countagb, low_agb, low_binary);
    yO_agb = (1-A) * integrate_arr(m_agb, yOagb, run_params.countagb, low_binary, up_agb) + integrate_arr(m_agb, yOagb, run_params.countagb, low_agb, low_binary);
    yC_sn =  (1-A) * integrate_arr(m_sn, yCsn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, yCsn, run_params.countsn, up_binary, up_sn);
    yO_sn =  (1-A) * integrate_arr(m_sn, yOsn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, yOsn, run_params.countsn, up_binary, up_sn);
    yMg_sn =  (1-A) * integrate_arr(m_sn, yMgsn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, yMgsn, run_params.countsn, up_binary, up_sn);
    ySi_sn =  (1-A) * integrate_arr(m_sn, ySisn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, ySisn, run_params.countsn, up_binary, up_sn);
    yS_sn =  (1-A) * integrate_arr(m_sn, ySsn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, ySsn, run_params.countsn, up_binary, up_sn);
    yCa_sn =  (1-A) * integrate_arr(m_sn, yCasn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, yCasn, run_params.countsn, up_binary, up_sn);
    yFe_sn =  (1-A) * integrate_arr(m_sn, yFesn, run_params.countsn, low_sn, up_binary) + integrate_arr(m_sn, yFesn, run_params.countsn, up_binary, up_sn);
    //printf("end integration");

    double yield_agb = yC_agb + yN_agb + yO_agb;
    double yield_snia = yCr_snia + yNi_snia + yFe_snia;
    double yield_sn = yC_sn + yO_sn + yMg_sn + ySi_sn + yS_sn + yCa_sn + yFe_sn;
    yield = (yield_agb + yield_snia + yield_sn);

//overall metallicity
  if(galaxies[p].ColdGas > 5.0e-2) {
        galaxies[p].MetalsColdGas += yield * dt;
  }
  else {
        galaxies[centralgal].MetalsHotGas += yield * dt;
  }
  XPRINT(galaxies[p].MetalsColdGas <= galaxies[p].ColdGas, "metallicity = %.3f, cold gas mass = %.3e, galaxy ID = %i \n", galaxies[p].MetalsColdGas / galaxies[p].ColdGas, galaxies[p].ColdGas, galaxies[p].GalaxyNr);

//mass of each element formed in each production channel, should satisfy same condition with overall metallicity
  if(galaxies[p].ColdGas > 1.0e-2) {
        Cr_snia = yCr_snia * dt;
        Fe_snia = yFe_snia * dt;
        Ni_snia = yNi_snia * dt;
        C_agb = yC_agb * dt;
        N_agb = yN_agb * dt;
        O_agb = yO_agb * dt;
        C_sn = yC_sn * dt;
        O_sn = yO_sn * dt;
        Mg_sn = yMg_sn * dt;
        Si_sn = ySi_sn * dt;
        S_sn = yS_sn * dt;
        Ca_sn = yCa_sn * dt;
        Fe_sn = yFe_sn * dt;

        double dustdot = 0;

        //produce dust
        //double dust_agb, dust_sn, dust_snia;
        double delta_agb = 0.2;  //should be free parameter!! (value based on Popping et al. 2017)
        double delta_sn = 0.2;  //should be free parameter!! (value based on Popping et al. 2017)
        double delta_snia = 0.15;  //should be free parameter!! (value based on Popping et al. 2017)

        assert(dt > 0 && "dt must be greater than 0");

        /* eq 4 and eq 5 Popping et al. 2017 */
        //from AGB
        if (C_agb/O_agb > 1) {
           dustdot += delta_agb * (C_agb - 0.75*O_agb) / dt;
        }
        else {
           dustdot += delta_agb * (C_agb + N_agb + O_agb) / dt;
        }

        /* eq 6 Popping et al. 2017 */
        //from SN
        dustdot += delta_sn * C_sn / dt;
        dustdot += delta_sn * O_sn / dt;
        dustdot += 16 * delta_sn * (Mg_sn/24 + Si_sn/28 + S_sn/32 + Ca_sn/40 + Fe_sn/56) / dt;

        /* eq 6 Popping et al. 2017 */
        //from SNIa
        //printf("dustdot before = %f \n", dustdot);
	//dustdot += 16 * delta_snia * (Fe_snia/56) / dt;
        //dustdot += delta_snia * (Cr_snia + Ni_snia) / dt;

	//printf("dustdot after = %f \n", dustdot);
      	galaxies[p].ColdDust += dustdot * dt;
	galaxies[p].MetalsColdGas -= dustdot * dt;
        galaxies[p].dustdotform[step] += dustdot;
        XPRINT(dustdot * dt >= 0, "dust mass = %.3e, delta dust = %.3e, galaxy id = %i \n", galaxies[p].ColdDust, dustdot*dt, galaxies[p].GalaxyNr);

   }
}

void accrete_dust(const double metallicity, const double dt, const int p, const int step, struct GALAXY *galaxies) {
//dust accretion in ISM : eq 20 Asano13
  double dustdot = 0;
  double tacc_zero = 20 * SEC_PER_MEGAYEAR / run_params.UnitTime_in_s; //should be free parameter! yr

  if (galaxies[p].MetalsColdGas > galaxies[p].ColdDust && metallicity > 0  ) {
    double tacc = tacc_zero * 0.02 / metallicity;
    dustdot += (1 - galaxies[p].ColdDust/galaxies[p].MetalsColdGas) * (galaxies[p].f_H2 * galaxies[p].ColdDust / tacc);

    galaxies[p].dustdotgrowth[step] += dustdot;
    galaxies[p].ColdDust += dustdot * dt;
    galaxies[p].MetalsColdGas -= dustdot * dt;
    XPRINT(galaxies[p].ColdDust >= 0, "dust mass = %.3e, delta dust = %.3e, galaxy id = %i \n", galaxies[p].ColdDust, dustdot*dt, galaxies[p].GalaxyNr);
  }

}

void destruct_dust(const double metallicity, const double stars, const double dt, const int p, const int step,  struct GALAXY *galaxies) {
//dust destruction : Asano et al. 13
  int i;
  double sfr;
  double dustdot = 0;
  double age[SNAPLEN], sfh[SNAPLEN];
  double m_low = 8; //lower limit of stellar mass that end up as SN
  double up_binary = 16; //upper limit for binary stars, Msun
  double m_up = 40; //upper limit of stellar mass that end up as SN
  double t_low = compute_taum(m_up); //the shortest stellar lifetime that end up as SN
  double eta = 0.1; //should be free parameter, we used value adopted by Asano et al. 13
  double m_swept = 1535 * pow((metallicity/0.02 + 0.039), -0.289) * run_params.Hubble_h / 1.e10; //eq. 14 Asano et al. 13
  int count = 20;
  double mass[count], phi[count], mphi[count];
  double A = run_params.BinaryFraction;
//  double m_swept = 600 * run_params.Hubble_h / 1.e10;

  for (i=0; i<SNAPLEN; i++)
  {
        age[i] = run_params.lbtime[i]; //change the variable name to represent the age of universe instead of lbtime
        sfh[i] = galaxies[p].Sfr[i];
  }

//Dwek & Cherchneff 2011
  if (age[galaxies[p].SnapNum] > t_low) {
    for(i=0; i<count; i++) {
        mass[i] = 1 + ((m_up - 1) / (count-i));
        phi[i] = compute_imf(mass[i]);
        mphi[i] = mass[i]*phi[i];
    }
/*
    for (i=0; i<count; i++){
	mass[i] = m_low + ((m_up - m_low) / (count-i));
	phi[i] = compute_imf(mass[i]);
        double taum = compute_taum(mass[i]);
        double time = age[galaxies[p].SnapNum] - taum;
        if(time < run_params.lbtime[0]){
                sfr=0;
        }
        else {
                sfr = interpolate_arr(age, sfh, SNAPLEN, time);
        }
	mphi[i] = sfr*phi[i];
    }
*/

    double mstar = integrate_arr(mass, mphi, count, mass[0], m_up) / integrate_arr(mass, phi, count, m_low, m_up);
    double mstar_ia = integrate_arr(mass, mphi, count, mass[0], m_up) / integrate_arr(mass, phi, count, m_low, up_binary);
    double mstar_ii = integrate_arr(mass, mphi, count, mass[0], m_up) / integrate_arr(mass, phi, count, up_binary, m_up);
    double Rsn_ia = A*stars / dt / mstar_ia;
    double Rsn_ii = (1-A) * stars / dt / mstar_ia + stars/dt/mstar_ii;
    //printf("%f \n", Rsn_ii/Rsn_ia);
    double Rsn = stars / dt / mstar;
    assert(m_swept > 0 && "mass of ISM swept by SN must be greater than 0");

//    if (Rsn > 0 && galaxies[p].ColdGas > 0 && galaxies[p].f_HI >0) {
    if (Rsn > 0 && galaxies[p].ColdGas > 0) {
       Rsn *=  run_params.UnitMass_in_g / run_params.UnitTime_in_s * SEC_PER_YEAR / SOLAR_MASS; //convert to Msun/yr
//       double tsn = galaxies[p].ColdGas * galaxies[p].f_HI/ (eta * m_swept * Rsn); //eq.12 Asano et al 13 [yr]
       double tsn = galaxies[p].ColdGas / (eta * m_swept * Rsn); //eq.12 Asano et al 13 [yr]
       tsn /= run_params.UnitTime_in_s / SEC_PER_YEAR; //back to internal unit
//       printf("coldgas = %.3e, dust = %.3e, m_swept = %.3e, Rsn = %.3e,  tsn = %.3e, dt = %.3e Myr \n", galaxies[p].ColdGas, galaxies[p].ColdDust,  m_swept, Rsn, tsn * run_params.UnitTime_in_s / SEC_PER_MEGAYEAR, dt * run_params.UnitTime_in_s / SEC_PER_MEGAYEAR);
//       assert(tsn > 0 && "tsn must be greater than 0");
       dustdot += galaxies[p].ColdDust / tsn;
    }

    if (galaxies[p].ColdDust - dustdot * dt > 0) {
  	  galaxies[p].dustdotdestruct[step] += dustdot;
  	  galaxies[p].ColdDust -= dustdot * dt;
  	  galaxies[p].MetalsColdGas += dustdot * dt;
    }
    else {
	galaxies[p].dustdotdestruct[step] += dustdot;
	galaxies[p].MetalsColdGas += galaxies[p].ColdDust;
	galaxies[p].ColdDust = 0;
    }

  }
}

void dust_thermal_sputtering(const int gal, const double dt, struct GALAXY *galaxies) //section 3.5 Popping 2017
{
   double adot;
   double a = 1e-5 / run_params.UnitLength_in_cm; //a = 0.1 micrometer
   double mdot = 0.0;

// thermal sputtering in hot dust
   if(galaxies[gal].HotDust > 0.0 && galaxies[gal].Vvir > 0.0) {
        const double temp0 = 2e6; //in Kelvin
        const double temp = 35.9 * galaxies[gal].Vvir * galaxies[gal].Vvir; //in Kelvin
        const double gamma = 2.5;

        double x = -3.2e-18 / PROTONMASS / (pow(temp0/temp, gamma) + 1); //in cm^4 / g /s
        x /= run_params.UnitLength_in_cm / run_params.UnitDensity_in_cgs / run_params.UnitTime_in_s; //in internal unit
        double rho = galaxies[gal].HotGas / (4 / 3 * M_PI * galaxies[gal].Rvir * galaxies[gal].Rvir * galaxies[gal].Rvir);
        adot = rho / x;
        a -= adot * dt;

        assert(a > 0);
        a /= 1e-5 / run_params.UnitLength_in_cm; //in 0.1 micrometer unit
        rho *= run_params.UnitDensity_in_cgs;
        const double tau0 = 170 * SEC_PER_MEGAYEAR / run_params.UnitTime_in_s; // 0.17Gyr to internal unit
        const double tau = tau0 * (a/rho) * (pow(temp0/temp, gamma) + 1);
        mdot = galaxies[gal].HotDust / tau / 3;

        galaxies[gal].HotDust -= mdot * dt;
        galaxies[gal].MetalsHotGas += mdot * dt;
   }

// thermal sputtering in ejected dust
   if(galaxies[gal].EjectedDust > 0.0 && galaxies[gal].Vvir > 0.0) {
        const double temp0 = 2e6; //in Kelvin
        const double temp = 35.9 * galaxies[gal].Vvir * galaxies[gal].Vvir; //in Kelvin
        const double gamma = 2.5;

        double x = -3.2e-18 / PROTONMASS / (pow(temp0/temp, gamma) + 1); //in cm^4 / g /s
        x /= run_params.UnitLength_in_cm / run_params.UnitDensity_in_cgs / run_params.UnitTime_in_s; //in internal unit
        double rho = galaxies[gal].EjectedMass / (4 / 3 * M_PI * galaxies[gal].Rvir * galaxies[gal].Rvir * galaxies[gal].Rvir);
        adot = rho / x;
        a -= adot * dt;

        assert(a > 0);
        a /= 1e-5 / run_params.UnitLength_in_cm; //in 0.1 micrometer unit
        rho *= run_params.UnitDensity_in_cgs;
        const double tau0 = 170 * SEC_PER_MEGAYEAR / run_params.UnitTime_in_s; // 0.17Gyr to internal unit
        const double tau = tau0 * (a/rho) * (pow(temp0/temp, gamma) + 1);
        mdot = galaxies[gal].EjectedDust / tau / 3;

        galaxies[gal].EjectedDust -= mdot * dt;
        galaxies[gal].MetalsEjectedMass += mdot * dt;
   }

}

double integrate_arr(const double arr1[MAX_STRING_LEN], const double arr2[MAX_STRING_LEN], const int npts, const double lower_limit, const double upper_limit)
{
        double Q;
        gsl_interp_accel *acc;
        gsl_spline *spl;

        acc = gsl_interp_accel_alloc ();
        spl = gsl_spline_alloc (gsl_interp_linear, npts);

        gsl_spline_init (spl, arr1, arr2, npts);
        Q = gsl_spline_eval_integ (spl, lower_limit, upper_limit, acc);

        gsl_spline_free (spl);
        gsl_interp_accel_free (acc);

        return Q;
}

double interpolate_arr(const double arr1[MAX_STRING_LEN], const double arr2[MAX_STRING_LEN], const int npts, const int xi)
{
        double Q;
        gsl_interp_accel *acc;
        gsl_spline *spl;

        acc = gsl_interp_accel_alloc ();
        spl = gsl_spline_alloc (gsl_interp_linear, npts);

        gsl_spline_init (spl, arr1, arr2, npts);
        Q = gsl_spline_eval (spl, xi, acc);

        gsl_spline_free (spl);
        gsl_interp_accel_free (acc);
        return Q;
}

double compute_imf (const double m)
{ //eq 11 Arrigoni et al. 2010
        double mass = m;
        double A = 0.9098, B = 0.2539, x = 1.3, sigma=0.69;
        double mc = 0.079; //Msun
        double phi;

        if (m < 1){
                phi = A * exp( -(log10(mass) - log10(mc)) / (2*sigma*sigma));
                }
        else {
                phi = B * pow(mass, -x);
                }

        return phi;
}

double compute_taum (const double m)
{ //eq 3 Raiteri et al. 1996
        double mass = m;
        double Z = 0.02;
        double a0 = 10.13 + 0.07547*log10(Z) - 0.008084*log10(Z)*log10(Z);
        double a1 = -4.424 - 0.7939*log10(Z) - 0.1187*log10(Z)*log10(Z);
        double a2 = 1.262 + 0.3385*log10(Z) + 0.05417*log10(Z)*log10(Z);

        double logt = a0 + a1*log10(mass) + a2*log10(mass)*log10(mass);
        double t = pow(10, logt) / 1e6  ; //in Myr/h

        /* convert into Myrs/h */
        return t;
}


/* ode-initval2/rk2imp.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* Based on rk2imp.c by Gerard Jungman */

/* Runge-Kutta 2, Gaussian implicit. Also known as implicit midpoint
   rule. Error estimation is carried out by the step doubling
   method.
 */

/* Reference: Ascher, U.M., Petzold, L.R., Computer methods for
   ordinary differential and differential-algebraic equations, SIAM,
   Philadelphia, 1998.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

#include "odeiv_util.h"
#include "rksubs.c"
#include "modnewton1.c"

/* Stage of method */
#define RK2IMP_STAGE 1

typedef struct
{
  gsl_matrix *A;                /* Runge-Kutta coefficients */
  double *y_onestep;            /* Result with one step */
  double *y_twostep;            /* Result with two half steps */
  double *ytmp;                 /* Temporary work space */
  double *y_save;               /* Backup space */
  double *YZ;                   /* Runge-Kutta points */
  double *fYZ;                  /* Derivatives at YZ */
  gsl_matrix *dfdy;             /* Jacobian matrix */
  double *dfdt;                 /* time derivative of f */
  modnewton1_state_t *esol;     /* nonlinear equation solver */
  double *errlev;               /* desired error level of y */
  const gsl_odeiv2_driver *driver;      /* pointer to driver object */
}
rk2imp_state_t;

static void *
rk2imp_alloc (size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) malloc (sizeof (rk2imp_state_t));

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

  state->A = gsl_matrix_alloc (RK2IMP_STAGE, RK2IMP_STAGE);

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

  state->y_onestep = (double *) malloc (dim * sizeof (double));

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

  state->y_twostep = (double *) malloc (dim * sizeof (double));

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

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y_save = (double *) malloc (dim * sizeof (double));

  if (state->y_save == 0)
    {
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_save", GSL_ENOMEM);
    }

  state->YZ = (double *) malloc (dim * RK2IMP_STAGE * sizeof (double));

  if (state->YZ == 0)
    {
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for YZ", GSL_ENOMEM);
    }

  state->fYZ = (double *) malloc (dim * RK2IMP_STAGE * sizeof (double));

  if (state->fYZ == 0)
    {
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for fYZ", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->dfdy = gsl_matrix_alloc (dim, dim);

  if (state->dfdy == 0)
    {
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->esol = modnewton1_alloc (dim, RK2IMP_STAGE);

  if (state->esol == 0)
    {
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for esol", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      modnewton1_free (state->esol);
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->driver = NULL;

  return state;
}

static int
rk2imp_apply (void *vstate, size_t dim, double t, double h,
              double y[], double yerr[],
              const double dydt_in[], double dydt_out[],
              const gsl_odeiv2_system * sys)
{
  /* Makes a Gaussian implicit 4th order Runge-Kutta step with size h
     and estimates the local error of the step by step doubling.
   */

  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  double *const y_onestep = state->y_onestep;
  double *const y_twostep = state->y_twostep;
  double *const ytmp = state->ytmp;
  double *const y_save = state->y_save;
  double *const YZ = state->YZ;
  double *const fYZ = state->fYZ;
  gsl_matrix *const dfdy = state->dfdy;
  double *const dfdt = state->dfdt;
  double *const errlev = state->errlev;

  const modnewton1_state_t *esol = state->esol;

  /* Runge-Kutta coefficients */
  gsl_matrix *A = state->A;
  const double b[] = { 1.0 };
  const double c[] = { 0.5 };
  gsl_matrix_set (A, 0, 0, 0.5);

  if (esol == NULL)
    {
      GSL_ERROR ("no non-linear equation solver speficied", GSL_EINVAL);
    }

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

  /* Evaluate Jacobian for modnewton1 */

  {
    int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Calculate a single step with size h */

  {
    int s = modnewton1_init ((void *) esol, A, h, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h, YZ, fYZ);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (y_onestep, h, y, fYZ, b, RK2IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Error estimation by step doubling */

  {
    int s = modnewton1_init ((void *) esol, A, h / 2.0, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* 1st half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h / 2.0, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      return s;
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h / 2.0, YZ, fYZ);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (ytmp, h / 2.0, y, fYZ, b, RK2IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Save original y values in case of error */

  DBL_MEMCPY (y_save, y, dim);

  /* 2nd half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t + h / 2.0, h / 2.0,
                              ytmp, sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0 + c[0] * h / 2.0, YZ, fYZ);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Note: rk2imp returns y using the results from two half steps
       instead of the single step since the results are freely
       available and more precise.
     */

    int s = rksubs (y_twostep, h / 2.0, ytmp, fYZ, b, RK2IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      {
        DBL_MEMCPY (y, y_save, dim);
        return s;
      }
  }

  DBL_MEMCPY (y, y_twostep, dim);

  /* Error estimation */

  {
    size_t i;
    for (i = 0; i < dim; i++)
      {
        yerr[i] = ODEIV_ERR_SAFETY * 0.5 *
          fabs (y_twostep[i] - y_onestep[i]) / 3.0;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          /* Restore original values */
          DBL_MEMCPY (y, y_save, dim);

          return s;
        }
    }

  return GSL_SUCCESS;
}

static int
rk2imp_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
rk2imp_reset (void *vstate, size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y_twostep, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y_save, dim);
  DBL_ZERO_MEMSET (state->YZ, dim);
  DBL_ZERO_MEMSET (state->fYZ, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk2imp_order (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;
  state = 0;                    /* prevent warnings about unused parameters */
  return 2;
}

static void
rk2imp_free (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  free (state->errlev);
  modnewton1_free (state->esol);
  gsl_matrix_free (state->dfdy);
  free (state->dfdt);
  free (state->fYZ);
  free (state->YZ);
  free (state->y_save);
  free (state->ytmp);
  free (state->y_twostep);
  free (state->y_onestep);
  gsl_matrix_free (state->A);
  free (state);
}

static const gsl_odeiv2_step_type rk2imp_type = {
  "rk2imp",                     /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &rk2imp_alloc,
  &rk2imp_apply,
  &rk2imp_set_driver,
  &rk2imp_reset,
  &rk2imp_order,
  &rk2imp_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_rk2imp = &rk2imp_type;


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/

/*

Performs y = ( G^T G )^{-1} ( G^T K G ) ( G^T G )^{-1} x
This preconditioner is defined as a PC rather than a MatShell
as we can define and compute the inverse easily without the
need of an additional KSP. Compare this to Q_S = G^T Q_K^-1 G,
where Q_S^-1 can only be performed implicitly via an iterative
method.

*/
#if 1

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

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>
#include <petscksp.h>
#include <petscpc.h>

#include "common-driver-utils.h"

#include <petscversion.h>
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR >=3) )
  #if (PETSC_VERSION_MINOR >=6)
     #include "petsc/private/pcimpl.h"
     #include "petsc/private/kspimpl.h"
  #else
     #include "petsc-private/pcimpl.h"
     #include "petsc-private/kspimpl.h"
  #endif
#else
  #include "private/pcimpl.h"
  #include "private/kspimpl.h"
#endif

#include "pc_GtKG.h"

#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>

#define PCTYPE_GtKG "gtkg"

/* private data */

typedef struct {
	Mat         G, K;   /* G \in [M x N], K \in [M x M] */
	Mat         M;      /* the velocity mass matrix */
	Vec         inv_diag_M;
	PetscTruth  form_GtG; /* don't allow anything else yet */
	Mat         GtG;
	KSP         ksp;
	Vec         s,t,X;  /* s \in [M], t \in [N], X \in [M] */
	PetscTruth  monitor_activated;
	PetscTruth  monitor_rhs_consistency;
} _PC_GtKG;

typedef _PC_GtKG* PC_GtKG;

/* private prototypes */
PetscErrorCode BSSCR_PCApply_GtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCApplyTranspose_GtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCSetUp_GtKG( PC pc );

PetscErrorCode BSSCR_pc_warn( PC pc, const char func_name[] )
{
	const PCType type;
	PCGetType( pc, &type );

	if( strcmp(type,PCTYPE_GtKG)!=0 ) {
		printf("Warning(%s): PC type (%s) should be gtkg \n",func_name, type );
		PetscFunctionReturn(0);
	}
	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_pc_error( PC pc, const char func_name[] )
{
	const PCType type;
	PCGetType( pc, &type );

	if( strcmp(type,PCTYPE_GtKG)!=0 ) {
		printf("Error(%s): PC type (%s) should be gtkg \n",func_name, type );
		PetscFinalize();
		exit(0);
	}
	PetscFunctionReturn(0);
}

void BSSCR_get_number_nonzeros_AIJ( Mat A, PetscInt *nnz )
{
	MatInfo info;

	MatGetInfo( A, MAT_GLOBAL_SUM, &info );
	*nnz = info.nz_used;

}

/*
I should not modify setup called!!
This is handled via petsc.
*/
PetscErrorCode BSSCR_PCSetUp_GtKG( PC pc )
{
	PC_GtKG    ctx = (PC_GtKG)pc->data;
	PetscReal  fill;
	Mat        Ident;
	Vec        diag;
	PetscInt   M,N, m,n;
	MPI_Comm   comm;
	PetscInt   nnz_I, nnz_G;
	const MatType mtype;
	const char *prefix;
	PetscTruth wasSetup;

	if( ctx->K == PETSC_NULL ) {
		Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: K not set" );
	}
	if( ctx->G == PETSC_NULL ) {
		Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: G not set" );
	}

	PetscObjectGetComm( (PetscObject)ctx->K, &comm );

	/* Check for existence of objects and trash any which exist */
	if( ctx->form_GtG == PETSC_TRUE && ctx->GtG != PETSC_NULL ) {
		Stg_MatDestroy(&ctx->GtG );
		ctx->GtG = PETSC_NULL;
	}

	if( ctx->s != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->s );
		ctx->s = PETSC_NULL;
	}
	if( ctx->X != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->X );
		ctx->X = PETSC_NULL;
	}
	if( ctx->t != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->t );
		ctx->t = PETSC_NULL;
	}
	if( ctx->inv_diag_M != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->inv_diag_M );
		ctx->inv_diag_M = PETSC_NULL;
	}

	/* Create vectors */
	MatGetVecs( ctx->K, &ctx->s, &ctx->X );
	MatGetVecs( ctx->G, &ctx->t, PETSC_NULL );

	if( ctx->M != PETSC_NULL ) {
		MatGetVecs( ctx->K, &ctx->inv_diag_M, PETSC_NULL );
		MatGetDiagonal( ctx->M, ctx->inv_diag_M );
		VecReciprocal( ctx->inv_diag_M );

		/* change the pc_apply routines */
		pc->ops->apply          = BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling;
		pc->ops->applytranspose = BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling;
	}

	/* Assemble GtG */
	MatGetSize( ctx->G, &M, &N );
	MatGetLocalSize( ctx->G, &m, &n );

	MatGetVecs( ctx->G, PETSC_NULL, &diag );
	VecSet( diag, 1.0 );

	MatCreate( comm, &Ident );
	MatSetSizes( Ident, m,m , M, M );
#if (((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=3)) || (PETSC_VERSION_MAJOR>3) )
        MatSetUp(Ident);
#endif

	MatGetType( ctx->G, &mtype );
	MatSetType( Ident, mtype );

	if( ctx->M == PETSC_NULL ) {
		MatDiagonalSet( Ident, diag, INSERT_VALUES );
	}
	else {
		MatDiagonalSet( Ident, ctx->inv_diag_M, INSERT_VALUES );
	}

	BSSCR_get_number_nonzeros_AIJ( Ident, &nnz_I );
	BSSCR_get_number_nonzeros_AIJ( ctx->G, &nnz_G );
	//fill = 1.0;
	/*
	Not sure the best way to estimate the fill factor.
	GtG is a laplacian on the pressure space.
	This might tell us something useful...
	*/
	fill = (PetscReal)(nnz_G)/(PetscReal)( nnz_I );
	MatPtAP( Ident, ctx->G, MAT_INITIAL_MATRIX, fill, &ctx->GtG );

	Stg_MatDestroy(&Ident);
	Stg_VecDestroy(&diag );

	Stg_KSPSetOperators( ctx->ksp, ctx->GtG, ctx->GtG, SAME_NONZERO_PATTERN );

	if (!pc->setupcalled) {
		wasSetup = PETSC_FALSE;

		PCGetOptionsPrefix( pc,&prefix );
		KSPSetOptionsPrefix( ctx->ksp, prefix );
		KSPAppendOptionsPrefix( ctx->ksp, "pc_gtkg_" ); /* -pc_GtKG_ksp_type <type>, -ksp_GtKG_pc_type <type> */
	}
	else {
		wasSetup = PETSC_TRUE;
	}

//	if (!wasSetup && pc->setfromoptionscalled) {
	if (!wasSetup) {
		KSPSetFromOptions(ctx->ksp);
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCDestroy_GtKG( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	if( ctx == PETSC_NULL ) {	PetscFunctionReturn(0); }

	if( ctx->form_GtG == PETSC_TRUE && ctx->GtG != PETSC_NULL ) {
		Stg_MatDestroy(&ctx->GtG );
	}
	if( ctx->ksp != PETSC_NULL ) {	Stg_KSPDestroy(&ctx->ksp );		}
	if( ctx->s != PETSC_NULL ) {	Stg_VecDestroy(&ctx->s );		}
	if( ctx->X != PETSC_NULL ) {	Stg_VecDestroy(&ctx->X );		}
	if( ctx->t != PETSC_NULL ) {	Stg_VecDestroy(&ctx->t );		}
	if( ctx->inv_diag_M != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->inv_diag_M );
		ctx->inv_diag_M = PETSC_NULL;
	}

	PetscFree( ctx );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCView_GtKG( PC pc, PetscViewer viewer )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	PetscViewerASCIIPushTab(viewer); //1

	if( ctx->M == PETSC_NULL ) {
		PetscViewerASCIIPrintf( viewer, "gtkg: Standard \n" );
	}else {
		PetscViewerASCIIPrintf( viewer, "gtkg: Least Squares Commutator \n" );
	}

	PetscViewerASCIIPrintf( viewer, "gtkg-ksp \n" );
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");
	PetscViewerASCIIPushTab(viewer);
		KSPView( ctx->ksp, viewer );
	PetscViewerASCIIPopTab(viewer);
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");

	PetscViewerASCIIPopTab(viewer); //1

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_Lp_monitor( KSP ksp, PetscInt index )
{
	PetscInt max_it;
	PetscReal rnorm;
	KSPConvergedReason reason;

	KSPGetIterationNumber( ksp, &max_it );
	KSPGetResidualNorm( ksp, &rnorm );
	KSPGetConvergedReason( ksp, &reason );
	if (ksp->reason > 0) {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve converged. its.=%.4d ; |r|=%5.5e ; Reason=%s\n",
				index, max_it, rnorm, KSPConvergedReasons[reason] );
	} else {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve did not converge. its.=%.4d ; |r|=%5.5e ; Reason=%s\n",
				index, max_it, rnorm, KSPConvergedReasons[reason]);
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCBFBTSubKSPMonitor( KSP ksp, PetscInt index, PetscLogDouble time )
{
        PetscInt max_it;
        PetscReal rnorm;
        KSPConvergedReason reason;

        KSPGetIterationNumber( ksp, &max_it );
        KSPGetResidualNorm( ksp, &rnorm );
        KSPGetConvergedReason( ksp, &reason );
        PetscPrintf(((PetscObject)ksp)->comm,"    PCBFBTSubKSP (%d): %D Residual norm; r0 %12.12e, r %12.12e: Reason %s: Time %5.5e \n",
		index, max_it, ksp->rnorm0, rnorm, KSPConvergedReasons[reason], time );

        PetscFunctionReturn(0);
}

/*
Checks rhs of Lp systems is in the null space of Lp, i.e. {rhs} \centerdot {null_space} = 0
Since Lp should contain the null space {1}, we just check the \sum_i rhs_i = 0
*/
PetscErrorCode BSSCRBSSCR_Lp_monitor_check_rhs_consistency( KSP ksp, Vec rhs, PetscInt index )
{
	PetscScalar dot;
#if 0
	Vec one;

	VecDuplicate( rhs, &one );
	VecSet( one, 1.0 );
	VecDot( rhs, one, &dot );
	if( PetscAbsReal(dot) > 1.0e-8 ) {
		PetscPrintf(ksp->comm,"    ($D) Lp z = r: ******* WARNING ******* RHS is not consistent. {b}.{1} = %5.5e \n", index, dot );
	}

	Stg_VecDestroy(&one );
	PetscFunctionReturn(0);
#endif
#if 0
	VecSum( rhs, &dot );
	if( PetscAbsReal(dot) > 1.0e-8 ) {
		PetscPrintf(((PetscObject)ksp)->comm,"    (%D) Lp z = r: ******* WARNING ******* RHS is not consistent. {b}.{1} = %5.5e \n",
				index, dot );
		BSSCR_VecRemoveConstNullspace( rhs, PETSC_NULL );
	}
#endif
	PetscFunctionReturn(0);
}

/*
Performs y <- S^{-1} x
S^{-1} = ( G^T G )^{-1} G^T K G ( G^T G )^{-1}
*/
PetscErrorCode BSSCR_PCApply_GtKG( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,1,(t1-t0));		}

	MatMult( G, t, s ); /* s <- G t */
	MatMult( K, s, X ); /* X <- K s */
	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,2,(t1-t0));		}

	PetscFunctionReturn(0);
}

/* Need to check this one if correct */
/*
S^{-1} = ( G^T G )^{-1} G^T K G ( G^T G )^{-1}
       = A C A
S^{-T} = A^T (A C)^T
       = A^T C^T A^T, but A = G^T G which is symmetric
       = A C^T A
       = A G^T ( G^T K )^T A
       = A G^T K^T G A

*/
PetscErrorCode BSSCR_PCApplyTranspose_GtKG( PC pc, Vec x, Vec y )
{

	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,1,(t1-t0));		}

	MatMult( G, t, s ); /* s <- G t */
	MatMultTranspose( K, s, X ); /* X <- K^T s */
	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,2,(t1-t0));		}

	PetscFunctionReturn(0);
}

/*
Performs y <- S^{-1} x
S^{-1} = ( G^T Di G )^{-1} G^T Di K Di G ( G^T Di G )^{-1}
where Di = diag(M)^{-1}
*/

PetscErrorCode BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X,di;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	di = ctx->inv_diag_M;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	KSPSolve( ksp, x, t );       /* t <- GtG_inv x */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	MatMult( G, t, s );          /* s <- G t */
	VecPointwiseMult( s, s,di ); /* s <- s * di */

	MatMult( K, s, X );          /* X <- K s */
	VecPointwiseMult( X, X,di ); /* X <- X * di */

	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	KSPSolve( ksp, t, y );       /* y <- GtG_inv t */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X,di;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	di = ctx->inv_diag_M;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	KSPSolve( ksp, x, t );       /* t <- GtG_inv x */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,1);		}

	MatMult( G, t, s );          /* s <- G t */
	VecPointwiseMult( s, s,di ); /* s <- s * di */

	MatMultTranspose( K, s, X ); /* X <- K^T s */
	VecPointwiseMult( X, X,di ); /* X <- X * di */

	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	KSPSolve( ksp, t, y );       /* y <- GtG_inv t */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	PetscFunctionReturn(0);
}

/*
Only the options related to GtKG should be set here.
*/
PetscErrorCode BSSCR_PCSetFromOptions_GtKG( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;
	PetscTruth ivalue, flg;

	PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor", &ivalue, &flg );
	if( flg==PETSC_TRUE ) {
		ctx->monitor_activated = ivalue;
	}

	PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor_rhs_consistency", &ivalue, &flg );
	if( flg==PETSC_TRUE ) {
		ctx->monitor_rhs_consistency = ivalue;
	}

	PetscFunctionReturn(0);
}

/* ---- Exposed functions ---- */

PetscErrorCode BSSCR_PCCreate_GtKG( PC pc )
{
	PC_GtKG         pc_data;
	PetscErrorCode  ierr;

	/* create memory for ctx */
	ierr = Stg_PetscNew( _PC_GtKG,&pc_data);CHKERRQ(ierr);

	/* init ctx */
	pc_data->K   = PETSC_NULL;
	pc_data->G   = PETSC_NULL;
	pc_data->M   = PETSC_NULL;
	pc_data->GtG = PETSC_NULL;
	pc_data->form_GtG          = PETSC_TRUE;
	pc_data->ksp               = PETSC_NULL;
	pc_data->monitor_activated = PETSC_FALSE;
	pc_data->monitor_rhs_consistency = PETSC_FALSE;

	pc_data->s = PETSC_NULL;
	pc_data->t = PETSC_NULL;
	pc_data->X = PETSC_NULL;
	pc_data->inv_diag_M = PETSC_NULL;

	/* create internals */
	KSPCreate( ((PetscObject)pc)->comm, &pc_data->ksp );

	/* set ctx onto pc */
	pc->data  = (void*)pc_data;

	ierr = PetscLogObjectMemory(pc,sizeof(_PC_GtKG));CHKERRQ(ierr);

	/* define operations */
	pc->ops->setup   = BSSCR_PCSetUp_GtKG;
	pc->ops->view    = BSSCR_PCView_GtKG;
	pc->ops->destroy = BSSCR_PCDestroy_GtKG;
	pc->ops->setfromoptions = BSSCR_PCSetFromOptions_GtKG;

	pc->ops->apply          = BSSCR_PCApply_GtKG;
	pc->ops->applytranspose = BSSCR_PCApplyTranspose_GtKG;

	PetscFunctionReturn(0);
}

/*
K & G must different to PETSC_NULL
M can be PETSC_NULL
*/
PetscErrorCode BSSCR_PCGtKGSet_Operators( PC pc, Mat K, Mat G, Mat M )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	ctx->K = K;
	ctx->G = G;
	ctx->M = M;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGSet_OperatorForAlgebraicCommutator( PC pc, Mat M )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	ctx->M = M;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGAttachNullSpace( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;
	MatNullSpace nsp;

	BSSCR_pc_error( pc, "__func__" );

	/* Attach a null space */
	MatNullSpaceCreate( PETSC_COMM_WORLD, PETSC_TRUE, PETSC_NULL, PETSC_NULL, &nsp );
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR <6) )
	KSPSetNullSpace( ctx->ksp, nsp );
#else
    Mat A;
    KSPGetOperators(ctx->ksp,&A,NULL);//Note: DOES NOT increase the reference counts of the matrix, so you should NOT destroy them.
    MatSetNullSpace( A, nsp);
#endif
	/*
	NOTE: This does NOT destroy the memory for nsp, it just decrements the nsp->refct, so that
	the next time MatNullSpaceDestroy() is called, the memory will be released. The next time this
	is called will be by KSPDestroy();
	*/
	MatNullSpaceDestroy( nsp );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGGet_KSP( PC pc, KSP *ksp )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	if( ksp != PETSC_NULL ) {
		(*ksp) = ctx->ksp;
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGSet_KSP( PC pc, KSP ksp )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	if( ctx->ksp != PETSC_NULL ) {
		Stg_KSPDestroy(&ctx->ksp);
	}
	ctx->ksp = ksp;

	PetscFunctionReturn(0);
}
#endif


#pragma once

#include <utility>
#include <initializer_list>
#include <exception>
#include <iostream>
#include <cmath>

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

#include "bits/matrix-view.h"
#include "utils/fcmp.h"
#include "vector.h"

namespace gsl_wrapper
{
  class Matrix
  {
  public:
    // Constructors and destructor
    Matrix(size_t i, size_t j);
    Matrix(size_t matrix_size);
    Matrix(std::initializer_list<std::initializer_list<double>> args);
    Matrix(const Vector &vec);

    Matrix(const Matrix &copy_from);
    Matrix(Matrix &&move_from);

    ~Matrix();

    // Member functions
    auto get_gsl_matrix() const -> gsl_matrix *;
    auto get_dimensions() const -> std::pair<size_t, size_t>;
    auto num_rows() const -> size_t;
    auto num_collumns() const -> size_t;

    // Operators
    auto operator=(const Matrix &copy_from) -> Matrix &;
    auto operator=(Matrix &&move_from) -> Matrix &;

    auto operator==(const Matrix &comparasion_matrix) const -> bool;
    auto operator!=(const Matrix &comparasion_matrix) const -> bool;

    auto operator[](const size_t index) const -> gsl_wrapper::bits::MatrixRow;
    auto operator*(const Matrix &mul) const -> Matrix;
    auto operator*(const double number) const -> Matrix;
    auto operator+(const Matrix &matrix) const -> Matrix;
    auto operator+(const double numer) const -> Matrix;

    // Friend declarations
    friend auto operator<<(std::ostream &stream, const Matrix &matrix) -> std::ostream &;
    friend auto operator*(const double number, const Matrix &matrix) -> Matrix;
    friend auto operator+(const double number, const Matrix &matrix) -> Matrix;

  private:
    gsl_matrix *m_matrixPtr;
    size_t m_numRows;
    size_t m_numCollumns;
  };

  inline Matrix::Matrix(size_t i, size_t j)
      : m_matrixPtr{gsl_matrix_calloc(i, j)}, m_numRows{i}, m_numCollumns{j}
  {
  }

  inline Matrix::Matrix(size_t matrix_size)
      : Matrix(matrix_size, matrix_size)
  {
  }

  inline Matrix::Matrix(std::initializer_list<std::initializer_list<double>> args)
      : m_matrixPtr{nullptr}, m_numCollumns{0}, m_numRows{0}
  {
    if (args.size() == 0)
      return;
    size_t num_rows = args.size();
    size_t num_collumns = (*args.begin()).size();

    // Setting object properties
    m_numCollumns = num_collumns;
    m_numRows = num_rows;
    m_matrixPtr = gsl_matrix_calloc(m_numRows, m_numCollumns);

    size_t row_iterator = 0;
    size_t collumn_iterator = 0;
    for (auto &&row : args)
    {
      if (row.size() != num_collumns)
        throw std::range_error{"Diffrent number of items in diffrent rows when creating matrix"};

      for (auto &&el : row)
      {
        (*this)[row_iterator][collumn_iterator++] = el;
      }
      collumn_iterator = 0;
      ++row_iterator;
    }
  }

  inline Matrix::Matrix(const Vector &vec)
      : m_matrixPtr{gsl_matrix_calloc(vec.size(), 1)},
        m_numCollumns{1},
        m_numRows{vec.size()}
  {
    for (size_t i = 0; i < m_numRows; i++)
    {
      (*this)[i][0] = vec[i];
    }
  }

  inline Matrix::Matrix(const Matrix &copy_from)
      : m_matrixPtr{gsl_matrix_calloc(copy_from.m_numRows, copy_from.m_numCollumns)},
        m_numRows{copy_from.m_numRows},
        m_numCollumns{copy_from.m_numCollumns}
  {
    gsl_matrix_memcpy(m_matrixPtr, copy_from.m_matrixPtr);
  }

  inline Matrix::Matrix(Matrix &&move_from)
      : m_matrixPtr{std::exchange(move_from.m_matrixPtr, nullptr)},
        m_numRows{std::exchange(move_from.m_numRows, 0)},
        m_numCollumns{std::exchange(move_from.m_numCollumns, 0)}
  {
  }

  inline Matrix::~Matrix()
  {
    gsl_matrix_free(m_matrixPtr);
  }

  inline auto Matrix::get_gsl_matrix() const -> gsl_matrix *
  {
    return m_matrixPtr;
  }

  inline auto Matrix::get_dimensions() const -> std::pair<size_t, size_t>
  {
    return {m_numRows, m_numCollumns};
  }

  inline auto Matrix::num_rows() const -> size_t
  {
    return m_numRows;
  }
  inline auto Matrix::num_collumns() const -> size_t
  {
    return m_numCollumns;
  }

  inline auto Matrix::operator=(const Matrix &copy_from) -> Matrix &
  {
    // Prevent self copy
    if (m_matrixPtr == copy_from.m_matrixPtr)
      return *this;

    gsl_matrix_free(m_matrixPtr);
    m_matrixPtr = gsl_matrix_calloc(copy_from.m_numRows, copy_from.m_numCollumns);
    gsl_matrix_memcpy(m_matrixPtr, copy_from.m_matrixPtr);

    m_numCollumns = copy_from.m_numCollumns;
    m_numRows = copy_from.m_numRows;

    return *this;
  }

  inline auto Matrix::operator=(Matrix &&move_from) -> Matrix &
  {
    // Prevent self move
    if (m_matrixPtr == move_from.m_matrixPtr)
      return *this;

    gsl_matrix_free(m_matrixPtr);
    m_matrixPtr = std::exchange(move_from.m_matrixPtr, nullptr);
    m_numRows = std::exchange(move_from.m_numRows, 0);
    m_numCollumns = std::exchange(move_from.m_numCollumns, 0);

    return *this;
  }

  inline auto Matrix::operator==(const Matrix &comparasion_matrix) const -> bool
  {

    if ((m_numCollumns != comparasion_matrix.m_numCollumns) || (m_numRows != comparasion_matrix.m_numRows))
      return false;

    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        bool test = ::gsl_wrapper::utils::equal((*this)[i][j], comparasion_matrix[i][j]);
        if (!test)
          return false;
      }
    }
    return true;
  }

  inline auto Matrix::operator!=(const Matrix &comparasion_matrix) const -> bool
  {
    return !(*this == comparasion_matrix);
  }

  inline auto Matrix::operator[](const size_t index) const -> gsl_wrapper::bits::MatrixRow
  {
    using bits::MatrixRow;
    gsl_vector_view view = gsl_matrix_row(m_matrixPtr, index);
    return MatrixRow(view);
  }

  inline auto Matrix::operator*(const Matrix &mul) const -> Matrix
  {
    // Check sizes
    if (m_numCollumns != mul.m_numRows)
      throw std::runtime_error{"Wrong matrix sizes!"};

    Matrix result(m_numRows, mul.m_numCollumns);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, m_matrixPtr, mul.m_matrixPtr, 0.0, result.get_gsl_matrix());

    return result;
  }

  inline auto Matrix::operator*(const double number) const -> Matrix
  {
    Matrix result = *this;
    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        result[i][j] *= number;
      }
    }

    return result;
  }

  inline auto Matrix::operator+(const Matrix &matrix) const -> Matrix
  {
    if ((m_numCollumns != matrix.m_numCollumns) || (m_numRows != matrix.m_numRows))
      throw std::range_error{"Wrong matrix sizes when adding"};

    Matrix result = *this;
    gsl_matrix_add(result.m_matrixPtr, matrix.m_matrixPtr);

    return result;
  }

  inline auto Matrix::operator+(const double number) const -> Matrix
  {
    Matrix result = *this;
    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        result[i][j] += number;
      }
    }

    return result;
  }

  inline auto operator<<(std::ostream &stream, const Matrix &matrix) -> std::ostream &
  {
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        stream << matrix[i][j] << " ";
      }
      stream << std::endl;
    }

    return stream;
  }

  inline auto operator*(const double number, const Matrix &matrix) -> Matrix
  {
    Matrix result = matrix;
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        result[i][j] *= number;
      }
    }

    return result;
  }

  inline auto operator+(const double number, const Matrix &matrix) -> Matrix
  {
    Matrix result = matrix;
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        result[i][j] += number;
      }
    }

    return result;
  }
}


/*
** LISA single subject statistical inference using 1st level GLM (general linear modeling).
** The prewhitening approach is adapted from Worsley et al (2002), Neuroimage 15(1).
**
** G.Lohmann, MPI-KYB, 2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>

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

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

extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);
extern void printmat(gsl_matrix *R,char *str);
extern void printvec(gsl_vector *x,char *str);

extern void prewhite(gsl_matrix_float** pinvX,gsl_matrix_float** invM,gsl_matrix_float* X,int);

extern void dfs(VFloat** Dfs,gsl_matrix_float* pinvX,gsl_matrix_float* X,gsl_matrix_float* con,int);

extern void whitecov(VImage rho_vol,gsl_matrix_float* Y,gsl_matrix_float* invM,
		     gsl_matrix_float* pinvX,gsl_matrix_float* X,int,int);

extern void whitecov2(VImage effect_image, VImage rho_vol,
		      gsl_matrix_float* Y, gsl_matrix_float* X, gsl_matrix_float* con,
		      VFloat* Dfs, int, int slice);

void CopySlice(gsl_matrix *Data,gsl_matrix_float *Y,VImage map,int slice)
{
  size_t nvox;
  size_t i=0,j=0;
  double u=0;
  int ncols = VPixel(map,0,3,2,VShort);

  gsl_matrix_float_set_zero(Y);

  for (nvox=0; nvox < Data->size1; nvox++) {
    int b = VPixel(map,0,0,nvox,VShort);
    if (b != slice) continue;
    int r = VPixel(map,0,1,nvox,VShort);
    int c = VPixel(map,0,2,nvox,VShort);
    i = c + ncols*r;
    if (i >= Y->size2) continue;

    for (j=0; j<Data->size2; j++) {
      u = gsl_matrix_get(Data,nvox,j);
      gsl_matrix_float_set(Y,j,i,(float)u);
    }
  }
}

gsl_matrix_float *GslFloatCpy(gsl_matrix *X)
{
  int i,j;
  double u;
  gsl_matrix_float *Z = gsl_matrix_float_calloc(X->size2,X->size1);
  for (i=0; i<X->size1; i++) {
    for (j=0; j<X->size2; j++) {
      u = gsl_matrix_get(X,i,j);
      gsl_matrix_float_set(Z,j,i,(float)u);
    }
  }
  return Z;
}

/*
** general linear model (GLM) using whitening
*/
void VWhiteGLM(gsl_matrix *Data,VImage map,gsl_matrix *X0,gsl_vector *con,int numlags,VImage zmap)
{
  int i,slice;
  int numcon=1;
  int nslices = VPixel(map,0,3,0,VShort);
  int nrows = VPixel(map,0,3,1,VShort);
  int ncols = VPixel(map,0,3,2,VShort);
  int npix = (nrows*ncols);
  int ntimesteps = Data->size2;

  gsl_set_error_handler_off();
  VFillImage(zmap,VAllBands,0);

  /* contrast vector */
  gsl_matrix_float *contrast = gsl_matrix_float_alloc(1,con->size);
  for (i=0; i<con->size; i++) gsl_matrix_float_set(contrast,0,i,(float)con->data[i]);

  /* copy design matrix */
  gsl_matrix_float *X = GslFloatCpy(X0);

  /* initialize rho_vol */
  VImage rho_vol = VCreateImage(npix,nslices,numlags,VFloatRepn);
  VFillImage(rho_vol,VAllBands,0);

  /* prewhitening */
  gsl_matrix_float *pinvX=NULL,*invM=NULL;
  prewhite(&pinvX,&invM,X,numlags);

  /* degrees of freedom */
  VFloat *dfs_gsl = (VFloat *)VMalloc(sizeof(VFloat)* (numcon+1));
  dfs(&dfs_gsl,pinvX,X,contrast,numlags);

  /* first pass */
  gsl_matrix_float *Y = gsl_matrix_float_calloc(ntimesteps,npix);
  for (slice=0; slice<nslices; slice++) {
    CopySlice(Data,Y,map,(int)slice);
    whitecov(rho_vol,Y,invM,pinvX,X,numlags,slice);
  }

  /* second pass */
  VImage dest = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(dest,VAllBands,0);
  for (slice=0; slice<nslices; slice++) {
    CopySlice(Data,Y,map,(int)slice);
    whitecov2(dest,rho_vol,Y,X,contrast,dfs_gsl,numlags,slice);
  }

  /* output */
  zmap = VCopyImagePixels(dest,zmap,VAllBands);

  /* cleanup */
  gsl_matrix_float_free(contrast);
  gsl_matrix_float_free(X);
  gsl_matrix_float_free(Y);
  gsl_matrix_float_free(pinvX);
  gsl_matrix_float_free(invM);
  VFree(dfs_gsl);
  VDestroyImage(rho_vol);
  VDestroyImage(dest);
}