/* population genetics tools- mainly for working with site frequency spectrum
/
/
/ Andrew Kern
*/

#include "stdio.h"
#include "math.h"
#include "popGenTools.h"
#include "numerical.h"
#include "assert.h"
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf.h>

/* function for sfs generation */
double my_f(double q, void * p){
  struct my_f_params * params = (struct my_f_params *) p;
  double beta = params->beta;
  int i = params->i;
  int n = params->n;

  return ((1.0 - exp(-2.0 * beta * (1.0 - q))) / (1.0 - exp(-2.0 * beta))) * (2.0 / (q * (1.0 - q))) * gsl_ran_binomial_pdf(i, q, n);
}

/* integral of f over allele freq */
double my_F(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  gsl_function f;
  gsl_integration_workspace * w = gsl_integration_workspace_alloc (100000);
  double result,error;

  if (beta == 0){
    gsl_integration_workspace_free(w);
    return 1.0 / params->i;
  }
  else{
    fParams.beta = beta;
    fParams.i = params->i;
    fParams.n = params->n;
    f.function = &(my_f);
    f.params = &fParams;
    gsl_integration_qags(&f,0.0,1.0,1e-5,1e-5, 100000,w, &result,&error);
    gsl_integration_workspace_free(w);
    return result;
  }
}

/* integral of f over allele freq using numerical recipes integration */
double my_F2(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  double result;

  if (beta == 0){
    return 1.0 / params->i;
  }
  else{
    fParams.beta = beta;
    fParams.i = params->i;
    fParams.n = params->n;
    result = d_qromb2(&my_f, 0.0, 1.0, &fParams);
    return result;
  }
}

/* this returns the prob of SNP freq i given n and beta */
double snpProb(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;
  long double tot;
  int i;
  gsl_function F;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;
    F.function =  &my_F;
    F.params = &tempParams;
    tot += GSL_FN_EVAL(&F, beta);
  }

  /* reset parameters */
  F.function =  &my_F;
  F.params = params;

  return   GSL_FN_EVAL(&F, beta) / tot ;
}

/* trying to speedup the snpProb routine with a lookup of the denominators (in snpProb_params) */
double snpProbDenomLookup(double beta, void * p){
  struct my_snpProb_params * params = (struct my_snpProb_params *) p;
  struct my_F_params tempParams;
  gsl_function F;

 /* set parameters */
  tempParams.n = params->n;
  tempParams.i = params->i;
  F.function =  &my_F;
  F.params = &tempParams;

  return   GSL_FN_EVAL(&F, beta) / params->denom ;
}

/* sampleSize vector - returns a vector of 1s or 0s depending on sample sizes in the data */
gsl_vector *sampleSizeVector(struct snp data[], int snpNumber, int maxSampleSize){
  gsl_vector *bools;
  int i;

  //alloc and initialize bools vector
  bools = gsl_vector_alloc(maxSampleSize + 1);
  gsl_vector_set_zero(bools);

  //go through data, set each sampleSize value to 1
  for(i = 0; i < snpNumber; i++){
    gsl_vector_set(bools,data[i].n, 1);
  }
  return(bools);
}

/* returns a vector of snpProbDenominators from i = 2 to n, 1 indexed */
gsl_vector *makeSnpProbDenomVector(double beta, int maxSampleSize, gsl_vector *sampleSizeVector){
  gsl_vector *probs;
  int i, j, test;
  double tot;
  struct my_F_params tempParams;
  gsl_function F;

  probs = gsl_vector_alloc(maxSampleSize + 1);

  /* go from n = 2 to maxSampleSize, calc total prob space for each, put in vector probs */
  for(j = 2; j < maxSampleSize + 1; j++){
    //check if sampleSize is included
    test = gsl_vector_get(sampleSizeVector,j);
      if (test){
      tot = 0.0;
      tempParams.n = j;
      for(i = 1; i < j ; i++){
	tempParams.i = i;
	F.function =  &my_F;
	F.params = &tempParams;
	tot += GSL_FN_EVAL(&F, beta);
      }
      gsl_vector_set(probs, j, tot);
    }
  }
   return probs;
}

/* snpProbDenom - returns denom for snpProb */
double snpProbDenom(double beta, int sampleSize){
  int i;
  double tot;
  struct my_F_params tempParams;
  gsl_function F;

  tot = 0.0;
  tempParams.n = sampleSize;
  for(i = 1; i < sampleSize ; i++){
	tempParams.i = i;
	F.function =  &my_F;
	F.params = &tempParams;
	tot += GSL_FN_EVAL(&F, beta);
      }
  return(tot);
}

/* snpProbMatrix- returns a matrix of log snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1) */
gsl_matrix *snpProbMatrix(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //get denoms
  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.denom = gsl_vector_get(denoms,i);
	  f.function = &snpProbDenomLookup;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, log(GSL_FN_EVAL(&f, beta)));
	}
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}
/* snpProbMatrix- returns a matrix of  snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1) */
gsl_matrix *snpProbMatrixNotLog(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //get denoms
  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.denom = gsl_vector_get(denoms,i);
	  f.function = &snpProbDenomLookup;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, beta));
	}
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}
/* snpProbMatrixNotLogFull- returns a matrix of  snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). All freqs/samplesizes included */
gsl_matrix *snpProbMatrixNotLogFull(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);

  //get denoms

  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //check for sampleSize
    if (gsl_vector_get(sampleSizeVector,i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//calc prob
	FParams.i = j;
	FParams.n = i;
	FParams.denom = gsl_vector_get(denoms,i);
	f.function = &snpProbDenomLookup;
	f.params = &FParams;
	gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, beta));
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}

/* snpProbVectorNotLog- returns a vector of log snpProbs at specified sampleSize columns represent freqs (1 to max -1) */
gsl_vector *snpProbVectorNotLog(double beta,  int sampleSize){
  double denom;
  gsl_vector *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i;

  //alloc probs
  probs = gsl_vector_alloc(sampleSize);
  gsl_vector_set_zero(probs);
  //get denoms
  denom = snpProbDenom(beta, sampleSize);

  //go through freqs
  for(i = 1; i < sampleSize; i++){
    FParams.i = i;
    FParams.n = sampleSize;
    FParams.denom = denom;
    f.function = &snpProbDenomLookup;
    f.params = &FParams;
    gsl_vector_set(probs, i, GSL_FN_EVAL(&f, beta));
  }
  return(probs);
}

/* likelihood function for SFS given the data; p here contains the data and the weights */
double my_lik(double beta, void * p){
	struct my_lik_params * params = (struct my_lik_params *) p;
	struct my_F_params FParams;
	gsl_function f;
	double lik;
	int i;

	lik = 0;
	for(i = 0; i < params->snpNumber; i++){
		FParams.i = params->data[i].i;
		FParams.n = params->data[i].n;
		f.function = &snpProb;
		f.params = &FParams;
		lik += log(GSL_FN_EVAL(&f, beta));
		}
	return -lik;
}

/* likelihood function for SFS where each SNP has independent selection coeff */
double sfsLikBetaVector(gsl_vector *betas, void * p){
	struct my_lik_params * params = (struct my_lik_params *) p;
	struct my_F_params FParams;
	gsl_function f;
	double lik;
	int i;

	lik = 0;
	for(i = 0; i < params->snpNumber; i++){
		FParams.i = params->data[i].i;
		FParams.n = params->data[i].n;
		f.function = &snpProb;
		f.params = &FParams;
		lik += log(GSL_FN_EVAL(&f, gsl_vector_get(betas, i)));
		}
	return -lik;
}

/* weighted likelihood function for SFS given the data; here contains the data and the weights */
double weightedLik(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  struct my_F_params FParams;
  gsl_function f;
  double lik;
  int i;

  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    FParams.i = params->data[i].i;
    FParams.n = params->data[i].n;
    f.function = &snpProb;
    f.params = &FParams;
    lik += gsl_vector_get(params->weights, i) * log(GSL_FN_EVAL(&f, beta));
  }
  return -lik;
}

/* weighted likelihood function for SFS given the data; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLook(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik;
  int i;
  gsl_matrix *probs;

  //make prob matrix (note this is in logs)
  probs = snpProbMatrix(beta, params->maxSampleSize, params->sampleSizeVector, params->sfsBools);
  //now go and tally likelihood
  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)  * gsl_vector_get(params->weights,i);
  }
  gsl_matrix_free(probs);
  return -lik;
}

double weightedLikLookCI(double beta, void * p){
  struct my_likCI_params *params = (struct my_likCI_params *) p;
  struct my_lik_params likParams;
  gsl_function l;
  double result;

  likParams.data = params->data;
  likParams.snpNumber = params->snpNumber;
  likParams.sampleSizeVector = params->sampleSizeVector;
  likParams.maxSampleSize = params->maxSampleSize;
  likParams.weights = params->weights;
  likParams.sfsBools = params->sfsBools;
  l.function = &weightedLikLook;
  l.params = &likParams;
  result = GSL_FN_EVAL(&l, beta) - (params->lMax) - (params->logUnits) ;
  //  printf("%f\t%f\t%f\n",params->lMax,GSL_FN_EVAL(&l, beta),result);
  return result;
}

/* wrapper for my_lik for use in mnbrak */
double likWrap(double beta){
	gsl_function f;

	f.function = &my_lik;
	f.params = NULL;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrap(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLik;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLook(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLook;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

double  wlikCIWrapLook(double beta, void * p){
	gsl_function f;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	f.function = &weightedLikLookCI;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/*error checking function for likelihood function */
void printProfile(double min, double max, void * p){
  gsl_function f;
  struct my_lik_params *params =  (struct my_lik_params *) p;
  int i;

  f.function = &weightedLikLook;
  f.params = params;
  for(i = min; i < max; i++){
    printf("%d\t%f\n",i, GSL_FN_EVAL(&f, i));
  }

}

/* get ML estimate of beta using likelihood function; BRENT method for gsl */
double ml_est(double * lik_beta_hat){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc;
	gsl_function L;

	/* get minimum bracket */
	ax = -10.0;
	bx = 0.0;
	cx = 10.0;

	mnbrak(&ax, &bx, &cx, &fa, &fb, &fc, &my_lik);

	/* check if there is a minimum, if not return inf */
	if (fa == fb || fc == fb || ax > cx){
		*lik_beta_hat = fa;
		return 666.0;
		}
	else{

		/* do mimization */

		L.function = &my_lik;
		//should have param line here but no params....

		T = gsl_min_fminimizer_brent;
		s = gsl_min_fminimizer_alloc(T);
		gsl_min_fminimizer_set(s, &L, bx, ax, cx);

		do{
			iter++;
			status = gsl_min_fminimizer_iterate(s);
			 m = gsl_min_fminimizer_x_minimum (s);
			  a = gsl_min_fminimizer_x_lower (s);
			  b = gsl_min_fminimizer_x_upper (s);

			  status = gsl_min_test_interval (a, b, 0.001, 0.0);

		}
	  while (status == GSL_CONTINUE && iter < max_iter);
	  gsl_min_fminimizer_free(s);

	  return m;
	}
}

/* get weighted ML estimate of beta using likelihood function; BRENT method for gsl */
double weighted_ml_est(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, min;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -10.0;
	bx = 1.0;
	cx = 10;
	L.function = &weightedLik;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrap, bx, params);

	//check if there is a minimum, if not return 666
	if (fa == fb || fc == fb || ax > cx){
	  min = fa;
	  if(fb < min){
	    min = fb;
	  }
	  if(fc < min){
	    min = fc;
	  }
	  *lik_beta_hat = min;

	  return 666.0;
	}
	else{
	  // do mimization

	  //initialize lik function
	  L.function = &weightedLik;
	  L.params = params;

	  //min routine
	  T = gsl_min_fminimizer_brent;
	  s = gsl_min_fminimizer_alloc(T);
	  gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	  do{
	    iter++;
	    status = gsl_min_fminimizer_iterate(s);
	    m = gsl_min_fminimizer_x_minimum (s);
	    a = gsl_min_fminimizer_x_lower (s);
	    b = gsl_min_fminimizer_x_upper (s);
	    status = gsl_min_test_interval (a, b, 0.001, 0.0);
	  }
	  while (status == GSL_CONTINUE && iter < max_iter);
	  *lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	  gsl_min_fminimizer_free(s);
	  return m;
	}
}

double weighted_ml_est_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLook;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLook, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}

	// do mimization

	//initialize lik function
	L.function = &weightedLikLook;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* same as above but outputs 666 on minimization error */
double weighted_ml_est_lookup_errReport(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLook;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLook, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	//check for error
	if (fb >= fc || fb >= fa){
	  return(666.0);
	}

	// do mimization

	//initialize lik function
	L.function = &weightedLikLook;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_set_error_handler_off ();
	status = gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	if (status == GSL_EINVAL){
	  return(666);
	}
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* get weighted ML lower CI beta using weightedLikLookCI method; BRENT method for gsl */
double weighted_ml_CILower_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;
	double m, a, b, ax, cx;
	gsl_function L;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	// get minimum bracket
	ax = -50.0;
	cx = params->beta_hat;

	// do root finding
	//initialize lik function
	L.function = &weightedLikLookCI;
	L.params = params;

	//min routine
	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &L, ax, cx);
	do{
	  iter++;
	  status = gsl_root_fsolver_iterate(s);
	  m = gsl_root_fsolver_root(s);
	  a = gsl_root_fsolver_x_lower(s);
	  b = gsl_root_fsolver_x_upper(s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	gsl_root_fsolver_free(s);
	return m;
}

/* get weighted ML upper CI beta using weightedLikLookCI method; BRENT method for gsl */
double weighted_ml_CIUpper_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;
	double m, a, b, ax, cx;
	gsl_function L;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	// get minimum bracket
	cx = 100.0;
	ax = params->beta_hat;

	// do root finding
	//initialize lik function
	L.function = &weightedLikLookCI;
	L.params = params;

	//min routine
	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &L, ax, cx);
	do{
	  iter++;
	  status = gsl_root_fsolver_iterate(s);
	  m = gsl_root_fsolver_root(s);
	  a = gsl_root_fsolver_x_lower(s);
	  b = gsl_root_fsolver_x_upper(s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	gsl_root_fsolver_free(s);
	return m;
}

/* summarizeSFS- returns a matrix of counts which summarize the SFS conditional
   upon sampleSize. rows = sampleSize, column = freqs */

gsl_matrix *summarizeSFS(int maxSampleSize, struct snp data[], int snpNumber){
  gsl_matrix *counts;
  int i;

  //alloc matrix of size maxSampleSize by maxSampleSize
  counts = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize + 1);
  gsl_matrix_set_zero(counts);
  assert(counts != NULL);

  gsl_matrix_set_zero(counts);

  //go through data, fill up matrix
  for(i = 0; i < snpNumber; i++){
    gsl_matrix_set(counts, data[i].n, data[i].i, gsl_matrix_get(counts, data[i].n, data[i].i) + 1);
  }
  return(counts);

}

/* summarizeSFS- returns a matrix of booleans which summarize the SFS conditional
   upon sampleSize. rows = sampleSize, column = freqs */

gsl_matrix *summarizeSFSBool(int maxSampleSize, struct snp data[], int snpNumber){
  gsl_matrix *counts;
  int i;

  //alloc matrix of size maxSampleSize by maxSampleSize
  counts = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize + 1);
  gsl_matrix_set_zero(counts);
  assert(counts != NULL);

  gsl_matrix_set_zero(counts);

  //go through data, fill up matrix
  for(i = 0; i < snpNumber; i++){
    gsl_matrix_set(counts, data[i].n, data[i].i, 1);
  }
  return(counts);

}

/* simulateSiteFreq- returns an int representing the frequency of
   of a site (unfolded), i, in sampleSize n, conditional on sel.
   coeff. beta , also needs a pointer to a gsl random number generator */

int simulateSiteFreq(int sampleSize, double alpha, void *r){
  int i;
  double sum, probs[sampleSize - 1], rand;
  gsl_function p;
  struct my_F_params simParams;
  gsl_rng * rn = (gsl_rng *) r;

  simParams.n = sampleSize;
  /* put probs in vector */
  for(i = 0; i < sampleSize - 1; i++){
    simParams.i = i + 1;
    p.function = &snpProb;
    p.params = &simParams;
    probs[i] = GSL_FN_EVAL(&p, alpha);
  }

  /* choose frequency */
  rand = gsl_rng_uniform(rn);
  sum = 0;
  for(i = 0; i < sampleSize - 1; i++){
    sum += probs[i];
    if (rand <= sum){
      return(i+1);
    }
  }
  return(666);
}

/* ascertainment stuff -- following Nielsen et al. 2004  */

/* simpleAscertain--  Nielsen et al. 2004 eqn 2 *, returns the prob of ascertainment
given a sample freq */
double simpleAscertain(double sampleFreq, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  double sampleSize, ascSize,tmpSampleFreq;
  double num1 = 0.0;
  double num2 = 0.0;

  sampleSize = params->n;
  ascSize = params->ascSize;
  tmpSampleFreq = (int) sampleFreq;

  //test for weirdness

  if (sampleFreq < ascSize){
    num1 = 0;
  }
  else{
    num1 = gsl_sf_choose(sampleFreq, ascSize);
  }

  if ((sampleSize -sampleFreq) < ascSize){
    num2 = 0;
  }
  else{
    num2 =  gsl_sf_choose(sampleSize - sampleFreq, ascSize);
  }

  return(1.0 - ((num1 + num2) / gsl_sf_choose(sampleSize, ascSize)));
}

/* outgroupAscertain-- equivalent to Nielsen et al. 2004 eqn 2,
 returns the prob of ascertainment given a sample freq, but
is for use when ascertainment was based on single outgroup comparisons
(e.g. divergence between reference genomes). specifically this
calculates the prob of sampling only ancestral alleles.
IMPORTANT- sampleFreq here is derived allele freq */
double outgroupAscertain(double sampleFreq, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  double sampleSize, ascSize;
  double num = 0.0;

  sampleSize = params->n;
  ascSize = params->ascSize;

  //test for weirdness
  if ((sampleSize - sampleFreq) < ascSize){
    num = 0;
  }
  else{
    num =  gsl_sf_choose((sampleSize - sampleFreq), ascSize);
  }

  return(num / gsl_sf_choose(sampleSize, ascSize));
}

/* probAscertainmentGivenModel-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model */
double probAscertainmentGivenModel(double beta, void *p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;

  long double tot, tmpres, tmpres2;
  int i=1;
  gsl_function F, G;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;
  tempParams.ascSize = params->ascSize;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;

    F.function =  &(snpProb);
    F.params = &tempParams;
    tmpres = 0.0;
    tmpres = GSL_FN_EVAL(&F, beta);
    G.function = &simpleAscertain;
    G.params = &tempParams;
    tmpres2 = GSL_FN_EVAL(&G, i) ;
    tot += tmpres * tmpres2;

  }

  return(tot) ;
}

/* probAscertainmentGivenModel-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model */
double probOutgroupAscertainmentGivenModel(double beta, void *p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;

  long double tot, tmpres, tmpres2;
  int i=1;
  gsl_function F, G;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;
  tempParams.ascSize = params->ascSize;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;

    F.function =  &(snpProb);
    F.params = &tempParams;
    tmpres = 0.0;
    tmpres = GSL_FN_EVAL(&F, beta);
    G.function = &outgroupAscertain;
    G.params = &tempParams;
    tmpres2 = GSL_FN_EVAL(&G, i) ;
    tot += tmpres * tmpres2;

  }

  return(tot) ;
}

/* probAscertainmentGivenModelLookup-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model, lookup version
   requires snpProb matrix and ascProb matrix */
double probAscertainmentGivenModelLookup(double beta, int sampSize, gsl_matrix *snpProbs, gsl_matrix *ascProbs){

  long double tot;
  int i=1;

  /* add up total prob space */
  tot = 0.0;

  for(i = 1; i < sampSize ; i++){
    tot += gsl_matrix_get(snpProbs, sampSize, i) * gsl_matrix_get(ascProbs, sampSize, i);
  }
  return(tot) ;
}

/* probAscertainmentGivenModelHemiLookup-- same
   as above but expects vector not matrix of snpProbs */
double probAscertainmentGivenModelHemiLookup(double beta, int sampSize, gsl_vector *snpProbs, gsl_matrix *ascProbs){

  long double tot;
  int i=1;

  /* add up total prob space */
  tot = 0.0;

  for(i = 1; i < sampSize ; i++){
    tot += gsl_vector_get(snpProbs, i) * gsl_matrix_get(ascProbs, sampSize, i);
  }
  return(tot) ;
}

/* ascSize vector - returns a vector of 1s or 0s depending on ascertainment sample sizes in the data */
gsl_vector *ascSizeVector(struct snp data[], int snpNumber, int maxSampleSize){
  gsl_vector *bools;
  int i;

  //alloc and initialize bools vector
  bools = gsl_vector_alloc(maxSampleSize + 1);
  gsl_vector_set_zero(bools);

  //go through data, set each sampleSize value to 1
  for(i = 0; i < snpNumber; i++){
    gsl_vector_set(bools,data[i].ascSize, 1);
  }
  return(bools);
}

/* snpAscMatrix- returns a matrix of simpleAscertainment probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size  */
gsl_matrix *snpAscMatrix(int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools, \
		 int ascSize){
  gsl_matrix *probs;
  struct my_F_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.ascSize = ascSize;
	  f.function = &simpleAscertain;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, j));
	}
      }
    }
  }
  return(probs);
}

/* snpOutgroupAscMatrix- returns a matrix of outgroupAscertainment probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size  */
gsl_matrix *snpOutgroupAscMatrix(int maxSampleSize, gsl_vector *sampleSizeVector, int ascSize){
  gsl_matrix *probs;
  struct my_F_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < i; j++){
	//calc prob
	FParams.i = j;
	FParams.n = i;
	FParams.ascSize = ascSize;
	f.function = &outgroupAscertain;
	f.params = &FParams;
	gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, j));
      }
    }
  }
  return(probs);
}

/*estimateAscSFS-- ML estimates of SFS, given ascertainment. returns
 a matrix of probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size */

gsl_matrix *estimateAscSFS(int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools, \
			   int ascSize, gsl_matrix *sfsSummary){
  gsl_matrix *probs, *ascProbs;
    int i, j;
  double ssTmp, p_hat;
  double xij;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //calculate ascProbs
  ascProbs = snpAscMatrix(maxSampleSize, sampleSizeVector, sfsBools, \
			  ascSize);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs to tally up denom at sampleSize
      ssTmp = 0.0;
      for(j = 1; j < maxSampleSize; j++){
	xij = gsl_matrix_get(sfsSummary, i, j);
	if (xij > 0){
	  ssTmp += xij / gsl_matrix_get(ascProbs, i, j);
	}
      }
      //go back through freqs to assign p_hats at sampleSize
      for(j = 1; j < maxSampleSize; j++){
	xij = gsl_matrix_get(sfsSummary, i, j);
	p_hat = (double) xij / gsl_matrix_get(ascProbs, i, j);
	gsl_matrix_set(probs, i, j, p_hat / ssTmp);
      }
    }
  }
  return(probs);
}

/* weighted likelihood function for SFS given the data and ascertainment; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLookAsc(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik, num;
  int i;
  gsl_matrix *probs, *ascProbs;
  gsl_vector *ascDenoms;

  //make prob matrix (note this is in logs)
  probs = snpProbMatrix(beta, params->maxSampleSize, params->sampleSizeVector, params->sfsBools);
  //make ascProb matrix (NOT in logs!)
  ascProbs = snpAscMatrix(params->maxSampleSize, params->sampleSizeVector, params->sfsBools, params->ascSize);

  //make vector of denoms
  ascDenoms = gsl_vector_alloc(params->maxSampleSize + 1);
  gsl_vector_set_zero(ascDenoms);
  for(i = params->ascSize; i <= params->maxSampleSize; i++){
     gsl_vector_set(ascDenoms,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbs));
  }

  //now go and tally likelihood
  lik = 0;
  num = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)	\
      + log(gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i)) \
      - log(gsl_vector_get(ascDenoms, params->data[i].n));
     }
  gsl_matrix_free(probs);
  gsl_matrix_free(ascProbs);
  gsl_vector_free(ascDenoms);

  return -lik;
}

/* weighted likelihood function for SFS given the data and outgroup ascertainment; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLookOutgroupAsc(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik, num;
  int i;
  gsl_matrix *probs, *ascProbs;
  gsl_vector *ascDenoms;

  //make prob matrix
  probs = snpProbMatrixNotLogFull(beta, params->maxSampleSize, params->sampleSizeVector);
  //make ascProb matrix
  ascProbs = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, params->ascSize);

  //make vector of denoms
  ascDenoms = gsl_vector_alloc(params->maxSampleSize + 1);
  gsl_vector_set_zero(ascDenoms);
  for(i = params->ascSize; i <= params->maxSampleSize; i++){
     gsl_vector_set(ascDenoms,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbs));
  }

  //now go and tally likelihood
  lik = 0;
  num = 0;
  for(i = 0; i < params->snpNumber; i++){
    num = (gsl_matrix_get(probs,params->data[i].n,params->data[i].i) * gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i)) \
      / gsl_vector_get(ascDenoms, params->data[i].n);
    lik += log(num);
    //printf("first: %f\t second: %f\t third: %f\n",gsl_matrix_get(probs,params->data[i].n,params->data[i].i),gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i),gsl_vector_get(ascDenoms, params->data[i].n));
    }

  gsl_matrix_free(probs);
  gsl_matrix_free(ascProbs);
  gsl_vector_free(ascDenoms);

  return -lik;
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLookAsc(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLookAsc;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLookOutgroupAsc(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLookOutgroupAsc;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* get weighted ML estimate of beta using likelihood function (lookup) conditional on ascertainment
; BRENT method for gsl */
double weighted_ml_est_lookup_asc(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLookAsc;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLookAsc, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	// do mimization

	//initialize lik function
	L.function = &weightedLikLookAsc;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* get weighted ML estimate of beta using likelihood function (lookup) conditional on outgroup ascertainment
; BRENT method for gsl */
double weighted_ml_est_lookup_outgroup_asc(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLookOutgroupAsc;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLookOutgroupAsc, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	// do mimization

	//initialize lik function
	L.function = &weightedLikLookOutgroupAsc;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* likelihood function for SFS where each SNP has
 independent selection coeff and outgroup ascertainment */
double sfsLikBetaVectorOutgroupAsc(gsl_vector *betas, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double pSNP, pAsc, pAscModel,lik, tmp;
  int i;
  gsl_matrix *ascProbs;
  gsl_vector *snpProbs;

  lik = 0;
  //make ascProb matrix
  ascProbs = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
				  params->ascSize);
  //go through snps
  for(i = 0; i < params->snpNumber; i++){
    //create vector of snpProbs, find pSNP_i
    snpProbs = snpProbVectorNotLog(gsl_vector_get(betas, i), params->data[i].n);
    pSNP = gsl_vector_get(snpProbs, params->data[i].i);

    //lookup pAsc_i
    pAsc = gsl_matrix_get(ascProbs, params->data[i].n, params->data[i].i);

    //get pAscModel
    pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
						      snpProbs, ascProbs);
    tmp = pSNP * pAsc / pAscModel;
    lik += log(tmp);
    gsl_vector_free(snpProbs);
  }
  gsl_matrix_free(ascProbs);
  return -lik;
}


/** Fit a set of points to a model that is subject to rotation and translation.
 *
 * The model is defined by a set of primitive faces such as plane, cylinder, sphere, etc.
 * The measured points are first translated by (xt, yt, zt),
 * then rotated about x-y-z axes by g, b, a angles in sequence,
 *
 * These 6 parameters are the output of the fit.
 *
 * Author: Yuan Mei
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>

#ifdef LINE_MAX
#undef LINE_MAX
#define LINE_MAX 4096
#endif

#ifndef LINE_MAX
#define LINE_MAX 4096
#endif

/** Check if the character is a field separator. */
#ifndef sepq
#define sepq(a) ((a)==' ' || (a)=='\t')
#endif

#define NDIM   3    /**< number of dimensions */

enum {
    FACE_PLANE=1,
    FACE_CYLINDER=2,
    FACE_SPHERE=3,
    FACE_LINE=12,
    FACE_POINT=13
};

struct face {
    int ftype; /**< type of face: plane, cylinder etc... */
    double x0; /**< origin */
    double y0;
    double z0;
    double nx; /**< normal vector */
    double ny;
    double nz;
    double r;  /**< radius */
};

struct data {
    double pr;   /**< probe radius */
    const struct face *faces;
    size_t n;    /**< number of measured data points */
    size_t *fid; /**< idx of face in the registered faces */
    double *x;   /**< measured point */
    double *y;
    double *z;
    double *w;   /**< weights */
    double *x1;  /**< after transformation */
    double *y1;
    double *z1;
};

/** Distance from measured point to face. */
int dist_f(const gsl_vector *p, void *data, gsl_vector *f)
{
    double pr = ((struct data *)data)->pr;
    const struct face *faces = ((struct data *)data)->faces;
    size_t n = ((struct data *)data)->n;
    size_t *fid = ((struct data *)data)->fid;
    double *x = ((struct data *)data)->x;
    double *y = ((struct data *)data)->y;
    double *z = ((struct data *)data)->z;

    double a  = gsl_vector_get(p, 0), Ca = cos(a), Sa = sin(a);
    double b  = gsl_vector_get(p, 1), Cb = cos(b), Sb = sin(b);
    double g  = gsl_vector_get(p, 2), Cg = cos(g), Sg = sin(g);
    double xt = gsl_vector_get(p, 3);
    double yt = gsl_vector_get(p, 4);
    double zt = gsl_vector_get(p, 5);

    size_t i;
    for (i = 0; i < n; i++) {
        int ftype = faces[fid[i]].ftype;
        double x0 = faces[fid[i]].x0;
        double y0 = faces[fid[i]].y0;
        double z0 = faces[fid[i]].z0;
        double nx = faces[fid[i]].nx;
        double ny = faces[fid[i]].ny;
        double nz = faces[fid[i]].nz;
        double r  = faces[fid[i]].r;

        double x1, y1, z1;
        x1 = Ca*Cb * (x[i]+xt) + (Ca*Sb*Sg-Sa*Cg) * (y[i]+yt) + (Ca*Sb*Cg+Sa*Sg) * (z[i]+zt);
        y1 = Sa*Cb * (x[i]+xt) + (Sa*Sb*Sg+Ca*Cg) * (y[i]+yt) + (Sa*Sb*Cg-Ca*Sg) * (z[i]+zt);
        z1 =  - Sb * (x[i]+xt) +            Cb*Sg * (y[i]+yt) +            Cb*Cg * (z[i]+zt);

        ((struct data *)data)->x1[i] = x1;
        ((struct data *)data)->y1[i] = y1;
        ((struct data *)data)->z1[i] = z1;

        double d, dist=0.0;
        switch (ftype) {
        case FACE_PLANE:
            d = -(nx*x0 + ny*y0 + nz*z0);
            dist = fabs(nx*x1 + ny*y1 + nz*z1 + d)
                / sqrt(nx*nx + ny*ny + nz*nz);
            dist -= pr;
            break;
        case FACE_CYLINDER:
        case FACE_LINE:
        {
            double dx = x1 - x0, dy = y1 - y0, dz = z1 - z0;
            double s1 = dy * nz - dz * ny, s2 = dz*nx - dx*nz, s3 = dx*ny - dy*nx;
            dist = fabs(sqrt((s1*s1 + s2*s2 + s3*s3)/(nx*nx + ny*ny + nz*nz)) - r);
            if (ftype == FACE_CYLINDER)
                dist -= pr;
        }
            break;
        case FACE_SPHERE:
        case FACE_POINT:
            dist = fabs(sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0) + (z1-z0)*(z1-z0)) - r);
            if (ftype == FACE_SPHERE)
                dist -= pr;
            break;
        default:
            dist = 0.0;
            break;
        }
        gsl_vector_set(f, i, dist);
    }

    return GSL_SUCCESS;
}

void callback(const size_t iter, void *params,
              const gsl_multifit_nlinear_workspace *w)
{
    gsl_vector *f = gsl_multifit_nlinear_residual(w);
    gsl_vector *x = gsl_multifit_nlinear_position(w);
    double rcond=0;

    /* compute reciprocal condition number of J(x) */
    /* cond(J) = %8.4f, 1.0 / rcond */
    gsl_multifit_nlinear_rcond(&rcond, w);

    fprintf(stderr, "iter %2zu: a = %7.4f, b = %7.4f, g = %7.4f, xt = %7.4f, yt = %7.4f, zt = %7.4f, |f(x)| = %.4f\n",
            iter,
            gsl_vector_get(x, 0),
            gsl_vector_get(x, 1),
            gsl_vector_get(x, 2),
            gsl_vector_get(x, 3),
            gsl_vector_get(x, 4),
            gsl_vector_get(x, 5),
            gsl_blas_dnrm2(f));
}

/** Read a long line from file.
 * @param[inout] s string of the line, is allocated when s==NULL and n==0 and grown as needed.
 * @param[inout] n current size of s
 * @return s
 */
static char *file_read_long_line(char **s, size_t *n, FILE *fp)
{
    const int bufsz = LINE_MAX;
    char *p;
    size_t cnt, sz;

    if ( *s == NULL && *n == 0 ) {
        *n = bufsz;
        if ( (*s = calloc(*n, sizeof(char))) == NULL ) exit(-1);
    }
    p = *s;
    sz = *n;
    while ( 1 ) {
        if ( fgets(p, sz, fp) == NULL ) return NULL;
        cnt = strlen(*s);
        if ( (*s)[cnt-1] == '\n' ) {
            break;
        } else { /* line too long, expand the buffer */
            *n += bufsz;
            if ( (*s = realloc(*s, (*n)*sizeof(char))) == NULL ) exit(-1);
            p = *s + cnt;
            sz = bufsz;
        }
    }

    return *s;
}

/** Read face definition file.
 *
 * @param[inout] n number of faces read from file.  If the given value
 * *n > 0, *n is interpreted as the number of elements in the faces
 * array and only up to *n faces will be read from file.  However, if
 * the file contains less than *n faces, *n will be updated to reflect
 * the available number of elements.
 *
 * @param[inout] faces array of faces.  If *faces != NULL, *faces will
 * be used rather than allocated.
 */
int read_faces(const char *fname, size_t *n, struct face **faces)
{
    char *linebuf = NULL;
    size_t linen = 0;
    FILE *fp;
    if ((fp = fopen(fname, "r"))==NULL) {
        perror(fname);
        return -1;
    }
    struct face fc;
    int fid;
    size_t lid = 0;
    ssize_t nelem = -1;
    if (*faces == NULL) {
        /* get number of elements in the file */
        while (file_read_long_line(&linebuf, &linen, fp)) {
            lid++;
            if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
            int ret = sscanf(linebuf, "%d %d %lf %lf %lf %lf %lf %lf %lf", &fid, &fc.ftype,
                             &fc.x0, &fc.y0, &fc.z0, &fc.nx, &fc.ny, &fc.nz, &fc.r);
            if (ret < 9 || fid < 0 || fc.ftype <= 0) {
                fprintf(stderr, "Malformatted face at line %zd\n", lid);
            } else {
                if (fid > nelem) nelem = fid;
            }
        }
        nelem++;
        if (nelem == 0) {
            fprintf(stderr, "No valid face in file.\n");
            return -1;
        } else {
            fprintf(stderr, "%zd faces available in file.\n", nelem);
        }
        *n = nelem;
        if ((*faces = calloc(nelem, sizeof(struct face))) == NULL) {
            perror("calloc *faces");
            return -1;
        }
    }
    rewind(fp);
    nelem = 0;
    lid = 0;
    while (file_read_long_line(&linebuf, &linen, fp) && (nelem < *n)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int ret = sscanf(linebuf, "%d %d %lf %lf %lf %lf %lf %lf %lf", &fid, &fc.ftype,
                         &fc.x0, &fc.y0, &fc.z0, &fc.nx, &fc.ny, &fc.nz, &fc.r);
        if (ret < 9 || fid < 0 || fc.ftype <= 0) {
            fprintf(stderr, "Malformatted face at line %zd, skipped.\n", lid);
        } else {
            if (fid >= *n) break;
            struct face *fc1 = *faces;
            memcpy(&fc1[fid], &fc, sizeof(fc));
            nelem++;
        }
    }

    fprintf(stderr, "%zd faces constructed.\n", nelem);
    free(linebuf);
    fclose(fp);
    return 0;
}

int read_points(const char *fname, size_t *n, struct data *data)
{
    char *linebuf = NULL;
    size_t linen = 0;
    FILE *fp;
    if ((fp = fopen(fname, "r"))==NULL) {
        perror(fname);
        return -1;
    }

    int fidmax = -1;
    *n = 0;
    size_t lid = 0;
    while (file_read_long_line(&linebuf, &linen, fp)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int ret = 0;
        for (int i=0; i<linen; i++) { /* count number of ';' */
            if (linebuf[i] == ';') ret++;
        }
        int fid;
        sscanf(linebuf, "%*d;%d", &fid);
        if (ret < 8 || fid < 0) {
            fprintf(stderr, "Malformatted point at line %zd\n", lid);
        } else {
            (*n)++;
            if (fid > fidmax) fidmax = fid;
        }
    }
    fprintf(stderr, "%zd points in file.  fidmax = %d\n", *n, fidmax);
    data->n = *n;
    data->fid = calloc(data->n, sizeof(size_t));
    data->x   = calloc(data->n, sizeof(double));
    data->y   = calloc(data->n, sizeof(double));
    data->z   = calloc(data->n, sizeof(double));
    data->w   = calloc(data->n, sizeof(double));
    data->x1  = calloc(data->n, sizeof(double));
    data->y1  = calloc(data->n, sizeof(double));
    data->z1  = calloc(data->n, sizeof(double));

    rewind(fp);

    lid = 0;
    size_t idx = 0;
    while (file_read_long_line(&linebuf, &linen, fp)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int fid = 0, ret = 0;
        double x, y, z, s;
        // ret = sscanf(linebuf, "%*d;%d;%*s ;%*d;%lf;%lf;%lf;;%lf", &fid, &x, &y, &z, &s);
        char *buf = linebuf, *endptr;
        for (; *buf != ';' ; buf++){;} buf++; /* find the next character past a ';' */
        fid = strtol(buf, &endptr, 0); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        x = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        y = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        z = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        s = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;

        if (ret < 5 || fid < 0) {
            fprintf(stderr, "Malformatted point at line %zd, skipped.\n", lid);
        } else {
            data->fid[idx] = fid;
            data->x[idx] = x;
            data->y[idx] = y;
            data->z[idx] = z;
            data->w[idx] = (fabs(s)>1e-8) ? (1.0/(s*s)) : 1.0;
            idx++;
        }
    }

    fprintf(stderr, "%zd points read.\n", idx);
    free(linebuf);
    fclose(fp);
    return 0;
}

int main(int argc, char **argv)
{
    if (argc != 10) {
        fprintf(stderr,
                "Usage: %s faces_file points_file pr a b g xt yt zt\n\n"
                "    pr is the probe radius.\n"
                "    The last 6 parameters are initial guesses.\n"
                "    Points are first translated, then rotated about x(g), y(b), z(a).\n"
                "    The 2nd column in points_file shall be faceid,\n"
                "        which is the first column of faces_file.\n"
                "    The last column in points_file shall be measurement sigma.\n", argv[0]);
        return EXIT_FAILURE;
    }
    size_t nfaces=0;
    struct face *faces=NULL;
    read_faces(argv[1], &nfaces, &faces);

    size_t nd=0;
    struct data data;
    read_points(argv[2], &nd, &data);
    data.faces = faces;

    data.pr = atof(argv[3]);

    #define np 6
    double p_init[np] = {0}; /* starting values */
    for (int i=0; i<np; i++) {
        p_init[i] = atof(argv[4+i]);
    }

    const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
    gsl_multifit_nlinear_workspace *w;
    gsl_multifit_nlinear_fdf fdf;
    gsl_multifit_nlinear_parameters fdf_params =
        gsl_multifit_nlinear_default_parameters();

    gsl_vector *f;
    gsl_matrix *J;
    gsl_matrix *covar = gsl_matrix_alloc(np, np);

    gsl_vector_view params = gsl_vector_view_array(p_init, np);
    gsl_vector_view wts = gsl_vector_view_array(data.w, nd);
    gsl_rng *r;
    double chisq, chisq0;
    int status, info;

    const double xtol = 1e-8;
    const double gtol = 1e-8;
    const double ftol = 1e-8;

    gsl_rng_env_setup();
    r = gsl_rng_alloc(gsl_rng_default);

    /* define the function to be minimized */
    fdf.f = dist_f;
    fdf.df = NULL;   /* set to NULL for finite-difference Jacobian */
    fdf.fvv = NULL;  /* not using geodesic acceleration */
    fdf.n = nd;
    fdf.p = np;
    fdf.params = &data; /* `data' supplied to the penalty function */

    /* allocate workspace with default parameters */
    w = gsl_multifit_nlinear_alloc(T, &fdf_params, nd, np);

    /* initialize solver with starting point and weights */
    gsl_multifit_nlinear_winit(&params.vector, &wts.vector, &fdf, w);

    /* compute initial cost function */
    f = gsl_multifit_nlinear_residual(w);
    gsl_blas_ddot(f, f, &chisq0);

    /* solve the system with a maximum of 100 iterations */
    status = gsl_multifit_nlinear_driver(100, xtol, gtol, ftol,
                                         callback, NULL, &info, w);

    /* compute covariance of best fit parameters */
    J = gsl_multifit_nlinear_jac(w);
    gsl_multifit_nlinear_covar(J, 0.0, covar);

    /* compute final cost */
    gsl_blas_ddot(f, f, &chisq);

#define FIT(i) gsl_vector_get(w->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))

    fprintf(stderr, "summary from method '%s/%s'\n",
            gsl_multifit_nlinear_name(w),
            gsl_multifit_nlinear_trs_name(w));
    fprintf(stderr, "number of iterations: %zu\n",
            gsl_multifit_nlinear_niter(w));
    fprintf(stderr, "function evaluations: %zu\n", fdf.nevalf);
    fprintf(stderr, "Jacobian evaluations: %zu\n", fdf.nevaldf);
    fprintf(stderr, "reason for stopping: %s\n",
            (info == 1) ? "small step size" : "small gradient");
    fprintf(stderr, "initial |f(x)| = %f\n", sqrt(chisq0));
    fprintf(stderr, "final   |f(x)| = %f\n", sqrt(chisq));

    {
        double dof = nd - np;
        double c = GSL_MAX_DBL(1, sqrt(chisq / dof));

        fprintf(stderr, "chisq/dof = %g\n", chisq / dof);

        fprintf(stderr, "a  = %16g +/- %g\n", FIT(0), c*ERR(0));
        fprintf(stderr, "b  = %16g +/- %g\n", FIT(1), c*ERR(1));
        fprintf(stderr, "g  = %16g +/- %g\n", FIT(2), c*ERR(2));
        fprintf(stderr, "xt = %16g +/- %g\n", FIT(3), c*ERR(3));
        fprintf(stderr, "yt = %16g +/- %g\n", FIT(4), c*ERR(4));
        fprintf(stderr, "zt = %16g +/- %g\n", FIT(5), c*ERR(5));
    }

    fprintf(stderr, "status = %s\n", gsl_strerror(status));

    /* compute residual of every point */
    dist_f(w->x, &data, f); // pure distance, no weights.
    printf("#    fid   ftype  distance          x1          y1          z1\n");
    for (int i=0; i<nd; i++) {
        printf("%8zd %7d %9.6f %11.5f %11.5f %11.5f\n", data.fid[i], faces[data.fid[i]].ftype,
               gsl_vector_get(f, i), data.x1[i], data.y1[i], data.z1[i]);
    }

    gsl_multifit_nlinear_free(w);
    gsl_matrix_free(covar);
    gsl_rng_free(r);

    free(faces);
    free(data.fid);
    free(data.x);
    free(data.y);
    free(data.z);
    free(data.w);
    free(data.x1);
    free(data.y1);
    free(data.z1);
    return EXIT_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////////////
// Stefano Allesina Dec 2009
// sallesina@uchicago.edu
// Given a matrix generated by the niche model and a vector containing
// the positions of the species used to generate the matrix, finds the likelihood.
//////////////////////////////////////////////////////////////////////////////////
// Compile
// gcc NewNicheLikelihood.c -o LNICHE -lgsl -lgslcblas -Wall -DHAVE_INLINE -O3
//////////////////////////////////////////////////////////////////////////////////
// Run
// ./LNICHE 4 Matrix4.txt Vec4.txt
// Where:
// * 4 is the number of species
// * Matrix4.txt is a text file containing a space separated adjacency matrix
//               where the rows are the resources and the cols are the consumers
// * Vec4.txt is a space separated text file containing the positions (in [0,1.0])
//            of the species
//////////////////////////////////////////////////////////////////////////////////
// Output
// NONE
//////////////////////////////////////////////////////////////////////////////////

#include<stdio.h>
#include<math.h>
#include <stdlib.h>
#include<gsl/gsl_math.h>
#include<gsl/gsl_integration.h>
#include<gsl/gsl_vector.h>
#include<gsl/gsl_matrix.h>
#include<gsl/gsl_rng.h>
#include<gsl/gsl_randist.h>
#include<gsl/gsl_cdf.h>
#include<gsl/gsl_blas.h>
#include<gsl/gsl_linalg.h>
#include<gsl/gsl_permutation.h>
#include<gsl/gsl_sort_vector_double.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>

// GLOBAL FILE FOR OUTPUT
FILE * FOUT;

// Structure needed for numerical integration
struct myP { double nlm1; double nl; double nr; double nrp1; double n; double beta;};

// UTILITIES
int PrintAdjMat(gsl_matrix * X, int a){
  int i,j;
  for (i=0;i<a;i++){
      for (j=0;j<a;j++){
	fprintf(FOUT,"%1.0f ",gsl_matrix_get(X,i,j));
      }
      fprintf(FOUT,"\n");
  }
  return 0;
}
int PrintVector(gsl_vector * X, int a){
  int i;
  for (i=0;i<a;i++){
    fprintf(FOUT,"%1.3f ",gsl_vector_get(X,i));
  }
  fprintf(FOUT,"\n");
  return 0;
}
// END UTILITIES

// INTEGRATION FOR CONSUMERS
double Inte(double x, void *p){
  struct myP * params  = (struct myP *)p;
  double nlm1=(params->nlm1);
  double nl=(params->nl);
  double nr=(params->nr);
  double nrp1=(params->nrp1);
  double n=(params->n);
  double beta=(params->beta);
  double res=0;
  res=gsl_ran_beta_pdf(x,1.0,beta);
  //fprintf(stderr,"Pdf %f\n",res);
  // Difference
  double c0,c1;
  double r2=(double )x*n/2.;
  if (x*n<=GSL_MIN_DBL(nr-nlm1,nrp1-nl)){
    // case '
    c0=GSL_MIN_DBL(n,nr-r2);
    c1=GSL_MIN_DBL(n,nl+r2);
  }
  else{
    if (x*n<=GSL_MAX_DBL(nr-nlm1,nrp1-nl)){
      // case ''
      if ((nr-nlm1)<=(nrp1-nl)){
	// a<=b
	c0=GSL_MIN_DBL(n,nlm1+r2);
	c1=GSL_MIN_DBL(n,nl+r2);
      }
      else{
	// b<a
	c0=GSL_MIN_DBL(n,nr-r2);
	c1=GSL_MIN_DBL(n,nrp1-r2);
      }
    }
    else{
      // case '''
      c0=GSL_MIN_DBL(n,nlm1+r2);
      c1=GSL_MIN_DBL(n,nrp1-r2);
    }
  }
  res*=(c1-c0);
  //fprintf(stderr,"Center %f (%f)\n",res,(c1-c0));
  // Normalization
  if ((n+r2)<1.0){
    res/=(n-r2);
  }
  else{
    res/=(1.0-2.0*r2);
  }
  //fprintf(stderr,"Rescale %f\n",res);
  return (res);
}

double Integrate (double nlm1, double nl, double n, double nr, double nrp1, double Beta){
  double error;
  double result;
  double low,upp;
  gsl_function F;
  struct myP params = { 0., 0., 0., 0., 0., 0.};
  // setup the environment
  gsl_integration_workspace * w= gsl_integration_workspace_alloc (10000);
  params.beta=Beta;
  params.nlm1=nlm1;
  params.nl=nl;
  params.n=n;
  params.nr=nr;
  params.nrp1=nrp1;

  F.function = &Inte;
  F.params = &params;
  low=GSL_MIN_DBL((nr-nl)/n,1.0);
  upp=GSL_MIN_DBL((nrp1-nlm1)/n,1.0);
  gsl_integration_qags(&F,low,upp,0,1e-4,10000,w,&result, &error);
  //fprintf(stderr,"Prob %f\n",result);
  if (result==0.0) {
    fprintf(FOUT,"The diet is incompatible with the niche model!!\n");
    return -10000.0; // The diet is impossible!
  }
  return log(result);
}

// INTEGRATION FOR BASAL SPECIES
double InteBasal(double x, void *p){
  struct myP * params  = (struct myP *)p;
  double nlm1=(params->nlm1);
  double nrp1=(params->nrp1);
  double n=(params->n);
  double beta=(params->beta);
  double res=0;
  res=gsl_ran_beta_pdf(x,1.0,beta);
  double r2=(double )x*n/2.;
  double c1=nrp1-r2;
  double c0=nlm1+r2;
  res*=(c1-c0);
  //fprintf(stderr,"Center %f (%f)\n",res,(c1-c0));
  // Normalization
  if ((n+r2)<1.0){
    res/=(n-r2);
  }
  else{
    res/=(1.0-2.0*r2);
  }
  return (res);
}

double IntegrateBasal(double GapLeft, double GapRight, double n, double Beta){
  double error;
  double result;
  double low,upp;
  gsl_function F;
  struct myP params = { 0., 0., 0., 0., 0., 0.};
  // setup the environment
  gsl_integration_workspace * w= gsl_integration_workspace_alloc (10000);
  params.beta=Beta;
  params.nlm1=GapLeft;
  params.nl=0.0;
  params.n=n;
  params.nr=0.0;
  params.nrp1=GapRight;

  F.function = &InteBasal;
  F.params = &params;
  low=GSL_MIN_DBL(0.0,1.0);
  upp=GSL_MIN_DBL((GapRight-GapLeft)/n,1.0);
  gsl_integration_qags(&F,low,upp,0,1e-4,10000,w,&result, &error);
  //fprintf(stderr,"Prob %f\n",result);
  if (result==0.0) {
    fprintf(FOUT,"The diet is incompatible with the niche model!!\n");
    return -10000000.0; // The diet is impossible!
  }
  return (result);
}

// COMPUTE THE LIKELIHOOD OF A MATRIX PERFECTLY COMPATIBLE WITH THE NICHE MODEL
double ComputeLikelihood(gsl_matrix *A, gsl_vector *Pos, int N, double Beta){
  double Prob=0.0;
  int i,j;
  double nlm1, nl, n, nr, nrp1;
  double TmpProb;
  for (i=0;i<N;i++){
    if (gsl_matrix_get(A,i,0)==1){
      fprintf(FOUT,"The first species is not basal!!\n");
      Prob-=100000000.0;
    }
  }
  if (Prob>-1.0) fprintf(FOUT,"First    Sp 0, LogLikelihood Diet 0.0 (1.0)\n");
  for (i=1;i<N;i++){
    n=gsl_vector_get(Pos,i);
    nlm1=nl=0.0;
    nr=nrp1=1.0;
    // Find first prey
    for (j=0;j<N;j++){
      if (gsl_matrix_get(A,j,i)==1){
	nl=gsl_vector_get(Pos,j);
	if (j>0){
	  nlm1=gsl_vector_get(Pos,j-1);
	}
	break;
      }
    }
    if (nl>0.0){
      // i is a predator
      // Find the last prey
      for (j=N-1;j>=0;j--){
	if (gsl_matrix_get(A,j,i)==1){
	  nr=gsl_vector_get(Pos,j);
	  if (j<(N-1)){
	    nrp1=gsl_vector_get(Pos,j+1);
	  }
	  break;
	}
      }
      // Now compute the probability
      TmpProb=0.0;
      //fprintf(stderr,"Computing probability of %d (%f %f %f %f %f) Beta: %f\n", i, nlm1, nl, n, nr, nrp1, Beta);
      TmpProb=Integrate(nlm1, nl, n, nr, nrp1, Beta);
      fprintf(FOUT,"Consumer Sp %d, LogLikelihood Diet %f (%f)\n",i,TmpProb,exp(TmpProb));
      Prob+=TmpProb;
    }
    else{
      TmpProb=0.0;
      // for each species compute the probability that the species is not eating any one in between
      double GapLeft, GapRight;
      for (j=0;j<=i;j++){
	if (j==0){
	  GapLeft=0.0;
	}
	else{
	  GapLeft=gsl_vector_get(Pos,j-1);
	}
	GapRight=gsl_vector_get(Pos,j);
	//fprintf(stderr,"Left: %f Right:%f Cumulative: %f\n",GapLeft,GapRight,TmpProb);
	TmpProb+=IntegrateBasal(GapLeft,GapRight, n,Beta);
      }
      fprintf(FOUT,"Basal    Sp %d, LogLikelihood Diet %f (%f)\n",i,log(TmpProb),TmpProb);
      // now that you have summed across all possibilities take the log and sum it
      Prob+=log(TmpProb);
    }
  }
  return Prob;
}

// READ THE MATRIX AND RUN THE PROBABILITY
// Main
int main(int argc, char *argv[]){
  // First argument: number of species
  int Sp=atoi(argv[1]);
  char * FileMat=(argv[2]);
  char * FilePos=(argv[3]);
  // Read the matrix
  FILE * F;
  // matrix
  gsl_matrix * A =gsl_matrix_calloc(Sp,Sp);
  F=fopen(FileMat,"rb");
  gsl_matrix_fscanf(F,A);
  fclose(F);
  int i,j;
  double C,Beta;
  C=Beta=0.0;
  // compute the C and Beta
  for (i=0;i<Sp;i++){
    for (j=0;j<Sp;j++){
      if(gsl_matrix_get(A,i,j)==1) C+=1.0;
    }
  }
  C=(double)C/(double)Sp/(double)Sp;
  Beta=((double) Sp -1.0)/(2.0*C*(double) Sp) -1.0;
  char OutFile[200];
  sprintf(OutFile,"Output-%s",FileMat);
  if ((FOUT = fopen(OutFile, "w")) == NULL){
    puts("Unable to open the file");
    return 0;
  }
  // read the vector
  gsl_vector * Eta=gsl_vector_calloc(Sp);
  F=fopen(FilePos,"rb");
  gsl_vector_fscanf(F,Eta);
  fclose(F);
  fprintf(FOUT,"Compatible Adjacency Matrix\n");
  PrintAdjMat(A,Sp);
  fprintf(FOUT,"Species Positions\n");
  PrintVector(Eta,Sp);
  fprintf(FOUT,"Beta: %f\n",Beta);
  fprintf(FOUT,"LogLikelihood: %f\n",ComputeLikelihood(A,Eta,Sp,Beta));
  gsl_matrix_free(A);
  gsl_vector_free(Eta);
  fclose(FOUT);
  return 0;
}


//This computes the IDFT (inverse discrete Fourier transformation) along dim of matrix X.
//This uses a CBLAS matrix multiplication by the IDFT matrix.

//The real-valued case is complicated (i.e., where only real part is output).
//This will only exactly invert with DFT if the DFT output all F=ndft/2+1 positive freqs.
//That is, use of this function to invert a specific DFT requires knowledge of how that DFT was run.

//Profile note: I tried splitting the matrix multiply into two halves,
//for positive and negative freqs, so that X would not need to be copied into Xc,
//but it was definitely slower.

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

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

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

int idft_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);

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

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

    //Scaling
    const float s = sc ? 2.0f*sqrtf(0.5f*(float)ndft)/(float)ndft : 1.0f/(float)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Init IDFT matrix
        const size_t NN = ndft * ndft;
        const float P2_N = (float)(2.0*M_PI/(double)ndft);
        float *IDFT;
        IDFT = (float *)aligned_alloc(sizeof(float),2u*NN*sizeof(float));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<ndft; ++l) { *IDFT++ = s; *IDFT++ = 0.0f; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<ndft; ++l)
            {
                *IDFT++ = s * cosf(P2_N*(float)n*(float)l);
                *IDFT++ = s * sinf(P2_N*(float)n*(float)l);
            }
        }
        IDFT -= 2u*NN;

        //Init complex matrix multiply
        const float z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};
        float *Xc, *Yc;
        Xc = (float *)aligned_alloc(sizeof(float),2u*ndft*sizeof(float));
        Yc = (float *)aligned_alloc(sizeof(float),2u*ndft*sizeof(float));
        if (!Xc) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        if (!Yc) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<2u*ndft; ++n, ++Xc) { *Xc = 0.0f; }
        Xc -= 2u*ndft;

        if (Lx==N)
        {
            //Copy into Xc
            for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
            if (ndft%2u==0u) { X -= 2; }
            for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
            Xc -= 2u*ndft;

            //Matrix multiply
            cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

            //Output real part only
            cblas_scopy((int)ndft,Yc,2,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=ndft)
                {
                    //Copy into Xc
                    for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
                    if (ndft%2u==0u) { X -= 2; }
                    for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
                    Xc -= 2u*ndft;
                    X += 2u*(ndft-ndft/2u-ndft%2u);

                    //Matrix multiply
                    cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                    //Output real part only
                    cblas_scopy((int)ndft,Yc,2,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u*K*(ndft-ndft/2u-ndft%2u-Lx)+2u, ++Y)
                    {
                        //Copy into Xc
                        for (size_t nf=Lx; nf>0u; --nf, X+=2u*K) { *Xc++ = *X; *Xc++ = *(X+1); }
                        if (ndft%2u==0u) { X -= 2u*K; }
                        for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2u*K; *Xc++ = *X; *Xc++ = -*(X+1); }
                        Xc -= 2u*ndft;

                        //Matrix multiply
                        cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                        //Output real part only
                        cblas_scopy((int)ndft,Yc,2,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT); free(Xc); free(Yc);
    }

    return 0;
}

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

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

    //Scaling
    const double s = sc ? 2.0*sqrt(0.5*(double)ndft)/(double)ndft : 1.0/(double)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Init IDFT matrix
        const size_t NN = ndft * ndft;
        const double P2_N = 2.0*M_PI/(double)ndft;
        double *IDFT;
        IDFT = (double *)aligned_alloc(sizeof(double),2u*NN*sizeof(double));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<ndft; ++l) { *IDFT++ = s; *IDFT++ = 0.0; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<ndft; ++l)
            {
                *IDFT++ = s * cos(P2_N*(double)n*(double)l);
                *IDFT++ = s * sin(P2_N*(double)n*(double)l);
            }
        }
        IDFT -= 2u*NN;

        //Init complex matrix multiply
        const double z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};
        double *Xc, *Yc;
        Xc = (double *)aligned_alloc(sizeof(double),2u*ndft*sizeof(double));
        Yc = (double *)aligned_alloc(sizeof(double),2u*ndft*sizeof(double));
        if (!Xc) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        if (!Yc) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<2u*ndft; ++n, ++Xc) { *Xc = 0.0; }
        Xc -= 2u*ndft;

        if (Lx==N)
        {
            //Copy into Xc
            for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
            if (ndft%2u==0u) { X -= 2; }
            for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
            Xc -= 2u*ndft;

            //Matrix multiply
            cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

            //Output real part only
            cblas_dcopy((int)ndft,Yc,2,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=ndft)
                {
                    //Copy into Xc
                    for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
                    if (ndft%2u==0u) { X -= 2; }
                    for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
                    Xc -= 2u*ndft;
                    X += 2u*(ndft-ndft/2u-ndft%2u);

                    //Matrix multiply
                    cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                    //Output real part only
                    cblas_dcopy((int)ndft,Yc,2,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u*K*(ndft-ndft/2u-ndft%2u-Lx)+2u, ++Y)
                    {
                        //Copy into Xc
                        for (size_t nf=Lx; nf>0u; --nf, X+=2u*K) { *Xc++ = *X; *Xc++ = *(X+1); }
                        if (ndft%2u==0u) { X -= 2u*K; }
                        for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2u*K; *Xc++ = *X; *Xc++ = -*(X+1); }
                        Xc -= 2u*ndft;

                        //Matrix multiply
                        cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                        //Output real part only
                        cblas_dcopy((int)ndft,Yc,2,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT); free(Xc); free(Yc);
    }

    return 0;
}

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

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

    //Scaling
    const float s = sc ? 2.0f*sqrtf(0.5f*(float)ndft)/(float)ndft : 1.0f/(float)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X * s; }
    }
    else
    {
        //Init IDFT matrix
        const size_t LN = Lx * ndft;
        const float P2_N = (float)(2.0*M_PI/(double)ndft);
        float *IDFT;
        IDFT = (float *)aligned_alloc(sizeof(float),2u*LN*sizeof(float));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_c: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l) { *IDFT++ = s; *IDFT++ = 0.0f; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<Lx; ++l)
            {
                *IDFT++ = s * cosf(P2_N*(float)n*(float)l);
                *IDFT++ = s * sinf(P2_N*(float)n*(float)l);
            }
        }
        IDFT -= 2u*LN;

        //Init complex matrix multiply
        const float z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};

        if (Lx==N)
        {
            //Matrix multiply
            cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=2u*ndft)
                {
                    //Matrix multiply
                    cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2, Y+=2)
                    {
                        //Matrix multiply
                        cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,(int)K,z,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT);
    }

    return 0;
}

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

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

    //Scaling
    const double s = sc ? 2.0*sqrt(0.5*(double)ndft)/(double)ndft : 1.0/(double)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X * s; }
    }
    else
    {
        //Init IDFT matrix
        const size_t LN = Lx * ndft;
        const double P2_N = 2.0*M_PI/(double)ndft;
        double *IDFT;
        IDFT = (double *)aligned_alloc(sizeof(double),2u*LN*sizeof(double));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_z: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l) { *IDFT++ = s; *IDFT++ = 0.0; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<Lx; ++l)
            {
                *IDFT++ = s * cos(P2_N*(double)n*(double)l);
                *IDFT++ = s * sin(P2_N*(double)n*(double)l);
            }
        }
        IDFT -= 2u*LN;

        //Init complex matrix multiply
        const double z[2] = {0.0,0.0}, o[2] = {1.0,0.0};

        if (Lx==N)
        {
            //Matrix multiply
            cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=2u*ndft)
                {
                    //Matrix multiply
                    cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2, Y+=2)
                    {
                        //Matrix multiply
                        cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,(int)K,z,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <stdio.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_math.h>

#include "timer.c"
#include "timer.h"

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

double dipole_approx (double r);

double gaussian (double *x, int dim);

int main (void)
{
    double res, err;

    size_t dim = 6;
    double x1[] = { 0., 0., 0., 0., 0., 0., };
    double xu[] = { 1., 1., 1., 1., 1., 1., };
    double distmin = 1.001;
    double distmax = 4.;
    double dist;
    int np = 20;
    double nt = (distmax - distmin) / (np - 1);
    double vegas[20], dipole[20], distance[20];

    gsl_rng *r = gsl_rng_alloc (gsl_rng_taus2);
    unsigned long seed = 1UL;

    gsl_rng_set (r, seed);

    size_t calls = 1000000;

    dist = distmin;

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

    gsl_monte_vegas_state *sv = gsl_monte_vegas_alloc (dim);

    gsl_monte_vegas_init (sv);

 // Vegas Integration
 /*commented so does not output in order to create a proper res
   printf ("#  Stat      Dist       E(r)        ErrEst      Dipolapprox\n");      */

  timer_start ();

  for (int i = 0; i < np; i++) {
     gsl_monte_vegas_integrate (&G, x1, xu, dim, calls / 5, r, sv, &res, &err);
  do
  {
  gsl_monte_vegas_integrate (&G, x1, xu, dim, calls, r, sv, &res, &err);          }
  while (fabs (gsl_monte_vegas_chisq (sv) - 1.0) > 0.2);

    fflush (stdout);
    dist += nt;
    vegas[i] = res;
    distance[i] = dist;
    dipole[i] = -2. / pow (dist, 3.);
    }

    timer_stop();

    gsl_monte_vegas_free (sv);

    double sum;
    double x[6];

    long i, j, nn;

    nn = 1000000;

    timer_start ();
    double home[20];

    dist = distmin;
    for (j = 0; j < np; j++)
    {
        sum = 0.;
        for (i = 0; i < nn; i++)
        {

        for (int k = 0; k < (int) dim; k++)
            {
                x[k] = gsl_rng_uniform (r);
            }
            sum += g (x, dim, &dist);
        }
        res = sum/nn;
        dist += nt;
        home[j] = res;
    }

     timer_stop ();

     gsl_rng_free (r);

     double homeerr = 0.0;

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

    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_core.h>
#include <gbpCosmo_NFW_etc.h>
#include <gsl/gsl_sf_expint.h>

void set_NFW_params(double M, double z, int mode, cosmo_info **cosmo, double *c_vir, double *R_vir) {
    if(mode != NFW_MODE_DEFAULT)
        SID_exit_error("Unknown mode (%d) in set_NFW_params()", SID_ERROR_LOGIC, mode);

    switch(ADaPS_exist(*cosmo, "M_WDM")) {
        case GBP_FALSE: {
            double Omega_M  = ((double *)ADaPS_fetch(*cosmo, "Omega_M"))[0];
            double h_Hubble = ((double *)ADaPS_fetch(*cosmo, "h_Hubble"))[0];

            // Mass-concentration from Munoz-Cuartas et al 2010
            double w     = 0.029;
            double m     = 0.097;
            double alpha = -110.001;
            double beta  = 2469.720;
            double gamma = 16.885;
            double a_z   = w * z - m;
            double b_z   = alpha / (z + gamma) + beta / pow(z + gamma, 2.);
            double Delta = Delta_vir(z, *cosmo);
            Delta        = 200.;

            (*c_vir) = take_alog10(a_z * take_log10(M / (M_SOL / h_Hubble)) + b_z);
            (*R_vir) = R_Delta_z(M, Delta, z, *cosmo); // Bullock et al '01
        } break;
        case GBP_TRUE:
            SID_exit_error("ENS not working.", SID_ERROR_LOGIC);
            //(*c_vir)=c_ENS(M,z,*cosmo);          // Eke, Navarro and Steinmetz
            //(*R_vir)=R_Delta_z(M,200.,z,*cosmo); // R_200
            break;
    }
}


#ifndef SIGNAL_H_
#define SIGNAL_H_

#include <gsl/gsl_statistics_double.h>
#include <complex>
#include <fftw3.h>
#include <math.h>
#include <iostream>
#include "fourier_transform.h"
#include "array.h"
#include "rng.h"
#include <assert.h>

// this is slowly turning into sth that should maybe rather be called
// "stochastic processes" rather than signal

namespace neurophys {

class SpectrumDescriptor
{
public:
    virtual double get(const double f) const = 0;
};

class WhiteSpectrum: public SpectrumDescriptor
{
public:
    WhiteSpectrum(const double s0): s0_(s0) {}
    virtual double get(const double f) const { return s0_; }
private:
    const double s0_;
};

class BandLimitedFlatSpectrum: public SpectrumDescriptor
{
public:
    BandLimitedFlatSpectrum(const double s0, const double f0, const double f1):
        s0_(s0), f0_(f0), f1_(f1)
        {}
    virtual double get(const double f) const
    {
        return (f >= f0_ && f < f1_) ? s0_ : 0.;
    }
private:
    const double s0_;
    const double f0_;
    const double f1_;
};

class PowerLawSpectrum: public SpectrumDescriptor
{
public:
    PowerLawSpectrum(const double expo, const double f0, const double f1):
        expo_(expo), f0_(f0), f1_(f1), A_(0)
    {
        // normalization, so that variance = 1
        A_ = 1./2 * (1.-expo) / (pow(f1,1.-expo) - expo*pow(f0,1.-expo));
    }
    virtual double get(const double f) const
    {
        if (f < f0_) return A_ * pow(f0_, -expo_);
        else if (f < f1_) return A_ * pow(f, -expo_);
        else return 0.;
    }
private:
    const double expo_;
    const double f0_;
    const double f1_;
    double A_;
};

namespace signal {

Array<double> generate_gaussian(RNG& rng,
        const C2RFourierTransform& c2rft, const SpectrumDescriptor& spec,
        const size_t N, const double T);

};

}

#endif /* SIGNAL_H */


#include <math.h>
#include <gmp.h>
#if BLAS_IMPL == openblas
#include <blas/cblas.h>
#else
#include <cblas.h>
#endif
#include "attrib.h"
#include "math.h"

#define CLEANUP \
    return err; \
    error: \
    if (ctx->err == 0) \
    	strcpy(ctx->err_msg, "Unexpected math."); \
    ctx->err = err; \
    return err;

#define CHECK(expr) \
    if ((err = (expr))) goto error;

VIO_CONST
uint32_t vmaxui(uint32_t a, uint32_t b) {
    return a > b ? a : b;
}

#define OP_IMPL(name) \
    vio_err_t generic_##name(vio_ctx *ctx, vio_val *a, vio_val *b) { \
        vio_err_t err = 0; \
        vio_val *va, *vb; \
        if (a->what == vv_str || b->what == vv_str || a->what == vv_quot || b->what == vv_quot || \
            a->what == vv_tagword || b->what == vv_tagword) { \
            err = vio_raise(ctx, VE_WRONG_TYPE, \
                            "Function '" #name "' expects numeric types or vectors; received '%s' and '%s' operands.", \
                            vio_val_type_name(a->what), vio_val_type_name(b->what));\
            goto error; \
        }

#define END_OP(_name) \
    CLEANUP \
}

#define GIVEN_VECF \
    if (a->what == vv_vecf && b->what == vv_vecf && a->vlen == b->vlen) {

#define DONE_VECF }

#define GIVEN_NUMERIC \
    else if (vio_is_numeric(a) && vio_is_numeric(b)) { \
        CHECK(vio_coerce(ctx, b, &vb, a->what)) \
        if (vb == NULL) { \
            CHECK(vio_coerce(ctx, a, &va, b->what)) \
            if (va == NULL) { \
                err = VE_NUMERIC_CONVERSION_FAIL; \
                goto error; \
            } \
            a = b; \
            b = va; \
        } \
        else \
            b = vb; \
        switch (a->what) {

#define DONE_NUMERIC \
        default: err = VE_WRONG_TYPE; goto error; \
        } \
    }

#define STANDARD_NUMERIC(op, fop) \
    GIVEN_NUMERIC \
        case vv_int: CHECK(vio_push_int(ctx, b->i32 op a->i32)) break; \
        case vv_float: CHECK(vio_push_float(ctx, b->f32 op a->f32)) break; \
        case vv_num: { mpf_t c; mpf_init(c); mpf_##fop(c, b->n, a->n); CHECK(vio_push_num(ctx, c)) break; } \
    DONE_NUMERIC

#define GIVEN_VEC \
    else CHECK(vio_coerce(ctx, a, &va, vv_vec)) \
    else CHECK(vio_coerce(ctx, b, &vb, vv_vec)) \
    else if (va && vb) { \

#define DONE_VEC }

#define AUTO_VECTORIZE(name) \
    GIVEN_VEC \
        uint32_t maxl = vmaxui(va->vlen, vb->vlen); \
        for (uint32_t i = 0; i < maxl; ++i) \
            CHECK(generic_##name(ctx, va->vv[i % va->vlen], vb->vv[i % vb->vlen])) \
        CHECK(vio_push_vec(ctx, maxl)) \
    DONE_VEC

OP_IMPL(add)
    GIVEN_VECF
        cblas_daxpy(b->vlen, 1, a->vf32, 1, b->vf32, 1);
        CHECK(vio_push_vecf32(ctx, b->vlen, b->vf32))
    DONE_VECF
    STANDARD_NUMERIC(+, add)
    AUTO_VECTORIZE(add)
END_OP(add)

OP_IMPL(sub)
    GIVEN_VECF
        cblas_daxpy(b->vlen, -1.0, a->vf32, 1, b->vf32, 1);
        CHECK(vio_push_vecf32(ctx, b->vlen, b->vf32))
    DONE_VECF
    STANDARD_NUMERIC(-, sub)
    AUTO_VECTORIZE(sub)
END_OP(sub)

OP_IMPL(mul)
    if (b->what == vv_vecf && a->what == vv_float) {
        va = a;
        a = b;
        b = va;
    }
    if (a->what == vv_vecf && b->what == vv_float) {
        cblas_dscal(a->vlen, b->f32, a->vf32, 1);
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    }
    else GIVEN_VECF
        for (uint32_t i = 0; i < a->vlen; ++i)
            a->vf32[i] *= b->vf32[i];
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    DONE_VECF
    STANDARD_NUMERIC(*, mul)
    AUTO_VECTORIZE(mul)
END_OP(mul)

OP_IMPL(div)
    if (b->what == vv_vecf && a->what == vv_float) {
        va = a;
        a = b;
        b = va;
    }
    if (a->what == vv_vecf && b->what == vv_float) {
        cblas_dscal(a->vlen, 1.0 / b->f32, a->vf32, 1);
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    }
    else GIVEN_VECF
        for (uint32_t i = 0; i < a->vlen; ++i)
            a->vf32[i] /= b->vf32[i];
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    DONE_VECF
    STANDARD_NUMERIC(/, div)
    AUTO_VECTORIZE(div)
END_OP(div)

#define BIN_OP(name) \
    vio_err_t vio_##name(vio_ctx *ctx) { \
        vio_err_t err = 0; \
        VIO__ENSURE_ATLEAST(2) \
\
        vio_val *a = ctx->stack[--ctx->sp], *b; \
        b = ctx->stack[--ctx->sp]; \
        err = generic_##name(ctx, a, b); \
\
        CLEANUP \
    }

BIN_OP(add)
BIN_OP(sub)
BIN_OP(mul)
BIN_OP(div)

#define UN_IMPL(f) \
    vio_err_t vio_##f(vio_ctx *ctx) { \
        vio_err_t err = 0; \
        vio_float x, y; \
        VIO__CHECK(vio_pop_float(ctx, &x)); \
        y = f(x); \
        VIO__CHECK(vio_push_float(ctx, y)); \
        return 0; \
        error: \
        return err; \
    }

LIST_MATH_UNARY(UN_IMPL)