/*
/
/ adkGSL.c
/
/ homebrewed addons to gsl */

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_statistics.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include "../extern/include/f2c.h"
#include "../extern/include/blaswrap.h"
#include "../extern/include/clapack.h"
//#include <vecLib/cblas.h>
//#include <accelerate/accelerate.h>
#include "adkGSL.h"

#define SUM_LOG_THRESHOLD  -10

//gsl_matrix_covariance-- computes sample covariance matrix and stores it in matrix cov
void gsl_matrix_covariance(gsl_matrix *data, gsl_matrix *cov){
	gsl_vector_view a, b;
	size_t i, j;
	double v;
	for (i = 0; i < data->size2; i++) {
 		for (j = 0; j < data->size2; j++) {
 			a = gsl_matrix_column (data, i);
 			b = gsl_matrix_column (data, j);
 			v = gsl_stats_covariance (a.vector.data, a.vector.stride,
 				b.vector.data, b.vector.stride, a.vector.size);
 			gsl_matrix_set (cov, i, j, v);
		}
	}
}

/*gsl_vector_includes() - returns 1 or 0 depending on result */
int gsl_vector_includes(gsl_vector *aVec, double aValue){
  int i;

  for(i = 0; i < aVec->size; i++){
    if (gsl_vector_get(aVec, i) == aValue){
      return(1);
    }
  }
  return(0);
}

double gsl_vector_sum(gsl_vector *aVec, int aVecSize){
	double sum;
	int i;

	sum = 0;
	for (i = 0; i < aVecSize; i++){
		sum += gsl_vector_get(aVec, i);
		}
	return sum;
}

double gsl_vector_dot_product(gsl_vector *vec1, gsl_vector *vec2){
  int i;
  double dot = 0.0;

  assert(vec1->size == vec2->size);
  for(i = 0; i < vec1->size; i++){
    dot += gsl_vector_get(vec1, i) *  gsl_vector_get(vec2, i);
  }
  return(dot);
}

void gsl_vector_outer_product(gsl_vector *vec1, gsl_vector *vec2, gsl_matrix *result){
	int i,j;

	assert(vec1->size == result->size1 || vec2->size == result->size2);
	for(i = 0; i < vec1->size; i++){
		for(j = 0; j < vec2->size; j++){
			gsl_matrix_set(result,i,j,gsl_vector_get(vec1,i) * gsl_vector_get(vec2,j));
		}
	}
}

double gsl_matrix_row_sum(gsl_matrix *mat, int iRow, int rowSize){
	gsl_vector_view aRow;
	double sum = 0;
	int i;

	aRow = gsl_matrix_row(mat, iRow);
	for (i = 0; i< rowSize; i++){
		sum += gsl_vector_get(&aRow.vector, i);
		}
	return sum;
}

/* efficiently compute log of sum of values, which themselves are
   stored as logs: that is, return log(sum_i exp(l_i)).  The largest
   of the elements of l (call it maxval) is factored out, so that
   log(sum_i(exp(l_i))) = maxval + log(1 + sum_i(exp(l_i-maxval))),
   where the new sum is taken over 2 <= i < n.  All of the quantities
   in the exp must be negative, and those smaller than some reasonable
   threshold can be ignored. [Thanks to David Haussler for showing me
   this trick].  This code was adapted from code kindly given to me by Adam Siepel */

double log_sum(gsl_vector *vec) {
  double maxval, expsum;
  int k;

  if (vec->size > 1){
  	gsl_sort_vector(vec);
	gsl_vector_reverse(vec);
	}

  maxval = gsl_vector_get(vec, 0);
  expsum = 1;
  k = 1;

  while (k < vec->size && gsl_vector_get(vec, k) - maxval > SUM_LOG_THRESHOLD){
    expsum += exp(gsl_vector_get(vec, k++) - maxval);
	}
  return maxval + log(expsum);
}

/*the following methods are wrappers for use with LAPACK */

void set_lapack_entry(double *A, int i, int j, int nrows, double val){
  A[j*nrows+i] = val;
}

double get_lapack_entry(double *a, int i, int j,int nrows){
  printf("ha");
  return a[j*nrows+i];
}

/*this is meant to be a conversion utility */
double *gsl_matrix_2_lapack(gsl_matrix *m){
  double *lapack_mat;
  int i, j;

  lapack_mat = malloc(m->size1 * m->size2 * sizeof(double));
  for(i = 0; i < m->size1; i++){
    for(j = 0; j < m->size2; j++){
      set_lapack_entry(lapack_mat, i, j,m->size1, gsl_matrix_get(m, i, j));
    }
  }
  return lapack_mat;
}

gsl_matrix *lapack_2_gsl_matrix(double *A, int nrows, int ncols){
  int i, j;
  gsl_matrix *new;

  new = gsl_matrix_alloc(nrows,ncols);
  for(i=0;i<nrows;i++){
    for(j=0;j<ncols;j++){
      gsl_matrix_set(new,i,j,get_lapack_entry(A,i,j,nrows));
    }
  }
  return new;
}

gsl_matrix *gsl_matrix_power_logs(gsl_matrix *aMatrix, int power){
  gsl_matrix *powerMat, *logMat;
  int i, j;
  double x;

  powerMat = gsl_matrix_power(aMatrix, power);
  logMat = gsl_matrix_alloc(aMatrix->size1, aMatrix->size2);
  for(i = 0; i < aMatrix->size1; i++){
    for(j = 0; j < aMatrix->size2; j++){
      x = gsl_matrix_get(powerMat, i, j);
      gsl_matrix_set(logMat, i, j, log(x));
    }
  }
  gsl_matrix_free(powerMat);
  return(logMat);
}

gsl_matrix *gsl_matrix_power(gsl_matrix *aMatrix, int power){
  gsl_matrix *revect, *levect;
  double *dataMat, *wr, *wi, *vl, *vr, *aMat, *bMat, *cMat, *dMat, *work;
  int lwork, i, j, n ,info;
  assert(aMatrix->size1 == aMatrix->size2);
  n = aMatrix->size1;

  if (power == 1){
    gsl_matrix * solMat = gsl_matrix_alloc(n, n);
    for(i=0; i < n; i++){
      for(j = 0; j< n; j++){
	gsl_matrix_set(solMat, i, j, gsl_matrix_get(aMatrix,i,j));
      }
    }
    return(solMat);
  }

  //allocate space for eigenvector matrices, and other result holders
  levect = gsl_matrix_alloc(n, n);
  revect = gsl_matrix_alloc(n, n);
  wr = malloc(n * sizeof(double));
  wi =  malloc(n * sizeof(double));
  vl = malloc(n * n * sizeof(double));
  vr = malloc(n * n * sizeof(double));
  lwork = 20 * n;
  work = malloc(lwork * sizeof(double));

  //move data to LAPACK (col major) format
  dataMat =  gsl_matrix_2_lapack(aMatrix);

  //run LAPACK routine dgeev
  #ifdef OSX
  dgeev('V','V',n, dataMat, n, wr, wi, vl, n, vr, n, work, lwork);
  #else
  dgeev_('V','V',n, dataMat, n, wr, wi, vl, n, vr, n, work, lwork, info);
  #endif
  //collect eigenvalues into matrices
  for(i=0; i < n; i++){
    for(j = 0; j< n; j++){
      gsl_matrix_set(revect, i, j, vr[j*2+i]);
      gsl_matrix_set(levect, j, i, vl[j*2+i]);
    }
  }

  /* rescale such that revect and levect are inverses */
  /* see Press et al. (Numerical Recipes) for a very clear explanation
     of the relationship between the left and right eigenvectors  */
   for(i = 0; i < n; i++){
    double dotProd = 0.0;
    /* compute dot product of row i in levect and col i in revect */
    for(j = 0; j< n; j++){
      dotProd += gsl_matrix_get(levect, i, j) * gsl_matrix_get(revect,j,i);
      }

      /* now rescale levect */
    for(j = 0; j< n; j++){
      double old = gsl_matrix_get(levect, i, j);
      double scaled = old / dotProd;
      gsl_matrix_set(levect, i, j, scaled);
      }
  }

   //prepare for matrix multiplication
   gsl_matrix * solMat = gsl_matrix_alloc(n, n);
   gsl_matrix * diag = gsl_matrix_alloc(n, n);
   gsl_matrix_set_all(diag, 0);
   for(j = 0; j< n; j++){
     gsl_matrix_set(diag, j,j, pow(wr[j],power));
   }

   aMat = gsl_matrix_2_lapack(diag);
   bMat = gsl_matrix_2_lapack(revect);
   cMat = malloc(n * n * sizeof(double));

   //compute a^t = s^-1 * diag^t * s
   cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, n, n, n, 1.0, aMat, n, bMat, n, 0.0, cMat, n);
   free(bMat);

   bMat = gsl_matrix_2_lapack(levect);
   dMat =  malloc(n * n * sizeof(double));
   cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, n, n, n, 1, cMat, n, bMat, n, 0.0, dMat, n);

   //put results into gsl_matrix
   for(i = 0; i < n; i++){
     for(j = 0; j < n; j++){
       gsl_matrix_set(solMat, i, j, dMat[j*n+i]);
     }
   }

   //cleanup
   gsl_matrix_free(levect);
   gsl_matrix_free(revect);
   gsl_matrix_free(diag);
   free(work);
   free(vl);
   free(vr);
   free(wi);
   free(wr);
   free(aMat);
   free(bMat);
   free(cMat);
   free(dMat);
   free(dataMat);
   return(solMat);
}

//returns the trace of a matrix-- sum of diag
double gsl_matrix_trace(gsl_matrix *x){
	int i;
	double sum = 0.0;
	for(i=0;i<x->size1;i++){
		sum += gsl_matrix_get(x,i,i);
	}
	return(sum);
}

//copies the lower triangle of src in to dest, sets others to zero
void gsl_matrix_lower_tri_copy(gsl_matrix *src, gsl_matrix *dest){
	int i, j;
	assert(src->size1 == dest->size1);
	assert(src->size2 == dest->size2);
	for(i = 0; i < src->size1; i++){
		for(j = 0; j < src->size2; j++){
			if(i<=j){
				gsl_matrix_set(dest,i,j,gsl_matrix_get(src,i,j));
			}
			else{
				gsl_matrix_set(dest,i,j,0.0);
			}
		}
	}
}

//copies the upper triangle of src in to dest, sets others to zero
void gsl_matrix_upper_tri_copy(gsl_matrix *src, gsl_matrix *dest){
	int i, j;
	assert(src->size1 == dest->size1);
	assert(src->size2 == dest->size2);
	for(i = 0; i < src->size1 ;i++){
		for(j = 0; j < src->size2; j++){
			if(i>=j){
				gsl_matrix_set(dest,i,j,gsl_matrix_get(src,i,j));
			}
			else{
				gsl_matrix_set(dest,i,j,0.0);
			}
		}
	}
}

//zeros out everything but lower tri-- in place
void gsl_matrix_lower_tri(gsl_matrix *x){
	int i, j;
	for(i = 0; i < x->size1; i++){
		for(j = 0; j < x->size2; j++){
			if(i>j)	gsl_matrix_set(x,i,j,0.0);
		}
	}
}

//zeros out everything but upper tri-- in place
void gsl_matrix_upper_tri(gsl_matrix *x){
		int i, j;
		for(i = 0; i < x->size1; i++){
			for(j = 0; j < x->size2; j++){
				if(i<j)	gsl_matrix_set(x,i,j,0.0);
			}
		}
}

//fillMatrixFromArray-- fills up a matrix based on nrow and ncol given *double
void fillMatrixFromArray(double *numbers, gsl_matrix *dest, int nrow, int ncol){
	int i, j, count = 0;
	assert(nrow == dest->size1);
	for(i = 0; i < nrow; i++){
		for(j=0;j<ncol;j++){
			gsl_matrix_set(dest,i,j,numbers[count++]);
		}
	}
}

//fillMatrixFromArray-- fills up a matrix based on nrow and ncol given gsl_vector
void fillMatrixFromVector(gsl_vector *numbers, gsl_matrix *dest, int nrow, int ncol){
	int i, j, count = 0;
	assert(nrow == dest->size1);
	for(i = 0; i < nrow; i++){
		for(j=0;j<ncol;j++){
			gsl_matrix_set(dest,i,j,gsl_vector_get(numbers,count++));
		}
	}
}

//fillArrayFromMatrix-- fills up a vector  based gsl_matrix
void fillArrayFromMatrix(gsl_matrix *src, gsl_vector *dest){
	int i, j, count = 0;

	for(i = 0; i < src->size1; i++){
		for(j=0;j<src->size2;j++){
			gsl_vector_set(dest,count++, gsl_matrix_get(src,i,j));
		}
	}
}

//fillMatrixFromCholArray-- fills up a matrix based on nrow and ncol given arraay
// representing the Cholesky decomp
void fillMatrixFromCholArray(double *numbers, gsl_matrix *dest, int nrow, int ncol){
	int i, j, count = 0;

	for(i = 0; i < nrow; i++){
		for(j=0;j<ncol;j++){
			if(i<=j)gsl_matrix_set(dest,i,j,numbers[count++]);
		}
	}
}

void fillCholArrayFromMatrix(gsl_matrix *src, gsl_vector *dest){
	int i, j, count = 0;

	for(i = 0; i < src->size1; i++){
		for(j=0;j<src->size2;j++){
			if(i<=j){
				 gsl_vector_set(dest,count++, gsl_matrix_get(src,i,j));
				//printf("i:%d j: %d val: %f\n",i,j,gsl_matrix_get(src,i,j) );
			}

		}
	}

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef OSX

/*some wrappers for use with the OS X vecLib LAPACK implementation */
/*wrapper for the lapack eigensystem routine dgeev_*/

int dgeev(char jobvl, char jobvr, int n,  double *a, int lda, double *wr, double *wi, double *vl,
	  int ldvl,double *vr, int ldvr, double *work, int lwork){

  __CLPK_integer _n, _lda, _ldvl, _ldvr, _lwork, info;

  _n = (__CLPK_integer) n;
  _lda =  (__CLPK_integer) lda;
  _ldvl =(__CLPK_integer) ldvl;
  _ldvr =(__CLPK_integer) ldvr;
  _lwork =(__CLPK_integer) lwork;

  dgeev_(&jobvl, &jobvr, &_n, a, &_lda, wr, wi, vl, &_ldvl, vr, &_ldvr, work, &_lwork, &info);
  return info;
}

/*wrapper for the lapack eigensystem routine dsyev_
int dsyev(char jobvl, char uplo, int n,  double *a, int lda, double *w, double *work, int lwork){

  __CLPK_integer _n, _lda, _lwork, info;

  _n = (__CLPK_integer) n;
  _lda =  (__CLPK_integer) lda;
  _lwork =(__CLPK_integer) lwork;

  dsyev_(&jobvl, &uplo, &_n, a, &_lda, w, work, &_lwork, &info);
  return info;
}

int dgehrd(int n, int ilo, int ihi, double *a, int lda, double *tau, double *work, int lwork){
  __CLPK_integer _n, _ilo, _ihi, _lda,  _lwork, info;

   _n = (__CLPK_integer) n;
  _lda =  (__CLPK_integer) lda;
  _lwork =(__CLPK_integer) lwork;
  _ilo =(__CLPK_integer) ilo;
  _ihi =(__CLPK_integer) ihi;

  dgehrd_(&_n, &_ilo, &_ihi, a, &_lda, tau, work, &_lwork, &info);
  return info;
}

int dgebal(char job, int n,  double *a, int lda, int ilo, int ihi, double *scale){
  __CLPK_integer _n, _ilo, _ihi, _lda, info;

   _n = (__CLPK_integer) n;
  _lda =  (__CLPK_integer) lda;
  _ilo =(__CLPK_integer) ilo;
  _ihi =(__CLPK_integer) ihi;

  dgebal_(&job, &_n, a, &_lda, &_ilo, &_ihi, scale, &info);
  return info;
}

int dhseqr(char job, char compz, int n, int ilo, int ihi,double *h, int ldh, double *wr, double *wi, double *z, int ldz, double *work, int lwork){
  __CLPK_integer _n, _ilo, _ihi, _ldh, _ldz, _lwork, info;

   _n = (__CLPK_integer) n;
  _ldh =  (__CLPK_integer) ldh;
  _ilo =(__CLPK_integer) ilo;
  _ihi =(__CLPK_integer) ihi;
  _ldz =  (__CLPK_integer) ldz;
  _lwork = (__CLPK_integer) lwork;
  dhseqr_(&job, &compz, &_n, &_ilo, &_ihi, h, &_ldh, wr, wi, z,&_ldz,  work, &_lwork, &info);
  return info;
}
#endif

int dgeev(char jobvl, char jobvr, int n,  double *a, int lda, double *wr, double *wi, double *vl,
          int ldvl,double *vr, int ldvr, double *work, int lwork){

  long int _n, _lda, _ldvl, _ldvr, _lwork, info;

  _n =  n;
  _lda =   lda;
  _ldvl = ldvl;
  _ldvr = ldvr;
  _lwork = lwork;

  dgeev_(&jobvl, &jobvr, &_n, a, &_lda, wr, wi, vl, &_ldvl, vr, &_ldvr, work, &_lwork, &info);
  return info;
}

int dsyev(char jobvl, char uplo, int n,  double *a, int lda, double *w, double *work, int lwork){

  long int _n, _lda, _lwork, info;

  _n =  n;
  _lda =   lda;
  _lwork = lwork;

  dsyev_(&jobvl, &uplo, &_n, a, &_lda, w, work, &_lwork, &info);
  return info;
}

int dgehrd(int n, int ilo, int ihi, double *a, int lda, double *tau, double *work, int lwork){
  long int _n, _ilo, _ihi, _lda,  _lwork, info;

   _n =  n;
  _lda =   lda;
  _lwork = lwork;
  _ilo = ilo;
  _ihi = ihi;

  dgehrd_(&_n, &_ilo, &_ihi, a, &_lda, tau, work, &_lwork, &info);
  return info;
}

int dgebal(char job, int n,  double *a, int lda, int ilo, int ihi, double *scale){
  long int _n, _ilo, _ihi, _lda, info;

   _n =  n;
  _lda =   lda;
  _ilo = ilo;
  _ihi = ihi;

  dgebal_(&job, &_n, a, &_lda, &_ilo, &_ihi, scale, &info);
  return info;
}

int dhseqr(char job, char compz, int n, int ilo, int ihi,double *h, int ldh, double *wr, double *wi, double *z, int ldz, double *work, int lwork){
  long int _n, _ilo, _ihi, _ldh, _ldz, _lwork, info;

   _n =  n;
  _ldh =   ldh;
  _ilo = ilo;
  _ihi = ihi;
  _ldz =   ldz;
  _lwork =  lwork;
  dhseqr_(&job, &compz, &_n, &_ilo, &_ihi, h, &_ldh, wr, wi, z,&_ldz,  work, &_lwork, &info);
  return info;
}
*/
#endif


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>

#define m(i,j) m[i*l+j]

double* calcMys(char m[],int n,int l)
{
  int i,j,k,k1,k2,k3,k4;
  char reslist[]={ 'A', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'K',
      'L', 'M', 'N', 'P', 'Q', 'R', 'S', 'T', 'V', 'W', 'Y'};
  int resnum=sizeof(reslist)/sizeof(char);
  double *simi;
  simi=malloc(n*n*sizeof(double));
  for (i=0;i<n;i++)
  {
    simi[i*n+i]=0.0;
    for (j=i+1;j<n;j++)
    {
      double allcount=0,same=0;
      for (k=0;k<l;k++)
      {
        if (m(i,k)!='-' || m(j,k)!='-')
        {
          allcount++;
          if (m(i,k)==m(j,k))
            same++;
        }
      }
      simi[i*n+j]=same/allcount;
      simi[j*n+i]=simi[i*n+j];
    }
  }
  int *edge,edgenow=0;
  edge=malloc(5*n*2*sizeof(int));
  for (i=0;i<5*n*2;i++)
  {
    edge[i]=-1;
  }
  for (i=0;i<n;i++)
  {
    int has=0,now=0,maxn[5],max5=-1;
    while (has<5)
    {
      if (now!=i)
      {
        maxn[has]=now;
        has++;
      }
      now++;
    }
    for(j=0;j<5;j++)
      if (max5==-1||simi[i*n+maxn[max5]]>simi[i*n+maxn[j]])
        max5=j;
    for (j=now;j<n;j++)
    {
      if (simi[i*n+j]>simi[i*n+maxn[max5]])
      {
        maxn[max5]=j;
        max5=-1;
        for(k=0;k<5;k++)
          if (max5==-1||simi[i*n+maxn[max5]]>simi[i*n+maxn[k]])
            max5=k;
      }
    }
    for (j=0;j<5;j++)
    {
      edge[edgenow*2+0]=i;
      edge[edgenow*2+1]=maxn[j];
      edgenow++;
    }
  }
  free(simi);
  double *mys;
  int n1=0,n2=0;
  mys=malloc(l*l*sizeof(double));
  for (i=0;i<l*l;i++)
    mys[i]=0.0;
  for (i=0;i<l;i++)
  {
    for (j=i+1;j<l;j++)
    {
      double *prop,*pro1,*pro2,allnumber=0,add=0;
      prop=malloc(20*20*20*20*sizeof(double));
      pro1=malloc(20*20*sizeof(double));
      pro2=malloc(20*20*sizeof(double));
      int count1=0,count2=0,count3=0,count4=0;
      for (k=0;k<160000;k++)
        prop[k]=0.0;
      for (k=0;k<400;k++)
      {
        pro1[k]=0;
        pro2[k]=0;
      }

      for (k=0;k<5*n;k++)
      {
        n1=edge[k*2];
        n2=edge[k*2+1];
        if (m(n1,i)!='-' && m(n1,j)!='-' && m(n2,i)!='-' && m(n2,j)!='-')
        {
          count1=0;
          count2=0;
          count3=0;
          count4=0;
          for (k1 = 0; k1 < 20; k1++)
          {
            if (reslist[k1]==m(n1,i))
            {
              count1=k1;
              break;
            }
          }
          for (k1 = 0; k1 < 20; k1++)
          {
            if (reslist[k1]==m(n2,i))
            {
              count2=k1;
              break;
            }
          }
          for (k1 = 0; k1 < 20; k1++)
          {
            if (reslist[k1]==m(n1,j))
            {
              count3=k1;
              break;
            }
          }
          for (k1 = 0; k1 < 20; k1++)
          {
            if (reslist[k1]==m(n2,j))
            {
              count4=k1;
              break;
            }
          }
          prop[(count1*20+count3)*400+(count2*20+count4)]+=1;
          prop[(count2*20+count4)*400+(count1*20+count3)]+=1;
          allnumber+=2;
        }
      }

      gsl_matrix_view m = gsl_matrix_view_array (prop, 400, 400);
      gsl_vector *eval = gsl_vector_alloc (400);
      gsl_matrix *evec = gsl_matrix_alloc (400, 400);
      gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (400);
      gsl_eigen_symmv (&m.matrix, eval, evec, w);
      gsl_eigen_symmv_free (w);
      gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
      gsl_matrix *mid = gsl_matrix_alloc (400, 400);
      gsl_matrix *right = gsl_matrix_alloc (400, 400);
      for (k1 = 0; k1 < 400; k1++)
      {
        double ser=gsl_vector_get(eval,k1);
        for (k2=0;k2<400;k2++)
        {
          gsl_matrix_set(mid,k1,k2,0);
          gsl_matrix_set(right,k1,k2,gsl_matrix_get(evec,k2,k1));
        }
        gsl_matrix_set(mid,k1,k1,ser/(ser+1));
      }
      gsl_matrix *temp= gsl_matrix_alloc (400, 400);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, evec, mid,
                  0.0, temp);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, temp, right,
                  0.0, evec);
      gsl_matrix_free(mid);
      gsl_matrix_free(right);
      gsl_vector_free (eval);
      for (k1=0;k1<400;k1++)
        for(k2=0;k2<400;k2++)
        {
          prop[k1*400+k2]=gsl_matrix_get(evec,k1,k2);
          if (prop[k1*400+k2]<0)
            prop[k1*400+k2]=-prop[k1*400+k2];
        }
      gsl_matrix_free (evec);
      gsl_matrix_free (temp);
      for (k1=0;k1<20;k1++)
        for(k2=0;k2<20;k2++)
          for(k3=0;k3<20;k3++)
            for(k4=0;k4<20;k4++)
            {
              pro1[k1*20+k2]+=prop[(k1*20+k3)*400+(k2*20+k4)];
              pro2[k3*20+k4]+=prop[(k1*20+k3)*400+(k2*20+k4)];
            }
      if (allnumber!=0)
      {
        for (k=0;k<160000;k++)
          prop[k]=prop[k]/allnumber;
        for (k=0;k<400;k++)
        {
          pro1[k]=pro1[k]/allnumber;
          pro2[k]=pro2[k]/allnumber;
        }
      }
      allnumber=0.0;
      for (k1=0;k1<20;k1++)
        for (k2=0;k2<20;k2++)
        {
          if (pro1[k1*20+k2]==0)
          {
            continue;
          }
          else
          {
            for (k3=0;k3<20;k3++)
              for (k4=0;k4<20;k4++)
              {
                if (pro2[k3*20+k4]==0||prop[(k1*20+k3)*400+(k2*20+k4)]==0)
                {
                  continue;
                }
                else
                {
                  allnumber+=prop[(k1*20+k3)*400+(k2*20+k4)]*log(prop[(k1*20+k3)*400+(k2*20+k4)]/pro1[k1*20+k2]/pro2[k3*20+k4]);
                }
              }
          }
        }
      mys[i*l+j]=allnumber;
      mys[j*l+i]=allnumber;
      free(prop);
      free(pro1);
      free(pro2);
    }
  }
  free(edge);
  return mys;
}


/*
 *  lowly.h
 *  low-ell likelihood in the pixel domain
 *
 *  JF Cardoso and Simon Prunet.  January 2008.
 *
 */

#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_errno.h>
#include <gsl/gsl_sf_gamma.h>

#ifdef HAS_PMC
#include "invgamma.h"
#endif
#include "lowly_common.h"
#include "chealpix.h"

#ifndef __LOWLY__
#define __LOWLY__

// llll = low ell log-likelihood
typedef struct  {
  long npix_seen ;
  long npix,nside ;
  double * X_seen  ;
  double * F_seen  ;
  double * NoiseVar ;
  double * CosMat ;
  long lmax ;
  double * Wl  ;
  double * q_buf;
  double * X_seen_buf;
  double * F_seen_buf;
  double * covmat_buf;
  double * buffer;
  int isLog;
  int partial_nindex;
  int* partial_index;
  double *partial_cl;
  double v2;
  STRUCT_TIMER_DEFS;
} llll_struct ;

typedef llll_struct llll_partial_struct;

// create and initialize the structure holding the material to compute the likelihood
llll_struct* init_lowly(
                        double *X,          // map size npix
                        unsigned char *Mask,       // mask size npix
                        double* Foreground, // if !=NULL a foreground map
                        long npix,
                        char* ordering,     // ring or nested
                        double noisevar,    // microK/pixel
                        double v2,          // sigma^2 for foreground marginalization
                        double *Wl,         // wl Cl unit lmax+1 values
                        double lmax,
                        error **err );

void free_lowly(llll_struct** self);

// return the low-ell log-likelihood for spectrum C_l
double lowly_lkl(
    void * llll_str ,
    double * C_l ,
    error **err
    ) ;

// return the low-ell log-likelihood for log(Cl)
double lowly_lkl_logcl(
    void * llll_str,
    double * logCl,
    error **err
    );

llll_struct * init_lowly_log(llll_struct* self,error **err);

// These are wrappers on top of lowly_lkl and lowly_lkl_logcl
// in case only a subset of Cls are varied as parameters
double lowly_partial_lkl(void *llll_str, double *cl, error **err);
double lowly_partial_lkl_logcl(void *llll_str, double *logcl, error **err);

void free_lowly_partial(llll_partial_struct **elf);
llll_partial_struct* init_lowly_partial( llll_struct *orig,int n,int* index, double* cl,error **err) ;
llll_partial_struct* init_lowly_partial_logcl( llll_struct *orig,int n,int* index, double* cl,error **err) ;
double lowly_lkl_with_noise(void *llll_str, double *cl, error **err);

// ------ internals.  Used for computation ---------------------

// assign and compute the npix_seen * npix_seen matrix of the cosines between seen pixels
double * build_cosine_matrix (
    const long nside,
    const long * pixel_indices ,
    const long npix_seen ,
    const int ordering,
    error **err
    ) ;

// assign and compute \sum_l W_l C_l P_l(cos) (2l+1)/4pi
double * build_cov_matrix (
    double *orig,
    const double * CosMat ,
    const double * NoiseVar,
    const long npix_seen ,
    const long lmax ,
    const double * q,
   error **err
    ) ;

// assign and compute inverse covariance matrix, same inputs as above
double * build_invcov_matrix (
    double *orig,
    const double * cosmat,
    const double * noisevar,
    const long npix_seen,
    const long lmax,
    const double *q,
    error **err
    ) ;

// assign and compute W_l P_l(cos) (2l+1)/4pi
double * build_dcov_matrix (
    double *orig,
    const double *cosmat,
    const long npix_seen,
    const long lmax,
    const double wl,
    error **err
    );

// compute signal covariance for one value of z=cos(theta_ij)
double scalCov (
    const double z,
    const long lmax,
    const double *q
    ) ;

// compute signal covariance derivative (with respect to C_l) for one value of z=cos(theta_ij)
double scaldCov (
    const double z,
    const double wl, // = Wl[ell]
    const long ell
    ) ;

// tentative computation of signal covariance matrix using matrix (blas) algebra
double * matCov(
    const double * cosmat,
    const long npix_seen,
    const long lmax,
    const double * q);

// Computes gradient of likelihood with respect to C_l
double * grad_lowly_lkl (
    double * orig,
    llll_struct * self,
    const double * covmat,
    const double * invcovmat,
    error **err
    ) ;

// Computes diagonal of fisher matrix, with respect to C_l
double * diag_fisher_lowly_lkl (
    double *orig,
    llll_struct * self,
    //const double * invcovmat,
    const double *q,
    error **err) ;

// Computes peak of posterior by pseudo-Newton iteration
double * pseudo_newton(
    llll_struct * self,
    double * cl,
    long niter,
    error **err
    ) ;

#ifdef HAS_PMC
typedef struct {
  invgamma * _ivg;
  double *data,*wl,*N;
  size_t ndim;
} invgamma_prop;

invgamma_prop* invgamma_prop_init(size_t ndim,int* ell,double* clhat, double* fsky, double* wl,double noisevar,long nside,error **err);
void free_invgamma_prop(invgamma_prop **pelf);
double invgamma_prop_log_pdf(invgamma_prop *self, double *x, error **err);
long simulate_invgamma_prop(pmc_simu *psim,invgamma_prop *proposal, gsl_rng * r,parabox *pb,error **err);
size_t get_importance_weight_invgamma_prop(pmc_simu *psim, const invgamma_prop *m,
                                           posterior_log_pdf_func *posterior_log_pdf,
                                           void *extra, error **err);
void update_invgamma_prop(invgamma_prop *g,pmc_simu *psim,error **err);
invgamma_prop* invgamma_prop_init_new(size_t ndim,int* ell,double* clhat, double* fsky, double* wl,double* N,error **err);
#endif

void computeAlphaBeta(double *alpha, double *beta,int ell, double clhat, double fsky, double wl, double Nl);
double cl2xl(double cl, double wl, double Nl);

#define lowly_base           -10 + clowly_base
#define lowly_outofrange     -2 + lowly_base
#define lowly_negcl          -4 + lowly_base
#define lowly_fish_negative  -5 + lowly_base
#define lowly_spec_negative  -6 + lowly_base

#endif


/******************************************************************************
CosmoLike Configuration Space Covariances for Projected Galaxy 2-Point Statistics
https://github.com/CosmoLike/CosmoCov
by CosmoLike developers
******************************************************************************/

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

#include <fftw3.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_erf.h>
#define NR_END 1
#define FREE_ARG char*

#define EXIT_MISSING_FILE(ein, purpose, filename) if (!ein) {fprintf(stderr, "Could not find %s file %s\n",purpose,filename);exit(1);}

static double darg __attribute__((unused)),maxarg1 __attribute__((unused)), maxarg2 __attribute__((unused));

#define FMAX(a,b) (maxarg1=(a), maxarg2=(b), (maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
#define FMIN(a,b) (maxarg1=(a), maxarg2=(b), (maxarg1) < (maxarg2) ? (maxarg1) : (maxarg2))

//Note: SQR*SQR....gives undefined warning
static double sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
static double cubearg;
#define CUBE(a) ((cubearg=(a)) == 0.0 ? 0.0 : cubearg*cubearg*cubearg)
static double pow4arg;
#define POW4(a) ((pow4arg=(a)) == 0.0 ? 0.0 : pow4arg*pow4arg*pow4arg*pow4arg)

void SVD_inversion(gsl_matrix *cov, gsl_matrix *inverseSVD,int Nmatrix);
double interpol2d(double **f, int nx, double ax, double bx, double dx, double x,   int ny, double ay, double by, double dy, double y, double lower, double upper);
double interpol2d_fitslope(double **f, int nx, double ax, double bx, double dx, double x, int ny, double ay, double by, double dy, double y, double lower);

double interpol(double *f, int n, double a, double b, double dx, double x, double lower, double upper);
double interpol_fitslope(double *f, int n, double a, double b, double dx, double x, double lower);
void free_double_vector(double *v, long nl, long nh);
long *long_vector(long nl, long nh);
int *int_vector(long nl, long nh);
double *create_double_vector(long nl, long nh);
void free_double_matrix(double **m, long nrl, long nrh, long ncl, long nch);
double **create_double_matrix(long nrl, long nrh, long ncl, long nch);
int line_count(char *filename);

void error(char *s);
void cdgamma(fftw_complex x, fftw_complex *res);

double int_gsl_integrate_insane_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter);
double int_gsl_integrate_high_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter);
double int_gsl_integrate_medium_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter);
double int_gsl_integrate_low_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter);
double int_gsl_integrate_cov_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter);

typedef struct {
     double pi;
     double pi_sqr;
     double twopi;
     double ln2;
     double arcmin;
     double lightspeed;
}con;

con constants = {
     3.14159265358979323846, //pi
     9.86960440108935861883, //pisqr
     6.28318530717958647693, //twopi
     0.69314718,
     2.90888208665721580e-4, //arcmin
     299792.458 //speed of light km/s
};

typedef struct {
     double low;
     double medium;
     double high;
     double insane;
}pre;

pre precision= {
  1e-2, //low
  1e-3, //medium
  1e-5, //high
  1e-7 //insane
};

typedef struct {
     double a_min;
     double k_min_mpc;
     double k_max_mpc;
     double k_max_mpc_class;
     double k_min_cH0;
     double k_max_cH0;
     double P_2_s_min;
     double P_2_s_max;
     double xi_via_hankel_theta_min;
     double xi_via_hankel_theta_max;
     double xi_3d_rmin;
     double xi_3d_rmax;
     double M_min;
     double M_max;
}lim;

lim limits = {
//	0.19, //a_min (in order to compute z=4 WFIRST)
    1./(1.+10.), //a_min (z=10, needed for CMB lensing)
	6.667e-6, //k_min_mpc
	1.e3, //k_max_mpc
	50., //k_max_mpc_class
	2.e-2, //k_min_cH0
	3.e+6,  //k_max_cH0
	0.1,//P_2_s_min
	1.0e5,//P_2_s_max
	3.0e-7,//xi_via_hankel_theta_min
	0.12, //xi_via_hankel_theta_max
    1.e-5,//xi_3d_rmin
    1.e+0,//xi_3d_rmax
	1.e+6, //M_min
	1.e+17,//M_max
};

typedef struct {
     int N_a ;
     int N_k_lin;
     int N_k_nlin;
     int N_ell;
     int N_theta;
     int N_thetaH;
     int N_S2;
     int N_DS;
     int N_norm;
     int N_r_3d;
     int N_k_3d;
     int N_a_halo;
}Ntab;

Ntab Ntable = {
100, //N_a
500, //N_k_lin
500, //N_k_nlin
200, //N_ell
200, //N_theta
2048, //N_theta for Hankel
1000, //N_S2
1000, //N_DS
50, //N_norm
50, //N_r_3d
25, //N_k_3d
20, //N_a_halo
};

struct cos{
     int ORDER ;
     double vt_max;
     double vt_min;
     double vt_bin_max;
     double vt_bin_min;
     int ni ;
     int nj ;
};

void SVD_inversion(gsl_matrix *cov, gsl_matrix *inverseSVD,int Nmatrix)
{
    int i,j;
    gsl_matrix *V=gsl_matrix_calloc(Nmatrix,Nmatrix);
    gsl_matrix *U=gsl_matrix_calloc(Nmatrix,Nmatrix);
    gsl_vector *S=gsl_vector_calloc(Nmatrix);
    gsl_vector *work=gsl_vector_calloc(Nmatrix);

    gsl_matrix_memcpy(U,cov);

    gsl_linalg_SV_decomp(U,V,S,work);

    for (i=0;i<Nmatrix;i++){
        gsl_vector *b=gsl_vector_calloc(Nmatrix);
        gsl_vector *x=gsl_vector_calloc(Nmatrix);
        gsl_vector_set(b,i,1.0);
        gsl_linalg_SV_solve(U,V,S,b,x);
        for (j=0;j<Nmatrix;j++){
            gsl_matrix_set(inverseSVD,i,j,gsl_vector_get(x,j));
        }
        gsl_vector_free(b);
        gsl_vector_free(x);
    }
    gsl_matrix_free(V);
    gsl_matrix_free(U);
    gsl_vector_free(S);
    gsl_vector_free(work);
}

void invert_matrix_colesky(gsl_matrix *A)
{
  gsl_linalg_cholesky_decomp (A);  // Adummy will be overwritten  */
  gsl_linalg_cholesky_invert (A);
}

double int_gsl_integrate_insane_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter)
{
  double res, err;
  gsl_integration_cquad_workspace *w = gsl_integration_cquad_workspace_alloc(niter);
  gsl_function F;
  F.function = func;
  F.params  = arg;
 gsl_integration_cquad(&F,a,b,0,precision.insane,w,&res,&err,0);
  if(NULL!=error)
    *error=err;
  gsl_integration_cquad_workspace_free(w);
  return res;
}

double int_gsl_integrate_high_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter)
{
  double res, err;
  gsl_integration_cquad_workspace *w = gsl_integration_cquad_workspace_alloc(niter);
  gsl_function F;
  F.function = func;
  F.params  = arg;
 gsl_integration_cquad(&F,a,b,0,precision.high,w,&res,&err,0);
  if(NULL!=error)
    *error=err;
  gsl_integration_cquad_workspace_free(w);
  return res;
}

double int_gsl_integrate_medium_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter)
{
  double res, err;
  gsl_integration_cquad_workspace *w = gsl_integration_cquad_workspace_alloc(niter);
  gsl_function F;
  F.function = func;
  F.params  = arg;
 gsl_integration_cquad(&F,a,b,0,precision.medium,w,&res,&err,0);
  if(NULL!=error)
    *error=err;
  gsl_integration_cquad_workspace_free(w);
  return res;
}

double int_gsl_integrate_low_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter)
{
  double res, err;
  gsl_integration_cquad_workspace *wcrude = gsl_integration_cquad_workspace_alloc(niter);
  gsl_function F;
  F.function = func;
  F.params  = arg;
 gsl_integration_cquad(&F,a,b,0,precision.low,wcrude,&res,&err,0);
  if(NULL!=error)
    *error=err;
  gsl_integration_cquad_workspace_free(wcrude);
  return res;
}

double int_gsl_integrate_cov_precision(double (*func)(double, void*),void *arg,double a, double b, double *error, int niter)
{
  double res, err;
  gsl_integration_cquad_workspace *wcrude = gsl_integration_cquad_workspace_alloc(niter);
  gsl_function F;
  F.function = func;
  F.params  = arg;
 gsl_integration_cquad(&F,a,b,1e-16,precision.high,wcrude,&res,&err,0);
  if(NULL!=error)
    *error=err;
  gsl_integration_cquad_workspace_free(wcrude);
  return res;
}

int line_count(char *filename)
{
  FILE *n;
  n = fopen(filename,"r");
  if (!n){printf("line_count: %s not found!\nEXIT!\n",filename);exit(1);}
  int ch =0, prev =0,number_of_lines = 0;

  do
    {
    prev = ch;
    ch = fgetc(n);
    if(ch == '\n')
    	number_of_lines++;
    } while (ch != EOF);
  fclose(n);
  // last line might not end with \n, but if previous character does, last line is empty
  if(ch != '\n' && prev !='\n' && number_of_lines != 0)
    number_of_lines++;
  return number_of_lines;
}

double **create_double_matrix(long nrl, long nrh, long ncl, long nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;

	/* allocate pointers to rows */
	m=(double **) calloc(nrow+NR_END,sizeof(double*));
	if (!m) error("allocation failure 1 in create_double_matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(double *) calloc(nrow*ncol+NR_END,sizeof(double));
	if (!m[nrl]) error("allocation failure 2 in create_double_matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

void free_double_matrix(double **m, long nrl, long nrh, long ncl, long nch)
		/* free a double matrix allocated by create_double_matrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

double *create_double_vector(long nl, long nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;

	v=(double *) calloc(nh-nl+1+NR_END,sizeof(double));
	if (!v) error("allocation failure in double vector()");
	return v-nl+NR_END;
}

int *int_vector(long nl, long nh)
			/* allocate a int vector with subscript range v[nl..nh] */
{
	int *v;

	v=(int *)calloc(nh-nl+1+NR_END, sizeof(int));
	if (!v) error("allocation failure in int vector()");
	return v-nl+NR_END;
}

long *long_vector(long nl, long nh)
			/* allocate a int vector with subscript range v[nl..nh] */
{
	long *v;

	v=(long *)calloc(nh-nl+1+NR_END,sizeof(long));
	if (!v) error("allocation failure in int vector()");
	return v-nl+NR_END;
}

void free_double_vector(double *v, long nl, long nh)
		/* free a double vector allocated with vector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void error(char *s)
{
	printf("error:%s\n ",s);
	exit(1);
}

/* ============================================================ *
 * Interpolates f at the value x, where f is a double[n] array,	*
 * representing a function between a and b, stepwidth dx.	*
 * 'lower' and 'upper' are powers of a logarithmic power law	*
 * extrapolation. If no	extrapolation desired, set these to 0	*
 * ============================================================ */
double interpol(double *f, int n, double a, double b, double dx, double x, double lower, double upper)
{
	double r;
	int  i;
	if (x < a) {
		if (lower==0.) {
			//error("value too small in interpol");
			return 0.0;
		}
		return f[0] + lower*(x - a);
	}
	r = (x - a)/dx;
	i = (int)(floor(r));
	if (i+1 >= n) {
		if (upper==0.0) {
			if (i+1==n) {
				return f[i];  /* constant extrapolation */
			} else {
				//error("value too big in interpol");
				return 0.0;
			}
		} else {
			return f[n-1] + upper*(x-b); /* linear extrapolation */
		}
	} else {
		return (r - i)*(f[i+1] - f[i]) + f[i]; /* interpolation */
	}
}
double interpol_fitslope(double *f, int n, double a, double b, double dx, double x, double lower)
{
	double r;
	int  i,fitrange;
	if (x < a) {
		if (lower==0.) {
			//error("value too small in interpol");
			return 0.0;
		}
		return f[0] + lower*(x - a);
	}
	r = (x - a)/dx;
	i = (int)(floor(r));
	if (i+1 >= n) {
		if (n > 50){fitrange =5;}
		else{fitrange = (int)floor(n/10);}
		double upper = (f[n-1] - f[n-1-fitrange])/(dx*fitrange);
		return f[n-1] + upper*(x-b); /* linear extrapolation */

	} else {
		return (r - i)*(f[i+1] - f[i]) + f[i]; /* interpolation */
	}
}

/* ============================================================ *
 * like interpol, but f beeing a 2d-function			*
 * 'lower' and 'upper' are the powers of a power law extra-	*
 * polation in the second argument				*
 * ============================================================ */
double interpol2d(double **f,
		      int nx, double ax, double bx, double dx, double x,
		      int ny, double ay, double by, double dy, double y,
		      double lower, double upper)
{
	double t, dt, s, ds;
	int i, j;
	if (x < ax) {
		return 0.;
//		error("value too small in interpol2d");
	}

	if (x > bx) {
		return 0.;//
//		printf("%le %le\n",x,bx);
	//	error("value too big in interpol2d");
	}
	t = (x - ax)/dx;
	i = (int)(floor(t));
	dt = t - i;
	if (y < ay) {
		return ((1.-dt)*f[i][0] + dt*f[i+1][0]) + (y-ay)*lower;
	} else if (y > by) {
		return ((1.-dt)*f[i][ny-1] + dt*f[i+1][ny-1]) + (y-by)*upper;
	}
	s = (y - ay)/dy;
	j = (int)(floor(s));
	ds = s - j;
	if ((i+1==nx)&&(j+1==ny)) {
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j];

	}
	if (i+1==nx){
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j]+ (1.-dt)*ds*f[i][j+1];
	}
	if (j+1==ny){
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j]+ dt*(1.-ds)*f[i+1][j];
	}
	return (1.-dt)*(1.-ds)*f[i][j] +(1.-dt)*ds*f[i][j+1] + dt*(1.-ds)*f[i+1][j] + dt*ds*f[i+1][j+1];
}
double interpol2d_fitslope(double **f,
		      int nx, double ax, double bx, double dx, double x,
		      int ny, double ay, double by, double dy, double y,
		      double lower)
{
	double t, dt, s, ds, upper;
	int i, j, fitrange;
	if (x < ax) {
		return 0.;
//		error("value too small in interpol2d");
	}

	if (x > bx) {
		return 0.;
//	printf("%le %le\n",x,bx);
//	  error("value too big in interpol2d");
	}
	t = (x - ax)/dx;
	i = (int)(floor(t));
	dt = t - i;
	if (y < ay) {
		return ((1.-dt)*f[i][0] + dt*f[i+1][0]) + (y-ay)*lower;
	} else if (y > by) {
		if (ny > 25){fitrange =5;}
		else{fitrange = (int)floor(ny/5);}
		upper = ((1.-dt)*(f[i][ny-1] - f[i][ny-1-fitrange])+dt*(f[i+1][ny-1] - f[i+1][ny-1-fitrange]))/(dy*fitrange);
		return ((1.-dt)*f[i][ny-1] + dt*f[i+1][ny-1]) + (y-by)*upper;
	}
	s = (y - ay)/dy;
	j = (int)(floor(s));
	ds = s - j;
	if ((i+1==nx)&&(j+1==ny)) {
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j];

	}
	if (i+1==nx){
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j]+ (1.-dt)*ds*f[i][j+1];
	}
	if (j+1==ny){
	  //printf("%d %d\n",i+1,j+1);
	  return (1.-dt)*(1.-ds)*f[i][j]+ dt*(1.-ds)*f[i+1][j];
	}
	return (1.-dt)*(1.-ds)*f[i][j] +(1.-dt)*ds*f[i][j+1] + dt*(1.-ds)*f[i+1][j] + dt*ds*f[i+1][j+1];
}

void cdgamma(fftw_complex x, fftw_complex *res)
{
	double  xr, xi, wr, wi, ur, ui, vr, vi, yr, yi, t;

	xr = (double) x[0];
	xi = (double) x[1];

	if (xr<0) {
		wr = 1 - xr;
		wi = -xi;
	} else {
		wr = xr;
		wi = xi;
	}

	ur = wr + 6.00009857740312429;
	vr = ur * (wr + 4.99999857982434025) - wi * wi;
	vi = wi * (wr + 4.99999857982434025) + ur * wi;
	yr = ur * 13.2280130755055088 + vr * 66.2756400966213521 +
			0.293729529320536228;
	yi = wi * 13.2280130755055088 + vi * 66.2756400966213521;
	ur = vr * (wr + 4.00000003016801681) - vi * wi;
	ui = vi * (wr + 4.00000003016801681) + vr * wi;
	vr = ur * (wr + 2.99999999944915534) - ui * wi;
	vi = ui * (wr + 2.99999999944915534) + ur * wi;
	yr += ur * 91.1395751189899762 + vr * 47.3821439163096063;
	yi += ui * 91.1395751189899762 + vi * 47.3821439163096063;
	ur = vr * (wr + 2.00000000000603851) - vi * wi;
	ui = vi * (wr + 2.00000000000603851) + vr * wi;
	vr = ur * (wr + 0.999999999999975753) - ui * wi;
	vi = ui * (wr + 0.999999999999975753) + ur * wi;
	yr += ur * 10.5400280458730808 + vr;
	yi += ui * 10.5400280458730808 + vi;
	ur = vr * wr - vi * wi;
	ui = vi * wr + vr * wi;
	t = ur * ur + ui * ui;
	vr = yr * ur + yi * ui + t * 0.0327673720261526849;
	vi = yi * ur - yr * ui;
	yr = wr + 7.31790632447016203;
	ur = log(yr * yr + wi * wi) * 0.5 - 1;
	ui = atan2(wi, yr);
	yr = exp(ur * (wr - 0.5) - ui * wi - 3.48064577727581257) / t;
	yi = ui * (wr - 0.5) + ur * wi;
	ur = yr * cos(yi);
	ui = yr * sin(yi);
	yr = ur * vr - ui * vi;
	yi = ui * vr + ur * vi;
	if (xr<0) {
		wr = xr * 3.14159265358979324;
		wi = exp(xi * 3.14159265358979324);
		vi = 1 / wi;
		ur = (vi + wi) * sin(wr);
		ui = (vi - wi) * cos(wr);
		vr = ur * yr + ui * yi;
		vi = ui * yr - ur * yi;
		ur = 6.2831853071795862 / (vr * vr + vi * vi);
		yr = ur * vr;
		yi = ur * vi;
	}

	(*res)[0]=yr; (*res)[1]=yi;
}


/*
** generate permutation table using exchangeability information
**
** G.Lohmann, July 2018
*/
#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/option.h>
#include <viaio/mu.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

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

extern gsl_matrix *XRead2ndLevel(VString);
extern void XReadExchange(VString filename,int *exchange,int n);
extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);

void genperm(long seed,int *exchange,int signswitch,
	     int **permtable,int **signtable,int nimages,int numperm,gsl_vector *contrast)
{
  int i,j,k,m,kmax;

  /* ini permutation and sign-switching tables */
  for (i = 0; i < numperm; i++) {
    permtable[i] = (int *) VCalloc(nimages,sizeof(int));
    signtable[i] = (int *) VCalloc(nimages,sizeof(int));
    for (j=0; j<nimages; j++) permtable[i][j] = j;
    for (j=0; j<nimages; j++) signtable[i][j] = 1;
  }

  /* ini random number generator */
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  /* sign switching only */
  if (signswitch >= 0) {
    for (i=0; i<numperm; i++) {
      for (j=0; j<nimages; j++) {
	if (gsl_ran_bernoulli (rx,(double)0.5) == 1) signtable[i][j] = -1;
      }
    }
    return;  /* stop here, no permutations */
  }

  /* get max number of exchange groups */
  kmax=0;
  for (j=0; j<nimages; j++) {
    if (exchange[j] > kmax) kmax = exchange[j];
  }

  /* permutations of rows of the design matrix */
  int *base = (int *) VCalloc(nimages,sizeof(int));
  for (i = 0; i < numperm; i++) {

    /* for each group */
    for (k=0; k<=kmax; k++) {
      m = 0;
      for (j=0; j<nimages; j++) {
	if (exchange[j] == k) {
	  base[m] = j;
	  m++;
	}
      }
      if (m < 1) continue;

      /* random shuffle within group */
      gsl_ran_shuffle (rx, base, m, sizeof (int));

      m = 0;
      for (j=0; j<nimages; j++) {
	if (exchange[j] == k) {
	  permtable[i][j] = base[m];
	  m++;
	}
      }
    }
  }
  VFree(base);
}

/* for zero variance replace permutations with sign switching (only for non-nuisance columns) */
int SignSwitch (gsl_matrix *X,gsl_vector *contrast,int *permflag)
{
  int i,j;
  double s1=0,s2=0,nx=0,u=0,mean=0,var=0;

  int signswitch = -1;
  for (j=0; j<X->size2; j++) {
    if (permflag[j] == 0) continue;
    if (fabs(contrast->data[j]) < TINY) continue;
    s1 = s2 = nx = 0;
    for (i=0; i<X->size1; i++) {
      u = gsl_matrix_get(X,i,j);
      s1 += u;
      s2 += u*u;
      nx++;
    }
    mean = s1/nx;
    var = (s2 - nx * mean * mean) / (nx - 1.0);
    if (var < TINY && signswitch >= 0)
      VError(" Implausible design, two columns have zero variance: %d %d",signswitch+1,j+1);
    if (var < TINY) signswitch = j;
  }
  return signswitch;
}


#ifndef TSNE_H
#define TSNE_H

/* Cmake will define ${LIBRARY_NAME}_EXPORTS on Windows when it
configures to build a shared library. If you are going to use
another build system on windows or create the visual studio
projects by hand you need to define ${LIBRARY_NAME}_EXPORTS when
building a DLL on windows.
*/
// We are using the Visual Studio Compiler and building Shared libraries

#if defined (_WIN32)
#if defined(examplelib_EXPORTS)
#define DLLEXPORT __declspec(dllexport)
#else
#define DLLEXPORT __declspec(dllimport)
#endif
#else
#define DLLEXPORT
#endif
#ifndef SWIG
#include <math.h>
#include <iostream>    // std::cout, std::fixed
#include <iomanip>      // std::setprecision
#include <stdlib.h>
#include <cstring>
#include <time.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <stdio.h>
#include <queue>
#include <limits>
#include <iostream>
#include <cfloat>
#include <limits>
#include <armadillo>
using namespace std;
using namespace arma;
#include "quadtree.h"
#include "vptree.h"

extern "C" {
#include <cblas.h>
}
#else
#endif
/*
 *  tsne.h
 *  Header file for t-SNE.
 *
 *  Created by Laurens van der Maaten.
 *  Copyright 2012, Delft University of Technology. All rights reserved.
 *
 */

static inline double sign(double x)
{
    return (x == .0 ? .0 : (x < .0 ? -1.0 : 1.0));
}

class DLLEXPORT TSNE {
public:
    void run(double* X, int N, int D, double* Y, int no_dims, double perplexity, double theta);
    bool load_data(const char * filename,double** data, int* n, int* d, double* theta, double* perplexity);
    void save_data(const char* filename,double* data, int* landmarks, double* costs, int n, int d);

    void symmetrizeMatrix(int** row_P, int** col_P, double** val_P, int N); // should be static?!

private:
    void computeGradient(double* P, int* inp_row_P, int* inp_col_P, double* inp_val_P, double* Y, int N, int D, double* dC, double theta);
    void computeExactGradient(double* P, double* Y, int N, int D, double* dC);
    double evaluateError(double* P, double* Y, int N);
    double evaluateError(int* row_P, int* col_P, double* val_P, double* Y, int N, double theta);
    void zeroMean(double* X, int N, int D);
    void computeGaussianPerplexity(double* X, int N, int D, double* P, double perplexity);
    void computeGaussianPerplexity(double* X, int N, int D, int** _row_P, int** _col_P, double** _val_P, double perplexity, int K);
    void computeGaussianPerplexity(double* X, int N, int D, int** _row_P, int** _col_P, double** _val_P, double perplexity, double threshold);
    void computeSquaredEuclideanDistance(double* X, int N, int D, double* DD);
    double randn();
};
/* the following Math  class is used to test the python wrapper work for basic pointer manipulation*/
class DLLEXPORT Math4py {
public:
    Math4py();
    int pi() const;
    void pi(int pi);
private:
    int _pi;
};
int DLLEXPORT fact(int n);

#endif


#ifndef _PK_SLOPE_H_
#define _PK_SLOPE_H_

#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include <vector_types.h>
#include <gsl/gsl_deriv.h>
#include "cspline.h"
#include "file_check.h"

template <typename T> class pkSlope{
    int N;
    std::vector<T> k, Pk, n;
    cspline<T> slope;

    void get_pk_spline();

    void calculate_n();

    void get_n_spline();

    public:
        cspline<T> power;

        pkSlope();

        pkSlope(std::string in_pk_file);

        void initialize(std::string in_pk_file);

        void calculate();

        void get_device_spline(std::vector<float4> &d_spline);

        void get_device_spline(std::vector<double4> &d_spline);

};

template <typename T> double f(double x, void *params) {
    cspline<T> Pow = *(cspline<T> *)params;
    return Pow.evaluate(x);
}

template <typename T> void pkSlope<T>::get_pk_spline() {
    pkSlope<T>::power.initialize(pkSlope<T>::k, pkSlope<T>::Pk);
}

template <typename T> void pkSlope<T>::calculate_n() {
    gsl_function F;
    double result, err;

    F.function = &f<T>;
    F.params = &power;

    for (int i = 0; i < pkSlope<T>::N; ++i) {
        if (i == 0) {
            gsl_deriv_forward(&F, pkSlope<T>::k[i], 1e-8, &result, &err);
        } else if (i == pkSlope<T>::N - 1) {
            gsl_deriv_backward(&F, pkSlope<T>::k[i], 1e-8, &result, &err);
        } else {
            gsl_deriv_central(&F, pkSlope<T>::k[i], 1e-8, &result, &err);
        }
        pkSlope<T>::n.push_back(result);
    }
}

template <typename T> void pkSlope<T>::get_n_spline() {
    pkSlope<T>::slope.initialize(pkSlope<T>::k, pkSlope<T>::n);
}

template <typename T> pkSlope<T>::pkSlope() {
    pkSlope<T>::N = 0;
}

template <typename T> pkSlope<T>::pkSlope(std::string in_pk_file) {
    pkSlope<T>::initialize(in_pk_file);
}

template <typename T> void pkSlope<T>::initialize(std::string in_pk_file) {
    if (check_file_exists(in_pk_file)) {
        std::ifstream fin(in_pk_file);
        while(!fin.eof()) {
            T kt, pt;
            fin >> kt >> pt;
            if (!fin.eof()) {
                pkSlope<T>::k.push_back(log10(kt));
                pkSlope<T>::Pk.push_back(log10(pt));
            }
        }
        fin.close();

        pkSlope<T>::N = pkSlope<T>::k.size();
    }
}

template <typename T> void pkSlope<T>::calculate() {
    pkSlope<T>::get_pk_spline();
    pkSlope<T>::calculate_n();
    pkSlope<T>::get_n_spline();
}

template <typename T> void pkSlope<T>::get_device_spline(std::vector<float4> &d_spline) {
    pkSlope<T>::slope.set_pointer_for_device(d_spline);
}

template <typename T> void pkSlope<T>::get_device_spline(std::vector<double4> &d_spline) {
    pkSlope<T>::slope.set_pointer_for_device(d_spline);
}

#endif


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_statistics_double.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_iarray.h"
#include "tz_stack_attribute.h"
#include "tz_image_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_voxel_linked_list.h"
#include "tz_stack_sampling.h"
#include "tz_matlabio.h"
#include "tz_darray.h"
#include "tz_stack_draw.h"
#include "tz_stack_code.h"
#include "tz_stack_bwmorph.h"
#include "tz_stack.h"
#include "tz_voxel_graphics.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[]) {
  char neuron_name[100];
  //char matlab_path[100];

  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  /*
  if (argc < 3) {
    strcpy(matlab_path, "/Applications/MATLAB74/bin/matlab");
  } else {
    strcpy(matlab_path, argv[2]);
  }
  */

  char file_path[100];
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);

  Stack *stack = Read_Stack(file_path);
  //Stack_Not(stack, stack);

  //Set_Matlab_Path(matlab_path);
  //Stack *dist = Stack_Bwdist(stack);
  /*
  Objlabel_Workspace *ow = New_Objlabel_Workspace();
  ow->conn = 18;
  Stack_Not(stack, stack);
  Stack *dist = Stack_Boundary_Code(stack, NULL, ow);
  Stack_Not(stack, stack);
  */

  Stack *dist = Stack_Bwdist_L_U16(stack, NULL, 1);

  sprintf(file_path, "../data/%s/dist.tif", neuron_name);
  Write_Stack(file_path, dist);
  printf("Distance map %s created.\n", file_path);

  /*
  Write_Stack(file_path,
	      Scale_Double_Stack((double *)dist->array, stack->width,
				 stack->height, stack->depth, GREY));
  */

  Kill_Stack(stack);

  //Stack *seeds = Stack_Local_Max(dist, NULL, STACK_LOCMAX_FLAT);
  //Stack_Clean_Locmax(dist, seeds);

  Stack *seeds = Stack_Locmax_Region(dist, 26);
  //Write_Stack("../data/test.tif", seeds);

  Object_3d_List *objs = Stack_Find_Object_N(seeds, NULL, 1, 0, 26);
  Zero_Stack(seeds);
  int objnum = 0;
  while (objs != NULL) {
    Object_3d *obj = objs->data;
    Voxel_t center;
    Object_3d_Central_Voxel(obj, center);
    Set_Stack_Pixel(seeds, center[0], center[1], center[2], 0, 1);
    objs = objs->next;
    objnum++;
  }
  printf("objnum: %d\n", objnum);
  sprintf(file_path, "../data/%s/seeds.tif", neuron_name);
  Write_Stack(file_path, seeds);
  printf("%s (seed mask) created.\n", file_path);

  /*
  int max_idx = 0;
  double max_dist = darray_max((double *) (dist->array),
			       Stack_Voxel_Number(dist),
			       &max_idx);
  */

  Voxel_List *list = Stack_To_Voxel_List(seeds);
  printf("%d\n", Voxel_List_Length(list));
  int *seed_offset = (int *) malloc(sizeof(int) * Voxel_List_Length(list));
  int i;
  int idx = Voxel_List_Length(list) - 1;
  int stack_length = Stack_Voxel_Number(seeds);
  for (i = 0; i < stack_length; i++) {
    if (seeds->array[i] > 0) {
      seed_offset[idx--] = i;
    }
  }

  if (idx != -1) {
    TZ_WARN(ERROR_DATA_VALUE);
  }

  sprintf(file_path, "../data/%s/seed_offset.ar", neuron_name);
  iarray_write(file_path, seed_offset, Voxel_List_Length(list));
  printf("%s (seed indices) created.\n", file_path);

  Pixel_Array *pa = Voxel_List_Sampling(dist, list);

  sprintf(file_path, "../data/%s/seeds.pa", neuron_name);
  Pixel_Array_Write(file_path, pa);
  printf("%s (seed values d^2) created.\n", file_path);

  //double *pa_array = (double *) pa->array;
  uint16 *pa_array16 = (uint16 *) pa->array;
  double *pa_array = darray_malloc(pa->size);
  for (i = 0; i < pa->size; i++) {
    pa_array[i] = sqrt(pa_array16[i]);
  }

  sprintf(file_path, "../data/%s/seeds.ar", neuron_name);
  darray_write(file_path, pa_array, pa->size);
  printf("%s (seed values d) created.\n", file_path);

  /*
  printf("mean: %g, std: %g\n", gsl_stats_mean(pa_array, 1, pa->size),
	 sqrt(gsl_stats_variance(pa_array, 1, pa->size)));
  printf("%g\n", gsl_stats_max(pa_array, 1, pa->size));
  */
  //int max_idx2;
  //double max_dist2 = darray_max(pa_array, pa->size, &max_idx2);

  int n = Stack_Voxel_Number(seeds);
  //double *dist_array = (double *) dist->array;
  uint16 *dist_array = (uint16 *) dist->array;
  for (i = 0; i < n; i++) {
    if (dist_array[i] == 1) {
      seeds->array[i] = 0;
    }
  }
  sprintf(file_path, "../data/%s/large_seeds.tif", neuron_name);
  Write_Stack(file_path, seeds);
  printf("%s (seeds for d > 1) created.\n", file_path);

  Kill_Stack(dist);
  Kill_Stack(seeds);
  free(seed_offset);
  Kill_Pixel_Array(pa);

  return 0;
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include "ray.h"

#define pi M_PI

//#############################################################################
//#############################################################################
//Mode finder

//Data structure for julia cavity data and mode parameters
typedef struct {
    //Julia cavity info and functions -----------------------------------------
    //pointer to Boundary object
    void *bnd;
    //pointer to radius function
    double (*rfunc_p)(void *bnd,double theta);
    //pointer to (mutating) radius and normal vector angle function
    void (*rsys_p)(void *bnd, double theta, double results[]);
    //pointer to RefractiveIndex object
    void *idx;
    //pointer to refractive index value function
    double (*nfunc_p)(void *idx, double r, double theta);
    //pointer to (mutating) refractive index value and derivative function
    void (*nderiv_p)(void *idx, double r, double theta, double results[]);

    //mode-specific parameters ------------------------------------------------
    long order; //number of bounces in a period
    double rtol; //stopping criterion tolerance

    //preallocated arrays -----------------------------------------------------
    double *raypath_r;
    double *raypath_theta;
    long *bounceindices;
    double *bouncepts_chi;
    long *lengths;
    double *modebounces;
} modeinfo;

//#############################################################################
//Function for rootfinding
int rootfunc(const gsl_vector *x, void *params, gsl_vector *f){

    //params is pointer to modeinfo struct containing both cavity info and
    //simulation parameters
    modeinfo *mip = (modeinfo *)params;
    const long order = (*mip).order;
    double *raypath_r = (double *)(*mip).raypath_r;
    double *raypath_theta = (double *)(*mip).raypath_theta;
    long *bounceindices = (long *)(*mip).bounceindices;
    double *bouncepts_chi = (double *)(*mip).bouncepts_chi;
    long *lengths = (long *)(*mip).lengths;

    //Extract initial condition data (print for debugging)
    const double theta0 = gsl_vector_get(x,0), chi0 = gsl_vector_get(x,1);
    //printf("theta0 = %.8f, chi0 = %.8f\n",theta0,chi0);

    //Check for sensible values
    if(fabs(chi0) > pi/2){
        //Out of domain
        return GSL_EDOM;
    }

    //Setup initial ODE coordinate vector components
    double results[2];
    (*(*mip).rsys_p)((*mip).bnd,theta0,results);
    //result[0] = r0, results[1] = normang0
    const double n = (*(*mip).nfunc_p)((*mip).idx,results[0],theta0);
    const double phi0 = results[1] + chi0;
    const double pr0 = n*cos(phi0-theta0);
    const double ptheta0 = n*results[0]*sin(phi0-theta0);

    //Compute ray
    rayevolve(
        //Storage arrays
        raypath_r,raypath_theta,bounceindices,bouncepts_chi,lengths,
        //Initial conditions
        results[0],theta0,pr0,ptheta0,
        //Simulation parameters
        200.0,order+1,1e-12,1e-12,
        //Cavity parameters
        (*mip).bnd,(*mip).rfunc_p,(*mip).rsys_p,
        (*mip).idx,(*mip).nfunc_p,(*mip).nderiv_p);

    //Store difference in initial and final bounce locations (print for debugging)
    const double dtheta = fmod(raypath_theta[bounceindices[order]-1] - raypath_theta[bounceindices[0]-1] + pi, 2*pi) - pi;
    const double dchi = bouncepts_chi[order] - bouncepts_chi[0];
    gsl_vector_set(f,0,dtheta);
    gsl_vector_set(f,1,dchi);
    //printf("dtheta = %.8f, dchi = %.8f\n",dtheta,dchi);

    //Test stopping criterion
    if(gsl_multiroot_test_residual(f,(*mip).rtol) != GSL_CONTINUE){
        //Record mode bounce data in results array (to avoid running simulation again)
        double *modebounces = (double *)(*mip).modebounces;
        //theta values stored in first half of array (first column after reshaping)
        //chi values stored in second half of array (second column after reshaping)
        int i;
        for(i=0; i<order; i+=1){
            modebounces[i] = raypath_theta[bounceindices[i]-1];
            modebounces[order+i] = bouncepts_chi[i];
        }
    }

    return GSL_SUCCESS;
}

//#############################################################################
//Solver function
int findmode(
    //mode-specific variables
    long order, double modebounces[],
    //initial position on PSS
    double theta0, double chi0,
    //root-finding algorithm parameters
    double rtol, long maxiter,
    //Cavity parameters
    void *bnd, double (*rfunc_p)(void *bnd,double theta),
    void (*rsys_p)(void *bnd, double theta,double results[]),
    void *idx, double (*nfunc_p)(void *idx, double r, double theta),
    void (*nderiv_p)(void *idx, double r, double theta,double results[])){

        //Preallocate arrays for reuse
        long bounceindices[30], lengths[2];
        double raypath_r[50000],raypath_theta[50000],bouncepts_chi[30];

        //Condense input mode data into modeinfo structs
        modeinfo mi = {bnd,rfunc_p,rsys_p,idx,nfunc_p,nderiv_p,order,rtol,
            raypath_r,raypath_theta,bounceindices,bouncepts_chi,lengths,modebounces};

        //Store initial guess
        gsl_vector *x0 = gsl_vector_alloc(2);
        gsl_vector_set(x0,0,theta0);
        gsl_vector_set(x0,1,chi0);

        //Initiate solver
        const gsl_multiroot_fsolver_type *fs_t = gsl_multiroot_fsolver_dnewton;
        gsl_multiroot_fsolver *fs = gsl_multiroot_fsolver_alloc(fs_t,2);
        gsl_multiroot_function mrf = {rootfunc,2,&mi};
        gsl_multiroot_fsolver_set(fs,&mrf,x0);
        gsl_set_error_handler_off();

        int status, count = 0;

        //Iterate solver
        while(count != maxiter){
            status = gsl_multiroot_fsolver_iterate(fs);
            if(status != GSL_SUCCESS) break; //Some error has occurred

            gsl_vector *f = gsl_multiroot_fsolver_f(fs);
            if(gsl_multiroot_test_residual(f,rtol) != GSL_CONTINUE) break; //Done!

            count += 1;
        }

        //Free memory
        gsl_multiroot_fsolver_free(fs);
        gsl_vector_free(x0);

        //In case of error or aborted computation, indicate no result
        if((status != GSL_SUCCESS) || (count == maxiter)) return 1;

        return 0;
}


/*
 * Copyright 2008-2016 Jan Gasthaus
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef RANDOM_H_
#define RANDOM_H_

#include <gsl/gsl_rng.h>
#include <vector>
#include <cassert>
#include <algorithm> // for lower_bound
// only for debugging
#include <iostream>
#include "libplump/utils.h"

namespace gatsby { namespace libplump {

/*******************************************************************************
 * RANDOM NUMBER GENERATION AND SAMPLING BASED ON GSL
 ******************************************************************************/

/**
 * Initialize global RNG.
 *
 * This function has to be called before using any of the sampling functions.
 *
 * The RNG type and seed are determined by the environment variables
 * GSL_RNG_TYPE and GSL_RNG_SEED.
 */
void init_rng();

/**
 * Free the global RNG.
 */
void free_rng();

/**
 * Returns true with probability true_prob.
 */
bool coin(double true_prob);

/**
 * Returns a uniform integer between 0 and max-1.
 */
long int uniform_int(long int max);

/**
 * Sample from a discrete distribution on 0,...,MAX with the given PDF.
 *
 * The probability if returning x for x \in 0,...,MAX is given by
 *   pdf[x] / (\sum_{i=0,...,end_pos} pdf[i])
 * i.e. pdf is normalized so that the sum of all elements up to and including
 * element end_pos is 1.
 *
 * Algorithm:
 *   1) Compute CDF; normalizing constant Z = CDF[end_pos]
 *   2) Sample z ~ Uniform(0,Z)
 *   3) find the smallest element x of CDF that is larger than i using binary
 *      search
 *   4) return x
 *
 * Complexity: O(log MAX)
 */
int sample_unnormalized_pdf(std::vector<double> pdf, int end_pos = 0);

////////////////////////////////////////////////////////////////////////////////
//////////////////////    INLINE FUNCTION DEFINITIONS   ////////////////////////
////////////////////////////////////////////////////////////////////////////////

extern gsl_rng* global_rng;

/**
 * Returns true with probability true_prob.
 */
inline bool coin(double true_prob) {
    return (true_prob>gsl_rng_uniform(global_rng));
}

/**
 * Returns a uniform integer between 0 and max-1.
 */
inline long int uniform_int(long int max) {
    return gsl_rng_uniform_int(global_rng, max);
}

/**
 * Sample from a discrete distribution on 0,...,MAX with the given PDF.
 *
 * The probability if returning x for x \in 0,...,MAX is given by
 *   pdf[x] / (\sum_{i=0,...,end_pos} pdf[i])
 * i.e. pdf is normalized so that the sum of all elements up to and including
 * element end_pos is 1.
 *
 * Algorithm:
 *   1) Compute CDF; normalizing constant Z = CDF[end_pos]
 *   2) Sample z ~ Uniform(0,Z)
 *   3) find the smallest element x of CDF that is larger than i using binary
 *      search
 *   4) return x
 *
 * Complexity: O(log MAX)
 */
inline int sample_unnormalized_pdf(std::vector<double> pdf, int end_pos) {
    assert(pdf.size() > 0);
    assert(end_pos < pdf.size());
    assert(end_pos >= 0);

    // if end_pos == 0, use entire vector
    if (end_pos == 0) {
        end_pos = pdf.size()-1;
    }

    // compute CDF (inplace)
    for (int i = 0; i < end_pos; ++i) {
        assert(pdf[i] >= 0);
        pdf[i+1] += pdf[i];
    }

    assert(pdf[end_pos] > 0);

    // sample pos ~ Unigorm(0,Z)
    double z = gsl_rng_uniform_pos(global_rng)*pdf[end_pos];

    assert((z >= 0) && (z <= pdf[end_pos]));

    // Perform binary search for z using std::lower_bound.
    // lower_bound(begin, end, x) returns the first element within [begin,end)
    // that is equal or larger than x.
    int x = std::lower_bound(pdf.begin(), pdf.begin() + end_pos + 1, z)
         - pdf.begin();

    assert(x == 0 || pdf[x-1] != pdf[x]);
    assert(x < pdf.size());
    return x;

}

} } // namespace gatsby::libplump

#endif // RANDOM_H_


/*
Copyright (C) 2019-2020 JingWeiZhangHuai <jingweizhanghuai@163.com>
Licensed under the Apache License, Version 2.0; you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
*/

// #include <cblas.h>
#include "morn_tensor.h"

struct TensorConnectPara
{
    MLayer *prev;

    int height;
    int width;
    int channel;

    int res_valid;

    float rate;
    float decay;
    float momentum;
};
void *mTensorConnectPara(MList *ini,char *name)
{
    struct TensorConnectPara *para=(struct TensorConnectPara *)mMalloc(sizeof(struct TensorConnectPara));

    para->prev = mNetworkLayer(ini,mINIRead(ini,name,"prev"));
    mException((para->prev == NULL),EXIT,"invalid prev");
    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    para->height = 1; mINIRead(ini,name,"height" ,"%d",&(para->height ));
    para->width  = 1; mINIRead(ini,name,"width"  ,"%d",&(para->width  ));
    para->channel= 1; mINIRead(ini,name,"channel","%d",&(para->channel));

    para->rate    =0.001;if(mINIRead(ini,name,"rate"    ,"%f",&(para->rate    ))==NULL) mINIRead(ini,"para","rate"    ,"%f",&(para->rate    ));
    para->momentum= 0.9 ;if(mINIRead(ini,name,"momentum","%f",&(para->momentum))==NULL) mINIRead(ini,"para","momentum","%f",&(para->momentum));
    para->decay   = 0.01;if(mINIRead(ini,name,"decay"   ,"%f",&(para->decay   ))==NULL) mINIRead(ini,"para","decay"   ,"%f",&(para->decay   ));
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    return para;
}

struct HandleTensorConnect
{
    MMemoryBlock *weight;
    MMemoryBlock *update;
};
void endTensorConnect(struct HandleTensorConnect *handle)
{
    if(handle->weight != NULL) mMemoryBlockRelease(handle->weight);
    if(handle->update != NULL) mMemoryBlockRelease(handle->update);
}
#define HASH_TensorConnect 0xb8986c4a

struct HandleTensorConnect *TensorConnectSet(MLayer *layer)
{
    struct TensorConnectPara *para = (struct TensorConnectPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->tns;
    int device = out->device;

    MHandle *hdl=mHandle(out,TensorConnect);
    struct HandleTensorConnect *handle = (struct HandleTensorConnect *)(hdl->handle);
    if(layer->state != DFLT) return handle;

    int weight_height= para->channel*para->height*para->width;
    int weight_width =   in->channel*  in->height*  in->width +1;
    int data_size = weight_height*weight_width;

    mTensorRedefine(out,in->batch,para->channel,para->height,para->width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);

        if(handle->update != NULL) mMemoryBlockRelease(handle->update);
        handle->update =mMemoryBlockCreate(data_size*sizeof(float),device);
        memset(handle->update->data,0,data_size*sizeof(float));
        handle->update->flag = MORN_HOST;
        mMemoryBlockWrite(handle->update);
    }

    if(handle->weight != NULL) mMemoryBlockRelease(handle->weight);
    handle->weight =mMemoryBlockCreate(data_size*sizeof(float),device);
    float *weight_data = (float *)(handle->weight->data);
    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/weight_width);
        for(int i=0;i<data_size;i++)
            weight_data[i] = scale*mNormalRand(0.0f,1.0f);//((float)mRand(-16384,16383))/16384.0f;
    }
    else
    {
        mNetworkParaRead(layer,"weight",weight_data,data_size*sizeof(float));
    }
    handle->weight->flag = MORN_HOST;
    mMemoryBlockWrite(handle->weight);

    hdl->valid = 1;
    return handle;
}

void mTensorConnectForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    mException(strcmp("Connect",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorConnectPara *para = (struct TensorConnectPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *out= layer->tns;
    int device = out->device;

    struct HandleTensorConnect *handle = TensorConnectSet(layer);

    int weight_height= out->height*out->width*out->channel;
    int weight_width =  in->height* in->width* in->channel +1;
    for(int b=0;b<in->batch;b++)
    {
        mSgemm(device,MORN_NO_TRANS,MORN_NO_TRANS,weight_height,1,weight_width,1.0f,handle->weight,weight_width,mTensorMemory(in,b),1,0.0f,mTensorMemory(out,b),1);
        printf("out_data[8]=%f\n",((float *)(out->data[b]))[8]);
    }

    layer->state = MORN_FORWARD;
}

void mTensorConnectBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    mException(strcmp("Connect",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorConnectPara *para = (struct TensorConnectPara *)(layer->para);
    MTensor *in  = para->prev->tns;
    MTensor *res = para->prev->res;
    MTensor *out = layer->res;
    int device = out->device;

    MHandle *hdl=mHandle(layer->tns,TensorConnect);
    struct HandleTensorConnect *handle = (struct HandleTensorConnect *)(hdl->handle);
    mException((hdl->valid==0),EXIT,"no forward operate");

    int weight_height= out->height*out->width*out->channel;
    int weight_width =  in->height* in->width* in->channel +1;
    float *update_data = (float*)(handle->update->data);

    mNetworkParaWrite(layer,"weight",handle->weight->data,weight_height*weight_width*sizeof(float));

    float beta = para->momentum;
    for(int b=0;b<in->batch;b++)
    {
        mSgemm(device,MORN_NO_TRANS,MORN_TRANS,weight_height,weight_width,1,1.0f,mTensorMemory(out,b),1,mTensorMemory(in,b),1,beta,handle->update,weight_width);
        printf("update_data[8]=%f\n",((float*)(handle->update->data))[8]);
        beta = 1.0;
    }

    if(para->res_valid) for(int b=0;b<in->batch;b++)
    {
        mSgemm(device,MORN_TRANS,MORN_NO_TRANS,weight_width,1,weight_height,1.0f,handle->weight,weight_width,mTensorMemory(out,b),1,((para->prev->state==MORN_FORWARD)?0.0f:1.0f),mTensorMemory(res,b),1);
        printf("res_data[8]=%f\n",((float *)(res->data[b]))[8]);
    }
    mSaxpby(device,weight_height*weight_width,(0.0f-(para->rate/(float)(in->batch))),handle->update,1,(1.0f-(para->decay*para->rate)),handle->weight,1);

    para->prev->state = MORN_BACKWARD;
}


#ifndef _GSLRNG_H
#define _GSLRNG_H

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <ctime>
#include <vector>
using namespace std;

namespace myGSL {
  class Rng;
  class PoissonGenerator;
};

typedef myGSL::Rng GSLRng;

class myGSL::Rng {
public:
  Rng(const gsl_rng_type * T = gsl_rng_taus2, unsigned long seed = time(NULL))
    : rng(gsl_rng_alloc(T)), discrete_t(NULL)
  { gsl_rng_set(rng,seed); }

  Rng(const Rng& R)
    : rng(gsl_rng_clone(R.constptr())), discrete_t(NULL)
  {}

  ~Rng() {
    gsl_rng_free(rng);
    if (discrete_t != NULL) discrete_free();
  }

  inline void set(unsigned long seed) { gsl_rng_set(rng,seed); }
  inline unsigned long min() { return gsl_rng_min(rng); }
  inline unsigned long max() { return gsl_rng_max(rng); }

  // uniform distributions
  inline unsigned long get() { return gsl_rng_get(rng); }
  inline double uniform() { return gsl_rng_uniform(rng); }
  inline double uniform_pos() { return gsl_rng_uniform_pos(rng); }
  inline double uniform(double a, double b) { return a + (b-a)*uniform(); }
  inline unsigned long uniform_int(unsigned long n) { return gsl_rng_uniform_int(rng,n); }

  // other distributions
  inline double gaussian(double mu, double s) { return gsl_ran_gaussian(rng,s) + mu; }
  inline unsigned long binomial(double r, unsigned n) { return gsl_ran_binomial(rng,r,n); }
  inline unsigned int poisson(double mu) { return gsl_ran_poisson(rng,mu); }
  inline double exponential(double mu) { return gsl_ran_exponential(rng,mu); }

  // discrete distributions
  inline void discrete_preproc(int K, const double * P) { discrete_t = gsl_ran_discrete_preproc(K,P); }
  inline int  get_discrete() { return gsl_ran_discrete(rng,discrete_t); }
  inline void discrete_free() { gsl_ran_discrete_free(discrete_t); discrete_t = NULL; }

  template<typename T> inline void shuffle(vector<T>& src, size_t first = 0,
      size_t last = -1) {
    if (first >= src.size()) return;
    if (last > src.size()) last = src.size();
    gsl_ran_shuffle(rng,&src[first],last-first,sizeof(T));
  }

  template<typename T> inline void choose(vector<T>& src, vector<T>& dest) {
    gsl_ran_choose(rng,&dest[0],dest.size(),&src[0],src.size(),sizeof(T));
  }

  inline gsl_rng * ptr() { return rng; }
  inline const gsl_rng * constptr() const { return rng; }

private:
  gsl_rng * rng;
  gsl_ran_discrete_t * discrete_t;
};

class myGSL::PoissonGenerator {
  public:
    double mu;
    Rng* rng;
    PoissonGenerator(double m, Rng* r) { mu = m; rng = r; }
    double operator()() { return rng->poisson(mu); }
};

#endif


/* Copyright 2017 Peter Williams and collaborators
 * Licensed under the MIT License.
 */

#include <Python.h>

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_poly.h>

#define COUNT(arr) (sizeof(arr) / sizeof(arr[0]))

typedef enum result_t {
    RESULT_OK = 0,
    RESULT_LAMBDA_FAILED = 1,
    RESULT_X_FAILED = 2,
    RESULT_GSL_ERROR = 3,
    RESULT_HIT_CRITICAL_POINT = 4,
} result_t;

typedef enum handedness_t {
    R_MODE = 1,
    L_MODE = -1,
} handedness_t;

typedef enum wave_filtering_t {
    WF_ALL_WAVES = 0,
    WF_NO_FORWARD_MODES = 1 << 0,
    WF_NO_BACKWARD_MODES = 1 << 1,
} wave_filtering_t;

typedef enum integration_type_t {
    ITYPE_BOUNCE_AVERAGED,
    ITYPE_LOCAL,
} integration_type_t;

typedef struct parameters_t {
    double E; /* dimensionless kinetic energy */
    double sin_alpha; /* sine of the pitch angle */
    double Omega_e; /* local electron gyrofrequency in rad/s */
    handedness_t handedness; /* handedness of wave; `s` in Summers/Shprits notation */
    double alpha_star; /* (cyclotron freq / plasma freq)**2 */
    double R; /* magnetic wave energy perturbation: (delta B / B)**2 */
    double x_m; /* center of wave frequency spectrum in units of cyclotron freq */
    double delta_x; /* width of wave frequency spectrum in units of cyclotron freq */
    double max_wave_latitude; /* maximum latitude at which waves are found, in radians */
    wave_filtering_t wave_filtering; /* which kinds of waves to filter out */
} parameters_t;

typedef struct state_t {
    parameters_t p;
    integration_type_t itype;
    result_t last_error;

    double gamma; /* Lorentz factor */
    double a; /* See Summers 2005, just after Eqn 24 */
    double b; /* See Summers 2005, just after Eqn 25 */
    double beta; /* velocity over speed of light */

    double latitude; /* current magnetic latitude, radians */
    double sin_alpha; /* current sin(alpha); varies since mu is conserved */
    double mu; /* current cos(alpha) */
    double Omega_e; /* current Omega_e; varies with latitude since it scales as B */

    int n_xy; /* Number of resonances encountered */
    double x[3]; /* x values of resonances */
    double y[3]; /* y values of resonances */
    double Q[3]; /* (1 - x cos alpha / y beta) */
    double R[3]; /* complex expression in summand */
} state_t;

typedef struct coefficients_t {
    double dimensionless_p; /* momentum over (m_0 * c) */
    double Daa;
    double err_Daa;
    double Dap_on_p;
    double err_Dpp_on_p2;
    double Dpp_on_p2;
    double err_Dap_on_p;
} coefficients_t;

static state_t *global_context = NULL;
static char global_err_msg[1024] = "";

static void
s05_error_handler(const char *reason, const char *file, int line, int gsl_errno)
{
    /* Go ahead and segfault if global_context is NULL. */

    int n;

    n = snprintf(global_err_msg, COUNT(global_err_msg),
                 "%s (%s:%d; %d = %s)", reason, file, line, gsl_errno, gsl_strerror(gsl_errno));
    if ((size_t) n > COUNT(global_err_msg))
        n = COUNT(global_err_msg) - 1;
    global_err_msg[n] = '\0';

    global_context->last_error = RESULT_GSL_ERROR;
}

void
summers2005_debug_hook(void)
{
    printf("Debug hook called.\n");
}

static gsl_integration_workspace *integ_workspace = NULL; /* We're lame and leak these */
static gsl_poly_complex_workspace *poly5_workspace = NULL;
static gsl_poly_complex_workspace *poly7_workspace = NULL;

const size_t INTEG_WS_SIZE = 512;
const double integ_abs_tol = 0.0;
const double integ_rel_tol = 1e-6;

const int lambda = -1; /* signifies that we're looking at electrons */
const double epsilon = 0.0005446; /* electron-to-proton mass ratio */
const double epsm1 =  -0.9994554; /* epsilon - 1; C is lame and can't do the math with consts! */
const double sigma = 2.; /* range of `x` that we consider */
const double pi_on_2nu = 0.89039; /* pi/2nu, nu = sqrt(pi)erf(sigma); see after Shprits 06 Eqn 5 */

static inline result_t
apply_latitude(double latitude, state_t *state)
{
    int i;

    if (state->last_error != RESULT_OK)
        return state->last_error;

    state->latitude = latitude;

    double scl = cos(latitude); /* sin of colatitude = cos of latitude */
    double r = sqrt(4 - 3 * scl * scl) / pow(scl, 6); /* B(lam) / B_eq */

    state->Omega_e = state->p.Omega_e * r;
    state->sin_alpha = state->p.sin_alpha * sqrt(r);
    state->mu = sqrt(1 - state->sin_alpha * state->sin_alpha); /* cos(alpha) */

    if (state->mu == 0.) /* happens at the very top of our bounce trajectory */
        state->mu = 1e-20;

    /* Find the critical `x` and `y` values. We do this following Appendix A
     * and Equation 24 of Summers 2005. */

    state->n_xy = 0;

    double c[5] = { 0 };
    double z[8] = { 0 };

    const int s = state->p.handedness;
    const double a = state->a;
    const double a2 = a * a;
    const double bm2 = pow(state->beta * state->mu, 2);

    c[0] = -a2 * epsilon;
    c[1] = a2 * s * epsm1 - 2 * a * epsilon;
    c[2] = a2 + 2 * a * s * epsm1 - epsilon + bm2 * (state->b + epsilon);
    c[3] = 2 * a + s * epsm1 - bm2 * s * epsm1;
    c[4] = 1 - bm2;

    gsl_poly_complex_solve(c, 5, poly5_workspace, z);

    if (state->last_error != RESULT_OK) {
        strncat(global_err_msg, " (while finding x/y roots)", COUNT(global_err_msg) - 1);
        return state->last_error;
    }

    for (i = 0; i < 4; i++) {
        if (z[2*i + 1] != 0.) /* Imaginary root? */
            continue;

        double x = z[2*i];

        if (x < 0) /* Non-physical root? */
            continue;

        if (x < state->p.x_m - sigma * state->p.delta_x || x > state->p.x_m + sigma * state->p.delta_x)
            continue; /* Out of waveband? */

        double y = (x + state->a) / (state->beta * state->mu);

        if ((state->p.wave_filtering & WF_NO_FORWARD_MODES) && y > 0)
            continue;

        if ((state->p.wave_filtering & WF_NO_BACKWARD_MODES) && y < 0)
            continue;

        state->x[state->n_xy] = x;
        state->y[state->n_xy] = y;
        state->n_xy++;
    }

    if (state->n_xy > 3) {
        snprintf(global_err_msg, COUNT(global_err_msg), "expect 0 to 3 X/Y solutions; got %d", state->n_xy);
        return RESULT_X_FAILED;
    }

    /* Now we calculate F(x,y) = dx/dy, following Appendix C of Summers 2005,
     * and related values that contribute to the sums in the integrands. `Q`
     * is `1 - x cosa / y beta` and `R` is the factor that is in common
     * between all three calculations. */

    for (i = 0; i < state->n_xy; i++) {
        double x = state->x[i];
        double y = state->y[i];

        c[0] = epsilon * (state->b + epsilon);
        c[1] = -0.5 * s * epsm1 * (state->b + 4 * epsilon);
        c[2] = 1 - 4 * epsilon + epsilon * epsilon;
        c[3] = 2 * s * epsm1;
        c[4] = 1.;

        double g = gsl_poly_eval(c, 5, x);
        double F = y * pow((x - s) * (x + s * epsilon), 2) / (x * g);

        state->Q[i] = 1 - x * state->mu / (y * state->beta);

        double k0 = pow((x - state->p.x_m) / state->p.delta_x, 2);
        double k1 = fabs(state->beta * state->mu - F);

        if (k1 < 1e-5) {
            snprintf(global_err_msg, COUNT(global_err_msg), "hit a critical point (%f, %f, %f)",
                     state->sin_alpha, x, y);
            return RESULT_HIT_CRITICAL_POINT;
        }

        state->R[i] = state->p.R * fabs(F) * exp(-k0) / (state->p.delta_x * k1);
    }

    return RESULT_OK;
}

typedef double (*gsl_signature)(double, void *);

static double
daa_integrand(double latitude, state_t *state)
{
    result_t r;

    if (state->last_error != RESULT_OK)
        return 0;

    if ((r = apply_latitude(latitude, state)) != RESULT_OK) {
        state->last_error = r;
        return 0;
    }

    int i;
    double v = 0;

    for (i = 0; i < state->n_xy; i++)
        v += state->R[i] * pow(state->Q[i], 2);

    if (state->itype == ITYPE_BOUNCE_AVERAGED)
        v *= state->mu * pow(cos(state->latitude), 7);

    return v * state->Omega_e;
}

static double
dap_on_p_integrand(double latitude, state_t *state)
{
    result_t r;

    if (state->last_error != RESULT_OK)
        return 0;

    if ((r = apply_latitude(latitude, state)) != RESULT_OK) {
        state->last_error = r;
        return 0;
    }

    int i;
    double v = 0;

    for (i = 0; i < state->n_xy; i++)
        v += state->R[i] * state->Q[i] * state->x[i] / state->y[i];

    /* Here we transform the sqrt(1 + 3 sin^2 lambda) term to use the cos
     * instead */

    double cos_lam = cos(state->latitude);
    double sa = state->sin_alpha;

    if (state->itype == ITYPE_BOUNCE_AVERAGED)
        v *= state->mu * cos_lam * sqrt(4 - 3 * cos_lam * cos_lam) / sa;

    return v * state->Omega_e * sa;
}

static double
dpp_on_p2_integrand(double latitude, state_t *state)
{
    result_t r;

    if (state->last_error != RESULT_OK)
        return 0;

    if ((r = apply_latitude(latitude, state)) != RESULT_OK) {
        state->last_error = r;
        return 0;
    }

    int i;
    double v = 0;

    for (i = 0; i < state->n_xy; i++)
        v += state->R[i] * pow(state->x[i] / state->y[i], 2);

    /* Same transform as in dap_on_p. */

    double cos_lam = cos(state->latitude);
    double sa = state->sin_alpha;

    if (state->itype == ITYPE_BOUNCE_AVERAGED)
        v *= cos_lam * sqrt(4 - 3 * cos_lam * cos_lam) / state->mu;

    return v * state->Omega_e * sa * sa;
}

static result_t
calc_coefficients(parameters_t *params, coefficients_t *coeffs)
{
    gsl_error_handler_t *prev_handler;
    gsl_function integrand;
    state_t state;
    double c[7] = { 0 };
    double z[12] = { 0 };
    double lambda_m = -1;
    double result, abserr, k;
    int i;

    if (params->sin_alpha > 0.99985) /* ~= 89 degrees */
        params->sin_alpha = 0.99985;

    global_context = &state;
    prev_handler = gsl_set_error_handler(s05_error_handler);

    if (integ_workspace == NULL) {
        integ_workspace = gsl_integration_workspace_alloc(INTEG_WS_SIZE);
        poly5_workspace = gsl_poly_complex_workspace_alloc(5);
        poly7_workspace = gsl_poly_complex_workspace_alloc(7);
    }

    state.p = *params;
    state.itype = ITYPE_BOUNCE_AVERAGED;
    state.last_error = RESULT_OK;

    /* Figure out the mirroring latitude; Shprits (2006) equation 10. */

    double f0 = pow(state.p.sin_alpha, 4);
    c[0] = -4 * f0;
    c[1] = 3 * f0;
    c[6] = 1.;

    gsl_poly_complex_solve(c, 7, poly7_workspace, z);

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while finding lambda_m)", COUNT(global_err_msg) - 1);
        return state.last_error;
    }

    for (i = 0; i < 6; i++) {
        if (z[2*i + 1] == 0. && z[2*i] > 0) {
            lambda_m = acos(sqrt(z[2*i]));
            break;
        }
    }

    if (!(lambda_m >= 0 && lambda_m <= M_PI_2)) {
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        snprintf(global_err_msg, COUNT(global_err_msg), "failed to compute lambda_m (%.16lf)", lambda_m);
        return RESULT_LAMBDA_FAILED;
    }

    /* It can happen that lambda_m is just a tiiiiiny bit too large such that
     * when we compute the sine of the pitch angle at lambda_m, we get
     * something bigger than one. Witness our beautiful workaround.
     */

    double cos_lat = cos(lambda_m);

    if (sqrt(4 - 3 * cos_lat * cos_lat) / (pow(cos_lat, 6) * state.p.sin_alpha * state.p.sin_alpha) > 1.)
        lambda_m -= 1e-7;

    /* If a latitude limit on waves is imposed, truncate lambda_m further. */

    if (lambda_m > state.p.max_wave_latitude)
        lambda_m = state.p.max_wave_latitude;

    /* Pre-compute quantities that do not depend on lambda. When lambda
     * changes, B and alpha change. Consequently Omega_e changes too. In our
     * model the other parameters stay fixed.
     */

    state.gamma = state.p.E + 1;
    state.a = ((int) state.p.handedness) * lambda / state.gamma;
    state.b = (1 + epsilon) / state.p.alpha_star;
    state.beta = sqrt(state.p.E * (state.p.E + 2)) / (state.p.E + 1);
    coeffs->dimensionless_p = state.gamma * state.beta;

    /* Shprits 2006 eqn 9, credited to Lencheck+ 1971 and Shultz & Lanzerotti 1974: */

    double s0 = 1.38 - 0.32 * (params->sin_alpha + sqrt(params->sin_alpha));
    double f1 = pi_on_2nu / (pow(state.p.E + 1, 2) * s0);

    /* Let's integrate! First, D_aa. */

    integrand.function = (gsl_signature) daa_integrand;
    integrand.params = &state;

    gsl_integration_qag(
        &integrand,
        0., /* lower limit */
        lambda_m, /* upper limit */
        integ_abs_tol,
        integ_rel_tol,
        INTEG_WS_SIZE, /* allocated workspace size */
        GSL_INTEG_GAUSS51, /* integration rule */
        integ_workspace, /* workspace */
        &result, /* output: value of the integral */
        &abserr /* estimated absolute error */
    );

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Daa)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    k = f1 / (1 - state.p.sin_alpha * state.p.sin_alpha);
    coeffs->Daa = result * k;
    coeffs->err_Daa = abserr * k;

    /* D_ap/p. */

    integrand.function = (gsl_signature) dap_on_p_integrand;

    gsl_integration_qag(
        &integrand,
        0., /* lower limit */
        lambda_m, /* upper limit */
        integ_abs_tol,
        integ_rel_tol,
        INTEG_WS_SIZE, /* allocated workspace size */
        GSL_INTEG_GAUSS51, /* integration rule */
        integ_workspace, /* workspace */
        &result, /* output: value of the integral */
        &abserr /* estimated absolute error */
    );

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Dap)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    k = -f1 * state.p.sin_alpha / (state.beta * sqrt(1 - state.p.sin_alpha * state.p.sin_alpha));
    coeffs->Dap_on_p = result * k;
    coeffs->err_Dap_on_p = abserr * fabs(k);

    /* D_pp/p^2. */

    integrand.function = (gsl_signature) dpp_on_p2_integrand;

    gsl_integration_qag(
        &integrand,
        0., /* lower limit */
        lambda_m, /* upper limit */
        integ_abs_tol,
        integ_rel_tol,
        INTEG_WS_SIZE, /* allocated workspace size */
        GSL_INTEG_GAUSS51, /* integration rule */
        integ_workspace, /* workspace */
        &result, /* output: value of the integral */
        &abserr /* estimated absolute error */
    );

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Dpp)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    k = f1 * pow(state.beta, -2);
    coeffs->Dpp_on_p2 = result * k;
    coeffs->err_Dpp_on_p2 = abserr * k;

    /* Huzzah, all done. */

    gsl_set_error_handler(prev_handler);
    global_context = NULL;
    return RESULT_OK;
}

static result_t
calc_unaveraged_coefficients(parameters_t *params, coefficients_t *coeffs)
{
    gsl_error_handler_t *prev_handler;
    state_t state;

    global_context = &state;
    prev_handler = gsl_set_error_handler(s05_error_handler);

    if (integ_workspace == NULL) {
        integ_workspace = gsl_integration_workspace_alloc(INTEG_WS_SIZE);
        poly5_workspace = gsl_poly_complex_workspace_alloc(5);
        poly7_workspace = gsl_poly_complex_workspace_alloc(7);
    }

    state.p = *params;
    state.itype = ITYPE_LOCAL;
    state.last_error = RESULT_OK;

    /* The structure here is paralleling calc_coefficients for maintainability. */

    state.gamma = state.p.E + 1;
    state.a = ((int) state.p.handedness) * lambda / state.gamma;
    state.b = (1 + epsilon) / state.p.alpha_star;
    state.beta = sqrt(state.p.E * (state.p.E + 2)) / (state.p.E + 1);
    coeffs->dimensionless_p = state.gamma * state.beta;

    double f1 = pi_on_2nu / pow(state.p.E + 1, 2);

    /* Compute! It all works if we pretend latitude = 0 and set `itype` to LOCAL. */

    coeffs->Daa = daa_integrand(0., &state);

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Daa)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    coeffs->Daa *= f1;
    coeffs->err_Daa = 0;

    /* D_ap/p. */

    coeffs->Dap_on_p = dap_on_p_integrand(0., &state);

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Dap)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    coeffs->Dap_on_p *= f1 / state.beta;
    coeffs->err_Dap_on_p = 0;

    /* D_pp/p^2. */

    coeffs->Dpp_on_p2 = dpp_on_p2_integrand(0., &state);

    if (state.last_error != RESULT_OK) {
        strncat(global_err_msg, " (while computing Dpp)", COUNT(global_err_msg) - 1);
        gsl_set_error_handler(prev_handler);
        global_context = NULL;
        return state.last_error;
    }

    coeffs->Dpp_on_p2 *= f1 * pow(state.beta, -2);
    coeffs->err_Dpp_on_p2 = 0.;

    /* Huzzah, all done. */

    gsl_set_error_handler(prev_handler);
    global_context = NULL;
    return RESULT_OK;
}

static PyObject*
get_coeffs(PyObject *self, PyObject* args)
{
    int modespec, handspec, wave_filtering;
    parameters_t params;
    coefficients_t coeffs = { 0 };
    result_t r;

    if (!PyArg_ParseTuple(args, "iiidddddddd", &modespec, &handspec, &wave_filtering,
                          &params.E,
                          &params.sin_alpha,
                          &params.Omega_e,
                          &params.alpha_star,
                          &params.R,
                          &params.x_m,
                          &params.delta_x,
                          &params.max_wave_latitude))
        return NULL;

    if (handspec == 0)
        params.handedness = R_MODE;
    else if (handspec == 1)
        params.handedness = L_MODE;
    else {
        PyErr_SetString(PyExc_RuntimeError, "unexpected handedness magic constant");
        return NULL;
    }

    params.wave_filtering = (wave_filtering_t) wave_filtering;

    if (modespec == 0)
        r = calc_coefficients(&params, &coeffs);
    else if (modespec == 1)
        r = calc_unaveraged_coefficients(&params, &coeffs);
    else {
        PyErr_SetString(PyExc_RuntimeError, "unexpected mode magic constant");
        return NULL;
    }

    if (r != RESULT_OK) {
        PyErr_SetString(PyExc_RuntimeError, global_err_msg);
        return NULL;
    }

    return Py_BuildValue("ddddddd", coeffs.dimensionless_p,
                         coeffs.Daa, coeffs.err_Daa,
                         coeffs.Dap_on_p, coeffs.err_Dap_on_p,
                         coeffs.Dpp_on_p2, coeffs.err_Dpp_on_p2);
}

static PyMethodDef methods[] = {
    { "get_coeffs", get_coeffs, METH_VARARGS|METH_KEYWORDS,
      "(mode, handedness, wfilt, E, sa, Oe, a*, R, xm, dx, mwl) -> "
      "(dp, daa, udaa, dap/p, udap/p, dpp/p2, udpp/p2)" },
    { NULL, NULL, METH_NOARGS, NULL },
};

#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef module_def = {
    PyModuleDef_HEAD_INIT,
    "_summers2005",
    NULL,
    0,
    methods,
    NULL,
    NULL,
    NULL,
    NULL
};

# define INIT_RET_TYPE PyObject *
#else
# define INIT_RET_TYPE void
#endif

INIT_RET_TYPE
PyInit__summers2005(void)
{
#if PY_MAJOR_VERSION >= 3
    return PyModule_Create(&module_def);
#else
    PyImport_AddModule("_summers2005");
    Py_InitModule("_summers2005", methods);
#endif
}