/*
  NAME:
     proj_gauss_mixtures
  PURPOSE:
     runs the full projected gaussian mixtures algorithms, with
     regularization and split and merge
  CALLING SEQUENCE:
     proj_gauss_mixtures(struct datapoint * data, int N,
     struct gaussian * gaussians, int K,bool * fixamp, bool * fixmean,
     bool * fixcovar, double * avgloglikedata, double tol,
     long long int maxiter, bool likeonly, double w, int splitnmerge,
     bool keeplog, FILE *logfile, FILE *convlogfile, bool noproj,
     bool diagerrs,noweight)
  INPUT:
     data        - the data
     N           - number of datapoints
     gaussians   - model gaussians (initial conditions)
     K           - number of model gaussians
     fixamp      - fix the amplitude?
     fixmean     - fix the mean?
     fixcovar    - fix the covar?
     tol         - proj_EM convergence limit
     maxiter     - maximum number of iterations in each proj_EM
     likeonly    - only compute the likelihood?
     w           - regularization paramter
     splitnmerge - split 'n' merge depth (how far down the list to go)
     keeplog     - keep a log in a logfile?
     logfile     - pointer to the logfile
     convlogfile - pointer to the convlogfile
     noproj      - don't perform any projections
     diagerrs    - the data->SS errors-squared are diagonal
     noweight    - don't use data-weights
  OUTPUT:
     updated model gaussians
     avgloglikedata - average log likelihood of the data
  REVISION HISTORY:
     2008-08-21 Written Bovy
     2010-03-01 Added noproj option - Bovy
     2010-04-01 Added noweight option - Bovy
*/
#ifdef _OPENMP
#include <omp.h>
#endif
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include "proj_gauss_mixtures.h"

void proj_gauss_mixtures(struct datapoint * data, int N,
			 struct gaussian * gaussians, int K,
			 bool * fixamp, bool * fixmean, bool * fixcovar,
			 double * avgloglikedata, double tol,
			 long long int maxiter, bool likeonly, double w,
			 int splitnmerge, bool keeplog, FILE *logfile,
			 FILE *convlogfile, bool noproj, bool diagerrs,
			 bool noweight){
  //Allocate some memory
  struct gaussian * startgaussians;
  startgaussians = gaussians;
  int d = (gaussians->VV)->size1;//dim of mm
  //Only give copies of the fix* vectors to the EM algorithm
  bool *fixamp_tmp, *fixmean_tmp,*fixcovar_tmp;
  fixamp_tmp = (bool *) malloc(K * sizeof (bool) );
  fixmean_tmp = (bool *) malloc(K * sizeof (bool) );
  fixcovar_tmp = (bool *) malloc(K * sizeof (bool) );
  int kk;
  for (kk = 0; kk != K; ++kk){
    *(fixamp_tmp++) = *(fixamp++);
    *(fixmean_tmp++) = *(fixmean++);
    *(fixcovar_tmp++) = *(fixcovar++);
  }
  fixamp -= K;
  fixamp_tmp -= K;
  fixmean -= K;
  fixmean_tmp -= K;
  fixcovar -= K;
  fixcovar_tmp -= K;
  //allocate the newalpha, newmm and newVV matrices
#ifdef _OPENMP
  nthreads = omp_get_max_threads();
#else
  nthreads = 1;
#endif
  newgaussians = (struct gaussian *) malloc(K * nthreads * sizeof (struct gaussian) );
  startnewgaussians = newgaussians;
  int ll;
  for (kk=0; kk != K*nthreads; ++kk){
    newgaussians->alpha = 0.0;
    newgaussians->mm = gsl_vector_calloc (d);
    newgaussians->VV = gsl_matrix_calloc (d,d);
    ++newgaussians;
  }
  newgaussians= startnewgaussians;
  double oldavgloglikedata;
  //allocate the q_ij matrix
  qij = gsl_matrix_alloc(N,K);
  double * qstarij = (double *) malloc(N * sizeof (double) );
  I = gsl_matrix_alloc(d,d);
  gsl_matrix_set_identity(I);//Unit matrix
  gsl_matrix_scale(I,w);//scaled to w
  //Also take care of the bbij's and the BBij's
  bs = (struct modelbs *) malloc(nthreads * K * sizeof (struct modelbs) );
  for (kk = 0; kk != nthreads*K; ++kk){
    bs->bbij = gsl_vector_alloc (d);
    bs->BBij = gsl_matrix_alloc (d,d);
    ++bs;
  }
  bs -= nthreads*K;
  //splitnmerge
  int maxsnm = K*(K-1)*(K-2)/2;
  int * snmhierarchy = (int *) malloc(maxsnm*3* sizeof (int) );
  int j,k,l;
  struct gaussian * oldgaussians = (struct gaussian *) malloc(K * sizeof (struct gaussian) );
  gsl_matrix * oldqij = gsl_matrix_alloc(N,K);
  for (kk=0; kk != K; ++kk){
    oldgaussians->mm = gsl_vector_calloc (d);
    oldgaussians->VV = gsl_matrix_calloc (d,d);
    ++oldgaussians;
  }
  oldgaussians -= K;

  //create temporary file to hold convergence info
  FILE *tmpconvfile;
  char * tmpfilename= tmpnam(NULL);;
  if (keeplog)
    tmpconvfile= fopen(tmpfilename,"w+");

  //proj_EM
  if (keeplog)
    fprintf(logfile,"#Initial proj_EM\n");
  //printf("Where's the segmentation fault?\n");
  proj_EM(data,N,gaussians,K,fixamp_tmp,fixmean_tmp,fixcovar_tmp,
	  avgloglikedata,tol,maxiter,likeonly,w,
	  keeplog,logfile,convlogfile,noproj,diagerrs,noweight);
  if (keeplog){
    fprintf(logfile,"\n");
    fprintf(convlogfile,"\n");
  }
  //reset fix* vectors
  for (kk = 0; kk != K; ++kk){
    *(fixamp_tmp++) = *(fixamp++);
    *(fixmean_tmp++) = *(fixmean++);
    *(fixcovar_tmp++) = *(fixcovar++);
  }
  fixamp -= K;
  fixamp_tmp -= K;
  fixmean -= K;
  fixmean_tmp -= K;
  fixcovar -= K;
  fixcovar_tmp -= K;

  //Run splitnmerge
  randgen = gsl_rng_alloc(gsl_rng_mt19937);
  bool weretrying = true;
  if (likeonly || splitnmerge == 0 || K < 3)
    ;
  else {
    while (weretrying){
      weretrying = false; /* this is set back to true if an improvement is found */
      //store avgloglike from normal EM and model parameters
      oldavgloglikedata = *avgloglikedata;
      gsl_matrix_memcpy(oldqij,qij);
      for (kk=0; kk != K; ++kk){
	oldgaussians->alpha = gaussians->alpha;
	gsl_vector_memcpy(oldgaussians->mm,gaussians->mm);
	gsl_matrix_memcpy(oldgaussians->VV,gaussians->VV);
	++oldgaussians;
	++gaussians;
      }
      gaussians -= K;
      oldgaussians -= K;
      //Then calculate the splitnmerge hierarchy
      calc_splitnmerge(data,N,gaussians,K,qij,snmhierarchy);
      //Then go through this hierarchy
      kk=0;
      while (kk != splitnmerge && kk != maxsnm){
	//splitnmerge
	j = *(snmhierarchy++);
	k = *(snmhierarchy++);
	l = *(snmhierarchy++);
	splitnmergegauss(gaussians,K,oldqij,j,k,l);
	//partial EM
	//Prepare fixed vectors for partial EM
	for (ll = 0; ll != K; ++ll){
	  *(fixamp_tmp++) = (ll != j && ll != k && ll != l);
	  *(fixmean_tmp++) = (ll != j && ll != k && ll != l);
	  *(fixcovar_tmp++) = (ll != j && ll != k && ll != l);
	}
	fixamp_tmp -= K;
	fixmean_tmp -= K;
	fixcovar_tmp -= K;
	if (keeplog)
	  fprintf(logfile,"#Merging %i and %i, splitting %i\n",j,k,l);
	proj_EM(data,N,gaussians,K,fixamp_tmp,fixmean_tmp,fixcovar_tmp,
		avgloglikedata,tol,maxiter,likeonly,w,keeplog,logfile,
		tmpconvfile,noproj,diagerrs,noweight);
	//reset fix* vectors
	for (ll = 0; ll != K; ++ll){
	    *(fixamp_tmp++) = *(fixamp++);
	    *(fixmean_tmp++) = *(fixmean++);
	    *(fixcovar_tmp++) = *(fixcovar++);
	}
	fixamp -= K;
	fixamp_tmp -= K;
	fixmean -= K;
	fixmean_tmp -= K;
	fixcovar -= K;
	fixcovar_tmp -= K;
	//Full EM
	if (keeplog){
	  fprintf(logfile,"#full EM:\n");
	  fprintf(tmpconvfile,"\n");
	}
	proj_EM(data,N,gaussians,K,fixamp_tmp,fixmean_tmp,fixcovar_tmp,
		avgloglikedata,tol,maxiter,likeonly,w,keeplog,logfile,
		tmpconvfile,noproj,diagerrs,noweight);
	if (keeplog){
	  fprintf(logfile,"\n");
	  fprintf(tmpconvfile,"\n");
	}
	//reset fix* vectors
	for (ll = 0; ll != K; ++ll){
	    *(fixamp_tmp++) = *(fixamp++);
	    *(fixmean_tmp++) = *(fixmean++);
	    *(fixcovar_tmp++) = *(fixcovar++);
	  }
	  fixamp -= K;
	  fixamp_tmp -= K;
	  fixmean -= K;
	  fixmean_tmp -= K;
	  fixcovar -= K;
	  fixcovar_tmp -= K;
	//Better?
	if (*avgloglikedata > oldavgloglikedata){
	  if (keeplog){
	    fprintf(logfile,"#accepted\n");
	    //Copy tmpfile into convfile
	    fseek(tmpconvfile,0,SEEK_SET);
	    while (feof(tmpconvfile) == 0)
	      fputc(fgetc(tmpconvfile),convlogfile);
	    fseek(convlogfile,-1,SEEK_CUR);
	    fclose(tmpconvfile);
	    tmpconvfile= fopen(tmpfilename,"w+");
	  }
	  weretrying = true;
	  ++kk;
	  break;
	}
	else {
	  if (keeplog){
	    fprintf(logfile,"#didn't improve likelihood\n");
	    fclose(tmpconvfile);
	    tmpconvfile= fopen(tmpfilename,"w+");
	  }
	  //revert back to the older solution
	  *avgloglikedata = oldavgloglikedata;
	  /*  gsl_matrix_memcpy(qij,oldqij);*/
	  for (ll=0; ll != K; ++ll){
	    gaussians->alpha = oldgaussians->alpha;
	    gsl_vector_memcpy(gaussians->mm,oldgaussians->mm);
	    gsl_matrix_memcpy(gaussians->VV,oldgaussians->VV);
	    ++oldgaussians;
	    ++gaussians;
	  }
	  gaussians -= K;
	  oldgaussians -= K;
	}
	++kk;
      }
      snmhierarchy -= 3*kk;
    }
  }

  if (keeplog){
    fclose(tmpconvfile);
    fprintf(convlogfile,"\n");
  }

  //Compute some criteria to set the number of Gaussians and print these to the logfile
  int ii;
  int npc,np;
  double pc,aic,mdl;
  if (keeplog){
    //Partition coefficient
    pc=0.;
    for (ii=0; ii != N; ++ii)
      for (kk=0; kk != K; ++kk)
	pc += pow(exp(gsl_matrix_get(qij,ii,kk)),2);
    pc /= N;
    fprintf(logfile,"Partition coefficient \t=\t%f\n",pc);
    //Akaike's information criterion
    npc = 1 + d + d * (d - 1) / 2;
    np = K * npc;
    aic= -2 * ((double) N - 1. - npc - 100)* *avgloglikedata + 3 * np;
    fprintf(logfile,"AIC \t\t\t=\t%f\n",aic);
    //MDL
    mdl = - *avgloglikedata * N + 0.5 * np * log(N);
    fprintf(logfile,"MDL \t\t\t=\t%f\n",mdl);
  }

  //Free memory
  gsl_matrix_free(I);
  gsl_matrix_free(qij);
  for (kk = 0; kk != nthreads*K; ++kk){
    gsl_vector_free(bs->bbij);
    gsl_matrix_free(bs->BBij);
    ++bs;
  }
  bs -= nthreads*K;
  free(bs);
  for (kk=0; kk != K*nthreads; ++kk){
    gsl_vector_free(newgaussians->mm);
    gsl_matrix_free(newgaussians->VV);
    ++newgaussians;
  }
  newgaussians= startnewgaussians;
  free(newgaussians);
  gsl_rng_free(randgen);

  gsl_matrix_free(oldqij);
  for (kk=0; kk != K; ++kk){
    gsl_vector_free(oldgaussians->mm);
    gsl_matrix_free(oldgaussians->VV);
    ++oldgaussians;
  }
  oldgaussians -= K;
  free(oldgaussians);
  free(qstarij);
  free(snmhierarchy);
  free(fixamp_tmp);
  free(fixmean_tmp);
  free(fixcovar_tmp);

  return;
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

int generate_MCMC_chain(MCMC_info *MCMC) {
    int              flag_success;
    int              i_P, i_DS;
    static char      format_string[256];
    FILE *           fp_report_props;
    static char      filename_report_props[SID_MAX_FILENAME_LENGTH];
    static int       n_P;
    static double *  P_new;
    static double ** M_new;
    static double *  P_last;
    static double *  P_chain;
    static double ** M_last;
    static double *  P_best;
    static double *  P_limit_min;
    static double *  P_limit_max;
    static RNG_info *RNG;
    static int       flag_report_props;
    static int       n_DS;
    static int *     n_M;

    // Initialize a few things on the first call
    switch(MCMC->first_chain_call) {
        case GBP_TRUE:
            n_P                    = MCMC->n_P;
            P_new                  = MCMC->P_new;
            M_new                  = MCMC->M_new;
            P_last                 = MCMC->P_last;
            M_last                 = MCMC->M_last;
            P_chain                = MCMC->P_chain;
            P_best                 = MCMC->P_best;
            P_limit_min            = MCMC->P_limit_min;
            P_limit_max            = MCMC->P_limit_max;
            RNG                    = MCMC->RNG;
            n_DS                   = MCMC->n_DS;
            n_M                    = MCMC->n_M;
            flag_report_props      = SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_REPORT_PROPS);
            MCMC->first_chain_call = GBP_FALSE;
            sprintf(filename_report_props, "%s/chains/report_props_%06d.dat", MCMC->filename_output_dir, MCMC->my_chain);
            break;
    }

    // We need to compute the likelihood of the initial conditions
    //   if we are just starting a chain
    if(MCMC->flag_init_chain)
        generate_MCMC_proposition(MCMC, GBP_TRUE);

    // Set the last new state to the new last state
    MCMC->ln_likelihood_last = MCMC->ln_likelihood_new;
    MCMC->ln_Pr_last         = MCMC->ln_Pr_new;
    memcpy(P_last, P_new, n_P * sizeof(double));
    for(i_DS = 0; i_DS < n_DS; i_DS++)
        memcpy(M_last[i_DS], M_new[i_DS], n_M[i_DS] * sizeof(double));

    // Keep generating parameter sets until the mapping function is satisfied
    generate_MCMC_proposition(MCMC, GBP_FALSE);

    // Decide if this is a successful proposition or not...
    if(MCMC->ln_likelihood_new > MCMC->ln_likelihood_chain) {
        MCMC->ln_Pr_new = 0.;
        flag_success    = GBP_TRUE;
    } else if(MCMC->my_chain == SID.My_rank) {
        MCMC->ln_Pr_new = MCMC->ln_likelihood_new - MCMC->ln_likelihood_chain;
        if((double)random_number(RNG) <= exp(MCMC->ln_Pr_new))
            flag_success = GBP_TRUE;
        else
            flag_success = GBP_FALSE;
    }
    // fprintf(stderr,"%d %le %d\n",flag_success,MCMC->ln_Pr_new,SID_CHECK_BITFIELD_SWITCH(MCMC->mode,MCMC_MODE_PARALLEL));
    // if(!SID_CHECK_BITFIELD_SWITCH(MCMC->mode,MCMC_MODE_PARALLEL))
    SID_Bcast(&flag_success, 1, SID_INT, SID_MASTER_RANK, MCMC->comm);

    // ... if it is, then update the chain ...
    if(flag_success) {
        memcpy(P_chain, P_new, (size_t)n_P * sizeof(double));
        MCMC->ln_likelihood_chain = MCMC->ln_likelihood_new;
        MCMC->ln_Pr_chain         = MCMC->ln_Pr_new;
        MCMC->n_success++;
    } else
        MCMC->n_fail++;
    MCMC->n_propositions++;

    // Report the proposition if asked to
    if(flag_report_props && MCMC->my_chain == SID.My_rank) {
        fp_report_props = fopen(filename_report_props, "w");
        if(flag_success)
            fprintf(fp_report_props, "Proposal #%09d: SUCCEEDED\n\n", MCMC->n_propositions);
        else
            fprintf(fp_report_props, "Proposal #%09d: REJECTED\n\n", MCMC->n_propositions);
        sprintf(format_string, "   %s = %%13.6le (was %%13.6le) (best is %%13.6le) (flat prior =%%13.6le -> %%13.6le)\n", MCMC->P_name_format);
        for(i_P = 0; i_P < n_P; i_P++)
            fprintf(fp_report_props, format_string, MCMC->P_names[i_P], P_new[i_P], P_last[i_P], P_best[i_P], P_limit_min[i_P], P_limit_max[i_P]);
        fprintf(fp_report_props, "\n");
        fprintf(fp_report_props,
                "ln(likelihood) =%13.6le+constant (was %13.6le+constant) (best is %13.6le+constant)\n",
                MCMC->ln_likelihood_new,
                MCMC->ln_likelihood_last,
                MCMC->ln_likelihood_best);
        fprintf(fp_report_props, "ln(probability)=%13.6le\n", MCMC->ln_Pr_new);
        fprintf(fp_report_props, "n_success      =%09d\n", MCMC->n_success);
        fprintf(fp_report_props, "n_fail         =%09d\n", MCMC->n_fail);
        fprintf(fp_report_props, "success rate   =%.2f %%\n", (double)MCMC->n_success / (double)(MCMC->n_fail + MCMC->n_success) * 100.0);
        fclose(fp_report_props);
    }

    return (flag_success);
}


//=========================================
// Main driver for the proper time integral
//=========================================

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <builtin_types.h>
#include "intT.h"

double* transformT(double t, void* p, int* order)
//Transforms the integral [0,infinity) to [0,1]
//x=(1-t)/t
{
	struct Wparams *params = (struct Wparams *) p;
	double* tfunc;
	double x = (1.0-t)/t;
	int i;

	tfunc = Tfunc(x,p, order);
	//loop over groups of worldlines
	for(i = 0; i < params->ng; i++)
	{
		tfunc[i] = (tfunc[i]/t)/t;

	}
	return tfunc;

}

void testval(double *val, int n, double t)
//used for outputting debugging information
{
  int i;
  double avg = 0.0;
  for(i = 0; i < n; i++)
  {
	if(isnan(val[i])) printf("for t=%f val[%d]=nan\n",t, i);
	avg += val[i];
  }
  avg /= n;

  printf("mean for Tt=%f, %f\n",t,avg);
}

void Tintout(double *val, int n, double rho, int fermion)
//Produces output about the T-integral
{
	int i;
	double avg = 0.0, SEM = 0.0;
  	for(i = 0; i < n; i++)
  	{
		if(isnan(val[i])) printf("for rho=%f val[%d]=nan\n",rho, i);
		avg += val[i];
  	}
  	avg /= n;
	for(i = 0; i < n; i++)
 	{
		SEM += (val[i]-avg)*(val[i]-avg);
  	}
  	SEM = sqrt(SEM/((double)n*((double)n-1.0)));
  	//rho Leff SEM-Leff

	if(fermion == 1)
  		printf("Leffvsrho: %e %e %e\n", rho, avg/(4.0*pi), SEM/(4.0*pi));
	else
  		printf("Leffvsrho: %e %e %e\n", rho, avg/(-2.0*pi), SEM/(2.0*pi));
}

int* shuffleWL(int Nl)
//Returns a vector representing a shuffled order of worldlines
{
  gsl_rng * r;
  int i;

  // select random number generator
  r = gsl_rng_alloc (gsl_rng_mt19937);

  int* order=(int*)malloc(Nl*sizeof(*order));

  for (i = 0; i < Nl; i++)
  {
  	order[i] = i;
  }

  //gsl_ran_shuffle (r, order, Nl, sizeof (int));
  free(r);

  return order;
}

double* integrateT(int n, void * p)
//Performs the proper time integral using Simpson's Method
//n = number of points to use for Simpson's method
//p = Wparams parameters for the integrand
{
        double* val;    //value of the integral for each group
	double* trt;    //Integrands evaluated at three points
	double* trtphalf;
	double* trtp1;
	int* order;     //Stores the order of worldlines
	struct Wparams *params = (struct Wparams *) p;
	int inx, ig, i;
	val = (double *)calloc(params->ng,sizeof(*val));
	double h = 1.0/((double) n);   //
	double t;
	if(!val) printf("integrate(): malloc failed\n");
	//initialize val to zero
	for(ig = 0; ig < params->ng; ig++)
	{
		val[ig] = 0.0;
	}
	//main integration loop
	for(inx = 0; inx < n; inx++)
	{
		printf("T integrate inx=%d\n",inx);
		//shuffle worldlines
		order=shuffleWL(params->Nl);
		t=h*((double)inx+1.0);
		trt=transformT(t,p,order);
		trtphalf=transformT(h*((double)inx + 0.5), p, order);
		trtp1=transformT(h*((double) inx+1.0), p, order);

		//A kludge to ignore possible infinities or nans
		for(ig = 0; ig < params->ng; ig++)
		{
			if(isinf(trt[ig]))
			{
				printf("Warning: trt[%d]=%f.  Setting to zero.\n",ig,trt[ig]);
				trt[ig]=0.0;
			}
			if(isinf(trtphalf[ig]))
			{
				printf("Warning: trtphalf[%d]=%f.  Setting to zero.\n",ig,trtphalf[ig]);
				trtphalf[ig]=0.0;
			}
			if(isinf(trtp1[ig]))
			{
				printf("Warning: trtp1[%d]=%f.  Setting to zero.\n",ig,trtp1[ig]);
				trtp1[ig]=0.0;
			}

        		val[ig]+=h/6.0*(
				trt[ig]
				+ 4.0*trtphalf[ig]
				+trtp1[ig]);
			if(isinf(val[ig])){
				printf("trt[%d]=%e\n",ig, trt[ig]);
				printf("trtphalf[%d]=%e\n",ig,trtphalf[ig]);
				printf("trtp1[%d]=%e\n",ig,trtp1[ig]);
				printf("val[%d]=%e\n",ig,val[ig]);

			}
			//printf("val[%d]=%e\n",ig,val[ig]);
		}
		testval(val, params->ng, t);
		free(order);

	}
	Tintout(val, params->ng, params->xcm.x, params->fermion);
	free(trt);
	free(trtphalf);
	free(trtp1);

        return val;

}


// -*- C++ -*-
//
// michael a.g. aïvázis
// orthologue
// (c) 1998-2021 all rights reserved
//

// get the headers
#include <cblas.h>

// smallest possible driver
int main() {
    // allocate the scala
    double alpha = 1;
    // allocate a vector
    double x[] = {0.0, 0.0, 0.0};
    double y[] = {0.0, 0.0, 0.0};

    // do it
    cblas_daxpy(3, alpha, x, 1, y, 1);

    // all done
    return 0;
}

// end of file


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics_double.h>
#include <math.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_voxel_linked_list.h"
#include "tz_stack_sampling.h"
#include "tz_voxel_graphics.h"
#include "tz_neurotrace.h"
#include "tz_stack_math.h"
#include "tz_stack_utils.h"
#include "tz_objdetect.h"
#include "tz_voxeltrans.h"
#include "tz_stack_stat.h"
#include "tz_stack_draw.h"
#include "tz_geo3d_vector.h"
#include "tz_stack_bwmorph.h"
#include "tz_stack_bwdist.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[])
{
#if 1
  Stack *stack = Read_Stack("../data/lobster_neuron_seed.tif");

  Voxel_List *list = Stack_To_Voxel_List(stack);

  //Print_Voxel_List(list);

  Pixel_Array *pa = Pixel_Array_Read("../data/lobster_neuron_seeds.pa");
  //Print_Pixel_Array(pa);

  Voxel *seed;

  int i;
  double *pa_array = (double *) pa->array;

  printf("mean: %g, std: %g\n", gsl_stats_mean(pa_array, 1, pa->size),
	 sqrt(gsl_stats_variance(pa_array, 1, pa->size)));

  double threshold = gsl_stats_mean(pa_array, 1, pa->size) +
    3.0 * sqrt(gsl_stats_variance(pa_array, 1, pa->size));

  double max_r = gsl_stats_max(pa_array, 1, pa->size);

  printf("%g, %g\n", threshold, max_r);

  max_r = 30.0;
  Set_Neuroseg_Max_Radius(max_r);

  dim_type dim[3];
  dim[0] = stack->width;
  dim[1] = stack->height;
  dim[2] = stack->depth;

  IMatrix *chord = Make_IMatrix(dim, 3);

  Stack *code = Make_Stack(GREY16, stack->width, stack->height, stack->depth);

  Kill_Stack(stack);

  stack = Read_Stack("../data/lobster_neuron_single.tif");

  Rgb_Color color;
  Set_Color(&color, 255, 0, 0);

  Stack *signal = Read_Stack("../data/lobster_neuron.tif");

  Stack *canvas = Translate_Stack(signal, COLOR, 0);

  /************** soma detection *************/
#if 0
  Struct_Element *se = Make_Ball_Se(((int) threshold));
  Stack *stack1 = Stack_Erode_Fast(stack, NULL, se);
  Stack *soma = Stack_Dilate(stack1, NULL, se);
  Kill_Stack(stack1);

  Stack_And(stack, soma, soma);

  Stack_Label_Bwc(canvas, soma, color);

  Kill_Stack(soma);

  Write_Stack("../data/test.tif", canvas);
  return 0;
#endif
  /*******************************************/

  Object_3d *obj = NULL;
  int seed_offset = -1;

  Neurochain *chain = NULL;

  double z_scale = 0.488 / 0.588;

  Set_Zscale(z_scale);

  Stack *traced =
    Make_Stack(GREY, signal->width, signal->height, signal->depth);
  One_Stack(traced);

  for (i = 0; i < pa->size; i++) {
    seed = Voxel_Queue_De(&list);
    printf("------------------------------------------> seed: %d\n", i);
    if (*STACK_PIXEL_8(traced, seed->x, seed->y, seed->z, 0) == 0) {
      continue;
    }

    double width = pa_array[i];

    if (width > max_r) {
      continue;
    }

    chain = New_Neurochain();

    Print_Voxel(seed);
    printf("%g\n", width);

    int max_level = (int) (width + 0.5);
    if (max_level < 6) {
      max_level = 6;
    }

    seed_offset = Stack_Util_Offset(seed->x, seed->y, seed->z,
				    stack->width, stack->height, stack->depth);
    Stack_Level_Code_Constraint(stack, code, chord->array, &seed_offset, 1,
				max_level + 1);

    Voxel_t v;
    Voxel_To_Tvoxel(seed, v);

    Print_Tvoxel(v);

    Stack *tmp_stack = Copy_Stack(stack);
    obj = Stack_Grow_Object_Constraint(tmp_stack, 1, v, chord, code,
				       max_level);
    Free_Stack(tmp_stack);

    Print_Object_3d_Info(obj);

    double vec[3];
    Object_3d_Orientation_Zscale(obj, vec, MAJOR_AXIS, z_scale);

    double theta, psi;
    Geo3d_Vector obj_vec;
    Set_Geo3d_Vector(&obj_vec, vec[0], vec[1], vec[2]);

    Geo3d_Vector_Orientation(&obj_vec, &theta, &psi);

    Set_Neuroseg(&(chain->seg), width, width, 10.0,
		 theta, psi);

    double cpos[3];
    cpos[0] = seed->x;
    cpos[1] = seed->y;
    cpos[2] = seed->z;
    cpos[2] *= z_scale;

    double bpos[3];
    Neuroseg_Pos_Center_To_Bottom(&(chain->seg), cpos, bpos);
    Set_Position(chain->position, bpos[0], bpos[1], bpos[2]);

    if (Initialize_Tracing(signal, chain, NULL, NULL) >= MIN_SCORE) {
      if ((chain->seg.r1 < max_r) && (chain->seg.r2 < max_r)) {
	chain = Trace_Neuron(signal, chain, BOTH, traced);

	Print_Neurochain(chain);

	//Stack_Draw_Object_Bwc(canvas, obj, color);

	Neurochain_Erase(traced, Neurochain_Head(chain));
	Neurochain_Label(canvas, Neurochain_Head(chain));
      }
    }

    Free_Neurochain(chain);

    free(seed);

    Kill_Object_3d(obj);
  }

  Write_Stack("../data/lobster_neuron_traced.tif", canvas);

#endif

  return 0;
}


#ifndef INCLUDED_TELEPATH_BLAS_1_H
#define INCLUDED_TELEPATH_BLAS_1_H

#include <complex>
#include <cblas.h>

namespace telepath{

    template< typename T >
    inline auto asum( std::size_t n, const T* x, std::size_t incX = 1 );

    template<> inline auto asum<double>( std::size_t n, const double* x,
        std::size_t incX ){ return cblas_dasum( n, x, incX ); }
    template<> inline auto asum<float>( std::size_t n, const float* x,
        std::size_t incX ){ return cblas_sasum( n, x, incX ); }
    template<> inline auto asum<std::complex<double> >( std::size_t n,
        const std::complex<double>* x, std::size_t incX ){
        return cblas_dzasum( n, (const double*) x, incX ); }
    template<> inline auto asum<std::complex<float> >( std::size_t n,
        const std::complex<float>* x, std::size_t incX ){
        return cblas_scasum( n, (const float*) x, incX ); }

} //namespace tblas

#endif


/* -*- c++ -*- */
/*
 * Copyright (C) 2017 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>

#define	NSTEPS		 10	// how many steps of mu are in the generated table
#define	MAX_NSTEPS	256
#define	NTAPS		  8	// # of taps in the interpolator
#define	MAX_NTAPS	128

extern void initpt (double x[], int ntaps);
extern double objective (double x[], int *ntaps);
extern double global_mu;
extern double global_B;
extern gsl_integration_workspace *global_gsl_int_workspace;

// fortran
extern double prax2_ (double (fct)(double x[], int *ntaps),
		      double initv[], int *ntaps, double result[]);

static void
usage (char *name)
{
  fprintf (stderr, "usage: %s [-v] [-n <nsteps>] [-t <ntaps>] [-B <bw>]\n", name);
  exit (1);
}

static void
printline (double x[], int ntaps, int imu, int nsteps)
{
  int	i;

  printf ("  { ");
  for (i = 0; i < ntaps; i++){
    printf ("%12.5e", x[i]);
    if (i != ntaps - 1)
      printf (", ");
    else
      printf (" }, // %3d/%d\n", imu, nsteps);
  }
}

int
main (int argc, char **argv)
{
  double	xx[MAX_NSTEPS+1][MAX_NTAPS];
  int	 	ntaps = NTAPS;
  int		nsteps = NSTEPS;
  int		i, j;
  double 	result;
  double	step_size;
  int		c;
  int		verbose = 0;

  global_B = 0.25;

  while ((c = getopt (argc, argv, "n:t:B:v")) != EOF){
    switch (c){
    case 'n':
      nsteps = strtol (optarg, 0, 0);
      break;

    case 't':
      ntaps = strtol (optarg, 0, 0);
      break;

    case 'B':
      global_B = strtod (optarg, 0);
      break;

    case 'v':
      verbose = 1;
      break;

    default:
      usage (argv[0]);
      break;
    }
  }

  if ((nsteps & 1) != 0){
    fprintf (stderr, "%s: nsteps must be even\n", argv[0]);
    exit (1);
  }

  if (nsteps > MAX_NSTEPS){
    fprintf (stderr, "%s: nsteps must be < %d\n", argv[0], MAX_NSTEPS);
    exit (1);
  }

  if ((ntaps & 1) != 0){
    fprintf (stderr, "%s: ntaps must be even\n", argv[0]);
    exit (1);
  }

  if (nsteps > MAX_NTAPS){
    fprintf (stderr, "%s: ntaps must be < %d\n", argv[0], MAX_NTAPS);
    exit (1);
  }

  if (global_B < 0 || global_B > 0.5){
    fprintf (stderr, "%s: bandwidth must be in the range (0, 0.5)\n", argv[0]);
    exit (1);
  }

  global_gsl_int_workspace = gsl_integration_workspace_alloc(4000);
  if (global_gsl_int_workspace == NULL) {
    fprintf (stderr, "%s: unable to allocate GSL integration work space\n",
             argv[0]);
    exit (1);
  }

  step_size = 1.0/nsteps;

  // compute optimal values for mu <= 1.0

  for (j = 0; j <= nsteps; j++){

    global_mu = j * step_size;		// this determines the MU for which we're computing the taps

    // initialize X to a reasonable starting value

    initpt (&xx[j][0], ntaps);

    // find the value of X that minimizes the value of OBJECTIVE

    result = prax2_ (objective, &xx[j][0], &ntaps, &xx[j][0]);

    if (verbose){
      fprintf (stderr, "Mu: %10.8f\t", global_mu);
      fprintf (stderr, "Objective: %g\n", result);
    }
  }

  gsl_integration_workspace_free(global_gsl_int_workspace);

  // now print out the table

  printf ("\
/*\n\
 * This file was machine generated by gen_interp_differentiator_taps.\n\
 * DO NOT EDIT BY HAND.\n\
 */\n\n");

  printf ("static const int	DNTAPS     = %4d;\n", ntaps);
  printf ("static const int	DNSTEPS    = %4d;\n", nsteps);
  printf ("static const double	DBANDWIDTH = %g;\n\n", global_B);

  printf ("static const float Dtaps[DNSTEPS+1][DNTAPS] = {\n");
  printf ("  //    -4            -3            -2            -1             0             1             2             3                mu\n");

  for (i = 0; i <= nsteps; i++)
    printline (xx[i], ntaps, i, nsteps);

  printf ("};\n\n");

  return 0;
}


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

int sum(double* arr, int size) {
    for (int i=0; i<size ; i++) {
        arr[i] += 10;
    }
    return 0;
}

gsl_vector* get_vector( int size )
{
    int i;
    gsl_vector * v = gsl_vector_alloc (size);

    for (i = 0; i < size; i++)
    {
        gsl_vector_set (v, i, 1.23 + i);

    }
    return v;
}

double * get_vector_pointer(gsl_vector* v){
    return gsl_vector_ptr(v, 0);
}

int print_array_data(double* arr, int size){
    for(int i=0; i<size; i++) {
        printf("%f\n", arr[i]);
    }
    return 0;
}

int free_vector(gsl_vector * v)
{
    gsl_vector_free(v);
    return 0;
}

int main(int argc, char *argv[]) {
    gsl_vector* vec = get_vector(1);
    double* darry = get_vector_pointer(vec);
    //incd(darry,1);
    printf("printing darry\n");
    print_array_data(darry, 1);
    free_vector(vec);

}

double* get_double_array(int size) {
    double* arr = (double*) malloc(size*sizeof(double));
    for (int i=0; i<size; i++ ) {
        arr[i] = i;
    }
    return arr;
}
int free_double_array(double* arr) {
    free(arr);
    return 0;
}

gsl_vector_view make_vector(double* arr, int size) {
    return gsl_vector_view_array(arr, size);
}


#include <stdio.h>
#include <stdlib.h>
#include "parmt_utils.h"
#ifdef PARMT_USE_INTEL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#include "iscl/array/array.h"

/*!
 * @brief The iloc'th location and iobs'th observation this sets the forward
 *        modeling matrix G s.t. Gm = u
 *
 * @param[in] iobs    observation number
 * @param[in] iloc    location number
 * @param[in] npmax   leading dimension of G
 * @param[in] data    data data structure with Green's functions which will be
 *                    set on G
 *
 * @param[out] G      column major modeling matrix s.t. Gm=u where
 *                    G is packed
 *                     \f$ \{ G_{xx}, G_{yy}, G_{zz},
 *                            G_{xy}, G_{xz}, G_{yz} \} \f$
 *                    and m is in NED ordering [npmax x 6]
 *
 * @result 0 indicates success
 *
 */
int parmt_utils_setDataOnG(const int iobs, const int iloc, const int npmax,
                           const struct parmtData_struct data,
                           double *__restrict__ G)
{
    const char *fcnm = "parmt_utils_setDataOnG\0";
    int k;
    if (iobs < 0 || iobs >= data.nobs)
    {
        printf("%s: Error invalid observation\n", fcnm);
        return -1;
    }
    if (iloc < 0 || iloc >= data.nlocs)
    {
        printf("%s: Error invalid location\n", fcnm);
        return -1;
    }
    k = iobs*data.nlocs + iloc;
    if (npmax > data.sacGxx[k].npts || npmax > data.sacGyy[k].npts ||
        npmax > data.sacGzz[k].npts || npmax > data.sacGxy[k].npts ||
        npmax > data.sacGxz[k].npts || npmax > data.sacGyz[k].npts)
    {
        printf("%s: npmax is too small\n", fcnm);
        return -1;
    }
    if (data.sacGxx[k].data == NULL || data.sacGyy[k].data == NULL ||
        data.sacGzz[k].data == NULL || data.sacGxy[k].data == NULL ||
        data.sacGxz[k].data == NULL || data.sacGyz[k].data == NULL)
    {
        printf("%s: Greens function is null\n", fcnm);
        return -1;
    }
    // Get the Green's functions onto the matrix G
    array_zeros64f_work(6*npmax, G);
    cblas_dcopy(data.sacGxx[k].header.npts,
                data.sacGxx[k].data, 1, &G[0*npmax], 1);
    cblas_dcopy(data.sacGyy[k].header.npts,
                data.sacGyy[k].data, 1, &G[1*npmax], 1);
    cblas_dcopy(data.sacGzz[k].header.npts,
                data.sacGzz[k].data, 1, &G[2*npmax], 1);
    cblas_dcopy(data.sacGxy[k].header.npts,
                data.sacGxy[k].data, 1, &G[3*npmax], 1);
    cblas_dcopy(data.sacGxz[k].header.npts,
                data.sacGxz[k].data, 1, &G[4*npmax], 1);
    cblas_dcopy(data.sacGyz[k].header.npts,
                data.sacGyz[k].data, 1, &G[5*npmax], 1);
    return 0;
}


/*
* -----------------------------------------------------------------
*  Thermochemistry Library --- thrm_lib.h
*  Version: 1.6180
*  Date: Dec 29, 2010
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  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.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the header file for a library with
*  thermochemistry routines.
* -----------------------------------------------------------------
*/

#ifndef __THRM_LIB_H__
#define __THRM_LIB_H__

#include <gsl/gsl_vector.h>
#include <nvector/nvector_serial.h>

/*
* ------------------------------------------------------------
*  Types: struct thrm_struct, thrm_wrk
* ------------------------------------------------------------
* This structure contains fields of the Chemkin workspace.
*
* last update: Mar 5, 2009
* ------------------------------------------------------------
*/

typedef struct thrm_struct
{
    int    *iwk;   /* integer work vector */
    double *rwk;   /* real work vector */
    char   *cwk;   /* character work vector */
    int leniwk;    /* iwk dimension */
    int lenrwk;    /* rwk dimension */
    int lencwk;    /* cwk dimension */
    int n_e;       /* # of chemical elements */
    int n_s;       /* # of elementary species */
    int n_r;       /* # of elementary reactions */
} thrm_wrk;
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
* function prototypes
*------------------------------------------------------------
*/

void thrm_title();

void thrm_mech(int n_e,
               int n_s,
               int n_r,
               char *mech);

int thrm_input(int *steps,
               double *t0,
               double *delta_t,
               double *Tmax,
               double *Tmin,
               double *tol);

thrm_wrk* thrm_alloc();

void thrm_free(void **thrm_bl);

int thrm_init(thrm_wrk *thrm);

void thrm_composition(thrm_wrk *thrm,
                      gsl_vector *phi);

void thrm_page_heading0(FILE *file);

double thrm_h2T(thrm_wrk *thrm,
                double *phi,
                double Tmax,
                double Tmin,
                double tol);

void thrm_temp_meanvar(int Np,
                       gsl_vector **ps,
                       thrm_wrk *thrm,
                       double Tmax,
                       double Tmin,
                       double tol,
                       double *mean,
                       double *var);

double thrm_rrsum(gsl_vector *phi);

double thrm_mfsum(gsl_vector *phi);

int thrm_mfsign(gsl_vector *phi);

int thrm_eqs(double t,
             N_Vector phi,
             N_Vector Rphi,
             void *thrm_data);

int thrm_eqs2(double t,
              N_Vector phi,
              N_Vector Sphi,
              void *thrm_data);

#endif /* __THRM_LIB_H__ */


/**
 * \file FIRFilter.h
 */

#ifndef ATK_EQ_FIRFILTER_H
#define ATK_EQ_FIRFILTER_H

#include <ATK/config.h>
#include <ATK/EQ/config.h>

#include <gsl/gsl>

#include <cassert>
#include <cstdint>
#include <vector>

namespace ATK
{
  /**
   * FIR filter template class
   */
  template<class Coefficients >
  class ATK_EQ_EXPORT FIRFilter final : public Coefficients
  {
  protected:
    using Parent = Coefficients;
    using typename Parent::DataType;
    using typename Parent::AlignedScalarVector;

    using Parent::converted_inputs;
    using Parent::outputs;
    using Parent::coefficients_in;
    using Parent::input_sampling_rate;
    using Parent::output_sampling_rate;
    using Parent::nb_input_ports;
    using Parent::nb_output_ports;

    using Parent::in_order;
    using Parent::input_delay;
    using Parent::setup;

  public:
    /*!
     * @brief Constructor
     * @param nb_channels is the number of input and output channels
     */
    explicit FIRFilter(gsl::index nb_channels = 1)
      :Parent(nb_channels)
    {
    }

    /// Move constructor
    FIRFilter(FIRFilter&& other)
    :Parent(std::move(other))
    {
    }

    void setup() override
    {
      Parent::setup();
      input_delay = in_order;
    }

    void process_impl(gsl::index size) const final
    {
      assert(input_sampling_rate == output_sampling_rate);
      assert(nb_input_ports == nb_output_ports);
      assert(coefficients_in.data());

      const auto* ATK_RESTRICT coefficients_in_ptr = coefficients_in.data();

      for (gsl::index channel = 0; channel < nb_input_ports; ++channel)
      {
        const DataType* ATK_RESTRICT input = converted_inputs[channel] - static_cast<int64_t>(in_order);
        DataType* ATK_RESTRICT output = outputs[channel];

        for (gsl::index i = 0; i < size; ++i)
        {
          output[i] = 0;
        }

        for (gsl::index j = 0; j < in_order + 1; ++j)
        {
          for (gsl::index i = 0; i < size; ++i)
          {
            output[i] += coefficients_in_ptr[j] * input[i + j];
          }
        }
      }

    }

    /// Returns the vector of internal coefficients for the MA section
    const AlignedScalarVector& get_coefficients_in() const
    {
      return coefficients_in;
    }
  };

}

#endif


#ifndef QR_H
#define QR_H

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

/**
 * Computes, stores, and maintains a QR decomposition of a gsl_matrix.
 *
 * A matrix-pointer is passed to the QR on construction. This pointer is kept through
 * the life-time of the QR object (but ownership is not assumed). If the contents
 * of the matrix is changed, the QR can be update to reflect the change using the
 * abstract UpdateFromMatrix function. This is not automatically done on construction.
 */
class QR{
 protected:
  const int m, n; ///< Dimensions of matrix

  /** Decomposes M into U*S*V^t.  */
  QR(gsl_matrix* M);
 public:

  /**
   * Constructs a QR object. If MKL is available it will be an MKLQR and if GSL
   * is available it will be GSLQR
   */
  static QR* createQR(gsl_matrix* M);

  virtual ~QR();

  /** Update Q and R to reflect the decomposition of the matrix. */
  virtual void updateFromMatrix() = 0;

  /** Print the QR to standard out */
  void print() const;

  gsl_matrix* getMatrix() const;
  gsl_matrix* getQ() const;
  gsl_matrix* getR() const;
 protected:
  gsl_matrix * const m_matrix;
  gsl_matrix * const m_Q;
  gsl_matrix * const m_R;

};

#endif


#ifndef LPC_H
#define LPC_H

#include <cmath>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_blas.h>

gsl_vector *hanning(size_t L);

double lpcCoeffs(double *lpc, gsl_vector *x, size_t order);

#endif // LPC_H


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

#include <options/options.h>

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

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

	const char*input_filename;
	const char*output_filename;
	double diff[3] = {0, 0, 0};

	double laser[3] ={0,0,0};

	double omega[2] ={0,0};
	double omega_vel = 0;
	double vel[2] ={0,0};
	double vel_omega = 0;

	struct option* ops = options_allocate(15);
	options_string(ops, "in", &input_filename, "stdin", "input file");
	options_string(ops, "out", &output_filename, "stdout", "output file");
	options_double(ops, "l_x", &(laser[0]), 0.0, "laser x (m)");
	options_double(ops, "l_y", &(laser[1]), 0.0, "laser y (m)");
	options_double(ops, "l_theta", &(laser[2]), 0.0, "laser theta (rad)");

	options_double(ops, "omega0", &(omega[0]), 0.0, "omega (rad)");
	options_double(ops, "omega1", &(omega[1]), 0.0, "omega (linear)");
	options_double(ops, "omega_vel", &(omega_vel), 0.0, "omega x vel");
	options_double(ops, "vel0", &(vel[0]), 0.0, "vel (m)");
	options_double(ops, "vel1", &(vel[1]), 0.0, "vel (linear)");
	options_double(ops, "vel_omega", &(vel_omega), 0.0, "vel x omega");

	if(!options_parse_args(ops, argc, argv)) {
		fprintf(stderr, " Corrects bias in odometry.\n");
		options_print_help(ops, stderr);
		return -1;
	}

	FILE * input_stream = open_file_for_reading(input_filename);
	FILE *output_stream = open_file_for_writing(output_filename);

	if(!input_stream || !output_stream) return -1;

	LDP laser_ref = ld_read_smart(input_stream);
	if(!laser_ref) {
		sm_error("Cannot read first scan.\n");
		return -2;
	}

	copy_d(laser_ref->odometry, 3, laser_ref->estimate);

	double old_odometry[3];
	copy_d(laser_ref->odometry, 3, old_odometry);

	LDP laser_sens;
	ld_write_as_json(laser_ref, output_stream);

	while((laser_sens = ld_read_smart(input_stream))) {
		double guess[3], old_guess[3];
		pose_diff_d(laser_sens->odometry, old_odometry, guess);
		pose_diff_d(laser_sens->odometry, old_odometry, old_guess);
		copy_d(laser_sens->odometry, 3, old_odometry);

		if(fabs(guess[2]) > 1e-7)
			guess[2] = old_guess[2] * omega[1] + omega_vel * guess[0] + omega[0];

		if(fabs(guess[0]) > 1e-7)
			guess[0] = old_guess[0] * vel[1] + vel_omega * guess[2] + vel[0];

		fprintf(stderr, "odo: %f %f %f Corr: %f rad \t%f m  \n", guess[0], guess[1], guess[2], guess[2]-old_guess[2], guess[0]-old_guess[0]);

		oplus_d(laser_ref->odometry, guess, laser_sens->odometry);
		oplus_d(laser_sens->odometry, laser, laser_sens->estimate);

		fprintf(stderr, "ref odo: %s  ref est: %s \n",
			friendly_pose(laser_ref->odometry),
			friendly_pose(laser_ref->estimate));
		fprintf(stderr, "sens odo: %s  sens est: %s \n",
			friendly_pose(laser_sens->odometry),
			friendly_pose(laser_sens->estimate));

			ld_write_as_json(laser_sens, output_stream);

		ld_free(laser_ref);
		laser_ref = laser_sens;
	}

	return 0;
}


/*
 *  iaf_cond_alpha_mc.h
 *
 *  This file is part of NEST.
 *
 *  Copyright (C) 2004 The NEST Initiative
 *
 *  NEST is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  NEST 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 NEST.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef IAF_COND_ALPHA_MC_H
#define IAF_COND_ALPHA_MC_H

// Generated includes:
#include "config.h"

#ifdef HAVE_GSL

// C++ includes:
#include <vector>

// C includes:
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

// Includes from nestkernel:
#include "archiving_node.h"
#include "connection.h"
#include "event.h"
#include "nest_types.h"
#include "recordables_map.h"
#include "ring_buffer.h"
#include "universal_data_logger.h"

// Includes from sli:
#include "dictdatum.h"
#include "name.h"

/* BeginDocumentation
Name: iaf_cond_alpha_mc - PROTOTYPE Multi-compartment conductance-based leaky
                          integrate-and-fire neuron model.

Description:
THIS MODEL IS A PROTOTYPE FOR ILLUSTRATION PURPOSES. IT IS NOT YET
FULLY TESTED. USE AT YOUR OWN PERIL!

iaf_cond_alpha_mc is an implementation of a multi-compartment spiking
neuron using IAF dynamics with conductance-based synapses. It serves
mainly to illustrate the implementation of multicompartment models in
NEST.

The model has three compartments: soma, proximal and distal dendrite,
labeled as s, p, and d, respectively. Compartments are connected through
passive conductances as follows

C_m.s d/dt V_m.s = ... - g_sp ( V_m.s - V_m.p )

C_m.p d/dt V_m.p = ... - g_sp ( V_m.p - V_m.s ) - g_pd ( V_m.p - V_m.d )

C_m.d d/dt V_m.d = ...                          - g_pd ( V_m.d - V_m.p )

A spike is fired when the somatic membrane potential exceeds threshold,
V_m.s >= V_th. After a spike, somatic membrane potential is clamped to
a reset potential, V_m.s == V_reset, for the refractory period. Dendritic
membrane potentials are not manipulated after a spike.

There is one excitatory and one inhibitory conductance-based synapse
onto each compartment, with alpha-function time course. The alpha
function is normalised such that an event of weight 1.0 results in a
peak current of 1 nS at t = tau_syn. Each compartment can also receive
current input from a current generator, and an external (rheobase)
current can be set for each compartment.

Synapses, including those for injection external currents, are addressed through
the receptor types given in the receptor_types entry of the state dictionary.
Note that in contrast to the single-compartment iaf_cond_alpha model, all
synaptic weights must be positive numbers!

Parameters:
The following parameters can be set in the status dictionary. Parameters
for each compartment are collected in a sub-dictionary; these sub-dictionaries
are called "soma", "proximal", and "distal", respectively. In the list below,
these parameters are marked with an asterisk.

V_m*         double - Membrane potential in mV
E_L*         double - Leak reversal potential in mV.
C_m*         double - Capacity of the membrane in pF
E_ex*        double - Excitatory reversal potential in mV.
E_in*        double - Inhibitory reversal potential in mV.
g_L*         double - Leak conductance in nS;
tau_syn_ex*  double - Rise time of the excitatory synaptic alpha function in ms.
tau_syn_in*  double - Rise time of the inhibitory synaptic alpha function in ms.
I_e*         double - Constant input current in pA.

g_sp         double - Conductance connecting soma and proximal dendrite, in nS.
g_pd         double - Conductance connecting proximal and distal dendrite, in
                      nS.
t_ref        double - Duration of refractory period in ms.
V_th         double - Spike threshold in mV.
V_reset      double - Reset potential of the membrane in mV.

Example:
See examples/nest/mc_neuron.py.

Remark:
This is a prototype for illustration which has undergone only limited testing.
Details of the implementation and user-interface will likely change.
USE AT YOUR OWN PERIL!

Sends: SpikeEvent

Receives: SpikeEvent, CurrentEvent, DataLoggingRequest

References:

Meffin, H., Burkitt, A. N., & Grayden, D. B. (2004). An analytical
model for the large, fluctuating synaptic conductance state typical of
neocortical neurons in vivo. J.  Comput. Neurosci., 16, 159-175.

Bernander, O ., Douglas, R. J., Martin, K. A. C., & Koch, C. (1991).
Synaptic background activity influences spatiotemporal integration in
single pyramidal cells.  Proc. Natl. Acad. Sci. USA, 88(24),
11569-11573.

Author: Plesser

SeeAlso: iaf_cond_alpha
*/

namespace nest
{
/**
 * Function computing right-hand side of ODE for GSL solver.
 * @note Must be declared here so we can befriend it in class.
 * @note Must have C-linkage for passing to GSL.
 * @note No point in declaring it inline, since it is called
 *       through a function pointer.
 */
extern "C" int
iaf_cond_alpha_mc_dynamics( double, const double*, double*, void* );

/**
 * @note All parameters that occur for both compartments
 *       and dendrite are stored as C arrays, with index 0 being soma.
 */
class iaf_cond_alpha_mc : public Archiving_Node
{

  // Boilerplate function declarations --------------------------------

public:
  iaf_cond_alpha_mc();
  iaf_cond_alpha_mc( const iaf_cond_alpha_mc& );
  ~iaf_cond_alpha_mc();

  /**
   * Import sets of overloaded virtual functions.
   * @see Technical Issues / Virtual Functions: Overriding, Overloading, and
   * Hiding
   */
  using Node::handle;
  using Node::handles_test_event;

  port send_test_event( Node&, rport, synindex, bool );

  void handle( SpikeEvent& );
  void handle( CurrentEvent& );
  void handle( DataLoggingRequest& );

  port handles_test_event( SpikeEvent&, rport );
  port handles_test_event( CurrentEvent&, rport );
  port handles_test_event( DataLoggingRequest&, rport );

  void get_status( DictionaryDatum& ) const;
  void set_status( const DictionaryDatum& );

private:
  void init_state_( const Node& proto );
  void init_buffers_();
  void calibrate();
  void update( Time const&, const long, const long );

  // Enumerations and constants specifying structure and properties ----

  //! Compartments, NCOMP is number
  enum Compartments_
  {
    SOMA = 0,
    PROX,
    DIST,
    NCOMP
  };

  /**
   * Minimal spike receptor type.
   * @note Start with 1 so we can forbid port 0 to avoid accidental
   *       creation of connections with no receptor type set.
   */
  static const port MIN_SPIKE_RECEPTOR = 1;

  /**
   * Spike receptors.
   */
  enum SpikeSynapseTypes
  {
    SOMA_EXC = MIN_SPIKE_RECEPTOR,
    SOMA_INH,
    PROX_EXC,
    PROX_INH,
    DIST_EXC,
    DIST_INH,
    SUP_SPIKE_RECEPTOR
  };

  static const size_t NUM_SPIKE_RECEPTORS =
    SUP_SPIKE_RECEPTOR - MIN_SPIKE_RECEPTOR;

  /**
   * Minimal current receptor type.
   *  @note Start with SUP_SPIKE_RECEPTOR to avoid any overlap and
   *        accidental mix-ups.
   */
  static const port MIN_CURR_RECEPTOR = SUP_SPIKE_RECEPTOR;

  /**
   * Current receptors.
   */
  enum CurrentSynapseTypes
  {
    I_SOMA = MIN_CURR_RECEPTOR,
    I_PROX,
    I_DIST,
    SUP_CURR_RECEPTOR
  };

  static const size_t NUM_CURR_RECEPTORS =
    SUP_CURR_RECEPTOR - MIN_CURR_RECEPTOR;

  // Friends --------------------------------------------------------

  friend int
  iaf_cond_alpha_mc_dynamics( double, const double*, double*, void* );

  friend class RecordablesMap< iaf_cond_alpha_mc >;
  friend class UniversalDataLogger< iaf_cond_alpha_mc >;

  // Parameters ------------------------------------------------------

  /**
   * Independent parameters of the model.
   * These parameters must be passed to the iteration function that
   * is passed to the GSL ODE solvers. Since the iteration function
   * is a C++ function with C linkage, the parameters can be stored
   * in a C++ struct with member functions, as long as we just pass
   * it by void* from C++ to C++ function. The struct must be public,
   * though, since the iteration function is a function with C-linkage,
   * whence it cannot be a member function of iaf_cond_alpha_mc.
   * @note One could achieve proper encapsulation by an extra level
   *       of indirection: Define the iteration function as a member
   *       function, plus an additional wrapper function with C linkage.
   *       Then pass a struct containing a pointer to the node and a
   *       pointer-to-member-function to the iteration function as void*
   *       to the wrapper function. The wrapper function can then invoke
   *       the iteration function on the node (Stroustrup, p 418). But
   *       this appears to involved, and the extra indirections cost.
   */
  struct Parameters_
  {
    double V_th;                //!< Threshold Potential in mV
    double V_reset;             //!< Reset Potential in mV
    double t_ref;               //!< Refractory period in ms
    double g_conn[ NCOMP - 1 ]; //!< Conductances connecting compartments,
                                //!< in nS
    double g_L[ NCOMP ];        //!< Leak Conductance in nS
    double C_m[ NCOMP ];        //!< Membrane Capacitance in pF
    double E_ex[ NCOMP ];       //!< Excitatory reversal Potential in mV
    double E_in[ NCOMP ];       //!< Inhibitory reversal Potential in mV
    double E_L[ NCOMP ]; //!< Leak reversal Potential (aka resting potential)
                         //!< in mV
    double tau_synE[ NCOMP ]; //!< Synaptic Time Constant Excitatory Synapse
                              //!< in ms
    double tau_synI[ NCOMP ]; //!< Synaptic Time Constant for Inhibitory
                              //!< Synapse in ms
    double I_e[ NCOMP ];      //!< Constant Current in pA

    Parameters_();                     //!< Sets default parameter values
    Parameters_( const Parameters_& ); //!< needed to copy C-arrays
    Parameters_& operator=( const Parameters_& ); //!< needed to copy C-arrays

    void get( DictionaryDatum& ) const; //!< Store current values in dictionary
    void set( const DictionaryDatum& ); //!< Set values from dicitonary
  };

  // State variables  ------------------------------------------------------

  /**
   * State variables of the model.
   * @note Copy constructor and assignment operator required because
   *       of C-style array.
   */
public:
  struct State_
  {

    /**
     * Elements of state vector.
     * For the multicompartmental case here, these are offset values.
     * The state variables are stored in contiguous blocks for each
     * compartment, beginning with the soma.
     */
    enum StateVecElems_
    {
      V_M = 0,
      DG_EXC,
      G_EXC,
      DG_INH,
      G_INH,
      STATE_VEC_COMPS
    };

    //! total size of state vector
    static const size_t STATE_VEC_SIZE = STATE_VEC_COMPS * NCOMP;

    //! neuron state, must be C-array for GSL solver
    double y_[ STATE_VEC_SIZE ];
    int r_; //!< number of refractory steps remaining

    State_( const Parameters_& ); //!< Default initialization
    State_( const State_& );
    State_& operator=( const State_& );

    void get( DictionaryDatum& ) const;
    void set( const DictionaryDatum&, const Parameters_& );

    /**
     * Compute linear index into state array from compartment and element.
     * @param comp compartment index
     * @param elem elemet index
     * @note compartment argument is not of type Compartments_, since looping
     *       over enumerations does not work.
     */
    static size_t
    idx( size_t comp, StateVecElems_ elem )
    {
      return comp * STATE_VEC_COMPS + elem;
    }
  };

private:
  // Internal buffers --------------------------------------------------------

  /**
   * Buffers of the model.
   */
  struct Buffers_
  {
    Buffers_( iaf_cond_alpha_mc& ); //!<Sets buffer pointers to 0
    //! Sets buffer pointers to 0
    Buffers_( const Buffers_&, iaf_cond_alpha_mc& );

    //! Logger for all analog data
    UniversalDataLogger< iaf_cond_alpha_mc > logger_;

    /** buffers and sums up incoming spikes/currents
     *  @note Using STL vectors here to ensure initialization.
     */
    std::vector< RingBuffer > spikes_;
    std::vector< RingBuffer > currents_;

    /** GSL ODE stuff */
    gsl_odeiv_step* s_;    //!< stepping function
    gsl_odeiv_control* c_; //!< adaptive stepsize control function
    gsl_odeiv_evolve* e_;  //!< evolution function
    gsl_odeiv_system sys_; //!< struct describing system

    // IntergrationStep_ should be reset with the neuron on ResetNetwork,
    // but remain unchanged during calibration. Since it is initialized with
    // step_, and the resolution cannot change after nodes have been created,
    // it is safe to place both here.
    double step_;            //!< step size in ms
    double IntegrationStep_; //!< current integration time step, updated by GSL

    /**
     * Input currents injected by CurrentEvent.
     * This variable is used to transport the current applied into the
     * _dynamics function computing the derivative of the state vector.
     * It must be a part of Buffers_, since it is initialized once before
     * the first simulation, but not modified before later Simulate calls.
     */
    double I_stim_[ NCOMP ]; //!< External Stimulus in pA
  };

  // Internal variables ---------------------------------------------

  /**
   * Internal variables of the model.
   */
  struct Variables_
  {
    /** initial value to normalise excitatory synaptic conductance */
    double PSConInit_E_[ NCOMP ];

    /** initial value to normalise inhibitory synaptic conductance */
    double PSConInit_I_[ NCOMP ];

    int RefractoryCounts_;
  };

  // Access functions for UniversalDataLogger -------------------------------

  /**
   * Read out state vector elements, used by UniversalDataLogger
   * First template argument is component "name", second compartment "name".
   */
  template < State_::StateVecElems_ elem, Compartments_ comp >
  double
  get_y_elem_() const
  {
    return S_.y_[ S_.idx( comp, elem ) ];
  }

  //! Read out number of refractory steps, used by UniversalDataLogger
  double
  get_r_() const
  {
    return Time::get_resolution().get_ms() * S_.r_;
  }

  // Data members ----------------------------------------------------

  Parameters_ P_;
  State_ S_;
  Variables_ V_;
  Buffers_ B_;

  //! Table of compartment names
  static std::vector< Name > comp_names_;

  //! Dictionary of receptor types, leads to seg fault on exit, see #328
  // static DictionaryDatum receptor_dict_;

  //! Mapping of recordables names to access functions
  static RecordablesMap< iaf_cond_alpha_mc > recordablesMap_;
};

inline port
iaf_cond_alpha_mc::send_test_event( Node& target,
  rport receptor_type,
  synindex,
  bool )
{
  SpikeEvent e;
  e.set_sender( *this );
  return target.handles_test_event( e, receptor_type );
}

inline port
iaf_cond_alpha_mc::handles_test_event( SpikeEvent&, rport receptor_type )
{
  if ( receptor_type < MIN_SPIKE_RECEPTOR
    || receptor_type >= SUP_SPIKE_RECEPTOR )
  {
    if ( receptor_type < 0 || receptor_type >= SUP_CURR_RECEPTOR )
    {
      throw UnknownReceptorType( receptor_type, get_name() );
    }
    else
    {
      throw IncompatibleReceptorType( receptor_type, get_name(), "SpikeEvent" );
    }
  }
  return receptor_type - MIN_SPIKE_RECEPTOR;
}

inline port
iaf_cond_alpha_mc::handles_test_event( CurrentEvent&, rport receptor_type )
{
  if ( receptor_type < MIN_CURR_RECEPTOR || receptor_type >= SUP_CURR_RECEPTOR )
  {
    if ( receptor_type >= 0 && receptor_type < MIN_CURR_RECEPTOR )
    {
      throw IncompatibleReceptorType(
        receptor_type, get_name(), "CurrentEvent" );
    }
    else
    {
      throw UnknownReceptorType( receptor_type, get_name() );
    }
  }
  return receptor_type - MIN_CURR_RECEPTOR;
}

inline port
iaf_cond_alpha_mc::handles_test_event( DataLoggingRequest& dlr,
  rport receptor_type )
{
  if ( receptor_type != 0 )
  {
    if ( receptor_type < 0 || receptor_type >= SUP_CURR_RECEPTOR )
    {
      throw UnknownReceptorType( receptor_type, get_name() );
    }
    else
    {
      throw IncompatibleReceptorType(
        receptor_type, get_name(), "DataLoggingRequest" );
    }
  }
  return B_.logger_.connect_logging_device( dlr, recordablesMap_ );
}

inline void
iaf_cond_alpha_mc::get_status( DictionaryDatum& d ) const
{
  P_.get( d );
  S_.get( d );
  Archiving_Node::get_status( d );

  ( *d )[ names::recordables ] = recordablesMap_.get_list();

  /**
   * @todo dictionary construction should be done only once for
   * static member in default c'tor, but this leads to
   * a seg fault on exit, see #328
   */
  DictionaryDatum receptor_dict_ = new Dictionary();
  ( *receptor_dict_ )[ names::soma_exc ] = SOMA_EXC;
  ( *receptor_dict_ )[ names::soma_inh ] = SOMA_INH;
  ( *receptor_dict_ )[ names::soma_curr ] = I_SOMA;

  ( *receptor_dict_ )[ names::proximal_exc ] = PROX_EXC;
  ( *receptor_dict_ )[ names::proximal_inh ] = PROX_INH;
  ( *receptor_dict_ )[ names::proximal_curr ] = I_PROX;

  ( *receptor_dict_ )[ names::distal_exc ] = DIST_EXC;
  ( *receptor_dict_ )[ names::distal_inh ] = DIST_INH;
  ( *receptor_dict_ )[ names::distal_curr ] = I_DIST;

  ( *d )[ names::receptor_types ] = receptor_dict_;
}

inline void
iaf_cond_alpha_mc::set_status( const DictionaryDatum& d )
{
  Parameters_ ptmp = P_; // temporary copy in case of errors
  ptmp.set( d );         // throws if BadProperty
  State_ stmp = S_;      // temporary copy in case of errors
  stmp.set( d, ptmp );   // throws if BadProperty

  // We now know that (ptmp, stmp) are consistent. We do not
  // write them back to (P_, S_) before we are also sure that
  // the properties to be set in the parent class are internally
  // consistent.
  Archiving_Node::set_status( d );

  // if we get here, temporaries contain consistent set of properties
  P_ = ptmp;
  S_ = stmp;
}

} // namespace

#endif // HAVE_GSL
#endif // IAF_COND_ALPHA_MC_H


/*
 * PIController.h
 *
 *  Created on: Mar 31, 2013
 *      Author: vgomez, Sep Thijssen
 */

#ifndef PICONTROLLER_H_
#define PICONTROLLER_H_

#define VERB_LEVEL 1

#include "global.h"
#include <boost/property_tree/ptree.hpp>

#include <iostream>
#include <fstream>
#include <algorithm>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <Eigen/Dense>
using namespace Eigen;

class PIController {
public:
	static double dt;		// time increment (required in Model)
	static int units;		// number of UAVs (required in PIController and Model)
	static double R;	    // control (required in Model)
	static double nu;		// noise level
	static double lambda;  	// PI scalar ratio = nu*R
	static int dtperstep;	// number of dt per control step
	static int H;			// number of steps in horizon
	static double dS;		// duration of a control step = dt*dtperstep
	static double stdv;		// local standard deviation = sqrt(nu/dS)
	static int dimUAVx;
	static int dimUAVu;
	static int seed;

protected:
	gsl_rng *r;					// Random seed.
	int dimX;					// dimension state
	int dimU;					// dimension control
	int N;						// number of rollouts applied each step
	double stdv_dt;				// new time scale for noise
	vvec u_exp;					// [H][dimU] current best, used for exploring
	std::string outfile_matlab;	// name of the output .m file
	std::string outfile_costs;	// name of a log file with costs (comma separated values format)
	boost::property_tree::ptree pt;

	// needed for feedback
	vector<MatrixXd> A,lhs,dlhs,rhs,drhs;
	int dimF;

public :

	// nested class Model used by the PIController to simulate dynamics
	class Model {
	protected:
		vec state;			// [dimX] running state
		PIController *ctrl; // pointer to PIController object (outer class)

	public:

		Model() {};
		virtual ~Model() {};

		// apply control uuu on the current state
		virtual void step(const vec&) = 0;

		// replaces the current state
		void setState(const vec& X0) { state = X0; }

		// returns the current state
		vec getState() const { return state; }

		// replaces the current state
		void setController(PIController *_ctrl) { ctrl = _ctrl; }

		// sets the concrete model (implemented by subclass)
		virtual void setProperties() = 0;

		// Output is the immediate state reward
		// writes in of the components of the reward to be logged
		virtual double immediateStateReward(std::ofstream &of) = 0;

		// Output is the immediate state reward
		// sets different attributes of the subclass to be logged if necessary
		virtual double immediateStateReward() = 0;

		// Output is the end state reward. Input is a state.
		double endStateReward() { return immediateStateReward(); };

	};

private:

protected:
	Model *model;			// The dynamical model the PIController uses

	public:
	PIController() {};
	virtual ~PIController() { unsetModel(); };

	void Init(const boost::property_tree::ptree &pt);

	void setSeed(const double &s) { seed = s; };

	// sets the concrete model (implemented by subclass)
	void setModel();

	// unsets the concrete model (implemented by subclass)
	void unsetModel();

	// Returns state based reward of a rollout
	double runningStateReward(const vec&, const vvec&);

	// Returns the cost of a control sequence.
	double runningControlCost(const vvec&) const;

	vec predictState(const vec& X, const vec& A);

	// Output is the immediate control cost. Input is a control action.
	double immediateControlCost(const vec&) const;

//	double immediateStateReward(const vec& X)	{	model->setState(X);	return model->immediateStateReward(); }

	void printTime() const;

	void plotCurrent(const vec&, const vec&) const;

	void plotSetup() const;

	boost::property_tree::ptree getProperties() { return pt; }

	// The PI-control algorithm, input is a state, output is control (VelHeight)
	vec computeControl(const vec &);

	// PI-Controller with feedback, input is a state, output is control (VelHeight)
	// property A is updated
	vec computeControlFeedback(const vec &);

};

#endif /* PICONTROLLER_H_ */


/* #################################################
 * # stochastic simulation of adapting populations #
 * # with varying constraint type                  #
 * # and exponential mutation kernel               #
 * #################################################
 *
 * 2012-2015, Lukas Geyrhofer, lukas.geyrhofer@ds.mpg.de
 *
 *
 * #################################################
 *
 * simplest usage (population size 1e7):
 * ./travelingwavepeak_exp -N 1e7 1> out.txt 2> conf.txt
 *
 * ################################################# */

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

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

#include <mcheck.h>

int space = 300;
int space0 = 100;
double dx = 1e-2;
int maxSteps = 1000;

int outputstep = 100;
int quiet = 0;

int noise = 0;		// noise can assume 4 different values:
			// value in code	cmdline	description
			// 0 (default)			populationsize is constrained to 1, no noise is added
			//				constraint could be any value
			// 1			N	fixedN, set by parameter -N POPSIZE
			// 2			u U	ustar, set by parameter -u UFILENAME
			//				if ustar-profile is in finer grid than simulation, use parameter -U LATTICERATIO
			// 3			n g	timetrace, population size is constrained using a (single column) textfile,
			//				where the entries are the current population size
			//				use also parameter -g STEPS to indicate the number of timesteps (steps! not time in generations)
			//				between two values in this textfile
			// 4			y U	ustar, use file from -y UFILENAME
			//				no stochastic dynamics, noise is not added, only the constraint is used
			//				if ustar-profile is in finer grid than simulation, use parameter -U LATTICERATIO
double populationsize = 1.;
double populationvariance;
double xg,varg,xcmax,xnose;
int correctformeanfitness =0;
int printhistotype = 1;
int printfixationprob = 0;

int allshifts = 0;
int shiftthreshold = 1;
double current_mean_fitness = 0.;

int read_from_file = 0;
int write_to_file = 0;
char c_infile[128],c_outfile[128];

int have_u_infile = 0;
char u_infile[128];
double *u_read,*u;
int dens_ustar_latticeratio = 1;
int u_space,u_space0;
double u_dx;
double wavespeed = 0.;
double speedprefactor;

double epsilon = 1e-2;
double twoepssqrt;
double *nn;
double *tmp;
double *x;

double mutationrate = 1e-5;
double *mutation_inflow;
double mutation_outflow;
double mutation_sigma = 1e-2;

gsl_rng* rg; /* gsl, global generator */
const gsl_rng_type* T;
unsigned long int randseed = 0;

int averagepopdens = 0;
int averagepopdens_center = 1;
int averagepopdens_resolution = 1;
int averagepopdens_havefile = 0;
double *averagepopdens_dens;
char averagepopdens_outputfile[128];
double averagepopdens_count = 0.;
double averagepopdens_dx;
int averagepopdens_space;
int averagepopdens_space0;
int averagepopdens_lower;
int averagepopdens_higher;

double *timetrace_popsize;
char timetrace_filename[128];
int timetrace_ps_interval = 100;
int timetrace_length;

int print_error(char *msg) {
  fprintf(stderr,"ERROR: %s\n",msg);
  exit(1);
}

// ************************************************************
// **   parameters
// ************************************************************

void parsecomamndline(int argn, char *argv[]) {
  char c;
  while((c = getopt(argn,argv,"s:z:d:S:e:D:O:qQi:o:R:N:CM:u:U:T:H:h:Pn:g:y:F")) != -1) {
    switch(c) {
      case 's':	space = atoi(optarg);
		break;
      case 'z':	space0 = atoi(optarg);
		break;
      case 'd':	dx = atof(optarg);
		break;
      case 'i':	strcpy(c_infile,optarg);
		read_from_file = 1;
		break;
      case 'o':	strcpy(c_outfile,optarg);
		write_to_file = 1;
		break;
      case 'u':	strcpy(u_infile,optarg);
		if(noise > 0)print_error("Only a single constraint-type can be used (either option -N POPSIZE or -u FILENAME or -n FILENAME_TIMETRACE)");
		noise = 2;
		break;
      case 'y':	strcpy(u_infile,optarg);
		if(noise > 0)print_error("Only a single constraint-type can be used (either option -N POPSIZE or -u FILENAME or -n FILENAME_TIMETRACE or -y FILENAME)");
		noise = 4;
		break;
      case 'U':	if((noise == 2)||(noise == 4)) {
		  dens_ustar_latticeratio = atoi(optarg);
		}else{
		  print_error("option -u FILENAME needed before option -U RATIO");
		}
		break;
      case 'S':	maxSteps = atoi(optarg);
		break;
      case 'e': epsilon = atof(optarg);
		break;
      case 'D':	mutationrate = atof(optarg);
		break;
      case 'O':	outputstep = atoi(optarg);
		break;
      case 'q':	quiet = 2;
		break;
      case 'Q':	quiet = 1;
		break;
      case 'R':	randseed =atoi(optarg);
		break;
      case 'N': populationsize = atof(optarg);
		if(noise > 0)print_error("Only a single constraint-type can be used (either option -N POPSIZE or -u FILENAME or -n FILENAME_TIMETRACE)");
		noise = 1;
		break;
      case 'C':	correctformeanfitness = 1;
		break;
      case 'M':	mutation_sigma = atof(optarg);
		break;
      case 'T':	shiftthreshold = atoi(optarg);
		break;
      case 'h':	strcpy(averagepopdens_outputfile,optarg);
		averagepopdens_havefile = 1;
		averagepopdens = 1;
		break;
      case 'H':	averagepopdens = 1;
		averagepopdens_resolution = atoi(optarg);
		if(averagepopdens_resolution < 0) {
		  averagepopdens_resolution *= -1;
		  averagepopdens_center = 0;
		}
		break;
      case 'P':	printhistotype = 0;
		break;
      case 'F': printfixationprob = 1;
		break;
      case 'n':	strcpy(timetrace_filename,optarg);
		if(noise > 0)print_error("Only a single constraint-type can be used (either option -N POPSIZE or -u FILENAME or -n FILENAME_TIMETRACE)");
		noise = 3;
		break;
      case 'g':	timetrace_ps_interval = atoi(optarg);
		break;
      }
  }
  if(randseed==0)randseed=time(NULL);
}

// ************************************************************
// **   input and output for configuration files
// ************************************************************

void read_popdens(int importparameters) {
  int i;
  int icount,dcount;
  int *ival;
  double *dval;
  int c_space,c_space0;
  double c_dx;
  FILE *fp;

  fp = fopen(c_infile,"rb");
  if(fp != NULL) {
    fread(&icount,sizeof(int),1,fp);
    fread(&dcount,sizeof(int),1,fp);
    fread(&c_dx,sizeof(double),1,fp);
    fread(&c_space,sizeof(int),1,fp);
    fread(&c_space0,sizeof(int),1,fp);

    if(importparameters) {
      space = c_space;
      space0 = c_space0;
      dx = c_dx;
    }

    if(icount>0) {
      ival = (int*)malloc(icount*sizeof(int));
      fread(ival,sizeof(int),icount,fp);
      free(ival);
    }

    if(dcount>0) {
      dval = (double*)malloc(dcount*sizeof(int));
      fread(dval,sizeof(double),dcount,fp);
      free(dval);
    }

    if(space != c_space)print_error("lattice does not match!");
    nn = (double*)malloc(space*sizeof(double));
    fread(nn,sizeof(double),space,fp);
    for(i=0;i<space;i++) {
      nn[i] *= dx;
    }

    fclose(fp);
  }else{
    print_error("could not open c-infile");
  }
}

void write_popdens() {
  int icount=0,dcount=0;
  int i;
  FILE *fpc;

  fpc=fopen(c_outfile,"wb");
  if(fpc != NULL) {
    fwrite(&icount,sizeof(int),1,fpc);
    fwrite(&dcount,sizeof(int),1,fpc);
    fwrite(&dx,sizeof(double),1,fpc);
    fwrite(&space,sizeof(int),1,fpc);
    fwrite(&space0,sizeof(int),1,fpc);
    for(i=0;i<space;i++)nn[i] /= dx;
    fwrite(nn,sizeof(double),space,fpc);
    fclose(fpc);
  }else{
    print_error("could not open c-outfile");
  }
}

void read_constraint(int importparameters) {
  int icount,dcount;
  int *ival;
  double *dval;
  FILE *fp;

  fp = fopen(u_infile,"rb");
  if(fp!=NULL) {
    fread(&icount,sizeof(int),1,fp);
    fread(&dcount,sizeof(int),1,fp);
    fread(&u_dx,sizeof(double),1,fp);
    fread(&u_space,sizeof(int),1,fp);
    fread(&u_space0,sizeof(int),1,fp);

    if(icount>0) {
      ival = (int*)malloc(icount*sizeof(int));
      fread(ival,sizeof(int),icount,fp);
      free(ival);
    }

    if(dcount>=3) {
      dval = (double*)malloc(dcount*sizeof(int));
      fread(dval,sizeof(double),dcount,fp);
      mutationrate = dval[0];
      wavespeed = dval[1];
      mutation_sigma = dval[2];
      free(dval);
    }else{
      print_error("not enough values in constraint file! need at least 3 double parameters: mutationrate, wavespeed, mutationsigma!");
    }

    if(importparameters) {
      space = u_space/dens_ustar_latticeratio;
      space0 = u_space0/dens_ustar_latticeratio;
      dx = u_dx*dens_ustar_latticeratio;
    }else{
      if((space*dens_ustar_latticeratio != u_space)||(space0*dens_ustar_latticeratio != u_space0))print_error("lattice does not match! u");
    }

    u_read = (double*)malloc(u_space*sizeof(double));
    fread(u_read,sizeof(double),u_space,fp);

    fclose(fp);
  }else{
    print_error("could not open ufile");
  }

  u = (double*)malloc(space*sizeof(double));

}

void flat_constraint(double size) {
  int i;
  u=(double*)malloc(space*sizeof(double));
  for(i=0;i<space;i++)u[i] = 1./size;
}

void initialize_with_gaussian_popdens() {
  int i;
  double startvariance;
  if((noise == 2)||(noise == 4)) {
    startvariance = wavespeed;
  }else{
    // Good et al., PNAS (2012)
    startvariance = mutation_sigma*mutation_sigma*2*log(mutation_sigma*populationsize)/(log(mutation_sigma/mutationrate)*log(mutation_sigma/mutationrate));
    // behaves weird for extremal parameter values.
    // catch those cases and set on standard value: density takes half of the box
    if(startvariance < 0) {
      startvariance = (0.25*space*space*dx*dx);
    }
  }
  for(i=0;i<space;i++) {
    nn[i] = exp(-(i-space0)*(i-space0)*dx*dx/(2.*startvariance));
  }
}

void print_u(int time) {
  int i;
  for(i=0;i<space;i++) {
    fprintf(stderr,"%10.6lf %10.6lf %16.10e\n",time*epsilon,(i-space0+allshifts)*dx,u[i]);
  }
  fprintf(stderr,"\n");
}

// ************************************************************
// **   timetrace
// ************************************************************

void read_timetrace() {
  int i;
  timetrace_length = (int)(maxSteps/timetrace_ps_interval)+1;
  FILE *fp;
  timetrace_popsize = (double*)malloc(timetrace_length*sizeof(double));
  fp = fopen(timetrace_filename,"r");
  if(fp != NULL) {
    for(i=0;i<timetrace_length;i++) {
      fscanf(fp,"%lf",&timetrace_popsize[i]);
    }
  }else{
    print_error("could not open timetrace file");
  }
  fclose(fp);
}

double get_timetrace_popsize(int step) {
  int i;
  if(step%timetrace_ps_interval == 0) {
    return timetrace_popsize[step/timetrace_ps_interval];
  }else{
    int lasttimepoint = (int)floor(step/timetrace_ps_interval);
    int nexttimepoint = lasttimepoint + 1;
    double fractimepoint = (1.*(step%timetrace_ps_interval))/(1.*timetrace_ps_interval);
    return fractimepoint*timetrace_popsize[nexttimepoint] + (1-fractimepoint)*timetrace_popsize[lasttimepoint];
  }
}

// ************************************************************
// **   screen output
// ************************************************************

void print_populationdensity(int timestep) {
  int i;
  double corr = allshifts*dx;
  if(correctformeanfitness)corr += current_mean_fitness;
  for(i=0;i<space;i++) {
    fprintf(stderr,"%lf %14.10lf %20.10e",timestep*epsilon,(i-space0)*dx+corr,nn[i]);
    if(printfixationprob)fprintf(stderr," %20.10e",u[i]);
    fprintf(stderr,"\n");

    if(printhistotype) {
      fprintf(stderr,"%lf %14.10lf %20.10e",timestep*epsilon,(i-space0+1)*dx+corr,nn[i]);
      if(printfixationprob)fprintf(stderr," %20.10e",u[i]);
      fprintf(stderr,"\n");
    }
  }
  fprintf(stderr,"\n");
}

// ************************************************************
// **   average popdens
// ************************************************************

void init_averagepopdens() {

  averagepopdens_dx = dx/(1.*averagepopdens_resolution);
  averagepopdens_space = space*averagepopdens_resolution;
  averagepopdens_space0 = space0*averagepopdens_resolution;

  averagepopdens_dens = (double*)calloc(averagepopdens_space,sizeof(double));

  averagepopdens_lower = (int)(-averagepopdens_resolution/2);
  averagepopdens_higher = (int)(averagepopdens_resolution/2);
  if(averagepopdens_higher - averagepopdens_lower < averagepopdens_resolution)averagepopdens_higher++;

}

void update_averagepopdens() {
  int i,j,idx;
  int offset=0;

  if(averagepopdens_center) {
    offset = (int)(current_mean_fitness/dx*averagepopdens_resolution);
  }

  for(i=0;i<space;i++) {
    for(j=averagepopdens_lower;j<averagepopdens_higher;j++) {
      idx = i*averagepopdens_resolution+j-offset;
      if((averagepopdens_space > idx) &&( idx >= 0))
	averagepopdens_dens[idx] += nn[i];
    }
  }

  averagepopdens_count += 1.;

}

void write_averagepopdens() {
  int i;
  FILE *fp;

  if(averagepopdens_havefile == 1) {
    fp = fopen(averagepopdens_outputfile,"w");
  }else{
    fp = stdout;
  }

  for(i=0;i<averagepopdens_space;i++) {
    fprintf(fp,"%lf %e\n",(i-averagepopdens_space0)*averagepopdens_dx,averagepopdens_dens[i]/averagepopdens_count);
  }

  if(averagepopdens_havefile == 1) {
    fclose(fp);
  }

  free(averagepopdens_dens);
}

// ************************************************************
// **   main algorithm
// ************************************************************

void update_u(int timestep) {
   // bug if shiftthreshold -T >1, use shiftthreshold only with fixedN
  int i,j;
  int baseshift;
  double fracshift;

  baseshift = (int)floor(current_mean_fitness/u_dx);
  fracshift = current_mean_fitness/u_dx - 1.*baseshift;
  i = 0;
  for(j=0;j<u_space;j++) {
    if ( j + baseshift + fracshift > (i+.5)*dens_ustar_latticeratio) {
      u[i] += (1.-fracshift)*u_read[j];
      u[i] /= (1.*dens_ustar_latticeratio);
      i++;
      if(i>=space)break;
      u[i] = fracshift*u_read[j];
    }else{
      u[i] += u_read[j];
    }
  }
}

void shift_population_backward(int step) {
  int i;
  for(i=0;i<space-step;i++)     nn[i] = nn[i+step];
  for(i=space-step;i<space;i++) nn[i] = 0.;
}

void shift_population_forward(int step) {
  int i;
  for(i=space-1;i>-step;i--) nn[i] = nn[i+step];
  for(i=-step;i>=0;i--)      nn[i] = 0.;
}

void shift_population(int timestep) {
  int shift = (int)floor(current_mean_fitness/dx);
  if(shift >= shiftthreshold) {shift_population_backward(shift);}
  if(shift <= -shiftthreshold) {shift_population_forward(-shift);}
  allshifts += shift;
  current_mean_fitness -= shift*dx;
}

void reproduce(int timestep) {
  int i,j;
  double tmpn;
  long int poiss;

  if((noise == 2)||(noise==4)) {
    current_mean_fitness = timestep*wavespeed*epsilon - allshifts*dx;
  }	// else:	current_mean_fitness was already updated at end of last timestep
	//		with function "populationconstraint" below and is still at the correct value

  if((current_mean_fitness/dx >= shiftthreshold) || (current_mean_fitness/dx <= -shiftthreshold)) {
    shift_population(timestep);
  }

  tmp[0] = 0.;
  tmp[space-1] = 0.;
  for(i=1;i<space-1;i++) {
    tmpn = nn[i]*(1.+(x[i]-current_mean_fitness)*epsilon-mutation_outflow);
    for(j = 1;j<i;j++) tmpn += mutation_inflow[j]*nn[i-j];
    if(tmpn<0) {
      tmpn = 0.;
    }else if((noise > 0)&&(noise != 4)) {
      if(tmpn < 1.e9) {	//	Poisson-RNG breaks down for parameters > 1e9. see GSL doc.
			//	use smaller bins if this occurs too often or population size too large
	tmp[i] = tmpn + twoepssqrt*(gsl_ran_poisson(rg,tmpn)-tmpn);
      }else{		//	neglect noise term if occupancy is too large,
	tmp[i] = tmpn;	//	dynamics can be assumed to be almost deterministic then
      }
    }else{
      tmp[i] = tmpn;
    }
  }

  memcpy(&nn[1],&tmp[1],(space-2)*sizeof(double));

}

double populationconstraint(int timestep) {
  int i;
  double constraint = 0., inv, sumxg = 0., sumxxg = 0.;

  double popdens_0thmom = 0.;
  double popdens_1stmom = 0.;
  double popdens_2ndmom = 0.;
  double cmax = 0.;
  int havenose = 0;
  if((noise == 2)||(noise == 4)) {
    update_u(timestep);
  }

  for(i=space-1;i>=0;i--) {
    if (nn[i] > cmax) {
      cmax=nn[i];
      xcmax = x[i];
    }
    if((havenose==0)&&(nn[i]>1)) {
      havenose = 1;
      xnose = x[i];
    }
    constraint += nn[i]*u[i];
    sumxg += x[i] * nn[i] * u[i];
    sumxxg += x[i] * x[i] * nn[i] * u[i];
    popdens_0thmom += nn[i];
    popdens_1stmom += x[i]*nn[i];
    popdens_2ndmom += x[i]*x[i]*nn[i];
  }

  current_mean_fitness = popdens_1stmom/popdens_0thmom;
  populationvariance = popdens_2ndmom/popdens_0thmom - current_mean_fitness*current_mean_fitness;
  populationsize = popdens_0thmom;
  xg = sumxg/constraint;
  varg = sumxxg/constraint - xg*xg;

  inv = 1./constraint;
  if(noise==3) {
    inv *= get_timetrace_popsize(timestep);
  }
  for(i=0;i<space;i++) nn[i] *= inv;
  populationsize *= inv;

  return inv;
}

// ************************************************************
// **   initialization
// ************************************************************

int initialize() {
  int i;

  if(read_from_file) {
    read_popdens(1);
    if(noise == 0)flat_constraint(1.);
    if(noise == 1)flat_constraint(populationsize);
    if((noise == 2)||(noise == 4)) {
      read_constraint(0);
      update_u(0);
    }
    if(noise == 3) {
      flat_constraint(1.);
      read_timetrace();
    }
  }else{
    if(noise == 0)flat_constraint(1.);
    if(noise == 1)flat_constraint(populationsize);
    if((noise == 2) || (noise == 4)) {
      read_constraint(1);
      update_u(0);
    }
    if(noise == 3) {
      flat_constraint(1.);
      read_timetrace();
    }
    nn = (double*)malloc(space*sizeof(double));
    initialize_with_gaussian_popdens();
  }
  tmp = (double*)malloc(space*sizeof(double));

  gsl_rng_env_setup();
  T = gsl_rng_default;
  rg = gsl_rng_alloc(T);
  gsl_rng_set(rg, randseed);

  twoepssqrt = sqrt(2.*epsilon);
  speedprefactor = wavespeed/dx*epsilon;

  x = (double*)malloc(space*sizeof(double));
  mutation_inflow = (double*)malloc(space*sizeof(double));
  mutation_outflow = 0.;
  mutation_inflow[0] = 0.;
  for(i=1;i<space;i++) {
    x[i] = (i-space0)*dx;
    mutation_inflow[i] = epsilon*mutationrate*exp(-i*dx/mutation_sigma)*(1.-exp(-dx/mutation_sigma));
    mutation_outflow += mutation_inflow[i];
  }

  if(quiet<2) {
    printf("#################################################################################\n");
    printf("# stochastic simulation of adapting population with exponential mutation kernel #\n");
    printf("#################################################################################\n");
    printf("#    mutationrate     = %e\n",mutationrate);
    printf("#    mutation_sigma   = %e\n",mutation_sigma);
    if(noise==0) {
      printf("#    constraint       = deterministic\n");
    }
    if(noise==1) {
      printf("#    constraint       = fixedN\n");
      printf("#    populationsize   = %e\n",populationsize);
    }
    if(noise==2) {
      printf("#    constraint       = ustar\n");
      printf("#    ufile            = %s\n",u_infile);
      printf("#    wavespeed        = %e\n",wavespeed);
    }
    if(noise==3) {
      printf("#    constraint       = fixedN, timetrace\n");
      printf("#    timetracefile    = %s\n",timetrace_filename);
    }
    if(noise==4) {
      printf("#    constraint       = ustar, non-stochastic\n");
      printf("#    ufile            = %s\n",u_infile);
      printf("#    wavespeed        = %e\n",wavespeed);
    }
    printf("#    (lattice) space  = %d\n",space);
    printf("#    (lattice) space0 = %d\n",space0);
    printf("#    (lattice) dx     = %e\n",dx);
    printf("#    randseed         = %d\n",randseed);
  }

}

// ************************************************************
// **   cleanup
// ************************************************************

void cleanup() {
  free(nn);
  free(tmp);
  free(x);
  free(mutation_inflow);
  free(u);
  if((noise == 2)||(noise == 4)) {
    free(u_read);
  }
  if(noise == 3) {
    free(timetrace_popsize);
  }
  gsl_rng_free(rg);
}

// ************************************************************
// **   main
// ************************************************************

int main(int argn, char *argv[]) {
  int i,j;
  double v,lambda = 1.;

  parsecomamndline(argn,argv);
  initialize();
  lambda = populationconstraint(0);
  if (quiet<2) fprintf(stdout,"%10.3lf %20.10e %.10e %.10e %.10e %.10e %.10e %.10e %.10e\n",0.0,current_mean_fitness,populationvariance,populationsize,lambda,xg,varg,allshifts*dx+xcmax,allshifts*dx+xnose);
  if(averagepopdens) {
    init_averagepopdens();
  }
  for(i=1;i<=maxSteps;i++) {
    reproduce(i);
    lambda = populationconstraint(i);

    if(i%outputstep == 0) {
      if (quiet<2)fprintf(stdout,"%10.3lf %20.10e %.10e %.10e %.10e %.10e %.10e %.10e %.10e\n",i*epsilon,allshifts*dx + current_mean_fitness,populationvariance,populationsize,lambda,xg,varg,allshifts*dx+xcmax,allshifts*dx+xnose);
      if (quiet==0) print_populationdensity(i);
      if (averagepopdens) update_averagepopdens();
    }
  }

  if(write_to_file)write_popdens();
  if(averagepopdens)write_averagepopdens();
  cleanup();
  return 0;
}


static char help[] =
"DMPlex mesh test suggested by Matt Knepley on petsc-users 11/9/21.\n"
"Shows DMPlex is more capable at the command line than I thought.\n\n";

/*
running with the -dm_view hdf5 seems to require a library I don't have:
mpiexec -n 2 ./meshtest -dm_plex_shape sphere -dm_refine_pre 3 -dm_extrude 5 -dm_plex_transform_extrude_thickness 0.1 -dm_distribute -dm_view hdf5:mesh.h5

this runs without complaint:
mpiexec -n 2 ./meshtest -dm_plex_shape sphere -dm_refine_pre 3 -dm_extrude 5 -dm_plex_transform_extrude_thickness 0.1 -dm_distribute -dm_view
*/

#include <petsc.h>

int main(int argc, char **argv)
{
  DM             dm;
  PetscErrorCode ierr;

  ierr = PetscInitialize(&argc, &argv, NULL, help);  if (ierr) return ierr;
  ierr = DMCreate(PETSC_COMM_WORLD, &dm); CHKERRQ(ierr);
  ierr = DMSetType(dm, DMPLEX); CHKERRQ(ierr);
  ierr = DMSetFromOptions(dm); CHKERRQ(ierr);
  ierr = DMViewFromOptions(dm, NULL, "-dm_view"); CHKERRQ(ierr);
  ierr = DMDestroy(&dm);CHKERRQ(ierr);
  return PetscFinalize();
}


/*
 Copyright (C) 2006-2007 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include "util.h"
#include "xc.h"

void func(double *x, int n, void *ex)
{
  int i;
  for(i=0; i<n;i++)
    x[i] = cos(x[i]);
}

void test_integration()
{
  double a, b, result;

  for(b=1e-8; b<5; b+=0.001){
    result = xc_integrate(func, NULL, a, b);
    printf("%lf %lf\n", b, result);
  }
}

#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_sf_bessel.h>

void test_expi()
{
  double b, r1, r2, r3, r4;
  int n;

  n = 1;
  for(b=1e-3; b<50; b+=0.01){
    r1 = xc_bessel_K0(b);
    //r2 = gsl_sf_bessel_K0(b);
    //r3 = bessi1(b);
    printf("%5.3lf %12.10lf %12.10lf %12.10lf\n", b, r1, r2, r3);
  }
}

void test_lda()
{
  xc_func_type l1, l2, l3;
  int i;

  //xc_func_init(&l1, XC_LDA_XC_KSDT2, XC_POLARIZED);
  //xc_func_init(&l1, XC_LDA_C_PW, XC_UNPOLARIZED);
  xc_func_init(&l3, XC_LDA_X, XC_UNPOLARIZED);

  //xc_lda_xc_ksdt_set_params(&l1, 1000.0);
  //xc_lda_c_1d_csc_set_params(&l2, 1, 1.0);

  for(i=0; i<=1000; i++){
    double dens, rs, zeta, rho[2];
    double ec1, vc1[2], fxc1[3], kxc1[4];
    double ec2, vc2[2], fxc2[3], kxc2[4];
    double ec3, vc3[2], fxc3[3], kxc3[4];

    //rs   = 0.5 + i/500.0;
    //zeta = -1.0 + 2.0*i/1000000000.0;

    //dens = 1.0/(4.0/3.0*M_PI*pow(rs,3)); /* 3D */
    //dens = 1.0/(2.0*rs); /* 1D */

    //rho[0] = dens*(1.0 + zeta)/2.0;
    //rho[1] = dens*(1.0 - zeta)/2.0;

    rho[0] = 0.05 + i/1000.0;
    rho[1] = 0.001;

    //rho[0] = 1.0/(2.0*rs);
    //rho[1] = 0.0;

    //dens = rho[0] + rho[1];

    //xc_lda_exc_vxc_fxc(&l1, 1, rho, &ec1, vc1, NULL, NULL);
    //xc_lda(&l2, 1, rho, &ec2, vc2, NULL, NULL);
    //xc_lda_fxc_fd(&l2, rho, fxc2);
    //xc_lda_kxc_fd(&l2, rho, kxc2);

    //rho[0] = dens; rho[1] = 0.0;
    //xc_lda(&l3, rho, &ec3, vc3, fxc3, kxc3);

    // printf("%e\t%e\t%e\n", dens, (fxc1[0]+2.0*fxc1[1]+fxc1[2])/4.0, fxc3[0]);
    // printf("%e\t%e\t%e\n", dens, (kxc1[0]+3.0*kxc1[1]+3.0*kxc1[2]+kxc1[3])/8.0, kxc3[0]);

    printf("%e\t%e\t%e\n", rho[0], (rho[0] + rho[1])*ec1, vc1[0]);
  }
}

void test_ak13()
{
  xc_func_type gga;
  double beta = 0.13;
  double x, rho[2] = {0.0, 0.0}, sigma[3] = {0.0, 0.0, 0.0}, zk, vrho[2], vsigma[3];
  double tmp1, tmp2;
  int i;

  xc_func_init(&gga,  XC_GGA_X_AK13,  XC_POLARIZED);

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

    x = 500.0*i/(10000.0);
    rho[0]   = 0.12*exp(-beta * x);
    sigma[0] = 0.12*0.12*beta*beta * rho[0]*rho[0];

    xc_gga_exc_vxc(&gga,  1, rho, sigma, &zk, vrho, vsigma);

    tmp2 = 1.74959015598863046792081721182*beta*x/3.0- 1.62613336586517367779736042170*log(x);
    fprintf(stderr, "%16.10lf\t%16.10lf\t%16.10lf\t%16.10lf\n", x, vrho[0], vsigma[0]*sqrt(sigma[0]),
	    -X_FACTOR_C*X2S*tmp2/2.0);
  }

}

void test_enhance()
{
  double x, f, dfdx, d2fdx2, d3fdx3;

  xc_func_type gga1, gga2, gga3, gga4;

  xc_func_init(&gga1,  XC_GGA_X_B88, XC_POLARIZED);
  xc_func_init(&gga2, XC_GGA_X_GG99, XC_POLARIZED);

  for(x=0.01; x<200; x+=0.01){
    //printf("%le", x);
    //xc_gga_x_b88_enhance(&gga1, 1, x, &f, &dfdx, &d2fdx2, &d3fdx3);
    //printf("\t%le", -f*X_FACTOR_C);
    //xc_gga_x_gg99_enhance(&gga2, 1, x, &f, &dfdx, &d2fdx2, &d3fdx3);
    printf("%le\t%le\t%le\n", x, f, dfdx);
  }
}

void test_gga()
{
  xc_func_type gga, ggap;
  int i, npoints = 1;
  double *rho, *sigma;
  double *zk, zkp, *vrho, vrhop[2], *vsigma, vsigmap[3];
  double *v2rho2, *v2rhosigma, *v2sigma2;
  double *v3rho3, *v3rho2sigma, *v3rhosigma2, *v3sigma3;

  rho         = libxc_malloc( 2*npoints*sizeof(double));
  sigma       = libxc_malloc( 3*npoints*sizeof(double));
  zk          = libxc_malloc( 1*npoints*sizeof(double));
  vrho        = libxc_malloc( 2*npoints*sizeof(double));
  vsigma      = libxc_malloc( 3*npoints*sizeof(double));
  v2rho2      = libxc_malloc( 3*npoints*sizeof(double));
  v2rhosigma  = libxc_malloc( 6*npoints*sizeof(double));
  v2sigma2    = libxc_malloc( 6*npoints*sizeof(double));
  v3rho3      = libxc_malloc( 4*npoints*sizeof(double));
  v3rho2sigma = libxc_malloc( 9*npoints*sizeof(double));
  v3rhosigma2 = libxc_malloc(12*npoints*sizeof(double));
  v3sigma3    = libxc_malloc(10*npoints*sizeof(double));

  xc_func_init(&gga,  XC_GGA_C_PW91,  XC_POLARIZED);

  /*
  for(i=1; i<=10000; i++){
    double x = 30.0*i/(10000.0), f, df, d2f, d3f;
    double c2 = 10.0/81.0;

    xc_gga_x_b88_enhance(&gga, 3, x, &f, &df, &d2f, &d3f);
    printf("%20.14e %20.14e", X2S*X2S*x*x, (f-1.0)/(c2*X2S*X2S*x*x));
    xc_gga_x_ev93_enhance(&gga, 3, x, &f, &df, &d2f, &d3f);
    printf(" %20.14e\n", (f-1.0)/(c2*X2S*X2S*x*x));
  }
  exit(0);
  */

   for(i=0; i<=10000; i++){
     rho[0]   = .01;
     rho[1]   = 0.2;
     sigma[0] = 0.1;
     sigma[1] = 0.00002;
     sigma[2] = 0.5 + i/1000.0;

     //xc_gga(&gga, 1, rho, sigma, zk, vrho, vsigma, v2rho2, v2rhosigma, v2sigma2, NULL, v3rho2sigma, v3rhosigma2, v3sigma3);

     fprintf(stderr, "%16.10le\t%16.10le\t%16.10le\n", sigma[2], vsigma[2], v2sigma2[5]);
   }

  /*
  for(i=0; i<npoints; i++){
    rho[2*i + 0]   = 0.048 + i/10000.0;
    rho[2*i + 1]   = 0.025;
    sigma[3*i + 0] = 0.0046;
    sigma[3*i + 1] = 0.0044;
    sigma[3*i + 2] = 0.0041;
  }

  xc_gga(&gga,  npoints, rho, sigma, zk,  vrho,  vsigma,  v2rho2,  v2rhosigma,  v2sigma2, v3rho3, v3rho2sigma, v3rhosigma2, v3sigma3);

  for(i=0; i<npoints; i++){
    fprintf(stderr, "%16.10lf\t%16.10lf\t%16.10lf\n", rho[2*i + 0], vrho[2*i + 0], v2rho2[3*i + 0]);
  }
  */

  xc_func_end(&gga);

  libxc_free(rho); libxc_free(sigma);
  libxc_free(zk); libxc_free(vrho); libxc_free(vsigma);
  libxc_free(v2rho2); libxc_free(v2rhosigma); libxc_free(v2sigma2);
  libxc_free(v3rho3); libxc_free(v3rho2sigma); libxc_free(v3rhosigma2); libxc_free(v3sigma3);
}

void test_mgga()
{
  xc_func_type mgga1, mgga2;
  int i;

  xc_func_init(&mgga1, XC_GGA_C_LYP, XC_POLARIZED);
  xc_func_init(&mgga2, XC_MGGA_X_SCAN, XC_POLARIZED);
  //xc_mgga_c_tpss_init(tpss2.mgga);

  for(i=0; i<=1000; i++){
    double rho[2], sigma[3], tau[2], lapl[2];
    double zk,   vrho[2],  vsigma[3],  vtau[2],  vlapl[2];
    double zk2, vrho2[2], vsigma2[3], vtau2[2], vlapl2[2];
    double v2rho2[3], v2sigma2[6], v2lapl2[3], v2tau2[3];
    double v2rhosigma[6], v2rholapl[3], v2rhotau[3];
    double v2sigmalapl[6], v2sigmatau[6], v2lapltau[3];

    rho[0]   = 0.3;
    rho[1]   = 0.4;
    sigma[0] = 0.2032882206468622;
    sigma[1] = 0.11;
    sigma[2] = 0.7 + i/10.0;
    tau[0]   = 1.0;
    tau[1]   = 0.15;
    lapl[0]  = -0.1518421131246519;
    lapl[1]  = 0.12;

    //xc_mgga(&mgga1, 1, rho, sigma, lapl, tau,
    //	     &zk,  vrho, vsigma, vlapl, vtau,
    //	     v2rho2, v2sigma2, v2lapl2, v2tau2, v2rhosigma, v2rholapl, v2rhotau,
    //	     v2sigmalapl, v2sigmatau, v2lapltau);
    //xc_mgga(&mgga2, 1, rho, sigma, lapl, tau,
    //	     &zk2,  vrho2, vsigma2, vlapl2, vtau2,
    // 	     NULL, v2sigma2, v2lapl2, v2tau2, v2rhosigma, v2rholapl, v2rhotau,
    //	     v2sigmalapl, v2sigmatau, v2lapltau);
    //xc_mgga_exc(&mgga2, 1, rho, sigma, lapl, tau,
    //		 &zk2);
    xc_gga_exc_vxc(&mgga1, 1, rho, sigma,
		     &zk,  vrho, vsigma);
    xc_mgga_exc_vxc(&mgga2, 1, rho, sigma, lapl, tau,
		     &zk2,  vrho2, vsigma2, vlapl2, vtau2);

    fprintf(stderr, "%16.10lf\t%16.10lf\t%16.10lf\n", sigma[2], zk2*(rho[0]+rho[1]), vsigma2[2]);
  }

  xc_func_end(&mgga1);
  xc_func_end(&mgga2);
}

void test_neg_rho()
{
  xc_func_type func;
  double rho[5][2] = {
    {9.03897273e-06, -1.00463992e-06},
    {8.48383564e-06, -3.51231267e-07},
    {1.45740621e-08, -2.94546705e-09},
    {2.62778445e-07, -1.00191745e-07},
    {2.55745103e-06, -1.54789964e-06}
  };
  double sigma[5][3] = {
    {1.20122271e-08, 4.83240746e-09, 6.24774836e-09},
    {1.54146602e-07, 1.41584609e-07, 1.36663204e-07},
    {2.75312438e-08, 2.75224049e-08, 2.75135719e-08},
    {1.90251649e-07, 1.91241798e-07, 1.92240989e-07},
    {9.29562712e-09, 7.83940082e-09, 8.05714636e-09}
  };
  double vsigma[5][3];
  double zk[5], vk[5][2];
  int i, func_id;

  for(func_id=1; func_id<1000; func_id++){
    if(xc_func_init(&func, func_id, XC_POLARIZED) != 0) continue;
    if(func_id == XC_LDA_C_2D_PRM || func_id == XC_GGA_X_LB) goto end;

    printf("\n%s:\n", func.info->name);

    switch(func.info->family){
    case XC_FAMILY_LDA:
      xc_lda_exc_vxc(&func, 5, &rho[0][0], zk, &vk[0][0]);
      break;
    case XC_FAMILY_GGA:
      xc_gga_exc_vxc(&func, 5, &rho[0][0], &sigma[0][0], zk, &vk[0][0], &vsigma[0][0]);
      break;
    }

    switch(func.info->family){
    case XC_FAMILY_LDA:
      for(i=0; i<5; i+=1)
        printf("%.8e %.8e %.8e %.8e %.8e\n",
               rho[i][0], rho[i][1], zk[i], vk[i][0], vk[i][1]);
      break;
    case XC_FAMILY_GGA:
      for(i=0; i<5; i+=1)
        printf("%.8e %.8e %.8e %.8e %.8e %.8e %.8e %.8e %.8e %.8e %.8e\n",
               rho[i][0], rho[i][1], sigma[i][0], sigma[i][1], sigma[i][2],
               zk[i], vk[i][0], vk[i][1], vsigma[i][0], vsigma[i][1], vsigma[i][2]);
      break;
    }

  end:
    xc_func_end(&func);
  }
}

double xc_mgga_x_mbrxc_get_x(double Q);
double xc_mgga_x_br89_get_x(double Q);
void test_mbrxc()
{
  double Q, x, rhs, Q2;

  for(Q=-10.1e4; Q<10.1e4; Q+=.01e4){
  printf("Q = %lf ", Q);
  fflush(stdout);

  //x = xc_mgga_x_mbrxc_get_x(Q);
  //rhs = pow(1.0 + x, 5.0/3.0)*exp(-2.0*x/3.0)/(x - 3.0);
  //Q2 = pow(32.0*M_PI, 2.0/3.0)/(6.0*rhs);

  x = xc_mgga_x_br89_get_x(Q);
  rhs = x*exp(-2.0*x/3.0)/(x - 2.0);
  Q2 = 2.0/3.0*pow(M_PI, 2.0/3.0)/rhs;
  printf("Q2 = %lf x = %lf\n", Q2, x);
  }
}

int main()
{
  //test_expi();
  //test_integration();
  //test_neg_rho();

  //test_lda();
  //test_enhance();
  //test_gga();
  //test_ak13();
  //test_mgga();
  test_mbrxc();

  //printf("number = '%d'; key = '%s'", 25, xc_functional_get_name(25));

  return 0;
}