/*
  NAME:
     proj_EM_step
  PURPOSE:
     one proj_EM step
  CALLING SEQUENCE:
     proj_EM_step(struct datapoint * data, int N, struct gaussian * gaussians,
     int K,bool * fixamp, bool * fixmean, bool * fixcovar,
     double * avgloglikedata, bool likeonly, double w, bool noproj,
     bool diagerrs, bool noweight)
  INPUT:
     data         - the data
     N            - number of data points
     gaussians    - model gaussians
     K            - number of model gaussians
     fixamp       - fix the amplitude?
     fixmean      - fix the mean?
     fixcovar     - fix the covar?
     likeonly     - only compute likelihood?
     w            - regularization parameter
     noproj       - don't perform any projections
     diagerrs     - the data->SS errors-squared are diagonal
     noweight     - don't use data-weights
  OUTPUT:
     avgloglikedata - average loglikelihood of the data
  REVISION HISTORY:
     2008-09-21 - Written Bovy
     2010-03-01 Added noproj option - Bovy
     2010-04-01 Added noweight option - Bovy
*/
#ifdef _OPENMP
#include <omp.h>
#endif

#include <math.h>
//#include <time.h>
//#include <sys/time.h>
#include <float.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <proj_gauss_mixtures.h>

#define CHUNKSIZE 1

void proj_EM_step(struct datapoint * data, int N,
		  struct gaussian * gaussians, int K,bool * fixamp,
		  bool * fixmean, bool * fixcovar, double * avgloglikedata,
		  bool likeonly, double w, bool noproj, bool diagerrs,
		  bool noweight){
  *avgloglikedata = 0.0;
  //struct timeval start,time1, time2, time3, time4, time5,end;
  struct datapoint * thisdata;
  struct gaussian * thisgaussian;
  struct gaussian * thisnewgaussian;
  int signum,di;
  double exponent;
  double currqij;
  struct modelbs * thisbs;
  int d = (gaussians->VV)->size1;//dim of mm

  //gettimeofday(&start,NULL);
  //Initialize new parameters
  int kk;
  for (kk=0; kk != K*nthreads; ++kk){
    newgaussians->alpha = 0.0;
    gsl_vector_set_zero(newgaussians->mm);
    gsl_matrix_set_zero(newgaussians->VV);
    ++newgaussians;
  }
  newgaussians= startnewgaussians;

  //gettimeofday(&time1,NULL);
  //check whether for some Gaussians none of the parameters get updated
  double sumfixedamps= 0;
  bool * allfixed = (bool *) calloc(K, sizeof (bool) );
  double ampnorm;
  for (kk=0; kk != K; ++kk){
    if (*fixamp == true){
      sumfixedamps += gaussians->alpha;
    }
    ++gaussians;
    if (*fixamp == true && *fixmean == true && *fixcovar == true)
      *allfixed= true;
    ++allfixed;
    ++fixamp;
    ++fixmean;
    ++fixcovar;
  }
  gaussians -= K;
  allfixed -= K;
  fixamp -= K;
  fixmean -= K;
  fixcovar -= K;

  //gettimeofday(&time2,NULL);

  //now loop over data and gaussians to update the model parameters
  int ii, jj, ll;
  double sumSV;
  int chunk;
  chunk= CHUNKSIZE;
#pragma omp parallel for schedule(static,chunk) \
  private(tid,di,signum,exponent,ii,jj,ll,kk,Tij,Tij_inv,wminusRm,p,VRTTinv,sumSV,VRT,TinvwminusRm,Rtrans,thisgaussian,thisdata,thisbs,thisnewgaussian,currqij) \
  shared(newgaussians,gaussians,bs,allfixed,K,d,data,avgloglikedata)
  for (ii = 0 ; ii < N; ++ii){
    thisdata= data+ii;
#ifdef _OPENMP
    tid= omp_get_thread_num();
#else
    tid = 0;
#endif
    di = (thisdata->SS)->size1;
    //printf("Datapoint has dimension %i\n",di);
    p = gsl_permutation_alloc (di);
    wminusRm = gsl_vector_alloc (di);
    TinvwminusRm = gsl_vector_alloc (di);
    Tij = gsl_matrix_alloc(di,di);
    Tij_inv = gsl_matrix_alloc(di,di);
    if ( ! noproj ) VRT = gsl_matrix_alloc(d,di);
    VRTTinv = gsl_matrix_alloc(d,di);
    if ( ! noproj ) Rtrans = gsl_matrix_alloc(d,di);
    for (jj = 0; jj != K; ++jj){
      //printf("%i,%i\n",(thisdata->ww)->size,wminusRm->size);
      gsl_vector_memcpy(wminusRm,thisdata->ww);
      //fprintf(stdout,"Where is the seg fault?\n");
      thisgaussian= gaussians+jj;
      //prepare...
      if ( ! noproj ) {
	if ( diagerrs ) {
	  gsl_matrix_set_zero(Tij);
	  for (ll = 0; ll != di; ++ll)
	    gsl_matrix_set(Tij,ll,ll,gsl_matrix_get(thisdata->SS,ll,0));}
	else
	  gsl_matrix_memcpy(Tij,thisdata->SS);
      }
      //Calculate Tij
      if ( ! noproj ) {
	gsl_matrix_transpose_memcpy(Rtrans,thisdata->RR);
	gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,thisgaussian->VV,Rtrans,0.0,VRT);//Only the upper right part of VV is calculated --> use only that part
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,thisdata->RR,VRT,1.0,Tij);}//This is Tij
      else {
	if ( diagerrs ) {
	  for (kk = 0; kk != d; ++kk){
	    gsl_matrix_set(Tij,kk,kk,gsl_matrix_get(thisdata->SS,kk,0)+gsl_matrix_get(thisgaussian->VV,kk,kk));
	    for (ll = kk+1; ll != d; ++ll){
	      sumSV= gsl_matrix_get(thisgaussian->VV,kk,ll);
	      gsl_matrix_set(Tij,kk,ll,sumSV);
	      gsl_matrix_set(Tij,ll,kk,sumSV);}}}
	else {
	  for (kk = 0; kk != d; ++kk){
	    gsl_matrix_set(Tij,kk,kk,gsl_matrix_get(thisdata->SS,kk,kk)+gsl_matrix_get(thisgaussian->VV,kk,kk));
	    for (ll = kk+1; ll != d; ++ll){
	      sumSV= gsl_matrix_get(thisdata->SS,kk,ll)+gsl_matrix_get(thisgaussian->VV,kk,ll);
	      gsl_matrix_set(Tij,kk,ll,sumSV);
	      gsl_matrix_set(Tij,ll,kk,sumSV);}}}}
      //gsl_matrix_add(Tij,thisgaussian->VV);}
      //Calculate LU decomp of Tij and Tij inverse
      gsl_linalg_LU_decomp(Tij,p,&signum);
      gsl_linalg_LU_invert(Tij,p,Tij_inv);
      //Calculate Tijinv*(w-Rm)
      if ( ! noproj ) gsl_blas_dgemv(CblasNoTrans,-1.0,thisdata->RR,thisgaussian->mm,1.0,wminusRm);
      else gsl_vector_sub(wminusRm,thisgaussian->mm);
      //printf("wminusRm = %f\t%f\n",gsl_vector_get(wminusRm,0),gsl_vector_get(wminusRm,1));
      gsl_blas_dsymv(CblasUpper,1.0,Tij_inv,wminusRm,0.0,TinvwminusRm);
      //printf("TinvwminusRm = %f\t%f\n",gsl_vector_get(TinvwminusRm,0),gsl_vector_get(TinvwminusRm,1));
      gsl_blas_ddot(wminusRm,TinvwminusRm,&exponent);
      //printf("Exponent = %f\nDet = %f\n",exponent,gsl_linalg_LU_det(Tij,signum));
      gsl_matrix_set(qij,ii,jj,log(thisgaussian->alpha) - di * halflogtwopi - 0.5 * gsl_linalg_LU_lndet(Tij) -0.5 * exponent);//This is actually the log of qij
      //printf("Here we have = %f\n",gsl_matrix_get(qij,ii,jj));
      //Now calculate bij and Bij
      thisbs= bs+tid*K+jj;
      gsl_vector_memcpy(thisbs->bbij,thisgaussian->mm);
      //printf("%i,%i,%i\n",tid,ii,jj);
      if ( ! noproj ) gsl_blas_dgemv(CblasNoTrans,1.0,VRT,TinvwminusRm,1.0,thisbs->bbij);
      else gsl_blas_dsymv(CblasUpper,1.0,thisgaussian->VV,TinvwminusRm,1.0,thisbs->bbij);
      //printf("bij = %f\t%f\n",gsl_vector_get(bs->bbij,0),gsl_vector_get(bs->bbij,1));
      gsl_matrix_memcpy(thisbs->BBij,thisgaussian->VV);
      if ( ! noproj ) {
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,VRT,Tij_inv,0.0,VRTTinv);
	gsl_blas_dgemm(CblasNoTrans,CblasTrans,-1.0,VRTTinv,VRT,1.0,thisbs->BBij);}
      else {
	gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,thisgaussian->VV,Tij_inv,0.0,VRTTinv);
	gsl_blas_dsymm(CblasRight,CblasUpper,-1.0,thisgaussian->VV,VRTTinv,1.0,thisbs->BBij);}
      gsl_blas_dsyr(CblasUpper,1.0,thisbs->bbij,thisbs->BBij);//This is bijbijT + Bij, which is the relevant quantity
      }
    gsl_permutation_free (p);
    gsl_vector_free(wminusRm);
    gsl_vector_free(TinvwminusRm);
    gsl_matrix_free(Tij);
    gsl_matrix_free(Tij_inv);
    if ( ! noproj ) gsl_matrix_free(VRT);
    gsl_matrix_free(VRTTinv);
    if ( ! noproj ) gsl_matrix_free(Rtrans);
    //Again loop over the gaussians to update the model(can this be more efficient? in any case this is not so bad since generally K << N)
#pragma omp critical
    {
      //Normalize qij properly
      *avgloglikedata += normalize_row(qij,ii,true,noweight,thisdata->logweight);
    }
      //printf("qij = %f\t%f\n",gsl_matrix_get(qij,ii,0),gsl_matrix_get(qij,ii,1));
      //printf("avgloglgge = %f\n",*avgloglikedata);
      for (jj = 0; jj != K; ++jj){
	currqij = exp(gsl_matrix_get(qij,ii,jj));
	//printf("Current qij = %f\n",currqij);
	thisbs= bs+tid*K+jj;
	thisnewgaussian= newgaussians+tid*K+jj;
	gsl_vector_scale(thisbs->bbij,currqij);
	gsl_vector_add(thisnewgaussian->mm,thisbs->bbij);
	gsl_matrix_scale(thisbs->BBij,currqij);
	gsl_matrix_add(thisnewgaussian->VV,thisbs->BBij);
	//printf("bij = %f\t%f\n",gsl_vector_get(bs->bbij,0),gsl_vector_get(bs->bbij,1));
	//printf("Bij = %f\t%f\t%f\n",gsl_matrix_get(bs->BBij,0,0),gsl_matrix_get(bs->BBij,1,1),gsl_matrix_get(bs->BBij,0,1));
      }
  }
  *avgloglikedata /= N;
  if (likeonly)
    return;
  //gettimeofday(&time3,NULL);

  //gather newgaussians
  if ( nthreads != 1 )
#pragma omp parallel for schedule(static,chunk) \
  private(ll,jj)
    for (jj = 0; jj < K; ++jj)
      for (ll = 1; ll != nthreads; ++ll) {
	gsl_vector_add((newgaussians+jj)->mm,(newgaussians+ll*K+jj)->mm);
	gsl_matrix_add((newgaussians+jj)->VV,(newgaussians+ll*K+jj)->VV);
      }

  //gettimeofday(&time4,NULL);

  //Now update the parameters
  //Thus, loop over gaussians again!
  double qj;
#pragma omp parallel for schedule(dynamic,chunk) \
  private(jj,qj)
  for (jj = 0; jj < K; ++jj){
    if (*(allfixed+jj)){
      continue;
    }
    else {
      qj = exp(logsum(qij,jj,false));
      (qj < DBL_MIN) ? qj = 0: 0;
      //printf("qj = %f\n",qj);
      if (*(fixamp+jj) != true) {
	(gaussians+jj)->alpha = qj;
	if (qj == 0) {//rethink this
	  *(fixamp+jj)=1;
	  *(fixmean+jj)=1;
	  *(fixcovar+jj)=1;
	  continue;
	}
      }
      gsl_vector_scale((newgaussians+jj)->mm,1.0/qj);
      if (*(fixmean+jj) != true){
	gsl_vector_memcpy((gaussians+jj)->mm,(newgaussians+jj)->mm);
    }
      if (*(fixcovar+jj) != true){
	//	if (*(fixmean+jj) != true)
	//  gsl_blas_dsyr(CblasUpper,-qj,(gaussians+jj)->mm,(newgaussians+jj)->VV);
	//else {
	gsl_blas_dsyr(CblasUpper,qj,(gaussians+jj)->mm,(newgaussians+jj)->VV);
	gsl_blas_dsyr2(CblasUpper,-qj,(gaussians+jj)->mm,(newgaussians+jj)->mm,(newgaussians+jj)->VV);
	//}
      if (w > 0.){
	gsl_matrix_add((newgaussians+jj)->VV,I);
	gsl_matrix_scale((newgaussians+jj)->VV,1.0/(qj+1.0));
      }
      else gsl_matrix_scale((newgaussians+jj)->VV,1.0/qj);
      gsl_matrix_memcpy((gaussians+jj)->VV,(newgaussians+jj)->VV);
      }
    }
  }
  //gettimeofday(&time5,NULL);

  //normalize the amplitudes
  if ( sumfixedamps == 0. && noweight ){
    for (kk=0; kk != K; ++kk){
      if ( noweight ) (gaussians++)->alpha /= (double) N;
    }
  }
  else {
    ampnorm= 0;
    for (kk=0; kk != K; ++kk){
      if (*(fixamp++) == false) ampnorm += gaussians->alpha;
      ++gaussians;
    }
    fixamp -= K;
    gaussians -= K;
    for (kk=0; kk != K; ++kk){
      if (*(fixamp++) == false){
	gaussians->alpha /= ampnorm;
	gaussians->alpha *= (1. - sumfixedamps);
      }
      ++gaussians;
    }
    fixamp -= K;
    gaussians -= K;
  }
  //gettimeofday(&end,NULL);
  //double diff, diff1, diff2, diff3, diff4, diff5,diff6;
  //diff= difftime (end.tv_sec,start.tv_sec)+difftime (end.tv_usec,start.tv_usec)/1000000;
  //diff1= (difftime(time1.tv_sec,start.tv_sec)+difftime(time1.tv_usec,start.tv_usec)/1000000)/diff;
  //diff2= (difftime(time2.tv_sec,time1.tv_sec)+difftime(time2.tv_usec,time1.tv_usec)/1000000)/diff;
  //diff3= (difftime(time3.tv_sec,time2.tv_sec)+difftime(time3.tv_usec,time2.tv_usec)/1000000)/diff;
  //diff4= (difftime(time4.tv_sec,time3.tv_sec)+difftime(time4.tv_usec,time3.tv_usec)/1000000)/diff;
  //diff5= (difftime(time5.tv_sec,time4.tv_sec)+difftime(time5.tv_usec,time4.tv_usec)/1000000)/diff;
  //diff6= (difftime(end.tv_sec,time5.tv_sec)+difftime(end.tv_usec,time5.tv_usec)/1000000)/diff;
  //printf("%f,%f,%f,%f,%f,%f,%f\n",diff,diff1,diff2,diff3,diff4,diff5,diff6);

  free(allfixed);

  return;
}


/*
 *  mvdens.h
 *  likely
 *
 *  Created by Karim Benabed on 10/03/08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef __MVDENS_H
#define __MVDENS_H
#ifdef __PLANCK__

#include "HL2_likely/tools/errorlist.h"
#include "HL2_likely/tools/io.h"
#include "HL2_likely/tools/maths_base.h"

#else

#include "errorlist.h"
#include "io.h"
#include "maths_base.h"

#endif

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_int.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>

/*#ifdef HAS_LAPACK
#ifdef HAS_MKL
#include "mkl_blas.h"
#include "mkl_lapack.h"
#else
#include "blas.h"
#include "lapack.h"
#endif
#endif
*/

#define __MVDENS_PARANOID_DEBUG__

/* errors */
#define mv_base       -700
#define mv_allocate   -1 + mv_base
#define mv_serialize  -2 + mv_base
#define mv_outOfBound -3 + mv_base
#define mv_badComm    -4 + mv_base
#define mv_negWeight  -5 + mv_base
#define mv_cholesky   -6 + mv_base
#define mv_negative   -7 + mv_base
#define mv_undef      -8 + mv_base
#define mv_file       -9 + mv_base
#define mv_io        -10 + mv_base
#define mv_tooManySteps -11 + mv_base
#define mv_dimension -12 + mv_base
#define mv_type      -13 + mv_base
#define mv_negHatCl  -14 + mv_base

#define PI         3.141592653589793
#define LOGSQRT2PI 0.918938533204673
#ifndef DYNMAX
#define DYNMAX 500.0
#endif
#define mv_INF 1e99

#define mv_EXP  2
#define mv_NORM 1
#define mv_UNORM 0

#define MVT_DF 3

typedef struct {
  size_t ndim;
  void *buf;
  int own_buf;
  double *mean;
  double *std;
  double *x_tmp;
  gsl_vector_view  mean_view_container;
  gsl_vector *mean_view;
  gsl_vector_view  x_tmp_view_container;
  gsl_vector *x_tmp_view;
  gsl_matrix_view  std_view_container;
  gsl_matrix  *std_view;
  size_t band_limit;
  int chol;
  int df;
  double detL;   /* Determinant of L, where L^t L = Covariance matrix */
} mvdens;

typedef struct {
  size_t ncomp, ndim;
  int init_cwght;
  double *wght, *cwght;
  gsl_vector_view wght_view_container, cwght_view_container;
  gsl_vector *wght_view, *cwght_view;
  mvdens **comp;
  void *buf_comp;
} mix_mvdens;

// init and stuff
mvdens * mvdens_alloc(size_t ndim, error **err);
mvdens * mvdens_init(mvdens *g, size_t nndim, double* mn, double* std, error **err);
void mvdens_empty(mvdens *g);
void mvdens_free(mvdens ** g);
void mvdens_free_void(void **g);
void mvdens_print(FILE* where, mvdens* what);
mvdens *mvdens_dwnp(FILE* where, error **err);
mvdens *mvdens_read_and_chol(FILE *where, error **err);
void mvdens_dump(FILE* where, mvdens* what);
void mvdens_chdump(const char *name, mvdens* what, error **err);
void mvdens_set_band_limit(mvdens *self, size_t value);
void mvdens_from_meanvar(mvdens *m, const double *pmean, const double *pvar, double correct);

//computations
double determinant(const double *L, size_t ndim);
void mvdens_cholesky_decomp(mvdens* self, error **err);
double* mvdens_ran(double* dest, mvdens * g, gsl_rng * r,error **err);
double scalar_product(mvdens *g, const double *x, error **err);
double mvdens_log_pdf(mvdens *g, const double * x, error ** err);
double mvdens_log_pdf_void(void *g, const double *x, error **err);
double mvdens_inverse(mvdens *m, error **err);

/* mix_mvdens functions */
mix_mvdens *mix_mvdens_alloc(size_t ncomp, size_t size,error **err);
void mix_mvdens_copy(mix_mvdens *target, const mix_mvdens *source, error **err);
void mix_mvdens_free_void(void **m);
void mix_mvdens_free(mix_mvdens **m);
void mix_mvdens_print(FILE* where,mix_mvdens* what);
void mix_mvdens_dump(FILE* where,mix_mvdens* what);
mix_mvdens * mix_mvdens_dwnp(FILE* where,error **err);
void mix_mvdens_set_band_limit(mix_mvdens *self, size_t value);
double effective_number_of_components(const mix_mvdens *self, error **err);

size_t mix_mvdens_size(size_t ncomp,size_t ndim);
void* serialize_mix_mvdens(mix_mvdens* self, size_t *sz,error** err);
mix_mvdens* deserialize_mix_mvdens(void* serialized, size_t sz,error **err);

void mix_mvdens_cholesky_decomp(mix_mvdens* self, error **err);
double* mix_mvdens_ran(double* dest,size_t *index, mix_mvdens *m, gsl_rng * r, error **err);
size_t ranindx(double *cw, size_t nE, gsl_rng * r, error ** err);
double mix_mvdens_log_pdf(mix_mvdens *m, const double *x, error **err);
double mix_mvdens_log_pdf_void(void *m, const double *x, error **err);

#define MALLOC_IF_NEEDED(ret,dest,size,err) { \
  if (dest==NULL) { \
    ret= (double*) malloc_err(size,err); \
  } else { \
    ret=dest; \
  } \
}

#ifdef HAS_HDF5
#include "hdf5.h"
#define hdf5_base mv_io
void mvdens_hdfdump_infile(mvdens* mv,hid_t loc_id,error **err);
void mvdens_hdfdump(mvdens* mv,char *fname,error **err);
void mix_mvdens_hdfdump(mix_mvdens* mmv,char *fname,error **err);
mvdens* mvdens_hdfdwnp_infile(mvdens* mv,hid_t loc_id,error **err);
mvdens* mvdens_hdfdwnp(char *fname,error **err);
mix_mvdens* mix_mvdens_hdfdwnp(char *fname,error **err);
#endif

#endif


#include <pthread.h>
#include <stdlib.h>
#include <string.h>

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

// MAX and MIN aren't defined in the standard library. O:
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))

typedef struct PopulationParametersStruct {
    int n; ///< Population size, used for determining the number of generations between events.
    float f; ///< Scaling factor for the total mutation rate. Smaller = shorter time to coalescence.
    float c; ///< Constant bias for the upward mutation rate.
    float d; ///< Linear bias for the downward mutation rate.
    int kappa; ///< Lower bound of repeat lengths.
    int omega; ///< Upper bound of repeat lengths.
} PopulationParameters;

typedef struct CoalescentStruct {
    PopulationParameters theta; ///< Mutation model parameters.
    int *coalescent_tree; ///< Pointer to our tree, stored as an array.
    int offset; ///< We generalize to include 1+ ancestors. Determine the offset for array representation of tree.
    gsl_rng *r; ///< Pointer to our RNG. This is preserved across the trace and evolve steps.
} PopulationTree;

int _triangle (int a) { return (int) (a * (a + 1) / 2.0); }
int _round_num (int a) { return (a < 0) ? (int) (a - 0.5) : (int) (a + 0.5); }
typedef int (*_mutate_f) (int, int, float, float, int, int, const gsl_rng *);

int _mutate_generation (int t, int ell, float c, float d, int kappa, int omega, const gsl_rng *r) {
    for (int k = 0; k < t; k++) {
        // Compute our upward mutation rate. We are bounded by omega.
        ell = (gsl_ran_flat(r, 0, 1) < c) ? MIN(omega, ell + 1) : ell;

        // Compute our downward mutation rate. We are bounded by kappa.
        ell = (gsl_ran_flat(r, 0, 1) < ell * d) ? MAX(kappa, ell - 1) : ell;
    }

    // Return the result of mutating over several generations.
    return ell;
}

int _mutate_draw (int t, int ell, float c, float d, int kappa, int omega, const gsl_rng *r) {
    return MIN((unsigned) omega, MAX((unsigned) kappa, ell + gsl_ran_poisson(r, t * c) -
                                                       gsl_ran_poisson(r, t * ell * d)));
}

void _trace_tree (int *coalescent_tree, int n, const gsl_rng *r) {
    for (int tau = 0; tau < 2 * n - 1; tau++) { // Diploid!
        int tau_0 = _triangle(tau + 0), tau_1 = _triangle(tau + 1); // Start at 2nd coalescence.

        // We save the indices of our ancestors to our descendants.
        for (int k = 0; k < tau_1 - tau_0; k++) {
        	coalescent_tree[tau_1 + k + 1] = tau_0 + k;
        }

        // Determine the individual whose frequency increases.
        gsl_ran_choose(r, &coalescent_tree[tau_1], 1, &coalescent_tree[tau_1 + 1], tau_1 - tau_0, sizeof(int));

        // Finally, shuffle the individuals in this generation.
        gsl_ran_shuffle(r, &coalescent_tree[tau_1], tau_1 - tau_0 + 1, sizeof(int));
    }
}

void _evolve_individual (int k, int *descendants, int t_coalescence, _mutate_f mutate, PopulationTree *p) {
    int *descendant_to_evolve = &(p->coalescent_tree[descendants[k]]);

    // Evolve each ancestor according to the average time to coalescence and the scaling factor f.
    *descendant_to_evolve = (*mutate)(t_coalescence, *descendant_to_evolve, p->theta.c, p->theta.d,
                                      p->theta.kappa, p->theta.omega, p->r);

    // Save our descendant state.
    descendants[k] = *descendant_to_evolve;
}

/**
 * We assumed our tree has been traced. Given the population tree structure and a pointer to a mutation function,
 * determine the repeat length of all individuals in our tree. We do so by iterating through each coalescent event.
 */
void _evolve_event (PopulationTree *p, _mutate_f mutate) {
    int descendants_size, expected_time, t_coalescence;
    int *descendants = NULL;

    for (int tau = p->offset; tau < 2 * p->theta.n - 1; tau++) {
        // We define our descendants for the tau'th coalescent.
        descendants = &(p->coalescent_tree[_triangle(tau + 1)]);
        descendants_size = _triangle(tau + 2) - _triangle(tau + 1);

        // Determine time to coalescence. This is exponentially distributed, but the mean stays the same. Scale by f.
        expected_time = (int) (p->theta.f * 2 * p->theta.n / (float) _triangle(tau + 1));
        t_coalescence = MAX(1, _round_num(gsl_ran_exponential(p->r, expected_time)));

        // Iterate through each of the descendants (currently indices) and determine each ancestor.
        for (int k = 0; k < descendants_size; k++) {
            _evolve_individual(k, descendants, t_coalescence, mutate, p);
        }
    }
}

void _evolve (int *i_0, int i_0_size, PopulationTree *p) {
    // Determine our offset, and seed our ancestors for the tree.
    p->offset = i_0_size - 1;
    for (int k = 0; k < i_0_size; k++) {
        p->coalescent_tree[k + _triangle(p->offset)] = i_0[k];
    }

    // From our common ancestors, descend forward in time and populate our tree with repeat lengths.
    _evolve_event(p, _mutate_draw);
}

void _cleanup (PopulationTree *p) {
    free(p->coalescent_tree);
    gsl_rng_free(p->r);
}


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

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

gsl_multifit_fdfsolver *
gsl_multifit_fdfsolver_alloc (const gsl_multifit_fdfsolver_type * T,
                              size_t n, size_t p)
{
  int status;

  gsl_multifit_fdfsolver * s;

  if (n < p)
    {
      GSL_ERROR_VAL ("insufficient data points, n < p", GSL_EINVAL, 0);
    }

  s = (gsl_multifit_fdfsolver *) calloc (1, sizeof (gsl_multifit_fdfsolver));
  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for multifit solver struct",
                     GSL_ENOMEM, 0);
    }

  s->x = gsl_vector_calloc (p);

  if (s->x == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->f = gsl_vector_calloc (n);

  if (s->f == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for f", GSL_ENOMEM, 0);
    }

  s->dx = gsl_vector_calloc (p);

  if (s->dx == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for dx", GSL_ENOMEM, 0);
    }

  s->g = gsl_vector_alloc (p);

  if (s->g == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for g", GSL_ENOMEM, 0);
    }

  s->sqrt_wts = gsl_vector_calloc (n);

  if (s->sqrt_wts == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for sqrt_wts", GSL_ENOMEM, 0);
    }

  s->state = calloc (1, T->size);

  if (s->state == 0)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to allocate space for multifit solver state",
                     GSL_ENOMEM, 0);
    }

  s->type = T ;

  status = (s->type->alloc)(s->state, n, p);

  if (status != GSL_SUCCESS)
    {
      gsl_multifit_fdfsolver_free (s);
      GSL_ERROR_VAL ("failed to set solver", status, 0);
    }

  s->fdf = NULL;

  s->niter = 0;

  return s;
}

int
gsl_multifit_fdfsolver_set (gsl_multifit_fdfsolver * s,
                            gsl_multifit_function_fdf * f,
                            const gsl_vector * x)
{
  return gsl_multifit_fdfsolver_wset(s, f, x, NULL);
}

int
gsl_multifit_fdfsolver_wset (gsl_multifit_fdfsolver * s,
                             gsl_multifit_function_fdf * f,
                             const gsl_vector * x,
                             const gsl_vector * wts)
{
  const size_t n = s->f->size;

  if (n != f->n)
    {
      GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
    }
  else if (s->x->size != x->size)
    {
      GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
    }
  else if (wts != NULL && n != wts->size)
    {
      GSL_ERROR ("weight vector length does not match solver", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      s->fdf = f;
      gsl_vector_memcpy(s->x, x);
      s->niter = 0;

      if (wts)
        {
          for (i = 0; i < n; ++i)
            {
              double wi = gsl_vector_get(wts, i);
              gsl_vector_set(s->sqrt_wts, i, sqrt(wi));
            }
        }
      else
        gsl_vector_set_all(s->sqrt_wts, 1.0);

      return (s->type->set) (s->state, s->sqrt_wts, s->fdf, s->x, s->f, s->dx);
    }
}

int
gsl_multifit_fdfsolver_iterate (gsl_multifit_fdfsolver * s)
{
  int status =
    (s->type->iterate) (s->state, s->sqrt_wts, s->fdf, s->x, s->f, s->dx);

  s->niter++;

  return status;
}

/*
gsl_multifit_fdfsolver_driver()
  Iterate the nonlinear least squares solver until completion

Inputs: s - fdfsolver
        maxiter - maximum iterations to allow
        xtol    - tolerance in step x
        gtol    - tolerance in gradient
        ftol    - tolerance in ||f||
        info    - (output) info flag on why iteration terminated
                  1 = stopped due to small step size ||dx|
                  2 = stopped due to small gradient
                  3 = stopped due to small change in f
                  GSL_ETOLX = ||dx|| has converged to within machine
                              precision (and xtol is too small)
                  GSL_ETOLG = ||g||_inf is smaller than machine
                              precision (gtol is too small)
                  GSL_ETOLF = change in ||f|| is smaller than machine
                              precision (ftol is too small)

Return: GSL_SUCCESS if converged, GSL_MAXITER if maxiter exceeded without
converging
*/
int
gsl_multifit_fdfsolver_driver (gsl_multifit_fdfsolver * s,
                               const size_t maxiter,
                               const double xtol,
                               const double gtol,
                               const double ftol,
                               int *info)
{
  int status;
  size_t iter = 0;

  do
    {
      status = gsl_multifit_fdfsolver_iterate (s);

      /*
       * if status is GSL_ENOPROG or GSL_SUCCESS, continue iterating,
       * otherwise the method has converged with a GSL_ETOLx flag
       */
      if (status != GSL_SUCCESS && status != GSL_ENOPROG)
        break;

      /* test for convergence */
      status = gsl_multifit_fdfsolver_test(s, xtol, gtol, ftol, info);
    }
  while (status == GSL_CONTINUE && ++iter < maxiter);

  /*
   * the following error codes mean that the solution has converged
   * to within machine precision, so record the error code in info
   * and return success
   */
  if (status == GSL_ETOLF || status == GSL_ETOLX || status == GSL_ETOLG)
    {
      *info = status;
      status = GSL_SUCCESS;
    }

  /* check if max iterations reached */
  if (iter >= maxiter && status != GSL_SUCCESS)
    status = GSL_EMAXITER;

  return status;
} /* gsl_multifit_fdfsolver_driver() */

int
gsl_multifit_fdfsolver_jac (gsl_multifit_fdfsolver * s, gsl_matrix * J)
{
  const size_t n = s->f->size;
  const size_t p = s->x->size;

  if (n != J->size1 || p != J->size2)
    {
      GSL_ERROR ("Jacobian dimensions do not match solver", GSL_EBADLEN);
    }
  else
    {
      return (s->type->jac) (s->state, J);
    }
} /* gsl_multifit_fdfsolver_jac() */

void
gsl_multifit_fdfsolver_free (gsl_multifit_fdfsolver * s)
{
  RETURN_IF_NULL (s);

  if (s->state)
    {
      (s->type->free) (s->state);
      free (s->state);
    }

  if (s->dx)
    gsl_vector_free (s->dx);

  if (s->x)
    gsl_vector_free (s->x);

  if (s->f)
    gsl_vector_free (s->f);

  if (s->sqrt_wts)
    gsl_vector_free (s->sqrt_wts);

  if (s->g)
    gsl_vector_free (s->g);

  free (s);
}

const char *
gsl_multifit_fdfsolver_name (const gsl_multifit_fdfsolver * s)
{
  return s->type->name;
}

gsl_vector *
gsl_multifit_fdfsolver_position (const gsl_multifit_fdfsolver * s)
{
  return s->x;
}

gsl_vector *
gsl_multifit_fdfsolver_residual (const gsl_multifit_fdfsolver * s)
{
  return s->f;
}

size_t
gsl_multifit_fdfsolver_niter (const gsl_multifit_fdfsolver * s)
{
  return s->niter;
}

/*
gsl_multifit_eval_wf()
  Compute residual vector y with user callback function, and apply
weighting transform if given:

y~ = sqrt(W) y

Inputs: fdf  - callback function
        x    - model parameters
        swts - weight matrix sqrt(W) = sqrt(diag(w1,w2,...,wn))
               set to NULL for unweighted fit
        y    - (output) (weighted) residual vector
               y_i = sqrt(w_i) f_i where f_i is unweighted residual
*/

int
gsl_multifit_eval_wf(gsl_multifit_function_fdf *fdf, const gsl_vector *x,
                     const gsl_vector *swts, gsl_vector *y)
{
  int s = ((*((fdf)->f)) (x, fdf->params, y));
  ++(fdf->nevalf);

  /* y <- sqrt(W) y */
  if (swts)
    gsl_vector_mul(y, swts);

  return s;
}

/*
gsl_multifit_eval_wdf()
  Compute Jacobian matrix J with user callback function, and apply
weighting transform if given:

J~ = sqrt(W) J

Inputs: fdf  - callback function
        x    - model parameters
        swts - weight matrix W = diag(w1,w2,...,wn)
               set to NULL for unweighted fit
        dy   - (output) (weighted) Jacobian matrix
               dy = sqrt(W) dy where dy is unweighted Jacobian
*/

int
gsl_multifit_eval_wdf(gsl_multifit_function_fdf *fdf, const gsl_vector *x,
                      const gsl_vector *swts, gsl_matrix *dy)
{
  int status = ((*((fdf)->df)) (x, fdf->params, dy));

  ++(fdf->nevaldf);

  /* J <- sqrt(W) J */
  if (swts)
    {
      const size_t n = swts->size;
      size_t i;

      for (i = 0; i < n; ++i)
        {
          double swi = gsl_vector_get(swts, i);
          gsl_vector_view v = gsl_matrix_row(dy, i);

          gsl_vector_scale(&v.vector, swi);
        }
    }

  return status;
}


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

int main(void)
{
	float a[] = { 1, 2, 3, 4, 5, 6 };
	float b[] = { 0, 0, 0, 0, 0, 0 };
	const int n = sizeof a / sizeof a[0];
	int i;

	cblas_scopy(n/2, a, 2, b, 2);	/* copy every other element */
	for (i = 0; i < n; ++i)
		printf("%f ", b[i]);
	putchar('\n');

	return 0;
}


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

/*
gsl_movstat_qqr()
  Apply a moving q-quantile range to an input vector

Inputs: endtype - how to handle end points
        x       - input vector, size n
        q       - quantile \in [0,0.5]
        xqqr    - (output) vector of q-quantile ranges of x, size n
                  xqqr_i = q-quantile range of i-th window:
        w       - workspace
*/

int
gsl_movstat_qqr(const gsl_movstat_end_t endtype, const gsl_vector * x, const double q,
                gsl_vector * xqqr, gsl_movstat_workspace * w)
{
  if (x->size != xqqr->size)
    {
      GSL_ERROR("x and xqqr vectors must have same length", GSL_EBADLEN);
    }
  else if (q < 0.0 || q > 0.5)
    {
      GSL_ERROR("q must be between 0 and 0.5", GSL_EDOM);
    }
  else
    {
      double qq = q;
      int status = gsl_movstat_apply_accum(endtype, x, gsl_movstat_accum_qqr, (void *) &qq, xqqr, NULL, w);
      return status;
    }
}