/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the instrumental noise for LISA-type detectors.
 *
 * Formulas taken from Królak&al gr-qc/0401108 (c.f. section III).
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "LISAnoise.h"

/* static double tflight_SI = L_SI/C_SI; */
/* static double twopitflight_SI = 2.*PI*L_SI/C_SI; */

/* Proof mass and optic noises - f in Hz */
/* Taken from (4) in McWilliams&al_0911 */
static double SpmLISA2010(const double f) {
  double invf2 = 1./(f*f);
  //return 2.5e-48 * invf2 * sqrt(1. + 1e-8*invf2);
  //const double Daccel=3.0e-15; //acceleration noise in m/s^2/sqrt(Hz)
  const double Daccel=3.0e-15; //scaled off L3LISA-v1 to for equal-SNR PE experiment
  const double SaccelFF=Daccel*Daccel/4.0/PI/PI/C_SI/C_SI; //f^-2 coeff for fractional-freq noise PSD from accel noise; yields 2.54e-48 from 3e-15;
  double invf8=invf2*invf2*invf2*invf2;
  //Here we add an eyeball approximation based on 4yrs integration with L3LISAReferenceMission looking at a private comm from Neil Cornish 2016.11.12
  double WDWDnoise=5000.0/sqrt(1e-21*invf8 + invf2 + 3e28/invf8)*SaccelFF*invf2;
  return SaccelFF * invf2 * sqrt(1. + 1e-8*invf2) + WDWDnoise;
}
static double SopLISA2010(const double f) {
  const double Dop=2.0e-11; //Optical path noise in m/rtHz (Standard LISA)
  const double SopFF=Dop*Dop*4.0*PI*PI/C_SI/C_SI; //f^2 coeff for OP frac-freq noise PSD.  Yields 1.76e-37 for Dop=2e-11.
  return SopFF * f * f;
}

/* Proof mass and optical noises - f in Hz */
/* L3 Reference Mission, from Petiteau LISA-CST-TN-0001 */
static double SpmLISA2017(const double f) {
  double invf2 = 1./(f*f);
  //double invf4=invf2*invf2;
  //double invf8=invf4*invf4;
  //double invf10=invf8*invf2;
  const double twopi2=4.0*PI*PI;
  double ddtsq=twopi2/invf2; //time derivative factor
  const double C2=1.0*C_SI*C_SI; //veloc to doppler
  const double Daccel_white=3.0e-15; //acceleration noise in m/s^2/sqrt(Hz)
  const double Daccel_white2=Daccel_white*Daccel_white;
  const double Dloc=1.7e-12; //local IFO noise in m/sqrt(Hz)
  const double Dloc2=Dloc*Dloc;
  double Saccel_white=Daccel_white2/ddtsq; //PM vel noise PSD (white accel part)
  //double Saccel_red=Saccel_white*(1.0 + 2.12576e-44*invf10 + 3.6e-7*invf2); //reddening factor from Petiteau Eq 1
  double Saccel_red=Saccel_white*(1.0 + 36.0*(pow(3e-5/f,10) + 1e-8*invf2)); //reddening factor from Petiteau Eq 1
  //Saccel_red*=4.0;//Hack to decrease low-freq sens by fac of 2.
  double Sloc=Dloc2*ddtsq/4.0;//Factor of 1/4.0 is in Petiteau eq 2
  double S4yrWDWD=5.16e-27*exp(-pow(f,1.2)*2.9e3)*pow(f,(-7./3.))*0.5*(1.0 + tanh(-(f-2.0e-3)*1.9e3))*ddtsq;//Stas' fit for 4yr noise (converted from Sens curve to position noise by multipyling by 3*L^2/80) which looks comparable to my fit), then converted to velocity noise
  double Spm_vel = ( Saccel_red + Sloc + S4yrWDWD );
  return Spm_vel / C2;//finally convert from velocity noise to fractional-frequency doppler noise.
}
static double SopLISA2017(const double f) {
  //double invf2 = 1./(f*f);
  const double twopi2=4.0*PI*PI;
  double ddtsq=twopi2*f*f; //time derivative factor
  const double C2=C_SI*C_SI; //veloc to doppler
  const double Dloc=1.7e-12; //local IFO noise in m/sqrt(Hz)
  const double Dsci=8.9e-12; //science IFO noise in m/sqrt(Hz)
  const double Dmisc=2.0e-12; //misc. optical path noise in m/sqrt(Hz)
  const double Dop2=Dsci*Dsci+Dloc*Dloc+Dmisc*Dmisc;
  double Sop=Dop2*ddtsq/C2; //f^2 coeff for OP frac-freq noise PSD.  Yields 1.76e-37 for Dop=2e-11.
  return Sop;
}

/* Proof mass and optical noises - f in Hz */
/* LISA Proposal, copied from the LISA Data Challenge pipeline */
static double SpmLISAProposal(const double f) {
  /* Acceleration noise */
  double noise_Sa_a = 9.e-30; /* m^2/sec^4 /Hz */
  /* In acceleration */
  double Sa_a = noise_Sa_a * (1.0 + pow(0.4e-3/f, 2)) * (1.0 + pow((f/8e-3), 4));
  /* In displacement */
  double Sa_d = Sa_a * pow(2.*PI*f, -4);
  /* In relative frequency unit */
  double Sa_nu = Sa_d * pow(2.*PI*f/C_SI, 2);
  double Spm = Sa_nu;
  return Spm;
}
static double SopLISAProposal(const double f) {
  /* Optical Metrology System noise */
  double noise_Soms_d = pow((10e-12), 2); /* m^2/Hz */
  /* In displacement */
  double Soms_d = noise_Soms_d * (1. + pow(2.e-3/f, 4));
  /* In relative frequency unit */
  double Soms_nu = Soms_d * pow(2.*PI*f/C_SI, 2);
  double Sop = Soms_nu;
  return Sop;
}

/* Compute proof mass and optical noises, for a given choice of noise - f in Hz */
static void ComputeLISASpmSop(double* Spm, double* Sop, const double f, LISANoiseType noise) {
  if(noise==LISAProposalnoise) {
    *Spm = SpmLISAProposal(f);
    *Sop = SopLISAProposal(f);
  }
  else if(noise==LISA2017noise) {
    *Spm = SpmLISA2017(f);
    *Sop = SopLISA2017(f);
  }
  else if(noise==LISA2010noise) {
    *Spm = SpmLISA2010(f);
    *Sop = SopLISA2010(f);
  }
  else {
    printf("Error in ComputeLISASpmSop: LISANoiseType not recognized.\n");
    exit(1);
  }
}

/* Noise Sn for TDI observables - factors have been scaled out both in the response and the noise */
/* Rescaled by 4*sin2pifL^2 */
double SnXYZ(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 4*( 2*(1. + c2*c2)*Spm + Sop );
}
/* No rescaling */
double Snalphabetagamma(const LISAconstellation *variant, double f) {
  double pifL = PI*variant->ConstL/C_SI*f;
  double s1 = sin(pifL);
  double s3 = sin(3*pifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*( (4*s3*s3 + 8*s1*s1)*Spm + 3*Sop );
}
/* Rescaled by 2*sin2pifL^2 */
double SnAXYZ(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*(3. + 2*c2 + c4)*Spm + (2 + c2)*Sop;
}
/* Rescaled by 2*sin2pifL^2 */
double SnEXYZ(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*(3. + 2*c2 + c4)*Spm + (2 + c2)*Sop;
}
/* Rescaled by 8*sin2pifL^2*sinpifL^2 */
double SnTXYZ(const LISAconstellation *variant, double f) {
  double pifL = PI*variant->ConstL/C_SI*f;
  double s1 = sin(pifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 4*s1*s1*Spm + Sop;
}
/* Rescaled by 8*sin2pifL^2 */
double SnAalphabetagamma(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*(3. + 2*c2 + c4)*Spm + (2 + c2)*Sop;
}
/* Rescaled by 8*sin2pifL^2 */
double SnEalphabetagamma(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*(3. + 2*c2 + c4)*Spm + (2 + c2)*Sop;
}
/* Rescaled by sin3pifL^2/sinpifL^2 */
double SnTalphabetagamma(const LISAconstellation *variant, double f) {
  double pifL = PI*variant->ConstL/C_SI*f;
  double s1 = sin(pifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 8*s1*s1*Spm + 2*Sop;
}

/* Noise functions for AET(XYZ) without rescaling */
/* Scaling by 2*sin2pifL^2 put back */
double SnAXYZNoRescaling(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double s2 = sin(twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*s2*s2 * (2*(3. + 2*c2 + c4)*SpmLISA2017(f) + (2 + c2)*SopLISA2017(f));
}
/* Scaling by 2*sin2pifL^2 put back */
double SnEXYZNoRescaling(const LISAconstellation *variant, double f) {
  double twopifL = 2.*PI*variant->ConstL/C_SI*f;
  double c2 = cos(twopifL);
  double c4 = cos(2*twopifL);
  double s2 = sin(twopifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 2*s2*s2 * (2*(3. + 2*c2 + c4)*Spm + (2 + c2)*Sop);
}
/* Scaling by 8*sin2pifL^2*sinpifL^2 put back*/
double SnTXYZNoRescaling(const LISAconstellation *variant, double f) {
  double pifL = PI*variant->ConstL/C_SI*f;
  double s1 = sin(pifL);
  double s2 = sin(2*pifL);
  double Spm = 0., Sop = 0.;
  ComputeLISASpmSop(&Spm, &Sop, f, variant->noise);
  return 8*s1*s1*s2*s2 * (4*s1*s1*Spm + Sop);
}

/* The noise functions themselves
/* Note - we factored out and cancelled the factors of the type sin(n pi f L) */
/* double NoiseSnA(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double cos1 = cos(twopifL); */
/*   double cos2 = cos(2*twopifL); */
/*   return 32*( (6 + 4*cos1 + 2*cos2)*Spm(f) + (2 + cos1)*Sop(f) ); */
/* } */
/* double NoiseSnE(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double cos1 = cos(twopifL); */
/*   double cos2 = cos(2*twopifL); */
/*   return 32*( (6 + 4*cos1 + 2*cos2)*Spm(f) + (2 + cos1)*Sop(f) ); */
/* } */
/* double NoiseSnT(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double sinhalf = sin(0.5*twopifL); */
/*   double cos1 = cos(twopifL); */
/*   return 8*( 4*sinhalf*sinhalf*Spm(f) + Sop(f) ); */
/* } */

/* Function returning the relevant noise function, given a set of TDI observables and a channel */
ObjectFunction NoiseFunction(const LISAconstellation *variant, const TDItag tditag, const int nchan)
{
  ObjectFunction fn;
  switch(tditag) {
  case TDIXYZ:
  case TDIX: {
    switch(nchan) {
    case 1: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnXYZ}; break;
    case 2: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnXYZ}; break;
    case 3: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnXYZ}; break;
    }
    break;
  }
  case TDIalphabetagamma:
  case TDIalpha: {
    switch(nchan) {
    case 1: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)Snalphabetagamma}; break;
    case 2: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)Snalphabetagamma}; break;
    case 3: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)Snalphabetagamma}; break;
    }
    break;
  }
  case TDIAETXYZ:
  case TDIAXYZ:
  case TDIEXYZ:
  case TDITXYZ: {
    switch(nchan) {
    case 1: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnAXYZ}; break;
    case 2: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnEXYZ}; break;
    case 3: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnTXYZ}; break;
    }
    break;
  }
  case TDIAETalphabetagamma:
  case TDIAalphabetagamma:
  case TDIEalphabetagamma:
  case TDITalphabetagamma: {
    switch(nchan) {
    case 1: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnAalphabetagamma}; break;
    case 2: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnEalphabetagamma}; break;
    case 3: fn = (ObjectFunction){variant,(RealObjectFunctionPtr)SnTalphabetagamma}; break;
    }
    break;
  }
  }
  if(fn.object==NULL) {
    printf("Error in NoiseFunction: incorrect argument.\n");
    exit(1);
  }
  return fn;
}

//Previous version - we had put a noise floor to mitigate cancellation lines
/* double NoiseSnA(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double sinhalf = sin(0.5*twopifL); */
/*   double sin3half = sin(1.5*twopifL); */
/*   double cos1 = cos(twopifL); */
/*   double cos2 = cos(2*twopifL); */
/*   double res = 32*sinhalf*sinhalf*sin3half*sin3half*( (6 + 4*cos1 + 2*cos2)*Spm(f) + (2 + cos1)*Sop(f) ); */
/*   return fmax(res, 1e-46); */
/* } */
/* double NoiseSnE(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double sinhalf = sin(0.5*twopifL); */
/*   double sin3half = sin(1.5*twopifL); */
/*   double cos1 = cos(twopifL); */
/*   double cos2 = cos(2*twopifL); */
/*   double res = 32*sinhalf*sinhalf*sin3half*sin3half*( (6 + 4*cos1 + 2*cos2)*Spm(f) + (2 + cos1)*Sop(f) ); */
/*   return fmax(res, 1e-46); */
/* } */
/* double NoiseSnT(const double f) { */
/*   double twopifL = 2.*PI*L_SI/C_SI*f; */
/*   double sinhalf = sin(0.5*twopifL); */
/*   double sin3half = sin(1.5*twopifL); */
/*   double cos1 = cos(twopifL); */
/*   double res = 8*(1+2*cos1)*(1+2*cos1)*sin3half*sin3half*( 4*sinhalf*sinhalf*Spm(f) + Sop(f) ); */
/*   return fmax(res, 1e-46); */
/* } */


#define S_FUNCTION_NAME  solve_chol
#define S_FUNCTION_LEVEL 2

// Example: w = solve_chol(R,b);

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include <string.h>
#include "simstruc.h"
#include <lapacke.h>

#if !defined(_WIN32) /* not Win32/Matlab */
  #define dpotrs dpotrs_
#endif
extern int_T dpotrs_(char *, int_T *, int_T *, real_T *, int_T *,
                                            real_T *, int_T *, int_T *);
/*=========*
 * Defines *
 *=========*/

//#define SIZE_IDX 50
//#define SIZE_PARAM(S) ssGetSFcnParam(S,SIZE_IDX)

#define NINPUTS  2
#define NOUTPUTS 1

static void mdlInitializeSizes(SimStruct *S)
{
    int_T needsInput   = 1;  /* direct feedthrough     */

	ssSetNumSFcnParams(S, 0);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /*
         * If the number of expected input parameters is not
         * equal to the number of parameters entered in the
         * dialog box, return. The Simulink engine generates an
         * error indicating that there is a parameter mismatch.
         */
        return;
	}

	ssSetNumContStates( S, 0);
    ssSetNumDiscStates( S, 0);

	if (!ssSetNumInputPorts(S, NINPUTS)) return;
	/* R: a dynamically sized 2-D matrix input */
	ssSetInputPortMatrixDimensions(S, 0, 50, 50);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    ssSetInputPortDirectFeedThrough(S, 0, 1);
	/* b: a dynamically sized 1-D matrix input */
	ssSetInputPortWidth(S, 0, 50);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    ssSetInputPortDirectFeedThrough(S, 0, 1);

	if (!ssSetNumOutputPorts(S, NOUTPUTS)) return;
	/* b: a dynamically sized 1-D matrix output */
    ssSetOutputPortWidth(S, 0, 50);

	/*
     * Set the number of sample times.     */
    ssSetNumSampleTimes(S, 1);

	/* Inherit the sample time. */
	ssSetInputPortSampleTime(S, 0, INHERITED_SAMPLE_TIME);
	ssSetInputPortSampleTime(S, 1, INHERITED_SAMPLE_TIME);
}

/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *      Port based sample times have already been configured, therefore this
 *	method doesn't need to perform any action (you can check the
 *	current port sample times).
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
	ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
    ssSetModelReferenceSampleTimeDefaultInheritance(S); /* Inherit sample time */
}

/* Function: mdlOutputs =======================================================
 * Abstract:
 *
 * solve_chol - solve a linear system A*X = B using the Cholesky factorization
 * of A (square, symmetric and positive definite) using LAPACK/DPOTRS.
 *
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
	int_T   *dims1   = ssGetInputPortDimensions(S, 0);
	int_T   *dims2   = ssGetInputPortDimensions(S, 1);
	int_T    n1      = dims1[0];	/* number of rows in inputs A*/
	int_T    m1      = dims1[1]; 	/* number of columns in inputs A*/
	int_T    n2      = dims2[0];	/* number of rows in inputs B*/
	int_T    m2      = dims2[1]; 	/* number of columns in inputs B*/
	int_T    q;
	real_T   *C 	= ssGetOutputPortSignal(S,0);
	real_T   *A  	= ssGetInputPortSignal(S,0);
    real_T   *B  	= ssGetInputPortSignal(S,1);

	UNUSED_ARG(tid); /* not used in single tasking mode */

	if (n1 != m1)
		mexErrMsgTxt("First argument matrix must be square.");

	if (n1 != n2)
		mexErrMsgTxt("Both argument matrices must have the same number of rows.");

	// C = mxCreateDoubleMatrix(n, m, mxREAL);         /* space for output X */

	if (n1 == 0) return;             /* if argument was empty matrix, do no more */
	//memcpy( C, mxGetPr(B), m*n*mxGetElementSize(C) );  /* copy data */
	dpotrs("U", &n1, &m2, mxGetPr(A), &n1, C, &n1, &q);       /* solve system */
	if (q < 0) mexErrMsgTxt("Error: illegal input to solve_chol");

}


/* dwt_undec.c
 * Paul Demorest, 2007/10
 *
 * Undecimated wavelet transform and inverse.  These
 * do the usual wavelet filter tree, but don't decimate
 * by 2 after each step.  This makes the transform
 * translation-invariant (to translations which are
 * integer numbers of samples anyways).
 *
 * Ends up with n*log2(n) sized array, from size n input.
 *
 * Wavelet coeffs g and h need to be externally supplied.
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_wavelet.h>

/* Do one UDWT step on input data, out-of-place (original
 * data array is unaltered).
 * Vars:
 *   in  pointer to input data (n doubles)
 *   outl pointer to low-pass output data (n doubles)
 *   outh pointer to high-pass output data (n doubles)
 *   n   size of in/out
 *   h   Wavelet coeffs, low-pass (nc doubles)
 *   g   Wavelet coeffs, high-pass (nc doubles)
 *   nc  size of h/g
 *   s   scale (filters expand by 2^s)
 */
void udwt_forward_step(double *in, double *outl, double *outh, size_t n,
        const double h[], const double g[], size_t nc, int s) {

    int i,j,idx;

    /* Zero output array */
    for (i=0; i<n; i++) { outl[i]=0.0; outh[i]=0.0; }

    /* Filter */
    for (i=0; i<n; i++) {
        for (j=0; j<nc; j++) {
            idx = (i + (1<<s)*j) % n;
            outl[i] += h[j] * in[idx];
            outh[i] += g[j] * in[idx];
        }
    }
}

/* Inverse UDWT step similar to above.  Remember to stay
 * consistent about which half contains h and which
 * contains g chunks!
 */
void udwt_inverse_step(double *inl, double *inh, double *out, size_t n,
        const double h[], const double g[], size_t nc, int s) {

    int i,j,idx;

    /* Zero output */
    for (i=0; i<n; i++) { out[i]=0.0; }

    /* filter, add */
    for (i=0; i<n; i++) {
        for (j=0; j<nc; j++) {
            idx = (i - (1<<s)*j) % n;
            out[i] += h[j] * inl[idx];
            out[i] += g[j] * inh[idx];
        }
    }

    /* Inverse needs a rescaling */
    for (i=0; i<n; i++) { out[i] *= 0.5; }
}

/* Return integer log 2 of input.
 * Error if not a power of 2?
 */
static int log2i(int n) {
    int rv=0;
    while ((n>>rv)>1) { rv++; }
    return(rv);
}

/* Do a undec wavelet transform on input data n.
 * Vars:
 *   in  pointer to input data (n doubles)
 *   out pointer to output data (n*log2(n) doubles)
 *   n   size of in
 *   h   Wavelet coeffs, high-pass (nc doubles)
 *   g   Wavelet coeffs, low-pass (nc doubles)
 *   nc  size of h/g
 *
 * This can be called using a GSL wavelet definition struct like:
 * gsl_wavelet *w;
 * (etc, init wavelet...)
 * dwt_undec_transform(in, &out, n, w->h1, w->g1, w->nc);
 */
int _dwt_undec_transform(double *in, double *out, size_t n,
        const double h[], const double g[], size_t nc) {

    /* Determine number of levels of decomp */
    int nl=0;
    nl = log2i(n);

    /* maybe do some value checks on n, nl .. */

    /* Temporary low-pass outputs */
    double *tmp1 = (double *)malloc(sizeof(double)*n);
    double *tmp2 = (double *)malloc(sizeof(double)*n);

    /* Loop to do dwts */
    int i;
    double *curi, *curol, *curoh, *swp;
    curi = in;
    curol = tmp1;
    curoh = out;
    for (i=0; i<nl; i++) {
        udwt_forward_step(curi, curol, curoh, n, h, g, nc, i);
        if (i==0) { curi=tmp2; }
        swp = curi;
        curi = curol;
        curol = swp;
        curoh = &(out[(i+1)*n]);
    }

    free(tmp1);
    free(tmp2);
    return(nl);
}

/* Inverse, similar to above, but switch in/out
 * This can be called using a GSL wavelet definition struct like:
 * gsl_wavelet *w;
 * (etc, init wavelet...)
 * dwt_undec_inverse(in, out, n, w->h2, w->g2, w->nc);
 */
int _dwt_undec_inverse(double *in, double *out, size_t n,
        const double h[], const double g[], size_t nc) {

    /* Determine number of levels of decomp */
    int nl=0;
    nl = log2i(n);

    /* maybe do some value checks on n, nl .. */

    /* Temporary low-pass outputs */
    int i;
    double *tmp1 = (double *)malloc(sizeof(double)*n);
    double *tmp2 = (double *)malloc(sizeof(double)*n);
    for (i=0; i<n; i++) { tmp1[i]=0.0; }

    /* Loop to do dwts */
    double *curil, *curih, *curo, *swp;
    curil = tmp1;
    curih = &in[(nl-1)*n];
    curo = tmp2;
    for (i=nl-1; i>=0; i--) {
        udwt_inverse_step(curil, curih, curo, n, h, g, nc, i);
        swp = curil;
        curil = curo;
        curo = swp;
        curih = &in[(i-1)*n];
    }

    /* We end up with final output in curil */
    for (i=0; i<n; i++) { out[i] = curil[i]; }

    free(tmp1);
    free(tmp2);
    return(nl);
}

/* Wrappers using GSL wavelet definitions */
int dwt_undec_transform(double *in, double *out, size_t n, const gsl_wavelet *w) {
    return _dwt_undec_transform(in, out, n, w->h1, w->g1, w->nc);
}
int dwt_undec_inverse(double *in, double *out, size_t n, const gsl_wavelet *w) {
    return _dwt_undec_inverse(in, out, n, w->h2, w->g2, w->nc);
}


// ${_warning_in_the_generated_file_not_to_edit}

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_errno.h>
#include <math.h>

#include "neqsys.h"

// Global counters
int NFEV = 0;
int NJEV = 0;
int NFJEV = 0;

int
func (const gsl_vector * x, void * params, gsl_vector * f)
{
  /*
    Best is to name all parameters k[0] ... k[P]
   */
  const double *k = (double *) params;
  const double * const y = (double *) x->data;

  /*
    Define variables for common subexpressions
   */
% for cse_token, cse_expr in func_cse_defs:
  const double ${cse_token} = ${cse_expr};
% endfor

  /*
    Calculate residuals
   */
% for i, expr in enumerate(func_new_code):
  gsl_vector_set(f, ${i}, ${expr});
% endfor

  NFEV++;
  return GSL_SUCCESS;
}

int
jac (const gsl_vector * x, void *params, gsl_matrix * J)
{
  const double *k = (double *) params;
  const double * const y = (double *) x->data;

  /*
    Define variables for common subexpressions
   */
% for cse_token, cse_expr in jac_cse_defs:
  const double ${cse_token} = ${cse_expr};
% endfor

  /*
    Populate the NY times NY Jacobian matrix
   */
% for i, expr in enumerate(jac_new_code):
  gsl_matrix_set (J, ${i // NX}, ${i % NX}, ${expr});
% endfor

  NJEV++;
  return GSL_SUCCESS;
}

int
fdf (const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix * J)
{
  const double * const k = (double *) params;
  const double * const y = (double *) x->data;

  /*
    Define variables for common subexpressions
   */
% for cse_token, cse_expr in fj_cse_defs:
  const double ${cse_token} = ${cse_expr};
% endfor

  /*
    Calculate residuals
   */
% for i, expr in enumerate(fj_func_new_code):
  gsl_vector_set(f, ${i}, ${expr});
% endfor

  /*
    Populate the NY times NY Jacobian matrix
   */
% for i, expr in enumerate(fj_jac_new_code):
  gsl_matrix_set (J, ${i // NX}, ${i % NX}, ${expr});
% endfor

  NFJEV++;
  return GSL_SUCCESS;
}


/* randist/gsl_randist.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_RANDIST_H__
#define __GSL_RANDIST_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif
#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.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

GSL_FUN unsigned int gsl_ran_bernoulli (const gsl_rng * r, double p);
GSL_FUN double gsl_ran_bernoulli_pdf (const unsigned int k, double p);

GSL_FUN double gsl_ran_beta (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_beta_pdf (const double x, const double a, const double b);

GSL_FUN unsigned int gsl_ran_binomial (const gsl_rng * r, double p, unsigned int n);
GSL_FUN unsigned int gsl_ran_binomial_knuth (const gsl_rng * r, double p, unsigned int n);
GSL_FUN unsigned int gsl_ran_binomial_tpe (const gsl_rng * r, double p, unsigned int n);
GSL_FUN double gsl_ran_binomial_pdf (const unsigned int k, const double p, const unsigned int n);

GSL_FUN double gsl_ran_exponential (const gsl_rng * r, const double mu);
GSL_FUN double gsl_ran_exponential_pdf (const double x, const double mu);

GSL_FUN double gsl_ran_exppow (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_exppow_pdf (const double x, const double a, const double b);

GSL_FUN double gsl_ran_cauchy (const gsl_rng * r, const double a);
GSL_FUN double gsl_ran_cauchy_pdf (const double x, const double a);

GSL_FUN double gsl_ran_chisq (const gsl_rng * r, const double nu);
GSL_FUN double gsl_ran_chisq_pdf (const double x, const double nu);

GSL_FUN void gsl_ran_dirichlet (const gsl_rng * r, const size_t K, const double alpha[], double theta[]);
GSL_FUN double gsl_ran_dirichlet_pdf (const size_t K, const double alpha[], const double theta[]);
GSL_FUN double gsl_ran_dirichlet_lnpdf (const size_t K, const double alpha[], const double theta[]);

GSL_FUN double gsl_ran_erlang (const gsl_rng * r, const double a, const double n);
GSL_FUN double gsl_ran_erlang_pdf (const double x, const double a, const double n);

GSL_FUN double gsl_ran_fdist (const gsl_rng * r, const double nu1, const double nu2);
GSL_FUN double gsl_ran_fdist_pdf (const double x, const double nu1, const double nu2);

GSL_FUN double gsl_ran_flat (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_flat_pdf (double x, const double a, const double b);

GSL_FUN double gsl_ran_gamma (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_gamma_int (const gsl_rng * r, const unsigned int a);
GSL_FUN double gsl_ran_gamma_pdf (const double x, const double a, const double b);
GSL_FUN double gsl_ran_gamma_mt (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_gamma_knuth (const gsl_rng * r, const double a, const double b);

GSL_FUN double gsl_ran_gaussian (const gsl_rng * r, const double sigma);
GSL_FUN double gsl_ran_gaussian_ratio_method (const gsl_rng * r, const double sigma);
GSL_FUN double gsl_ran_gaussian_ziggurat (const gsl_rng * r, const double sigma);
GSL_FUN double gsl_ran_gaussian_pdf (const double x, const double sigma);

GSL_FUN double gsl_ran_ugaussian (const gsl_rng * r);
GSL_FUN double gsl_ran_ugaussian_ratio_method (const gsl_rng * r);
GSL_FUN double gsl_ran_ugaussian_pdf (const double x);

GSL_FUN double gsl_ran_gaussian_tail (const gsl_rng * r, const double a, const double sigma);
GSL_FUN double gsl_ran_gaussian_tail_pdf (const double x, const double a, const double sigma);

GSL_FUN double gsl_ran_ugaussian_tail (const gsl_rng * r, const double a);
GSL_FUN double gsl_ran_ugaussian_tail_pdf (const double x, const double a);

GSL_FUN void gsl_ran_bivariate_gaussian (const gsl_rng * r, double sigma_x, double sigma_y, double rho, double *x, double *y);
GSL_FUN double gsl_ran_bivariate_gaussian_pdf (const double x, const double y, const double sigma_x, const double sigma_y, const double rho);

GSL_FUN int gsl_ran_multivariate_gaussian (const gsl_rng * r, const gsl_vector * mu, const gsl_matrix * L, gsl_vector * result);
GSL_FUN int gsl_ran_multivariate_gaussian_log_pdf (const gsl_vector * x,
                                           const gsl_vector * mu,
                                           const gsl_matrix * L,
                                           double * result,
                                           gsl_vector * work);
GSL_FUN int gsl_ran_multivariate_gaussian_pdf (const gsl_vector * x,
                                       const gsl_vector * mu,
                                       const gsl_matrix * L,
                                       double * result,
                                       gsl_vector * work);
GSL_FUN int gsl_ran_multivariate_gaussian_mean (const gsl_matrix * X, gsl_vector * mu_hat);
GSL_FUN int gsl_ran_multivariate_gaussian_vcov (const gsl_matrix * X, gsl_matrix * sigma_hat);

GSL_FUN int gsl_ran_wishart (const gsl_rng * r,
                     const double df,
                     const gsl_matrix * L,
                     gsl_matrix * result,
                     gsl_matrix * work);
GSL_FUN int gsl_ran_wishart_log_pdf (const gsl_matrix * X,
                             const gsl_matrix * L_X,
                             const double df,
                             const gsl_matrix * L,
                             double * result,
                             gsl_matrix * work);
GSL_FUN int gsl_ran_wishart_pdf (const gsl_matrix * X,
                         const gsl_matrix * L_X,
                         const double df,
                         const gsl_matrix * L,
                         double * result,
                         gsl_matrix * work);

GSL_FUN double gsl_ran_landau (const gsl_rng * r);
GSL_FUN double gsl_ran_landau_pdf (const double x);

GSL_FUN unsigned int gsl_ran_geometric (const gsl_rng * r, const double p);
GSL_FUN double gsl_ran_geometric_pdf (const unsigned int k, const double p);

GSL_FUN unsigned int gsl_ran_hypergeometric (const gsl_rng * r, unsigned int n1, unsigned int n2, unsigned int t);
GSL_FUN double gsl_ran_hypergeometric_pdf (const unsigned int k, const unsigned int n1, const unsigned int n2, unsigned int t);

GSL_FUN double gsl_ran_gumbel1 (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_gumbel1_pdf (const double x, const double a, const double b);

GSL_FUN double gsl_ran_gumbel2 (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_gumbel2_pdf (const double x, const double a, const double b);

GSL_FUN double gsl_ran_logistic (const gsl_rng * r, const double a);
GSL_FUN double gsl_ran_logistic_pdf (const double x, const double a);

GSL_FUN double gsl_ran_lognormal (const gsl_rng * r, const double zeta, const double sigma);
GSL_FUN double gsl_ran_lognormal_pdf (const double x, const double zeta, const double sigma);

GSL_FUN unsigned int gsl_ran_logarithmic (const gsl_rng * r, const double p);
GSL_FUN double gsl_ran_logarithmic_pdf (const unsigned int k, const double p);

GSL_FUN void gsl_ran_multinomial (const gsl_rng * r, const size_t K,
                          const unsigned int N, const double p[],
                          unsigned int n[] );
GSL_FUN double gsl_ran_multinomial_pdf (const size_t K,
                                const double p[], const unsigned int n[] );
GSL_FUN double gsl_ran_multinomial_lnpdf (const size_t K,
                           const double p[], const unsigned int n[] );

GSL_FUN unsigned int gsl_ran_negative_binomial (const gsl_rng * r, double p, double n);
GSL_FUN double gsl_ran_negative_binomial_pdf (const unsigned int k, const double p, double n);

GSL_FUN unsigned int gsl_ran_pascal (const gsl_rng * r, double p, unsigned int n);
GSL_FUN double gsl_ran_pascal_pdf (const unsigned int k, const double p, unsigned int n);

GSL_FUN double gsl_ran_pareto (const gsl_rng * r, double a, const double b);
GSL_FUN double gsl_ran_pareto_pdf (const double x, const double a, const double b);

GSL_FUN unsigned int gsl_ran_poisson (const gsl_rng * r, double mu);
GSL_FUN void gsl_ran_poisson_array (const gsl_rng * r, size_t n, unsigned int array[],
                            double mu);
GSL_FUN double gsl_ran_poisson_pdf (const unsigned int k, const double mu);

GSL_FUN double gsl_ran_rayleigh (const gsl_rng * r, const double sigma);
GSL_FUN double gsl_ran_rayleigh_pdf (const double x, const double sigma);

GSL_FUN double gsl_ran_rayleigh_tail (const gsl_rng * r, const double a, const double sigma);
GSL_FUN double gsl_ran_rayleigh_tail_pdf (const double x, const double a, const double sigma);

GSL_FUN double gsl_ran_tdist (const gsl_rng * r, const double nu);
GSL_FUN double gsl_ran_tdist_pdf (const double x, const double nu);

GSL_FUN double gsl_ran_laplace (const gsl_rng * r, const double a);
GSL_FUN double gsl_ran_laplace_pdf (const double x, const double a);

GSL_FUN double gsl_ran_levy (const gsl_rng * r, const double c, const double alpha);
GSL_FUN double gsl_ran_levy_skew (const gsl_rng * r, const double c, const double alpha, const double beta);

GSL_FUN double gsl_ran_weibull (const gsl_rng * r, const double a, const double b);
GSL_FUN double gsl_ran_weibull_pdf (const double x, const double a, const double b);

GSL_FUN void gsl_ran_dir_2d (const gsl_rng * r, double * x, double * y);
GSL_FUN void gsl_ran_dir_2d_trig_method (const gsl_rng * r, double * x, double * y);
GSL_FUN void gsl_ran_dir_3d (const gsl_rng * r, double * x, double * y, double * z);
GSL_FUN void gsl_ran_dir_nd (const gsl_rng * r, size_t n, double * x);

GSL_FUN void gsl_ran_shuffle (const gsl_rng * r, void * base, size_t nmembm, size_t size);
GSL_FUN int gsl_ran_choose (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;
GSL_FUN void gsl_ran_sample (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;

typedef struct {                /* struct for Walker algorithm */
    size_t K;
    size_t *A;
    double *F;
} gsl_ran_discrete_t;

GSL_FUN gsl_ran_discrete_t * gsl_ran_discrete_preproc (size_t K, const double *P);
GSL_FUN void gsl_ran_discrete_free(gsl_ran_discrete_t *g);
GSL_FUN size_t gsl_ran_discrete (const gsl_rng *r, const gsl_ran_discrete_t *g);
GSL_FUN double gsl_ran_discrete_pdf (size_t k, const gsl_ran_discrete_t *g);

__END_DECLS

#endif /* __GSL_RANDIST_H__ */


#include <iostream>
#include <vector>
#include <memory>
#include <cstdlib>
#include <cstddef>
#ifdef ENABLE_GSL
#include <gsl/gsl_integration.h>
#include <gsl/gsl_deriv.h>
#include <cmath>

#endif

#ifndef PI
#define PI  3.141593
#endif

#ifndef Grav
#define Grav 4.7788e-20  // Newton's over c^2 in Mpc / Msun
#endif

#ifndef lightspeed
#define lightspeed 2.99792458e5  // km / s
#endif

#ifndef error_message
#define error_message
#define ERROR_MESSAGE() std::cerr << "ERROR: file: " << __FILE__ << " line: " << __LINE__ << std::endl;
#endif

#define CRITD 2.49783e18    /* critical density / Ho^2 solar/Mpc */
#define CRITD2 2.7752543e11 /* critical density / h^2 M_sun/Mpc^3 */

#ifndef cosmo_declare

enum CosmoParamSet {WMAP5yr,Millennium,Planck1yr,Planck15,Planck18,BigMultiDark,none};

std::ostream &operator<<(std::ostream &os, CosmoParamSet p);

/**
 *
 * \brief The cosmology and all the functions required to calculated quantities based on the cosmology.
 *
 * This class is used to store the cosmological parameters, calculate cosmological distances, calculate
 * the power spectrum of density fluctuations and the mass function of halos.
 *
 * As set now, there are no baryon acoustic oscillations in the power spectrum, but this can be changed
 * at the expense of not including neutrinos.
 */
class COSMOLOGY{
public:

  COSMOLOGY(CosmoParamSet cosmo_p = WMAP5yr);
  /// if justdistances== true
  COSMOLOGY(double omegam,double omegal,double h, double w = -1,double wa = 0
            ,bool justdistances=false /// if true the internals needed calculate structure formation will not be calculated making constructuion faster
  );
  COSMOLOGY(const COSMOLOGY &cosmo);
  ~COSMOLOGY();

  COSMOLOGY& operator=(const COSMOLOGY &cosmo);

  // returns the parameter set if any
  CosmoParamSet ParamSet(){return cosmo_set;}

  void PrintCosmology(short physical = 0) const;
  std::string print(short physical= 0) const;

  // Lengths
  double rcurve() const;
  //double emptyDist(double zo,double z);
  double coorDist(double zo,double z) const;
  double coorDist(double z) const {return coorDist(0,z);}
  double radDist(double zo,double z) const;
  double radDist(double z) const {return radDist(0,z);}
  double angDist(double zo,double z) const;
  double angDist(double z) const {return angDist(0,z);}
  double lumDist(double zo,double z) const;
  double lumDist(double z) const {return lumDist(0,z);}

  /// derivative of coordinate distance with respect to Omo in flat cosmology.  Useful for Fisher matrix calculations
  double d_coorDist_dOmo(double zo,double z) const;
  /// derivative of coordinate distance with respect to w.  Useful for Fisher matrix calculations
  double d_coorDist_dw(double zo,double z) const;
  /// derivative of coordinate distance with respect to w1.  Useful for Fisher matrix calculations
  double d_coorDist_dw1(double zo,double z) const;

  double DRradius(double zo,double z,double pfrac);
  double DRradius2(double zo,double z);

  double invCoorDist(double d) const;
  double invRadDist(double d) const;
  double invComovingDist(double d) const;

  double scalefactor(double rad) const;
  double Omegam(double z) const;
  double rho_crit_comoving(double z) const;
  //double drdz_empty(double x);
  double drdz(double x) const;
  //double adrdz(double x) const;
  double drdz_dark(double x) const;
  inline double dark_factor(double x) const;
  //double adrdz_dark(double x) const;

  /// the critical over density
  double delta_c() const;

  double DeltaVir(double z,int caseunit=0) const;
  double Deltao(double m) const;
  double time(double z) const;
  double nonlinMass(double z) const;

  // Stuff having to do with the power spectrum
  double power_normalize(double sigma8);
  /**
   * \brief Linear power spectrum P(k,z)/a^2
   */
  double power_linear(double k,double z);
  double Dgrowth(double z) const;

  /**
   * \brief  powerCDM.c calculates the nonlinear P(k,z)/a(r)^2
   *
   * The method of Peacock & Dodds 1996 is used to convert the linear
   * power spectrum into the nonlinear one.
   * This could be updated to a more recent nonlinear power spectrum
   */
  double powerCDMz(double k,double z);
  double psdfdm(double z,double m,int caseunit=0);
  double halo_bias (double m, double z, int t=0);
  double stdfdm(double z,double m,int caseunit=0);
  double powerlawdfdm(double z,double m,double alpha,int caseunit=0);
  double haloNumberDensity(double m,double z,double a, int t,double alpha = 0.0);

  /** \brief Dark matter correlation function
   *
   *  This integrates powerCDMz() to get the correlation function as a function of comoving radius.
   *  Care should be taken that the right range of k is integrated over if the radius is very small or large.
   */
  double CorrelationFunction(double radius,double redshift
                      ,double k_max = 100,double k_min = 1.0e-3);

  struct CorrFunctorType{
    CorrFunctorType(COSMOLOGY *cosmo,double radius,double redshift)
    : cosmology(cosmo),z(redshift),r(radius)
    {
      norm = 0.5/PI/PI/(1+z)/(1+z);
    };

    COSMOLOGY *cosmology;
    double z;
    double r;

    double norm;

    double operator () (double k) {

      double rk = r*k;
      double jo = sin(rk)/rk;
      if(rk < 1.0e-3) jo = 1.0;

      return norm*jo*k*k*cosmology->powerCDMz(k,z);
    }
  };

  double haloNumberDensityOnSky (double m,double z1,double z2,int t,double alpha = 0.0);
  double haloNumberInBufferedCone (double mass ,double z1,double z2,double fov,double buffer ,int type ,double alpha=0.0);
  double haloMassInBufferedCone (double mass ,double z1,double z2,double fov,double buffer ,int type ,double alpha=0.0);

  double TopHatVariance(double m) const;
  double TopHatVarianceR(double R,double z);
  double TopHatVarianceM(double M,double z);
  //double gradius(double R,double rd);

  double getTimefromDeltaC(double dc);
  double getZfromDeltaC(double dc);

  /// accesser functions:

  /// Hubble paremters in units of 100 km/s/Mpc, renormalizes P(k) to keep sig8 fixed
  void sethubble(double ht){ h = ht; TFmdm_set_cosm();
    power_normalize(sig8); 	calc_interp_dist(); cosmo_set = none;}
  double gethubble() const {return h;}
  /// Hubble parameter in 1/Mpc units
  double getHubble() const {return 100*h/lightspeed;}

  /// Primordial spectral index, renormalizes P(k) to keep sig8 fixed
  void setindex(double nn){ n = nn;  power_normalize(sig8); cosmo_set = none;}
  double getindex() const { return n;}

  /// Omega matter, renormalizes P(k) to keep sig8 fixed
  void setOmega_matter(double Omega_matter,bool FLAT = false){Omo = Omega_matter; if(FLAT) Oml = 1-Omo ; TFmdm_set_cosm(); power_normalize(sig8); calc_interp_dist(); cosmo_set = none;}
  double getOmega_matter() const {return Omo;}

  /// Omega lambda, renormalizes P(k) to keep sig8 fixed
  void setOmega_lambda(double Omega_lambda,bool FLAT = false){Oml = Omega_lambda;  if(FLAT) Oml = 1-Omo ; TFmdm_set_cosm(); power_normalize(sig8); calc_interp_dist(); cosmo_set = none;}
  double getOmega_lambda() const {return Oml;}

  /// Omega baryon, renormalizes P(k) to keep sig8 fixed
  void setOmega_baryon(double Omega_baryon){Omb = Omega_baryon; TFmdm_set_cosm(); power_normalize(sig8); calc_interp_dist(); cosmo_set = none;
  }
  double getOmega_baryon() const {return Omb;}

  /// Omega neutrino, renormalizes P(k) to keep sig8 fixed
  void setOmega_neutrino(double Omega_neutrino){Omnu = Omega_neutrino; TFmdm_set_cosm(); power_normalize(sig8); 	calc_interp_dist(); cosmo_set = none;
  }
  double getOmega_neutrino() const {return Omnu;}

  /// Number of neutrino species, renormalizes P(k) to keep sig8 fixed
  void setNneutrino(double Nneutrino){Nnu = Nneutrino; TFmdm_set_cosm(); power_normalize(sig8); cosmo_set = none;}
  double getNneutrino() const {return Nnu;}

  /// Dark energy equation of state parameter p/rho = w + w_1 (1+z)
  void setW(double w){ww = w; 	calc_interp_dist(); cosmo_set = none;}
  double getW() const {return ww;}
  void setW1(double w){ww1 = w; 	calc_interp_dist(); cosmo_set = none;}
  double getW1() const {return ww1;}

  /// Running of primordial spectral index, P(k)_primordial \propto pow(k/h,n+dndlnk*log(k)), renormalizes P(k) to keep sig8 fixed
  void setdndlnk(double w){dndlnk = w; power_normalize(sig8); cosmo_set = none;}
  double getdndlnk() const {return dndlnk;}

  /// Alternative to w for dark energy/ alt. grav. structure evolution
  void setgamma(double gamm){gamma = gamm; cosmo_set = none;}
  double getgamma() const {return gamma;}
  // If physical = 1 all Omega are Omega*h^2 otherwise they have the usual definitions.
  // 2 gamma parameterization is used for dark energy
  void setDEtype(short tt){darkenergy = tt; cosmo_set = none;}
  short getDEtype() const {return darkenergy;}

  void setSigma8(double my_sig8){power_normalize(my_sig8); cosmo_set = none;}
  double getSigma8() const {return sig8;}

  void dzdangDist(double D,double z[],double dzdD[]);

  double totalMassDensityinHalos(int t,double alpha,double m_min,double z,double z1,double z2);

  /// set interpolation range and number of points
  void setInterpolation(double z_interp, std::size_t n_interp);

  /// The lensing critical density in Msun / Mpc^2
  double SigmaCrit(double zlens,double zsource) const;

protected:
  void SetConcordenceCosmology(CosmoParamSet cosmo_p);

  CosmoParamSet cosmo_set;

  // structure rappers to make integration thread safe
  struct drdz_struct{
    drdz_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.drdz(x);}
    COSMOLOGY const &cos;
  };
  struct drdzdark_struct{
    drdzdark_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.drdz_dark(x);}
    COSMOLOGY const &cos;
  };
  struct adrdz_struct{
    adrdz_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.drdz(x)/x;}
    COSMOLOGY const &cos;
  };
  struct adrdzdark_struct{
    adrdzdark_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.drdz_dark(x)/x;}
    COSMOLOGY const &cos;
  };

  struct normL_struct{
    normL_struct(COSMOLOGY &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.normL(x);}
    COSMOLOGY &cos;
  };

  double ddrdzdOmo(double x) const;
  double ddrdzdw(double x) const;
  double ddrdzdw1(double x) const;

  struct ddrdzdOmo_struct{
    ddrdzdOmo_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.ddrdzdOmo(x);}
    COSMOLOGY const &cos;
  };
  struct ddrdzdw_struct{
    ddrdzdw_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.ddrdzdw(x);}
    COSMOLOGY const &cos;
  };
  struct ddrdzdw1_struct{
    ddrdzdw1_struct(COSMOLOGY const &cosmo):cos(cosmo){};
    double operator()(double x) const {return cos.ddrdzdw1(x);}
    COSMOLOGY const &cos;
  };

  bool init_structure_functions;

  /// Hubble paremters in units of 100 km/s/Mpc
  double h;
  /// Primordial spectral index
  double n;
  /// Omega matter, dark matter + baryons
  double Omo;
  /// Omega lambda
  double Oml;
  /// Omega baryon
  double Omb;
  /// Omega neutrino
  double Omnu;
  /// Number of neutrino species
  double Nnu;
  /// Dark energy equation of state parameter p/rho = ww + ww_1 (1+z)
  double ww;
  /// Dark energy equation of state parameter p/rho = ww + ww_1 (1+z)
  double ww1;
  /// Running of primordial spectral index, P(k)_primordial \propto pow(k/h,n+dndlnk*log(k))
  double dndlnk;

  /// Alternative to w for dark energy/ alt. grav. structure evolution
  double gamma;
  // If physical = 1 all Omega are Omega*h^2 otherwise they have the usual definitions.
  //short physical;
  // 2 gamma parameterization is used for dark energy
  short darkenergy;

  /* table for growth parameter */
  //std::auto_ptr<double> *aa;
  //std::auto_ptr<double> *growth;
  //int Ntable;

  double A;
  double sig8;  /* do not access these normalization outside */

  double Rtophat;
  double ztmp;

  double powerEH(double k,double z);
  double powerEHv2(double k);
  double powerloc(double k,double z);
  double npow(double k);
  double normL(double lgk);
  double gradius(double R,double rd) const;

  int nbin;  // number of points when binning to interpolate
  std:: vector<double> vDeltaCz,vlz,vt;
  double DpropDz(double z);
  double dsigdM(double m);
  double timeEarly(double a) const;
  double dNdz(double z);

  typedef double (COSMOLOGY::*pt2MemFunc)(double) const;
  typedef double (COSMOLOGY::*pt2MemFuncNonConst)(double);

  double nintegrateDcos(pt2MemFunc func,double a,double b,double tols) const;
  double trapzdDcoslocal(pt2MemFunc func, double a, double b, int n, double *s2) const;
  void polintD(double xa[], double ya[], int n, double x, double *y, double *dy) const;
  double nintegrateDcos(pt2MemFuncNonConst func,double a,double b,double tols);
  double trapzdDcoslocal(pt2MemFuncNonConst func, double a, double b, int n, double *s2);
  double dfridrDcos(pt2MemFunc func, double x, double h, double *err);
  double f4(double u) const;

  int ni;
  std::vector<float> xf, wf;

  // The parameters used in Eisenstein & Hu power spectrum
  /* in powerEH.c */
  short TFmdm_set_cosm_change_z(double redshift);
  short TFmdm_set_cosm();
  double TFmdm_onek_mpc(double kk);
  double TFmdm_onek_hmpc(double kk);

  double f_baryon;	// Baryon fraction
  double f_bnu;		// Baryon + Massive Neutrino fraction
  double f_cb;		// Baryon + CDM fraction
  double f_cdm;		// CDM fraction
  double f_hdm;		// Massive Neutrino fraction

  double alpha_gamma;	 // sqrt(alpha_nu)
  double alpha_nu;	     // The small-scale suppression
  double beta_c;		 // The correction to the log in the small-scale
  double num_degen_hdm;	 // Number of degenerate massive neutrino species
  double growth_k0;	     // D_1(z) -- the growth function as k->0
  double growth_to_z0;	 // D_1(z)/D_1(0) -- the growth relative to z=0
  double k_equality;	 // The comoving wave number of the horizon at equality
  double obhh;		     // Omega_baryon * hubble^2
  double omega_curv;	 // = 1 - omega_matter - omega_lambda
  double omega_lambda_z; // Omega_lambda at the given redshift
  double omega_matter_z; // Omega_matter at the given redshift
  double omhh;		     // Omega_matter * hubble^2
  double onhh;		     // Omega_hdm * hubble^2
  double p_c;		     // The correction to the exponent before drag epoch
  double p_cb;		     // The correction to the exponent after drag epoch
  double sound_horizon_fit;  // The sound horizon at the drag epoch
  double theta_cmb;	     // The temperature of the CMB, in units of 2.7 K
  double y_drag;		 // Ratio of z_equality to z_drag
  double z_drag;		 // Redshift of the drag epoch
  double z_equality;	 // Redshift of matter-radiation equality

  void setinternals();
  // in powerEHv2.c
  void TFset_parameters(double omega0hh, double f_baryon, double Tcmb);
  double TFfit_onek(double k, double *tf_baryon, double *tf_cdm);

  double R_drag;		// Photon-baryon ratio at drag epoch
  double R_equality;	// Photon-baryon ratio at equality epoch
  double sound_horizon;	// Sound horizon at drag epoch, in Mpc
  double k_silk;		// Silk damping scale, in Mpc^-1
  double alpha_c;	    // CDM suppression
  double alpha_b;	    // Baryon suppression
  double beta_b;		// Baryon envelope shift
  double beta_node;	// Sound horizon shift
  double k_peak;		// Fit to wavenumber of first peak, in Mpc^-1

  // temporary variables for doing interations
  int tmp_type;
  double tmp_alpha;
  double tmp_mass;
  double tmp_a;

  // interpolation of functions
  double z_interp;
  std::size_t n_interp;
  void calc_interp_dist();
  double interp(const std::vector<double>& table, double z) const;
  double invert(const std::vector<double>& table, double f_z) const;
  std::vector<double> redshift_interp;
  std::vector<double> coorDist_interp;
  std::vector<double> radDist_interp;
};

typedef COSMOLOGY *CosmoHndl;
/**
 *  \brief Class for calculating properties of NFW halo profile.
 *
 *   This class does not take into affect the cosmological correlation between concentration and mass.
 *   For this see the HALOCalculator class.
 */
class NFW_Utility {
public:
  NFW_Utility(){return;}
  ~NFW_Utility(){};

  // methods for NFW profile
  double NFW_V200(double M200,double R200);
  double NFW_Vmax(double cons,double M200,double R200);
  double NFW_Vr(double x,double cons,double M200,double R200);
  double NFW_M(double x,double cons,double M200);
  double NFW_deltac(double cons);
  double NFW_Concentration(double Vmax,double M200,double R200);
  double NFW_rho(double cons,double x);

  void match_nfw(float Vmax,float R_half,float mass,float *cons,float *Rsize);
  float Rsize(float cons,float Vmax,float mass);
  float g_func(float x);

private:
  float Vmax,R_half,mass;       /// Mass (solar masses)

  typedef float (NFW_Utility::*MemFunc)(float);
  float zbrentD(MemFunc func, float a,float b,float tols);
  float nfwfunc(float cons);
  float funcforconcentration(float cons);
};

/// wrapper functions for gsl integration / differentiation
double drdz_wrapper(double x, void *params);
double drdz_dark_wrapper(double x, void *params);
double Deltao_wrapper(double m, void *params);

#define cosmo_declare
#endif

/*** in cosmo.c ***/
int cosmo_compare(CosmoHndl cos1,CosmoHndl cos2);
void cosmo_copy(CosmoHndl cos1,CosmoHndl cos2);
void ders(double z,double Da[],double dDdz[]);
void dir(double r,double a[],double dadr[]);
double arctanh(double x);
double fmini(double a,double b);
double fmaxi(double a,double b);
double **dmatrixcos(long nrl, long nrh, long ncl, long nch);
void free_dmatrixcos(double **m, long nrl, long nrh, long ncl, long nch);

/* in nfw.c */

/* in powerEHv2.c */
void TFset_parameters(double omega0hh, double f_baryon, double Tcmb);
double TFfit_onek(double k, double *tf_baryon, double *tf_cdm);


#include <stdio.h>
#include <math.h>
#include "mf.h"
#include <gsl/gsl_integration.h>

//****************************the functions are listed below**********
double func1 (double y, double T)  //The self-consistent equation f(y)=0
{
	double res = tanh(3*y/T)-y;
	return res;
}
double func2 (double y, double T) //The free energy per site
{
	double res = 0.5*3*y*y-T*log(2*cosh(3*y)/T);
	return res;
}
//*****************************************************************************
int main()
{
	FILE *fp;
	int anyi;
	if((fp=fopen(filename,"w"))==NULL)
	{
		printf("cannot open the file: %s\n",filename);
		printf("Please input an integer to end the program:");
		scanf("%d",&anyi);
		return 0;
	}

	FILE *ffp;
	if((ffp=fopen(filename2,"w"))==NULL)
	{
		printf("cannot open the file: %s\n",filename2);
		printf("Please input an integer to end the program:");
		scanf("%d",&anyi);
		return 0;
	}

	double tem0;          //judge indicator
	double T_step;             //the step of Temperature
	T_step = (T_high-T_low)/(N_T-1);    //note N_T is the tatal points of T
//	printf("The temperature step is %lf\n",T_step);

	double y_1;
	double y_2;

	double tem_1;
	double tem_2;

	double T = T_low;

	int i,j;    //loop indicator
	for(i=0;i<N_T;i++)
	{
		T = T_low + i*T_step;

		y_1=1e-1;
		y_2=1e7;
		tem_1 =  func1(y_1, T);
		tem_2 =  func1(y_2, T);
		//if(tem_1<0||tem_2>0)
		if(tem_1*tem_2>0)
		{
			printf("The two points in the boundary isn't good! Give up them now. Do it, baby!\n");
			return 0;
		}

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

		for(j=10;j>1;j++)
		{
			tem_1 = func1(y_1, T);
			tem_2 = func1(y_2, T);
			tem0 = func1((y_1+y_2)/2.,T);
		//	printf("tem_1:%lf\ttem_2:%lf\n",tem_1,tem_2);
			if((tem_1<precision&&tem_1>=0)||(tem_1>-precision&&tem_1<=0))
			{
				fprintf(fp,"%lf\t%lf\n",T,y_1);
				fprintf(ffp,"%lf\t%lf\t%lf\n",T,func2(y_1,T),func2(-y_1,T));
				j=0;
			}
			if((tem_2<precision&&tem_2>=0)||(tem_2>-precision&&tem_2<=0))
			{
				fprintf(fp,"%lf\t%lf\n",T,y_2);
				fprintf(ffp,"%lf\t%lf\t%lf\n",T,func2(y_2,T),func2(-y_2,T));
				j=0;
			}
			if(tem_1*tem0>0)
			{
				y_1 = (y_1+y_2)/2;
			}
			if(tem_2*tem0>0)
			{
				y_2 = (y_1+y_2)/2;
			}
			printf("%lf\n",(y_1+y_2)/2); //the mediate point value

		}
	}
	return 1;
}


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rstat.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_ieee_utils.h>

double *
random_data(const size_t n, gsl_rng *r)
{
  size_t i;
  double *data = malloc(n * sizeof(double));

  for (i = 0; i < n; ++i)
    data[i] = gsl_rng_uniform(r);

  return data;
}

void
test_basic(const size_t n, const double data[], const double tol)
{
  gsl_rstat_workspace *rstat_workspace_p = gsl_rstat_alloc();
  const double expected_mean = gsl_stats_mean(data, 1, n);
  const double expected_var = gsl_stats_variance(data, 1, n);
  const double expected_sd = gsl_stats_sd(data, 1, n);
  const double expected_sd_mean = expected_sd / sqrt((double) n);
  const double expected_skew = gsl_stats_skew(data, 1, n);
  const double expected_kurtosis = gsl_stats_kurtosis(data, 1, n);
  double expected_rms = 0.0;
  double mean, var, sd, sd_mean, rms, skew, kurtosis;
  size_t i, num;
  int status;

  /* compute expected rms */
  for (i = 0; i < n; ++i)
    expected_rms += data[i] * data[i];

  expected_rms = sqrt(expected_rms / n);

  /* add data to rstat workspace */
  for (i = 0; i < n; ++i)
    gsl_rstat_add(data[i], rstat_workspace_p);

  mean     = gsl_rstat_mean(rstat_workspace_p);
  var      = gsl_rstat_variance(rstat_workspace_p);
  sd       = gsl_rstat_sd(rstat_workspace_p);
  sd_mean  = gsl_rstat_sd_mean(rstat_workspace_p);
  rms      = gsl_rstat_rms(rstat_workspace_p);
  skew     = gsl_rstat_skew(rstat_workspace_p);
  kurtosis = gsl_rstat_kurtosis(rstat_workspace_p);
  num      = gsl_rstat_n(rstat_workspace_p);

  gsl_test_int(num, n, "n n=%zu" , n);
  gsl_test_rel(mean, expected_mean, tol, "mean n=%zu", n);
  gsl_test_rel(var, expected_var, tol, "variance n=%zu", n);
  gsl_test_rel(sd, expected_sd, tol, "stddev n=%zu", n);
  gsl_test_rel(sd_mean, expected_sd_mean, tol, "stddev_mean n=%zu", n);
  gsl_test_rel(rms, expected_rms, tol, "rms n=%zu", n);
  gsl_test_rel(skew, expected_skew, tol, "skew n=%zu", n);
  gsl_test_rel(kurtosis, expected_kurtosis, tol, "kurtosis n=%zu", n);

  status = gsl_rstat_reset(rstat_workspace_p);
  gsl_test_int(status, GSL_SUCCESS, "rstat returned success");
  num = gsl_rstat_n(rstat_workspace_p);

  gsl_test_int(num, 0, "n n=%zu" , n);

  gsl_rstat_free(rstat_workspace_p);
}

void
test_quantile(const double p, const double data[], const size_t n,
              const double expected, const double tol, const char *desc)
{
  gsl_rstat_quantile_workspace *w = gsl_rstat_quantile_alloc(p);
  double result;
  size_t i;

  for (i = 0; i < n; ++i)
    gsl_rstat_quantile_add(data[i], w);

  result = gsl_rstat_quantile_get(w);

  if (fabs(expected) < 1.0e-4)
    gsl_test_abs(result, expected, tol, "%s p=%g", desc, p);
  else
    gsl_test_rel(result, expected, tol, "%s p=%g", desc, p);

  gsl_rstat_quantile_free(w);
}

int
main()
{
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  const double tol1 = 1.0e-8;
  const double tol2 = 1.0e-3;

  gsl_ieee_env_setup();

  {
    const size_t N = 2000000;
    double *data = random_data(N, r);
    double data2[] = { 4.0, 7.0, 13.0, 16.0, -5.0 };
    size_t i;

    test_basic(2, data, tol1);
    test_basic(100, data, tol1);
    test_basic(1000, data, tol1);
    test_basic(10000, data, tol1);
    test_basic(50000, data, tol1);
    test_basic(80000, data, tol1);
    test_basic(1500000, data, tol1);
    test_basic(2000000, data, tol1);

    for (i = 0; i < 5; ++i)
      data2[i] += 1.0e9;

    test_basic(5, data2, tol1);

    free(data);
  }

  {
    /* dataset from Jain and Chlamtac paper */
    const size_t n_jain = 20;
    const double data_jain[] = {  0.02,  0.15,  0.74,  3.39,  0.83,
                                  22.37, 10.15, 15.43, 38.62, 15.92,
                                  34.60, 10.28,  1.47,  0.40,  0.05,
                                  11.39,  0.27,  0.42,  0.09, 11.37 };
    double expected_jain = 4.44063435326;

    test_quantile(0.5, data_jain, n_jain, expected_jain, tol1, "jain");
  }

  {
    size_t n = 1000000;
    double *data = malloc(n * sizeof(double));
    double *sorted_data = malloc(n * sizeof(double));
    gsl_rstat_workspace *rstat_workspace_p = gsl_rstat_alloc();
    double p;
    size_t i;

    for (i = 0; i < n; ++i)
      {
        data[i] = gsl_ran_gaussian_tail(r, 1.3, 1.0);
        gsl_rstat_add(data[i], rstat_workspace_p);
      }

    memcpy(sorted_data, data, n * sizeof(double));
    gsl_sort(sorted_data, 1, n);

    /* test quantile calculation */
    for (p = 0.1; p <= 0.9; p += 0.1)
      {
        double expected = gsl_stats_quantile_from_sorted_data(sorted_data, 1, n, p);
        test_quantile(p, data, n, expected, tol2, "gauss");
      }

    /* test mean, variance */
    {
      const double expected_mean = gsl_stats_mean(data, 1, n);
      const double expected_var = gsl_stats_variance(data, 1, n);
      const double expected_sd = gsl_stats_sd(data, 1, n);
      const double expected_skew = gsl_stats_skew(data, 1, n);
      const double expected_kurtosis = gsl_stats_kurtosis(data, 1, n);
      const double expected_median = gsl_stats_quantile_from_sorted_data(sorted_data, 1, n, 0.5);

      const double mean = gsl_rstat_mean(rstat_workspace_p);
      const double var = gsl_rstat_variance(rstat_workspace_p);
      const double sd = gsl_rstat_sd(rstat_workspace_p);
      const double skew = gsl_rstat_skew(rstat_workspace_p);
      const double kurtosis = gsl_rstat_kurtosis(rstat_workspace_p);
      const double median = gsl_rstat_median(rstat_workspace_p);

      gsl_test_rel(mean, expected_mean, tol1, "mean");
      gsl_test_rel(var, expected_var, tol1, "variance");
      gsl_test_rel(sd, expected_sd, tol1, "stddev");
      gsl_test_rel(skew, expected_skew, tol1, "skew");
      gsl_test_rel(kurtosis, expected_kurtosis, tol1, "kurtosis");
      gsl_test_abs(median, expected_median, tol2, "median");
    }

    free(data);
    free(sorted_data);
    gsl_rstat_free(rstat_workspace_p);
  }

  gsl_rng_free(r);

  exit (gsl_test_summary());
}


//
//  gemm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/30/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _gemm_h
#define _gemm_h

/// @file gemm.h Computes general matrix-matrix products.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Computes products of real matrices.
   ///
   /// For real matrices A,B,C and real scalars alpha, beta
   ///
   ///        C := alpha * op(A) * op(B) + beta * C
   ///
   /// is calculated, where op(X) = X or X'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param transA Specifies whether op(A)=A or A' as follows:
   ///
   ///        if transA = 'N' or 'n' then op(A)=A
   ///        if transA = 'T' or 't' then op(A)=A'
   ///        if transA = 'C' or 'c' then op(A)=A'
   ///
   /// @param transB Specifies whether op(B)=B or B' as follows:
   ///
   ///        if transB = 'N' or 'n' then op(B)=B
   ///        if transB = 'T' or 't' then op(B)=B'
   ///        if transB = 'C' or 'c' then op(B)=B'
   ///
   /// @param m The number of rows of the matrices C and op(A).  m>=0
   /// @param n The number of columns of the matrices C and op(B).  n>=0
   /// @param k The number of columns of the matrix op(A), and the number of rows of the matrix op(B).  k>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix A, where op(A) is m-by-k.
   /// @param ldA Column length of the matrix A.  If transA='N' or 'n', ldA>=m; otherwise, ldA>=k.
   /// @param B Pointer to real matrix B, where op(B) is k-by-n.
   /// @param ldB Column length of the matrix B.  If transB='N' or 'n', ldB>=k; otherwise, ldA>=n.
   /// @param beta Real scalar.
   /// @param C Pointer to real matrix C, where C is m-by-n.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template<typename real_t>
   int GEMM(char transA, char transB, int_t m, int_t n, int_t k, real_t alpha, real_t *A, int_t ldA, real_t *B, int_t ldB, real_t beta, real_t *C, int_t ldC)
   {
      using std::toupper;
      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,l;
      real_t s,t;
      real_t *a,*b,*c;

      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;
      else if((m==0)||(n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='N'))
      {
         c=C;
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            for(l=0;l<k;l++)
            {
               for(i=0;i<m;i++)
               {
                  c[i]+=alpha*b[l]*a[i];
               }
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA!='N')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB!='N'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*b[j];
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }

      }
      return 0;
   }

   /// @brief Computes products of complex matrices.
   ///
   /// For complex matrices A,B,C and complex scalars alpha, beta
   ///
   ///        C := alpha * op(A) * op(B) + beta * C
   ///
   /// is calculated, where op(X) = X, X' or X.'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param transA Specifies whether op(A)=A, A.' or A' as follows:
   ///
   ///        if transA = 'N' or 'n' then op(A)=A
   ///        if transA = 'T' or 't' then op(A)=A.'
   ///        if transA = 'C' or 'c' then op(A)=A'
   ///
   /// @param transB Specifies whether op(B)=B, B.' or B' as follows:
   ///
   ///        if transB = 'N' or 'n' then op(B)=B
   ///        if transB = 'T' or 't' then op(B)=B.'
   ///        if transB = 'C' or 'c' then op(B)=B'
   ///
   /// @param m The number of rows of the matrices C and op(A).  m>=0
   /// @param n The number of columns of the matrices C and op(B).  n>=0
   /// @param k The number of columns of the matrix op(A), and the number of rows of the matrix op(B).  k>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix A, where op(A) is m-by-k.
   /// @param ldA Column length of the matrix A.  If transA='N' or 'n', ldA>=m; otherwise, ldA>=k.
   /// @param B Pointer to complex matrix B, where op(B) is k-by-n.
   /// @param ldB Column length of the matrix B.  If transB='N' or 'n', ldB>=k; otherwise, ldA>=n.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex matrix C, where C is m-by-n.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template<typename real_t>
   int GEMM(char transA, char transB, int_t m, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::conj;
      using std::toupper;
      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*b,*c;
      complex<real_t> s,t;

      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;
      else if((m==0)||(n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='N'))
      {
         c=C;
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            for(l=0;l<k;l++)
            {
               for(i=0;i<m;i++)
               {
                  c[i]+=alpha*b[l]*a[i];
               }
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*b[j];
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*conj(b[j]);
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*conj(b[j]);
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*conj(b[j]);
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template<> int GEMM<float>(char transA, char transB, int_t m, int_t n, int_t k, float alpha, float *A, int_t ldA, float *B, int_t ldB, float beta, float *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_sgemm(CblasColMajor,TransA,TransB,m,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template<> int GEMM<double>(char transA, char transB, int_t m, int_t n, int_t k, double alpha, double *A, int_t ldA, double *B, int_t ldB, double beta, double *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_dgemm(CblasColMajor,TransA,TransB,m,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template<> int GEMM<float>(char transA, char transB, int_t m, int_t n, int_t k, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB, complex<float> beta, complex<float> *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_cgemm(CblasColMajor,TransA,TransB,m,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

   template<> int GEMM<double>(char transA, char transB, int_t m, int_t n, int_t k, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB, complex<double> beta, complex<double> *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_zgemm(CblasColMajor,TransA,TransB,m,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

#endif
}
#endif