#include <limits>
#include <ctime>
#include <fstream>
#include <cmath>
#include <random>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/special_functions/digamma.hpp>

#define LNINVSQRT2PI -0.918938533
#define INVSQRT2PI 0.3989422804
#define LN2 0.69314718056
#define PEAKWIDTH 0.325
#define MAX_EM_ITERATIONS 500
#define EM_CONVERGE 1e-3
//this is used for the minimum logValue as well - to distinguish it from an undef value which is 0
#define MIN_NOISE 1e-8
 //lookup tables
static double logx[32768]; //log base2
static double lnx[32768];  //natural logs

void init_logx();
template <class T> int fitLogNormEM(T *smooth, int nBins,vector<T> &peakBins,vector<T> &peakValues, vector<T> &peakMeans);
using namespace std;
using namespace boost::math;

template <class T> class MedianRecord{
	public:
	 vector<T> means0;
	 vector<T> means1;
	 T median0;
	 T median1;
		MedianRecord(){
   blank();
	 }
  void blank(){
			const T null=std::numeric_limits<T>::quiet_NaN();
			means0.clear();
			means1.clear();
			median0=null;
		 median1=null;
		}

	 void addValues(T mean0, T mean1){
			if(!mean0 || !mean1 || isnan(mean0) || isnan(mean1)) return;
			means0.push_back(mean0);
			means1.push_back(mean1);
		}
		void calculate_medians()	{
			if( means0.size() &&  means1.size() ){
    median0=median(means0.begin(),means0.end());
	   median1=median(means1.begin(),means1.end());
			}
		}
		private:
  template <typename Iterator>  T median(Iterator begin, Iterator end) {
	 Iterator middle = begin + (end - begin)/2;
	 std::nth_element(begin, middle, end);
  if ((end - begin) % 2) return *middle;
		Iterator lower_middle = std::max_element(begin, middle);
		return (*middle + *lower_middle) / 2.0;
 }
};

template <class T> class DeconRecord{
	public:
	T means0 [501];
	T means1 [501];
	T stdevs0 [501];
	T stdevs1 [501];
	T a0[501];
	T a1[501];
	T noiseFractions[501];
	T meansEst0[501];
	T meansEst1[501];
	int dataSizes[501];
	T loglikelihoods[501];
	bool flipped[501];
	bool converged[501];
	DeconRecord(){
		blank();
	}
	void blank(){
		const T null=std::numeric_limits<T>::quiet_NaN();
		fill(means0,means0+501,null);
		fill(means1,means1+501,null);
		fill(stdevs0,stdevs0+501,null);
		fill(stdevs1,stdevs1+501,null);
		fill(a0,a0+501,null);
		fill(a1,a1+501,null);
		fill(loglikelihoods,loglikelihoods+501,null);
		fill(meansEst0,meansEst0+501,null);
		fill(meansEst1,meansEst1+501,null);
		fill(dataSizes,dataSizes+501,0);
		fill(noiseFractions,noiseFractions+501,0);
		fill(flipped,flipped+501,0);
		fill(converged,converged+501,0);
	}
	void transfer(int i,T *meanVars){
		meansEst0[i]=meanVars[0];
		meansEst1[i]=meanVars[1];
	}

	void print_binary(FILE *fp){
		//prints out a short binary file with just the fields
		for (int i=11;i<501;i++){
			fwrite(means0+i,sizeof(T),1,fp);
			fwrite(means1+i,sizeof(T),1,fp);
		}
	}
	void print_binary(FILE *fp,int colorStart,int colorEnd){
		//prints out a short binary file with just the fields
		for (int i=colorStart;i<=colorEnd;i++){
			fwrite(means0+i,sizeof(T),1,fp);
			fwrite(means1+i,sizeof(T),1,fp);
		}
	}
	void print_text(FILE *fp,int verbosity){
		if(verbosity <0){
			print_binary(fp);
			return;
  }
		string headers[14]={"Analyte","Mean1","Mean2","StDev1","StDev2","a1","a2","LogLike","DataSize","NoiseFraction","Converged","Flip","CMean1","CMean2"};
		int nfields=3;
		if(verbosity ==1) nfields=8;
		else if (verbosity >1) nfields=14;
		for(int i=0;i<nfields-1;i++){
			fprintf(fp,"%s\t",headers[i].c_str());
		}
		fprintf(fp,"%s\n",headers[nfields-1].c_str());
		for (int i=1;i<501;i++){
			if(verbosity==0) fprintf(fp,"%3d\t%f\t%f\n",i,means0[i],means1[i]);
			else if(verbosity==1)	fprintf(fp,"%3d\t%f\t%f\t%f\t%f\t%f\t%f\t%e\n",i,means0[i],means1[i],stdevs0[i],stdevs1[i],a0[i],a1[i],loglikelihoods);
   else{
				if((means0[i] > means1 [i] && 	meansEst0[i] < meansEst1[i]) ||  (means0[i] < means1 [i] && 	meansEst0[i] > meansEst1[i])){
					flipped[i]=1;
				}
				fprintf(fp,"%3d\t%f\t%f\t%f\t%f\t%f\t%f\t%e\t%d\t%f\t%s\t%s\t%f\t%f\n",i,means0[i],means1[i],stdevs0[i],stdevs1[i],a0[i],a1[i],loglikelihoods[i],dataSizes[i],noiseFractions[i],"false\0true"+6*(int)converged[i],"false\0true"+6*(int)flipped[i],meansEst0[i],meansEst1[i]);
			}
		}
	}
	void print_text(FILE *fp,int colorStart,int colorEnd,int verbosity){
		if(verbosity <0){
			print_binary(fp,colorStart,colorEnd);
			return;
  }
		string headers[14]={"Analyte","Mean1","Mean2","StDev1","StDev2","a1","a2","LogLike","DataSize","NoiseFraction","Converged","Flip","CMean1","CMean2"};
		int nfields=3;
		if(verbosity ==1) nfields=8;
		else if (verbosity >1) nfields=14;
		for(int i=0;i<nfields-1;i++){
			fprintf(fp,"%s\t",headers[i].c_str());
		}
		fprintf(fp,"%s\n",headers[nfields-1].c_str());
		for (int i=colorStart;i<=colorEnd;i++){
			if(verbosity==0) fprintf(fp,"%3d\t%f\t%f\n",i,means0[i],means1[i]);
			else if(verbosity==1)	fprintf(fp,"%3d\t%f\t%f\t%f\t%f\t%f\t%f\t%e\n",i,means0[i],means1[i],stdevs0[i],stdevs1[i],a0[i],a1[i],loglikelihoods);
   else{
				if((means0[i] > means1 [i] && 	meansEst0[i] < meansEst1[i]) ||  (means0[i] < means1 [i] && 	meansEst0[i] > meansEst1[i])){
					flipped[i]=1;
				}
				//something wrong with format string near space <-fix
				fprintf(fp,"%3d\t%f\t%f\t%f\t%f\t%f\t%f\t%e\t%d\t%f\t%s\t%s \t%f\t%f\n",i,means0[i],means1[i],stdevs0[i],stdevs1[i],a0[i],a1[i],loglikelihoods[i],dataSizes[i],noiseFractions[i],"false\0true"+6*(int)converged[i],"false\0true"+6*(int)flipped[i],meansEst0[i],meansEst1[i]);
			}
		}
	}
};
template <class T> class EM_GMM{
	public:
	 //these are for lognormals usually expressed using base 2 - we change to natural logs so that normalization works
	 T var0,var1,var_est,var_lower,var_upper,mean0,mean1,meanEst0,meanEst1,mean_lower,mean_upper,a0,a1,p_est,p_upper,p_sigma;
	 T *data;
	 int nBins;
	 int nPoints;
	 int maxValue;
	 int minValue;
	 T loglikelihood;
	 bool converged;
	 bool normed; //normalized data?

	 EM_GMM(T *_data,bool _normed,int _nBins,int _nPoints, int _minValue,int _maxValue,T _mus[2],T _vars[2],T _a, T _p_est, T _p_sigma,T _p_upper, T _sigma_lower, T _sigma_est, T _sigma_upper){
			nPoints=_nPoints;
			mean0=_mus[0]*LN2;
			mean1=_mus[1]*LN2;
			meanEst0=mean0;
			meanEst1=mean1;
			var0=_vars[0]*LN2*LN2;
			var1=_vars[1]*LN2*LN2;;
			var_est=_sigma_est*_sigma_est*LN2*LN2;
			var_lower=_sigma_lower*_sigma_lower*LN2*LN2;
			var_upper=_sigma_upper*_sigma_upper*LN2*LN2;
			a0=_a;
			a1=1.0-a0;
			data=_data;
			nBins=_nBins;
			minValue=_minValue;
			mean_lower=lnx[minValue];
			maxValue=_maxValue;
			mean_upper=lnx[maxValue];
			p_est=_p_est;
   p_upper=_p_upper;
   p_sigma=_p_sigma;
   loglikelihood=0;
   normed=_normed;
			//hard estimate for p distorts the distribution
			//with large number of points (i.e. composite of many wells) the spread of possible p values needs incresased as using a binomial correction is the same as a hard limit
			//we assume that the mixture value is not exactly _p since we have estimates from .625 - .700 and there is probably some mixture error in addition to sampling variation which would be binomial
			//for lower numbers (i.e. individual wells) we use the estimate of mixture error to determine the beta distribution and add binomial correction
			//for higher numbers the data swamps the prior if we do this - so we explicitly calculate beta to limit the possible values

			//when given limiting based on distribution
			//calculate alpha and beta of beta distribution
			//alpha=((1-_p)/(_pErr*_pErr) -1/_p)*_p*_p
			//beta=alpha*(1/_p-1)
			//update is now (alpha+k)/(alpha+beta+n) instead of k/n
			//use this formula to derive beta-binomial correction to loglikelihood

			//will not use k/n as estimate of partition ratio - instead calculate an alpha and beta so that the
			//so that the maximum value if the entire sample partitioned 1 way the value equal to the limit
   //alpha =-(N*p_est*p_upper-N*p_est)/(p_upper-p_est)
   //beta=((N*p_est-N)*p_upper-N*p_est+N)/(p_upper-p_est)

   //for betabinomial loglikelihood for y successes in n trials
   //lgamma(n+a+b)-lgamma(y+alpha) -lgamma(n-y+beta) +(alpha+y-1)log(p)+(b+n-y-1)*log(1-p)

   //NB check and flip a1 a0 if a1>a0 - check - define alarger asmaller
	 }
	 void reset(T _mean0,T _mean1, T _var0,T _var1, T _a){
			mean0=_mean0*LN2;;
			mean1=_mean1*LN2;;
			var0=_var0*LN2*LN2;
			var1=_var1*LN2*LN2;
			a0=_a;
			a1=1.0-a0;
			loglikelihood=0;
		}
	 int seed_mclust(T input_p){
   //use m-clust to provide initial points
   //very similar to optimize function but with different estimates
   const int maxIterations=MAX_EM_ITERATIONS;
   const double converge=EM_CONVERGE;
   bool done=0;
   int nIterations=0;
   const int last=(nBins-1<maxValue)? nBins-1 : maxValue;
   T df=3.0,kp=0.01,svar=var_est;

  	//T alpha1=((1.0-p_est)/(p_sigma*p_sigma) -1.0/p_est)*p_est*p_est;
			//T beta1=alpha1*(1.0/(p_est-1));

   //fit mean stdevs to normals norma a*invsqrtpi/sigma * exp((-(x-mean)**2)/(2*var*var))
   //however the  invsqrtpi/sigma needs to be in natural log scale to properly normalize

   //adjust ln
   vector <int> map;
   vector <T> mylnx; //mapped copy of lnx
   vector <T> mydata;
   for (int j=minValue;j<=last; j++){
			//check for underflow
			 if(data[j] && !isnan(data[j])){
					map.push_back(j);
					mylnx.push_back(lnx[j]);
					mydata.push_back(data[j]);
				}
			}

   while(!done){
    double term0=log(a0)+LNINVSQRT2PI-log(sqrt(var0));
    double term1=log(a1)+LNINVSQRT2PI-log(sqrt(var1));
    double inv2Var0=1.0/(2.0*var0);
    double inv2Var1=1.0/(2.0*var1);

  //calculate weighted percentage that points in j come from distribution 1 or 2
  //then calculate stdevs and mean for expectation maximization
   double L=0,sum0=0,sumsq0=0,sum1=0,sumsq1=0,counts0=0,counts1=0;
   const int setSize=map.size();
   for (int m=0;m<setSize; m++){
				const int j=map[m];
			//check for underflow
			 	T logLike0=term0-inv2Var0*(mylnx[m]-mean0)*(mylnx[m]-mean0);
			 	T logLike1=term1-inv2Var1*(mylnx[m]-mean1)*(mylnx[m]-mean1);

			 	//check for underflows
			 	if((!logLike1 || std::isnan(logLike1)) && logLike0 && !std::isnan(logLike0)){
		     L+=mydata[m]*logLike0;
			 		 counts0+=mydata[m];
			 		 sum0+=mylnx[m]*mydata[m];
			 		 sumsq0+=mylnx[m]*mylnx[m]*mydata[m];
			 	}
			 	else if	((!logLike0 || std::isnan(logLike0)) && logLike1 && !std::isnan(logLike1)){
		     L+=mydata[m]*logLike1;
			 		 counts1+=mydata[m];
			 		 sum1+=mylnx[m]*mydata[m];
			 		 sumsq1+=mylnx[m]*mylnx[m]*mydata[m];
			 	}
			 	else{
			 		const T logRatio=logLike0-logLike1;
			 	 if(logRatio > 15){
		     L+=mydata[m]*logLike0;
			 		 counts0+=mydata[m];
			 		 sum0+=mylnx[m]*mydata[m];
			 		 sumsq0+=mylnx[m]*mylnx[m]*mydata[m];
						}
						else if (logRatio < -15){
		     L+=mydata[m]*logLike1;
			 		 counts1+=mydata[m];
			 		 sum1+=mylnx[m]*mydata[m];
			 		 sumsq1+=mylnx[m]*mylnx[m]*mydata[m];
						}
						else{
			 		 const T ratio=exp(logRatio);
			 		 const T r0=ratio/(ratio+1.0);
			 		 const T r1=1.0-r0;

					  T nPoints0=mydata[m]*r0;
				 	 T nPoints1=mydata[m]*r1;

				   counts0+=nPoints0;
				   counts1+=nPoints1;
				   L+=nPoints0*logLike0+nPoints1*logLike1;
				 	 sum0+=nPoints0*mylnx[m];
					  sumsq0+=nPoints0*mylnx[m]*mylnx[m];
				 	 sum1+=nPoints1*mylnx[m];
					  sumsq1+=nPoints1*mylnx[m]*mylnx[m];
					 }
					}
			}
		 //calculate estimate of mean
		 T mean0MLE=(sum0+kp*meanEst0)/(kp+counts0);
   T mean1MLE=(sum1+kp*meanEst1)/(kp+counts1);

   mean0MLE= (mean0MLE > mean_upper)? mean_upper : mean0MLE;
   mean0MLE= (mean0MLE < mean_lower)? mean_lower: mean0MLE;
   mean1MLE= (mean1MLE > mean_upper)? mean_upper: mean1MLE;
   mean1MLE= (mean1MLE < mean_lower)? mean_lower : mean1MLE;

   //estimate a0 a1
   //check if estimated a1 > a0 - if so switch means and var
   T a0MLE,a1MLE;
   if(counts1) a0MLE = counts0/(counts0+counts1);
   else a0MLE=1;

   if(a0MLE < .5){
			//swap terms
			 swap(mean0,mean1);
			 swap(var0,var1);
			 swap(mean0MLE,mean1MLE);
			 swap(counts1,counts0);
			 swap(a0,a1);
			 a0MLE=1.0-a0;
		 }
		 //find unormalized counts
		 T ucounts,ucounts0,ucounts1;
		 if(normed){
				T totalCounts=counts0+counts1;
				ucounts=(T) nPoints * totalCounts;
				ucounts0=(T) nPoints * counts0;
				ucounts1=(T) nPoints * counts1;
			}
		 else{
				ucounts=counts0+counts1;
				ucounts0=counts0;
				ucounts1=counts1;
			}

   a0MLE=(a0MLE > MIXTURE_RATIO + 2*MIXTURE_STD)?MIXTURE_RATIO + 2*MIXTURE_STD :a0MLE;
   a0MLE=(a0MLE < MIXTURE_RATIO - 2*MIXTURE_STD)?MIXTURE_RATIO - 2*MIXTURE_STD :a0MLE;
   a1MLE=1.0-a0MLE;
   T rawVar0MLE= sumsq0-mean0MLE*mean0MLE*counts0/(counts0+6.0);
   T rawVar1MLE= sumsq1-counts1-mean1MLE*mean1MLE*counts1/(counts1+6.0);

   if(isnan(rawVar0MLE)){
  		rawVar0MLE=(var_est+(kp*counts0)*(meanEst0-mean0MLE)*(meanEst0-mean0MLE))/(counts0+kp);
			}
			else{
				rawVar0MLE+=(var_est+(kp*counts0)*(meanEst0-mean0MLE)*(meanEst0-mean0MLE))/(counts0+kp);
			}
   if(isnan(rawVar1MLE)){
  		rawVar1MLE=(var_est+(kp*counts1)*(meanEst1-mean1MLE)*(meanEst1-mean1MLE))/(counts1+kp);
			}
			else{
				rawVar1MLE+=(var_est+(kp*counts1)*(meanEst1-mean1MLE)*(meanEst1-mean1MLE))/(counts1+kp);
			}

   T var0MLE=rawVar0MLE;
   T var1MLE=rawVar1MLE;

  //enforce limits
   var0MLE=(var0MLE > var_upper)? var_upper : var0MLE;
   var0MLE=(var0MLE < var_lower)? var_lower : var0MLE;
   var1MLE=(var1MLE > var_upper)? var_upper : var1MLE;
   var1MLE=(var1MLE < var_lower)? var_lower : var1MLE;

   if(fabs(mean0MLE-mean0) > converge || fabs(mean1MLE-mean1) > converge){
	   mean0=mean0MLE;mean1=mean1MLE;
	   var0=var0MLE;var1=var1MLE;
	   a0=a0MLE;a1=a1MLE;
		 }
		 else{
				loglikelihood=L;
				converged=1;
    var0=rawVar0MLE;var1=rawVar1MLE;
			 var0=(var0 > var_upper*2.0)? var_upper*2.0 : var0;
    var0=(var0 < var_lower/2.0)? var_lower/2.0 : var0;
    var1=(var1 > var_upper*2.0)? var_upper*2.0 : var1;
    var1=(var1< var_lower/2.0)? var_lower/2.0 : var1;
    a1=1.0-a0;
     mean0=mean0MLE;mean1=mean1MLE;
    return(1);
			}
				//adjustment for binomial - seems to give worse results - enough to soft constrain p
				//loglikelihood=L+(ucounts0+alpha1-1)*log(p_est)+(ucounts1+beta1-1)*log(1.0-p_est);				return(1);

		 if(++nIterations > maxIterations){
				loglikelihood=L;
				var0=rawVar0MLE;var1=rawVar1MLE;

			 var0=(var0 > var_upper*2.0)? var_upper*2.0 : var0;
    var0=(var0 < var_lower/2.0)? var_lower/2.0 : var0;
    var1=(var1 > var_upper*2.0)? var_upper*2.0 : var1;
    var1=(var1< var_lower/2.0)? var_lower/2.0 : var1;
    a1=1.0-a0;
				mean0=mean0MLE;mean1=mean1MLE;
				return(0);
			}
		}
	}
  int optimize(T input_p){
   //optimize mean and use common variance
    //return loglikelihood after convergence with betabinomial term
   const int maxIterations=MAX_EM_ITERATIONS;
   const double converge=EM_CONVERGE;
   bool done=0;
   int nIterations=0;
   const int last=(nBins-1<maxValue)? nBins-1 : maxValue;

  	//T alpha1=((1.0-p_est)/(p_sigma*p_sigma) -1.0/p_est)*p_est*p_est;
			//T beta1=alpha1*(1.0/(p_est-1));

   //fit mean stdevs to normals norma a*invsqrtpi/sigma * exp((-(x-mean)**2)/(2*var*var))
   //however the  invsqrtpi/sigma needs to be in natural log scale to properly normalize

   //adjust ln
   vector <int> map;
   vector <T> mylnx; //mapped copy of lnx
   vector <T> mydata;
   for (int j=minValue;j<=last; j++){
			//check for underflow
			 if(data[j] && !isnan(data[j])){
					map.push_back(j);
					mylnx.push_back(lnx[j]);
					mydata.push_back(data[j]);
				}
			}
   while(!done){
    double term0=log(a0)+LNINVSQRT2PI-log(sqrt(var0));
    double term1=log(a1)+LNINVSQRT2PI-log(sqrt(var1));
    double inv2Var0=1.0/(2.0*var0);
    double inv2Var1=1.0/(2.0*var1);

  //calculate weighted percentage that points in j come from distribution 1 or 2
  //then calculate stdevs and mean for expectation maximization
   double L=0,sum0=0,sumsq0=0,sum1=0,sumsq1=0,counts0=0,counts1=0;
   const int setSize=map.size();
   for (int m=0;m<setSize; m++){
			//check for underflow
			 	T logLike0=term0-inv2Var0*(mylnx[m]-mean0)*(mylnx[m]-mean0);
			 	T logLike1=term1-inv2Var1*(mylnx[m]-mean1)*(mylnx[m]-mean1);

			 	//check for underflows
			 	if((!logLike1 || std::isnan(logLike1)) && logLike0 && !std::isnan(logLike0)){
		     L+=mydata[m]*logLike0;
			 		 counts0+=mydata[m];
			 		 sum0+=mylnx[m]*mydata[m];
			 		 sumsq0+=mylnx[m]*mylnx[m]*mydata[m];
			 	}
			 	else if	((!logLike0 || std::isnan(logLike0)) && logLike1 && !std::isnan(logLike1)){
		     L+=mydata[m]*logLike1;
			 		 counts1+=mydata[m];
			 		 sum1+=mylnx[m]*mydata[m];
			 		 sumsq1+=mylnx[m]*mylnx[m]*mydata[m];
			 	}
			 	else{
			 		const T logRatio=logLike0-logLike1;
			 	 if(logRatio > 15){
		     L+=mydata[m]*logLike0;
			 		 counts0+=mydata[m];
			 		 sum0+=mylnx[m]*mydata[m];
			 		 sumsq0+=mylnx[m]*mylnx[m]*mydata[m];
						}
						else if (logRatio < -15){
		     L+=mydata[m]*logLike1;
			 		 counts1+=mydata[m];
			 		 sum1+=mylnx[m]*mydata[m];
			 		 sumsq1+=mylnx[m]*mylnx[m]*mydata[m];
						}
						else{
			 		 const T ratio=exp(logRatio);
			 		 const T r0=ratio/(ratio+1.0);
			 		 const T r1=1.0-r0;

					  T nPoints0=mydata[m]*r0;
				 	 T nPoints1=mydata[m]*r1;
				   counts0+=nPoints0;
				   counts1+=nPoints1;
				   L+=nPoints0*logLike0+nPoints1*logLike1;
				 	 sum0+=nPoints0*mylnx[m];
					  sumsq0+=nPoints0*mylnx[m]*mylnx[m];
				 	 sum1+=nPoints1*mylnx[m];
					  sumsq1+=nPoints1*mylnx[m]*mylnx[m];
					 }
					}
			}
		 //calculate estimate of mean
		 T mean0MLE=sum0/(counts0);
   T mean1MLE=sum1/(counts1);

   mean0MLE= (mean0MLE > mean_upper)? mean_upper : mean0MLE;
   mean0MLE= (mean0MLE < mean_lower)? mean_lower: mean0MLE;
   mean1MLE= (mean1MLE > mean_upper)? mean_upper: mean1MLE;
   mean1MLE= (mean1MLE < mean_lower)? mean_lower : mean1MLE;

   //estimate a0 a1
   //check if estimated a1 > a0 - if so switch means and var
   T a0MLE,a1MLE;
   if(counts1) a0MLE = counts0/(counts0+counts1);
   else a0MLE=1;

   if(a0MLE < .5){
			//swap terms
			 swap(mean0,mean1);
			 swap(var0,var1);
			 swap(mean0MLE,mean1MLE);
			 swap(counts1,counts0);
			 swap(a0,a1);
			 a0MLE=1.0-a0;
		 }
		 //find unormalized counts
		 T ucounts,ucounts0,ucounts1;
		 if(normed){
				T totalCounts=counts0+counts1;
				ucounts=(T) nPoints * totalCounts;
				ucounts0=(T) nPoints * counts0;
				ucounts1=(T) nPoints * counts1;
			}
		 else{
				ucounts=counts0+counts1;
				ucounts0=counts0;
				ucounts1=counts1;
			}

   a0MLE=(a0MLE > MIXTURE_RATIO + 2*MIXTURE_STD)?MIXTURE_RATIO + 2*MIXTURE_STD :a0MLE;
   a0MLE=(a0MLE < MIXTURE_RATIO - 2*MIXTURE_STD)?MIXTURE_RATIO - 2*MIXTURE_STD :a0MLE;
   a1MLE=1.0-a0MLE;
   T rawVar0MLE=(sumsq0/(counts0)-mean0MLE*mean0MLE)*.6;
   T rawVar1MLE=(sumsq1/(counts1)-mean1MLE*mean1MLE)*.6;
   if(isnan( rawVar0MLE))rawVar0MLE=var_lower/2.0;
   if(isnan( rawVar1MLE))rawVar1MLE=var_lower/2.0;
   T var0MLE=rawVar0MLE;
   T var1MLE=rawVar1MLE;

  //enforce limits
   var0MLE=(var0MLE > var_upper)? var_upper : var0MLE;
   var0MLE=(var0MLE < var_lower)? var_lower : var0MLE;
   var1MLE=(var1MLE > var_upper)? var_upper : var1MLE;
   var1MLE=(var1MLE < var_lower)? var_lower : var1MLE;

   if(fabs(mean0MLE-mean0) > converge || fabs(mean1MLE-mean1) > converge){
	   mean0=mean0MLE;mean1=mean1MLE;
	   var0=var0MLE;var1=var1MLE;
	   a0=a0MLE;a1=a1MLE;
		 }
		 else{
				loglikelihood=L;
				converged=1;
    var0=rawVar0MLE;var1=rawVar1MLE;
			 var0=(var0 > var_upper*2.0)? var_upper*2.0 : var0;
    var0=(var0 < var_lower/2.0)? var_lower/2.0 : var0;
    var1=(var1 > var_upper*2.0)? var_upper*2.0 : var1;
    var1=(var1< var_lower/2.0)? var_lower/2.0 : var1;
    a1=1.0-a0;
     mean0=mean0MLE;mean1=mean1MLE;
    //fprintf(stderr,"%f %f %f %f %f %f\n",mean0MLE*INVLOG2,mean1MLE*INVLOG2,var0MLE*INVLOG2*INVLOG2,var1MLE*INVLOG2*INVLOG2,a0MLE,a1MLE);

    return(1);
			}
				//adjustment for binomial - seems to give worse results - enough to soft constrain p
				//loglikelihood=L+(ucounts0+alpha1-1)*log(p_est)+(ucounts1+beta1-1)*log(1.0-p_est);				return(1);

		 if(++nIterations > maxIterations){
				loglikelihood=L;
				var0=rawVar0MLE;var1=rawVar1MLE;

			 var0=(var0 > var_upper*2.0)? var_upper*2.0 : var0;
    var0=(var0 < var_lower/2.0)? var_lower/2.0 : var0;
    var1=(var1 > var_upper*2.0)? var_upper*2.0 : var1;
    var1=(var1< var_lower/2.0)? var_lower/2.0 : var1;
    a1=1.0-a0;
				mean0=mean0MLE;mean1=mean1MLE;
			 //fprintf(stderr,"%f %f %f %f %f %f\n",mean0MLE*INVLOG2,mean1MLE*INVLOG2,var0MLE*INVLOG2*INVLOG2,var1MLE*INVLOG2*INVLOG2,a0MLE,a1MLE);

				return(0);
			}
		}
	}
};

void init_logx(){
	logx[0]=0;
	lnx[0]=0;
 for (int i=1;i<32768;i++){
		lnx[i]=log((double)i);
  logx[i]=lnx[i]*INVLOG2;
	}
}
template <class T> void transferData(int color,DeconRecord<T> &deconRecord,EM_GMM<T> &EM){

	deconRecord.means0[color]=(EM.mean0*INVLOG2 < MIN_NOISE)? MIN_NOISE : EM.mean0*INVLOG2;
	deconRecord.means1[color]=(EM.mean1*INVLOG2 < MIN_NOISE)? MIN_NOISE : EM.mean1*INVLOG2;
	deconRecord.meansEst0[color]=EM.meanEst0*INVLOG2;
	deconRecord.meansEst1[color]=EM.meanEst1*INVLOG2;
	deconRecord.stdevs0[color]=sqrt(EM.var0)*INVLOG2;
	deconRecord.stdevs1[color]=sqrt(EM.var1)*INVLOG2;
	deconRecord.a0[color]=EM.a0;
	deconRecord.a1[color]=EM.a1;
	deconRecord.loglikelihoods[color]=-EM.loglikelihood;
	deconRecord.dataSizes[color]=EM.nPoints;
	deconRecord.converged[color]=EM.converged;
	if((EM.meanEst0-EM.meanEst1)*(EM.mean0-EM.mean1) <0) deconRecord.flipped[color]=1;
	else	 deconRecord.flipped[color]=0;
}

template <typename T1, typename T2> class wellRecord{
	//qnorm is done by well
	//these are then combined
	public:
	 vector <T1> values[NCOLORS1OFF]; //original values in int16
	 vector <string> groupNames;
	 vector <T2> offsets[NCOLORS1OFF];
	 vector <int> nBeads;
	 vector <int> groupOffsets; //the well where the group starts
	 int nTotalWells;

	 int minValue; //minimum value for peak
	 int maxValue; //maximum value for peak
	 float peakGridSize; //gridSize for peakSearch
	 float smoothHalfWindowSize; //half window size for smoothing
	 wellRecord(string list,float minLogValue, float maxLogValue,float _smoothHalfWindowSize,float _peakGridSize){
	  int16_t header[2];
		 int16_t v;
		 nTotalWells=0;
		 minValue=pow(2.0,minLogValue); //round down bin
		 maxValue=pow(2.0,maxLogValue)+1;
		 if(maxValue > 32767){
				maxValue=32767;
			}
			smoothHalfWindowSize=_smoothHalfWindowSize;
			peakGridSize=_peakGridSize;
		 FILE *fp=fopen(list.c_str(),"r");
		 //can have very long lines
		 fseek(fp,0,SEEK_END);
		 int size=ftell(fp)+1;
		 fseek(fp,0,SEEK_SET);
		 char *line=new char[size];
		 //cerr << "allocated " << size << " bytes"<<endl;
		 if(!line){
				cerr << "unable to allocate " << size << " bytes" <<endl;
				exit(0);
			}
		 int n=0;
		 for(int i=1;i<NCOLORS1OFF;i++){
		 	offsets[i].push_back(0);
		 }
		 groupOffsets.push_back(0);
   while(fgets(line,size,fp) != NULL){
				//format is name\tfileName1\tfileName2...fileNamen\n
				vector <string> fileNames;
				string groupName;
				const char delimit[]=" \t\r\n\v\f";
				char *pch = strtok (line,delimit);
			 groupName=(pch);
			 pch = strtok (NULL,delimit);
			 //start extracting list of fileNames

			 //check for case when there is only one element per line in which case the groupName is also the file
			 if(pch == NULL){
					fileNames.push_back(groupName);
				}
				else{
				 while (pch != NULL){
      fileNames.push_back(pch);
      pch = strtok (NULL,delimit);
				 }
				}
				//now extract the information from the filenames
				int nFilesRead=0;
				for (int k=0;k<fileNames.size();k++){
				//	fprintf(stderr,"opening %s\n",fileNames[k].c_str());
			  FILE *fp1=fopen(fileNames[k].c_str(),"r");
			  if(fp1 == NULL){
			   fprintf(stderr,"unable to open %s\n",fileNames[k].c_str());
			   continue;
			  }
			  while(fread(&header,2*sizeof(int16_t),1,fp1)){
			 	 const int id=header[0];
      for(int k=0;k<header[1];k++){
		     if(fread(&v,sizeof(int16_t),1,fp1)){
				  	 if(v>=0){values[id].push_back(v);}
				 	 }
			   }
			  }
		   fclose(fp1);
		   //save sizes to offset
		   int totalRead=0;
		   for(int i=1;i<NCOLORS1OFF;i++){
			  	offsets[i].push_back(values[i].size());
			  	totalRead+=values[i].size()-offsets[i][nBeads.size()];
			  }
		   nBeads.push_back(totalRead);
					nFilesRead++;
			 }
			 if(nFilesRead){
				 groupOffsets.push_back(groupOffsets[groupOffsets.size()-1]+nFilesRead);
				 groupNames.push_back(groupName);
				 nTotalWells+=nFilesRead;
				}
		 }
		 fprintf(stderr,"closing file\n");
		 fclose(fp);
		 delete[] line;
	 }
  wellRecord(const wellRecord &A): values(A.values),groupNames(A.groupNames),groupOffsets(A.groupOffsets),offsets(A.offsets),maxValue(A.maxValue),minValue(A.minValue),smoothHalfWindowSize(A.smoothHalfWindowSize),peakGridSize(A.peakGridSize),nTotalWells(A.nTotalWells),nBeads(A.nBeads){}

	void dumpEnsemble(string outputFile){
		cerr << "dumping density to " << outputFile <<endl;
		FILE *fp=fopen(outputFile.c_str(),"w");
		int counts[32768];
		fill(counts,counts+32768,0);
		//the 0th slice can be used to store errors in the old PERL script - keep it in case this is to be implemented later in C++
		fwrite(counts,sizeof(int),32768,fp);
		for(int id=1;id<NCOLORS1OFF;id++){
			fill(counts,counts+32768,0);
			for(int k=0;k<values[id].size();k++){
				counts[values[id][k]]++;
			}
			fwrite(counts,sizeof(int),32768,fp);
		}
		fclose(fp);
	}
	void dumpWells(string outputDir){
		cerr << "dumping density to directory " << outputDir <<endl;
		for(int n=0;n<nTotalWells;n++){
			ostringstream nstream;
			nstream << "." << n;

			string outputFile=outputDir+"/"+groupNames[n]+nstream.str()+".den";
	 	FILE *fp;
	 	if(fp=fopen(outputFile.c_str(),"w")){
	 	 int counts[32768];
	 	 fill(counts,counts+32768,0);
		  //the 0th slice can be used to store errors in the old PERL script - keep it in case this is to be implemented later in C++
    fwrite(counts,sizeof(int),32768,fp);
		  for(int id=1;id<NCOLORS1OFF;id++){
					fill(counts,counts+32768,0);
			  for(int k=offsets[id][n];k<offsets[id][n+1];k++){
			  	counts[values[id][k]]++;
			  }
		  	fwrite(counts,sizeof(int),32768,fp);
		  }
		  fclose(fp);
	  }
		}
	}
	void dumpSingleWell(string outputFile,int slice){
		cerr << "dumping density of well " << slice << " to file " << outputFile <<endl;
	 FILE *fp;
	 if(fp=fopen(outputFile.c_str(),"w")){
	  int counts[32768];
	  fill(counts,counts+32768,0);
		 //the 0th slice can be used to store errors in the old PERL script - keep it in case this is to be implemented later in C++
   fwrite(counts,sizeof(int),32768,fp);
		 for(int id=1;id<NCOLORS1OFF;id++){
				fill(counts,counts+32768,0);
			 for(int k=offsets[id][slice];k<offsets[id][slice+1];k++){
			  counts[values[id][k]]++;
			 }
		  fwrite(counts,sizeof(int),32768,fp);
		 }
		 fclose(fp);
	 }
	}
	void dumpDensity(string outputFile,float densitySmoothHalfWindowSize){
	 FILE *fp;
	 if(fp=fopen(outputFile.c_str(),"w")){
	 	int counts[32768];
	 	fill(counts,counts+32768,0);
	 	float smooth[32768];
	 	fill(smooth,smooth+32768,0);
   fwrite(smooth,1,sizeof(float)*32768,fp);//0 well - keep it for errors
		 int upper[32768];
		 int lower [32768];
		 memset (lower,0,32768*sizeof(int));
		 memset (upper,0,32768*sizeof(int));
		 const float upperConstant=pow(2,densitySmoothHalfWindowSize);
		 const float lowerConstant=1.0/upperConstant;
		 for (int i=0;i<32768;i++){
		 	lower[i]= (int) (((float)i)*lowerConstant+.5);
    upper[i]= (int) (((float)i)*upperConstant+.5);
    if(upper[i] > 32767)upper[i]=32767;
	  }
		 for (int i=1; i<=NCOLORS; i++){
			 fill(smooth,smooth+32768,0);
			 int nBins=logWindowSmooth(lower,upper,smooth,&(values[i][0]),values[i].size());
		 	fwrite(smooth,1,32768*sizeof(float),fp);
			}
		 fclose(fp);
		}
	}
template<class Ta, class Tb> int logWindowSmooth(int *lower,int *upper,Ta *smooth,Tb *colorValues,int nValues){
		int maxBin=0;
		int counts[32768];
		fill(counts,counts+32768,0);
		double totalWeight=0;
		for(int i=0;i<nValues;i++){
		 counts[colorValues[i]]++;
	 }
		for(int i=1;i<32768;i++){
		 if(counts[i])maxBin=i;
	 }
		for(int i=1;i<=maxBin;i++){
   int sum=0;
   //const int limit=(upper[i]<maxBin)? maxBin:upper[i];
	 	for(int k=lower[i];k<=upper[i];k++){
				sum+=counts[k];
			}
			smooth[i]=(Ta)sum/(Ta)(upper[i]-lower[i]+1);
			//fprintf(stderr,"%d %d %d %e\n",i,lower[i],upper[i],smooth[i]);
		 totalWeight+=smooth[i];
		}
		double invTotalWeight=1.0/totalWeight;
		for(int i=0;i<=maxBin;i++){
		 smooth[i]*=invTotalWeight;
	 }
		return(maxBin+1);
	}
	template <class T> double dampenLowNoise(T *smooth,int nBins){
		//version for a single file in wells
	 double lowNoiseFraction=0;
  for(int i=0;i<minValue;i++){
  	const double factor= exp(-0.3*(minValue-i));
  	lowNoiseFraction +=(1.0-factor)*smooth[i];
  	smooth[i]*=factor;
		}
  //renormalize
  double normFactor=1.0/(1.0-lowNoiseFraction);
  for(int i=minValue;i<nBins;i++)
   smooth[i]*=normFactor;
	 return (lowNoiseFraction);
	}
	template <class T> void findPeaks(T *smooth, int nBins, T gridSize,vector<int> &peakBins, vector<T> &peakValues, vector<T> &peakMeans){
		//finds peaks using a interval (grid) search
		//uses equal log intervals and logSmoothed data
		//an interval contains a peak if the previous interval and and the following interval have lower mean values
		//the bin with the maxCounts in that interval is the location of a peak
		//peaks are then sorted by mean values (max values is more vulnerable to spikes)
		//a good value for grid size is roughly the std deviation of the expected peaks

  vector<T> maxValues,meanValues;
  vector<int>maxBins;
  if(nBins <4){
			//edge case - return the middle bin
			peakBins.resize(1);
		 peakMeans.resize(1);
		 peakValues.resize(1);
			peakBins[0]=nBins/2;
			peakMeans[0]=smooth[nBins/2];
			peakValues[0]=smooth[nBins/2];
			return;
		}

	 int minBin=pow(2,minValue);int maxBin=(pow(2,maxValue) < nBins)?pow(2,maxValue) : nBins;
	 T gridFactor=pow(2,gridSize);

	 int i=minBin;
		while(i<nBins){
			int upperBin=(int) ((T)i*gridFactor + 0.5);
	  if (upperBin >nBins-1) upperBin=nBins-1;
		 T maxValue=smooth[i];
	  int maxBin=i;
   double sum=0;
   for (int k=i+1;k<=upperBin;k++){
		 	if(smooth[k] > maxValue){
				 maxValue=smooth[k];
				 maxBin=k;
			 }
 		 sum+=smooth[k];
		 }
		 maxValues.push_back(maxValue);
		 maxBins.push_back(maxBin);
		 //meanValues.push_back((T)sum/(T)(upperBin-i+1));
		 meanValues.push_back((T)sum);
		 i=upperBin+1;
		}
		vector <T> myPeakValues,myPeakBins,myPeakMeans;
	 for  (int i=2;i<maxBins.size()-2;i++){
			if(maxBins[i-2] > minValue && maxBins[i+1] < maxValue){ //-2 necessary otherwise there is an edge effect
 		 if(meanValues[i] > meanValues[i-1] && meanValues[i] > meanValues[i+1]){
		  	myPeakBins.push_back(maxBins[i]);
			  myPeakValues.push_back(maxValues[i]);
			  myPeakMeans.push_back(meanValues[i]);
		  }
			}
		}
		//check for empty case - no peaks - seed with middle
		if(!myPeakBins.size()){
			for  (int i=maxBins.size()/3;i<maxBins.size();i+=maxBins.size()/2){
			 myPeakBins.push_back(maxBins[i]);
			 myPeakValues.push_back(maxValues[i]);
			 myPeakMeans.push_back(meanValues[i]);
			}
		}

		vector<int> index;
		index.resize(myPeakMeans.size());
		peakBins.resize(1);
		peakMeans.resize(1);
		peakValues.resize(1);
		sort_by_scores(myPeakMeans.size(),myPeakMeans.data(),index.data(),0);

		peakBins[0]=myPeakBins[index[0]];
		peakValues[0]=myPeakValues[index[0]];
		peakMeans[0]=myPeakMeans[index[0]];
	 for (int i=1;i<index.size();i++){
			const int s=index[i];
			if(myPeakMeans[s] < 0.2*myPeakMeans[0])break;
		 peakBins.push_back(myPeakBins[s]);
		 peakValues.push_back(myPeakValues[s]);
		 peakMeans.push_back(myPeakMeans[s]);
		}
	}
	template <class T> void findPeaks(int *counts, int nBins, T gridSize,vector<int> &peakBins, vector<T> &peakValues, vector<T> &peakMeans){
		//finds peaks using a interval (grid) search
		//uses equal log intervals
		//an interval contains a peak if the previous interval and and the following interval have lower mean values
		//the bin with the maxCounts in that interval is the location of a peak
		//peaks are then sorted by mean values (max values is more vulnerable to spikes)
		//a good value for grid size is roughly the std deviation of the expected peaks

  vector<T> maxValues,meanValues;
  vector<int>maxBins;
  if(nBins <4){
			//edge case - return the middle bin
			peakBins.resize(1);
		 peakMeans.resize(1);
		 peakValues.resize(1);
			peakBins[0]=nBins/2;
			peakMeans[0]=counts[nBins/2];
			peakValues[0]=counts[nBins/2];
			return;
		}

	 int minBin=pow(2,minValue);int maxBin=(pow(2,maxValue) < nBins)?pow(2,maxValue) : nBins;
	 T gridFactor=pow(2,gridSize);

	 int i=minBin;
		while(i<nBins){
			int upperBin=(int) ((T)i*gridFactor + 0.5);
	  if (upperBin >nBins-1) upperBin=nBins-1;
		 T maxValue=counts[i];
	  int maxBin=i;
   double sum=0;
   for (int k=i+1;k<=upperBin;k++){
		 	if(counts[k] > maxValue){
				 maxValue=counts[k];
				 maxBin=k;
			 }
 		 sum+=counts[k];
		 }
		 maxValues.push_back(maxValue);
		 maxBins.push_back(maxBin);
		 //meanValues.push_back((T)sum/(T)(upperBin-i+1));
		 meanValues.push_back((T)sum);
		 i=upperBin+1;
		}
		vector <T> myPeakValues,myPeakBins,myPeakMeans;
	 for  (int i=2;i<maxBins.size()-2;i++){
			if(maxBins[i-2] > minValue && maxBins[i+1] < maxValue){ //-2 necessary otherwise there is an edge effect
 		 if(meanValues[i] > meanValues[i-1] && meanValues[i] > meanValues[i+1]){
		  	myPeakBins.push_back(maxBins[i]);
			  myPeakValues.push_back(maxValues[i]);
			  myPeakMeans.push_back(meanValues[i]);
		  }
			}
		}
		//check for empty case - no peaks - seed with middle
		if(!myPeakBins.size()){
			for  (int i=maxBins.size()/3;i<maxBins.size();i+=maxBins.size()/2){
			 myPeakBins.push_back(maxBins[i]);
			 myPeakValues.push_back(maxValues[i]);
			 myPeakMeans.push_back(meanValues[i]);
			}
		}

		vector<int> index;
		index.resize(myPeakMeans.size());
		peakBins.resize(1);
		peakMeans.resize(1);
		peakValues.resize(1);
		sort_by_scores(myPeakMeans.size(),myPeakMeans.data(),index.data(),0);

		peakBins[0]=myPeakBins[index[0]];
		peakValues[0]=myPeakValues[index[0]];
		peakMeans[0]=myPeakMeans[index[0]];
	 for (int i=1;i<index.size();i++){
			const int s=index[i];
			if(myPeakMeans[s] < 0.2*myPeakMeans[0])break;
		 peakBins.push_back(myPeakBins[s]);
		 peakValues.push_back(myPeakValues[s]);
		 peakMeans.push_back(myPeakMeans[s]);
		}
	}
	template <class T> int deconvolute(DeconRecord<T> &deconRecord,FILE *fpRaw,FILE *fpSmooth){
	 if(!logx[2]){init_logx();}
		T smooth[32768];
		int counts[32768];
		fill(smooth,smooth+32768,0);
		fill(counts,counts+32768,0);
		if(fpSmooth) fwrite(smooth,1,sizeof(T)*32768,fpSmooth);
	 if(fpRaw) fwrite(counts,1,sizeof(T)*32768,fpRaw);
		int upper[32768];
		int lower [32768];
		memset (lower,0,32768*sizeof(int));
		memset (upper,0,32768*sizeof(int));
		const T upperConstant=pow(2,smoothHalfWindowSize);
		const T lowerConstant=1.0/upperConstant;
		for (int i=0;i<32768;i++){
			lower[i]= (int) (((T)i)*lowerConstant+.5);
   upper[i]= (int) (((T)i)*upperConstant+.5);
   if(upper[i] > 32767)upper[i]=32767;
	 }
		for (int i=1; i<=NCOLORS; i++){
			fill(smooth,smooth+32768,0);
			vector<T> peakValues,peakMeans;
			vector<int>peakBins;
			int nBins=logWindowSmooth(lower,upper,smooth,&(values[i][0]),values[i].size());
			double lowNoiseFraction=dampenLowNoise(smooth,nBins);
			if(fpSmooth)fwrite(smooth,1,32768*sizeof(T),fpSmooth);
			fill(counts,counts+32768,0);
			for(int k=0;k<values[i].size();k++)
			 counts[values[i][k]]++;

			if(fpRaw){
    fwrite(counts,1,32768*sizeof(T),fpRaw);
			}
   findPeaks<T>(smooth,nBins,0.2,peakBins,peakValues,peakMeans);
   if(i >10){
				T nPoints=(double)values[i].size()-lowNoiseFraction*(double)values[i].size();
			 for(int k=0;k<32768;k++){
	 			smooth[k]=counts[k];
 			}
    fitLogNormEM(smooth,0,nBins,nPoints,peakBins,deconRecord,i,1,0);
    deconRecord.noiseFractions[i]=lowNoiseFraction;
			}
		}
	}
	template <class T> int deconvolute(int nFold,int n,int nGroups,vector<DeconRecord<T>> &deconRecords,vector<MedianRecord<T>>&medianRecords,FILE *fpRaw){
		return deconvolute_peakSeed(nFold,n,nGroups,deconRecords,medianRecords,fpRaw);
		//return deconvolute_peakSeed(n,nGroups,deconRecords[0],fpRaw);
	}
 template <class T> int deconvolute_peakSeed(int n,int nGroups,DeconRecord<T> &deconRecord,FILE *fpRaw){
	 if(!logx[2]){init_logx();}
		int counts[32768];
		fill(counts,counts+32768,0);
	 if (fpRaw) fwrite(counts,1,sizeof(T)*32768,fpRaw);
	 int finalGroupOffset= (n+nGroups >= groupOffsets.size())? groupOffsets[groupOffsets.size()-1] : groupOffsets[n+nGroups];
	 //fprintf(stderr,"%d start %d final offset %d\n",n,groupOffsets[n],finalGroupOffset);
		for (int i=1; i<=NCOLORS; i++){
			const int start=offsets[i][groupOffsets[n]];
			const int finish= offsets[i][finalGroupOffset];
			int nBins=0;
			T nPoints=finish-start;
			if(nPoints >= MINBEADS){
			 vector<int>peakBins;
			 peakBins.resize(2);

			 fill(counts,counts+32768,0);
			 for(int k=start;k<finish;k++){
					if(values[i][k]>nBins) nBins=values[i][k]+1;
			  counts[values[i][k]]++;
				}
			 if(fpRaw) fwrite(counts,1,32768*sizeof(int),fpRaw);
    peakBins[0]=pow(2,8.1);
    peakBins[1]=pow(2,7.9);
    if(i >10){
					T tcounts[32768];
					fill(tcounts,tcounts+32768,0);
					int k=0;
					while (k<minValue){
						nPoints-=counts[k];
						//tcounts[k]=counts[k];
						k++;
					}
					while (k<32768){
	 			 tcounts[k]=counts[k];
	 			 k++;
 			 }
     fitLogNormEM(tcounts,0,nBins,nPoints,peakBins,deconRecord,i,1,1);
     deconRecord.noiseFractions[i]=1.0-(nPoints/(T)(finish-start));
				}
			}
			else{
			 deconRecord.dataSizes[i]=finish-start;
				if(fpRaw){
		   fill(counts,counts+32768,0);
		   for(int k=start;k<finish;k++)
			   counts[values[i][k]]++;
	    fwrite(counts,1,sizeof(int)*32768,fpRaw);
			 }
			}
		}
		return(1);
	}
	template <class T> int deconvolute_peakSeed(int nFold,int n,int nGroups,vector<DeconRecord<T>> &deconRecords,vector<MedianRecord<T>>&medianRecords,FILE *fpRaw){
  //this overload takes 1/10 slices and finds the median value
	 if(!logx[2]){init_logx();}
		int counts[32768];
	 if (fpRaw) fwrite(counts,1,sizeof(T)*32768,fpRaw);
	 int finalGroupOffset= (n+nGroups >= groupOffsets.size())? groupOffsets[groupOffsets.size()-1] : groupOffsets[n+nGroups];
	 //fprintf(stderr,"%d start %d final offset %d\n",n,groupOffsets[n],finalGroupOffset);
		for (int i=1; i<=NCOLORS; i++){
			const int start=offsets[i][groupOffsets[n]];
			const int finish= offsets[i][finalGroupOffset];
			if(i >10){
			 int nBins=0;
			 int totalPoints=finish-start;
			 if(totalPoints >= MINBEADS){
			  vector<int>peakBins(2);
			  vector<unsigned int>foldIndices=getFolds(totalPoints,nFold);;
			  for(int f=0;f<nFold;f++){
						int nPoints=0;
						fill(counts,counts+32768,0);
						unsigned int j=0;
		  	 for(int k=start;k<finish;k++){
							if(nFold >1 && foldIndices[j++] == f ) continue;
		  			if(values[i][k]>nBins) nBins=values[i][k]+1;
		 	   counts[values[i][k]]++;
		 	   nPoints++;
		 	 	}
		 	  if(fpRaw) fwrite(counts,1,32768*sizeof(int),fpRaw);
      peakBins[0]=pow(2,8.1);
      peakBins[1]=pow(2,7.9);

			 		T tcounts[32768];
			 		fill(tcounts,tcounts+32768,0);
			 		int k=0;
			 		while (k<minValue){
				 		nPoints-=counts[k];
			 			//tcounts[k]=counts[k];
				 		k++;
				 	}
				 	while (k<32768){
	 		 	 tcounts[k]=counts[k];
	 		 	 k++;
 			  }
      fitLogNormEM(tcounts,0,nBins,nPoints,peakBins,deconRecords[f],i,1,1);
      medianRecords[i].addValues(deconRecords[f].means0[i], deconRecords[f].means1[i]);
      deconRecords[f].noiseFractions[i]=1.0-(nPoints/(T)(totalPoints));
				 }
				}
			}
			else{
				//write a blank Deconrecord with nPoints
				for(int f=0;f<nFold;f++){
				 deconRecords[f].dataSizes[i]=finish-start;
				}
				if(fpRaw){
		   fill(counts,counts+32768,0);
		   for(int k=start;k<finish;k++)
			   counts[values[i][k]]++;
	    fwrite(counts,1,sizeof(int)*32768,fpRaw);
			 }
			}
		}
		return(1);
	}
 template <class T> int fitLogNormEM(T *smooth,bool normed, int nBins,int nPoints,vector<int> &peakBins,DeconRecord<T> &deconRecord,int color,bool isMode,bool seed){
  const T s0=PEAKWIDTH;
  const T s1=PEAKWIDTH;
  T sigmasqs[2]={s0*s0,s1*s1};
  vector<T> mus;
  mus.resize(peakBins.size());
  if(isMode){
   for(int i=0;i<peakBins.size();i++)
    mus[i]=logx[peakBins[i]]-s0*s0;
		}
		else{
	  for(int i=0;i<peakBins.size();i++)
    mus[i]=logx[peakBins[i]];
		}
  //starting for search is both lognormals at the estimated medians

  //keep record of initial mode estimate
		deconRecord.meansEst0[color]=logx[peakBins[0]];
		deconRecord.meansEst1[color]=logx[peakBins[1]];
		T input_mus[2]={mus[0]+ (T).05,mus[0]-(T).05};
		EM_GMM <float> EM (smooth,normed,nBins,nPoints,minValue,maxValue,input_mus,sigmasqs,.5, MIXTURE_RATIO, 0.15,0.7,.275,PEAKWIDTH,.355);
		if (seed){
			EM.reset(mus[0],mus[1],sigmasqs[0],sigmasqs[1],MIXTURE_RATIO);
			EM.seed_mclust(0.5);
			mus.resize(2);
			if(isnan(EM.mean0)){
				mus[0]=EM.mean1*INVLOG2+.05;
			 mus[1]=EM.mean1*INVLOG2-.05;
			}
			else if(isnan(EM.mean1)){
			 mus[0]=EM.mean0*INVLOG2+.05;
			 mus[1]=EM.mean0*INVLOG2-.05;
			}
			else{
				mus[0]=EM.mean0*INVLOG2+.05;
			 mus[1]=EM.mean1*INVLOG2-.05;
			}
			EM.reset(mus[0]+.05,mus[1]-.05,sigmasqs[0],sigmasqs[1],MIXTURE_RATIO);
		}

  int converged=EM.optimize(MIXTURE_RATIO);
  transferData(color,deconRecord,EM);
  double best_likelihood=EM.loglikelihood;

  for(int j=0;j<mus.size()-1;j++){
			for (int k=j+1;k<mus.size();k++){
				EM.reset(mus[j],mus[k],sigmasqs[0],sigmasqs[1],MIXTURE_RATIO);
				int converged=EM.optimize(MIXTURE_RATIO);
				double likelihood=EM.loglikelihood;
				if(likelihood < best_likelihood){
					best_likelihood=likelihood;
				 transferData(color,deconRecord,EM);
				 deconRecord.meansEst0[color]=mus[j];deconRecord.meansEst1[color]=mus[k];
				}
				EM.reset(mus[k],mus[j],sigmasqs[1],sigmasqs[0],MIXTURE_RATIO);
				converged=EM.optimize(MIXTURE_RATIO);
				likelihood=EM.loglikelihood;
				if(likelihood < best_likelihood){
					best_likelihood=likelihood;
				 transferData(color,deconRecord,EM);
				 deconRecord.meansEst0[color]=mus[j];deconRecord.meansEst1[color]=mus[k];
				}
			}
		}
 }
	vector<unsigned int> getFolds(unsigned int size, unsigned int nFold){
		//for assigning nfolds
		vector<unsigned int> shuffledIndices(size);
  std::random_device rd;
  std::mt19937 rng(rd());
		for(unsigned int i=0;i<size;i++){
			shuffledIndices[i]=i%nFold;
		}
		for	(unsigned int i=0;i<size;i++){
			std::uniform_int_distribution<int> uni(i,size-1);
			unsigned int j=uni(rng);
 	 swap(shuffledIndices[i],shuffledIndices[j]);
		}
		return shuffledIndices;
	}
};


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include "utils.h"
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>

TEST_CASE("Precomputed Lagrange Interpolation", "[util]") {
  utils::init_ntl_extension_field(utils::NTL_INSTANCE::GF2_128);
  size_t dimension = 20;
  vec_GF2E x_values;
  vec_GF2E y_values1;
  vec_GF2E y_values2;
  for (size_t i = 0; i < dimension; i++) {
    x_values.append(random_GF2E());
    y_values1.append(random_GF2E());
    y_values2.append(random_GF2E());
  }

  // builtin interpolate
  GF2EX poly1 = interpolate(x_values, y_values1);
  GF2EX poly2 = interpolate(x_values, y_values2);
  // precomputed interpolate
  auto precomputation = utils::precompute_lagrange_polynomials(x_values);
  GF2EX poly1_with_precom =
      utils::interpolate_with_precomputation(precomputation, y_values1);
  GF2EX poly2_with_precom =
      utils::interpolate_with_precomputation(precomputation, y_values2);

  REQUIRE(poly1 == poly1_with_precom);
  REQUIRE(poly2 == poly2_with_precom);
}


//
// Created by Sylvain  on 5/30/20.
//

#include "SatOps/MagneticField.h"
#include "SatOps/constants.h"
#include "SatOps/ReferenceFrame.h"
#include <cmath>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <boost/math/special_functions/factorials.hpp>

MagneticField::MagneticField(Model model, unsigned int max_degree, const std::filesystem::path& file, double year)
    :
    PotentialField(max_degree),
    m_model(model)
{
    // Allocates and initializes vectors containing the coefficients
    unsigned int num_entries = (max_degree + 1) * (max_degree + 2) / 2;
    m_c_coeffs = std::vector(num_entries, 0.0);
    m_s_coeffs = std::vector(num_entries, 0.0);

    switch (m_model) {
        case Model::IGRF13:
            if (m_max_degree > 13)
                throw std::invalid_argument("MagneticField: The degree of expansion is out of range (max 13).");
            if (year < 1900. || year > 2025.)
                throw std::invalid_argument("MagneticField: The year is out of range. The model is valide from 1900 to 2025.");

            loadIGRF13Coeffs(file, year);
            m_radius = constants::IGRF13_RADIUS;
            m_scale_factor = 1E-9 * constants::IGRF13_RADIUS * constants::IGRF13_RADIUS;
            break;
    }
}

// The coefficients of the model are computed for the initial epoch of the simulation
void MagneticField::loadIGRF13Coeffs(const std::filesystem::path& path, double year) {

    // Gets reference epoch of the model and difference with desired time
    int T0 = static_cast<int>(year) - static_cast<int>(std::floor(year)) % 5;
    double dt = year - T0;
    int year_col = (T0 - 1900) / 5 + 3;

    // Opens file containing the geomagnetic coefficients and populates arrays
    std::ifstream data_file(path);
    if (data_file.is_open()) {

        // Skips first four lines (header)
        std::string str;
        for (int i = 0; i < 4; ++i) {
            std::getline(data_file, str);
        }

        // Reads required lines
        while (std::getline(data_file, str)) {
            std::istringstream iss(str);
            std::string value;
            std::string coeff;
            int col = 0, degree = 0, order = 0;
            double main_field_coeff = 0., secular_variation_coeff = 0.;

            while (iss >> value) {
                if (col == 0)
                    coeff = value;
                else if (col == 1)
                    degree = stoi(value);
                else if (col == 2)
                    order = stoi(value);
                else if (col == year_col)
                    main_field_coeff = stod(value);
                else if (col == 28) // if last column, secular variation is directly given
                    secular_variation_coeff = stod(value);
                else if (col == year_col + 1)
                    secular_variation_coeff = (stod(value) - main_field_coeff) / 5.0;

                ++col;
            }

            if (degree > m_max_degree)
                break;

            int i = getIndex(degree, order);
            double kron_delta = (0 == order) ? 1. : 0.;
            double unnormalization_factor = std::sqrt(2.0 * boost::math::factorial<double>(degree-order) / boost::math::factorial<double>(degree+order) - kron_delta);
            if (coeff == "g") {
                m_c_coeffs[i] = unnormalization_factor * (main_field_coeff + dt * secular_variation_coeff);
            } else if (coeff == "h") {
                m_s_coeffs[i] = unnormalization_factor * (main_field_coeff + dt * secular_variation_coeff);
            }
        }
        data_file.close();
    } else {
        throw std::invalid_argument("MagneticField: The file containing the magnetic coefficients could not be opened.");
    }
}

Vector3d MagneticField::getMagneticField(const Vector3d& itrf93_position, Matrix3d* jacobian) {
    double x = itrf93_position[0];
    double y = itrf93_position[1];
    double z = itrf93_position[2];
    double r = itrf93_position.norm();

    std::array<double, 12> sums{};
    sums.fill(0.0);
    sums[0] = m_radius / (r*r) * (3. * (m_c_coeffs[getIndex(1,1)] * x + m_s_coeffs[getIndex(1,1)] * y) + 2. * m_c_coeffs[getIndex(1,0)] * z);
    sums[1] = m_radius / r * m_c_coeffs[getIndex(1,0)];
    sums[2] = m_radius / r * m_c_coeffs[getIndex(1,1)];
    sums[3] = m_radius / r * m_s_coeffs[getIndex(1,1)];
    sums[4] = 2.0;

    Vector3d mag_field = computeGradient(x, y, z, sums, jacobian);
    mag_field.setFrame(ReferenceFrame::ITRF93);
    return mag_field;
}


#pragma once

/// @file

#include <cstdint>
#include <utility>
#include <Eigen/Core>

namespace neon
{
using indices = Eigen::Array<std::int32_t, Eigen::Dynamic, Eigen::Dynamic>;

/// Type alias for whatever type is returned from these views
using index_view = decltype(std::declval<const indices>()(Eigen::all, 0l));
}


#include <stdio.h>
#include <stdlib.h>
#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

int main(void)
{
///read
  ofstream myFile;
  vec v(7);
  for (int i = 0;i<7;i++)
    v(i) = i;
   // v = {1, 2, 3, 4, 5, 6, 7};
  myFile.open("Collector/todelete.log");
  for (int i = 0;i<20;i++)
    myFile<<v;
  myFile.close();

    /* read */

      std::ifstream ifile("Collector/todelete.log", std::ios::in);
      // std::ifstream ifile("Collector/posCollected.txt", std::ios::in);

      std::vector<double> scores;

      //check to see that the file was opened correctly:
      if (!ifile.is_open()) {
          std::cerr << "There was a problem opening the input file!\n";
          exit(1);//exit or do additional error checking
      }

      int DIM = 7;
      double num = 0.0;
      int i = 0;

      //keep storing values from the text file so long as data exists:
      while (ifile >> num) {
          scores.push_back(num);
      }

      mat m(DIM,int(scores.size()/DIM));
      int t=-1;

      for (int i = 0; i < int(scores.size()/DIM); ++i)
      {
        for (int e = 0; e<DIM; e++)
        {
          t++;
          m(e,i) = scores[t] ;
        }
      }
cout<<"start out m"<<endl;
      //verify that the scores were stored correctly:
      for (int i = 0; i < int(scores.size()/DIM); ++i) {
          for (int e = 0; e<DIM; e++)
          {
            // std::cout << scores[i+e] << "\t";
            std::cout << m(e,i) << "\t" ;
          }
          cout<<endl;
      }

  //// get
    // FILE *fp;
    // char ch=' ';
    // int cnt=0;
   	// fp=fopen("Collector/todelete.log","r");
    // if (fp==NULL) exit(2);
    // vec vv(1);
    // while((ch=fgetc(fp))!=EOF)
    // {
	  //    vv(1) = putchar(ch);
    //
    // }
    //
    // fclose(fp);
    // printf("\n b meros\n");
    // //------------------------------------------
    // FILE *fp1;
    // int ar0,ar1,ar2,ar3,ar4,ar5,ar6,k;
    //  fp1=fopen("Collector/todelete.log","r");
    //  if (fp1==NULL)
    //  {
    //     puts("Provlima sto anoigma toy arxeioy");
    //     exit(2);
    //  }
    //
    //   while (k=fscanf(fp1,"%d %d %d %d %d %d %d", &ar0, &ar1, &ar2, &ar3, &ar4, &ar5, &ar6)>=1)
    //  {
    //
	  //  //  a++;
	  //  //  printf("%d",a);
    //
    //      printf("%d %d %d %d %d %d %d \n",ar0,ar1,ar2, ar3,ar4,ar5,ar6);
    //      fgetc(fp1);
    //  }
    //  fclose(fp1);

    return 0;
}


/*
 * OrientationBox.hpp
 *
 *  Created on: 01.11.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_STATEBOXES_HOMOCOORDBOX_HPP_
#define ESTIMATORS_STATEBOXES_HOMOCOORDBOX_HPP_

#include "StateBox.hpp"
#include <eigen3/Eigen/Core>
#include <Eigen_Utils.hpp>
#include <stdio.h>

namespace zavi
::estimator::state_boxes {
	/**
	 * A Box which contains homogenous coordinates
	 * This is basically the calibration matrix for the offset between IMU and body coordinates
	 */
	class HomoCoordBox : public StateBox<16,6,0> {
		template<typename T>
		using ROT_MATRIX= Eigen::Matrix<T,3,3>;

		template<typename T>
		inline static const Eigen::Matrix<T,4,4> remap(const OUTER_T<T> & state){
			return Eigen::Map<const Eigen::Matrix<T,4,4>>(state.data());
		}

	public:
		HomoCoordBox(std::shared_ptr<plugin::SensorPlugin> sensor):StateBox<outer_size,inner_size,input_size>(sensor) {
		}
		virtual ~HomoCoordBox() {};

		template<typename T>
		inline static OUTER_T<T> boxPlus(const OUTER_T<T> &state,const INNER_T<T> &delta) {
			assert_inputs(state,delta);
			Eigen::Matrix<T,4,4> result=result.Identity();
			result.template block<3,3>(0,0)=zavi::eigen_util::boxPlusOrientation<T>(remap(state).template block<3,3>(0,0),delta.template block<3,1>(0,0));
			result.template block<3,1>(0,3)=state.template block<3,1>(12,0)+delta.template block<3,1>(3,0);
			return Eigen::Map<OUTER_T<T>>(result.data());
		}
		template<typename T>
		inline static INNER_T<T> boxPlusInnerSpace(const INNER_T<T> &delta1,const INNER_T<T> &delta2) {
			assert_inputs(delta1,delta2);
			return delta1+delta2;
		}

		template<typename T>
		inline static INNER_T<T> boxMinus(const OUTER_T<T> &a,const OUTER_T<T> &b) {
			assert_inputs(a,b);
			INNER_T<T> result=result.Zero();
			result.template block<3,1>(0,0)=zavi::eigen_util::boxMinusOrientation<T,T>(remap(a).template block<3,3>(0,0),remap(b).template block<3,3>(0,0));
			result.template block<3,1>(3,0)=b.template block<3,1>(12,0)-a.template block<3,1>(12,0);
			return result;
		}

		template<typename T>
		inline static OUTER_T<T> stateTransition(const OUTER_T<T> & state,double time_diff ) {
			assert_inputs(state,time_diff);
			return state;
		}
		inline INNER_T<double> getSTD(double time_diff) {
			INNER_T<double> noise=INNER_T<double>::Ones()*30*time_diff;
			return noise;
		}
	};
}
//zavi::estimator::state_boxes

#endif /* ESTIMATORS_STATEBOXES_HOMOCOORDBOX_HPP_ */


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

/// Calculate course (bearing) between two points.
template <typename ReturnType, typename Point1, typename Point2>
inline ReturnType course(Point1 const& p1, Point2 const& p2)
{
    // http://williams.best.vwh.net/avform.htm#Crs
    ReturnType dlon = get_as_radian<0>(p2) - get_as_radian<0>(p1);
    ReturnType cos_p2lat = cos(get_as_radian<1>(p2));

    // An optimization which should kick in often for Boxes
    //if ( math::equals(dlon, ReturnType(0)) )
    //if ( get<0>(p1) == get<0>(p2) )
    //{
    //    return - sin(get_as_radian<1>(p1)) * cos_p2lat);
    //}

    // "An alternative formula, not requiring the pre-computation of d"
    // In the formula below dlon is used as "d"
    return atan2(sin(dlon) * cos_p2lat,
        cos(get_as_radian<1>(p1)) * sin(get_as_radian<1>(p2))
        - sin(get_as_radian<1>(p1)) * cos_p2lat * cos(dlon));
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP


#include <boost/math/distributions/cauchy.hpp>


// Copyright (C) 2021 Igor A. Baratta
//
// This file is part of DOLFINx_CUAS
//
// SPDX-License-Identifier:    MIT

#include "volume.h"
#include <basix/finite-element.h>
#include <basix/quadrature.h>
#include <boost/program_options.hpp>
#include <dolfinx.h>
#include <dolfinx/fem/petsc.h>
#include <dolfinx_cuas/QuadratureRule.hpp>
#include <dolfinx_cuas/kernels.hpp>
#include <dolfinx_cuas/matrix_assembly.hpp>
#include <dolfinx_cuas/utils.hpp>
#include <xtensor/xio.hpp>

using namespace dolfinx;
namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  common::subsystem::init_logging(argc, argv);
  common::subsystem::init_petsc(argc, argv);

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "kernel", po::value<std::string>()->default_value("mass"),
      "kernel (mass or stiffness)")("degree", po::value<int>()->default_value(1),
                                    "Degree of function space (1-5)");
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << "\n";
    return 0;
  }
  const std::string problem_type = vm["kernel"].as<std::string>();
  const int degree = vm["degree"].as<int>();

  MPI_Comm mpi_comm{MPI_COMM_WORLD};

  std::shared_ptr<mesh::Mesh> mesh = std::make_shared<mesh::Mesh>(
      mesh::create_box(mpi_comm, {{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}}, {10, 10, 10},
                       mesh::CellType::tetrahedron, mesh::GhostMode::none));

  mesh->topology().create_entity_permutations();

  auto kappa = std::make_shared<fem::Constant<PetscScalar>>(1.0);

  // Define variational forms
  ufcx_form form;
  std::shared_ptr<fem::FunctionSpace> V;
  dolfinx_cuas::Kernel kernel_type;

  int q_degree = 0;
  if (problem_type == "mass")
  {
    q_degree = 2 * degree;
    kernel_type = dolfinx_cuas::Kernel::Mass;
    std::vector spaces_mass = {functionspace_form_volume_a_mass1, functionspace_form_volume_a_mass2,
                               functionspace_form_volume_a_mass3, functionspace_form_volume_a_mass4,
                               functionspace_form_volume_a_mass5};
    std::vector forms_mass = {form_volume_a_mass1, form_volume_a_mass2, form_volume_a_mass3,
                              form_volume_a_mass4, form_volume_a_mass5};
    V = std::make_shared<fem::FunctionSpace>(
        fem::create_functionspace(spaces_mass[degree - 1], "v_0", mesh));
    form = *forms_mass[degree - 1];
  }
  else if (problem_type == "stiffness")
  {
    q_degree = 2 * (degree - 1);
    kernel_type = dolfinx_cuas::Kernel::Stiffness;
    std::vector spaces_stiffness
        = {functionspace_form_volume_a_stiffness1, functionspace_form_volume_a_stiffness2,
           functionspace_form_volume_a_stiffness3, functionspace_form_volume_a_stiffness4,
           functionspace_form_volume_a_stiffness5};
    std::vector forms_stiffness
        = {form_volume_a_stiffness1, form_volume_a_stiffness2, form_volume_a_stiffness3,
           form_volume_a_stiffness4, form_volume_a_stiffness5};
    V = std::make_shared<fem::FunctionSpace>(
        fem::create_functionspace(spaces_stiffness[degree - 1], "v_0", mesh));
    form = *forms_stiffness[degree - 1];
  }
  else
    throw std::runtime_error("Unsupported kernel");

  auto a = std::make_shared<fem::Form<PetscScalar>>(
      fem::create_form<PetscScalar>(form, {V, V}, {}, {{"kappa", kappa}}, {}));

  // Matrix to be used with custom assembler
  la::petsc::Matrix A = la::petsc::Matrix(fem::petsc::create_matrix(*a), false);
  MatZeroEntries(A.mat());

  // Matrix to be used with custom DOLFINx/FFCx
  la::petsc::Matrix B = la::petsc::Matrix(fem::petsc::create_matrix(*a), false);
  MatZeroEntries(B.mat());

  // Generate Kernel
  dolfinx_cuas::QuadratureRule q_rule(mesh->topology().cell_type(), q_degree,
                                      mesh->topology().dim(), basix::quadrature::type::Default);
  auto kernel = dolfinx_cuas::generate_kernel<PetscScalar>(kernel_type, degree,
                                                           V->dofmap()->index_map_bs(), q_rule);

  // Define active cells
  const std::int32_t tdim = mesh->topology().dim();
  const std::int32_t ncells = mesh->topology().index_map(tdim)->size_local();
  xt::xarray<std::int32_t> active_cells = xt::arange<std::int32_t>(0, ncells);
  const std::vector<PetscScalar> coeffs(0);
  const std::vector<PetscScalar> consts(0);

  common::Timer t0("~Assemble Matrix Custom");
  dolfinx_cuas::assemble_matrix<PetscScalar>(la::petsc::Matrix::set_block_fn(A.mat(), ADD_VALUES),
                                             V, {}, active_cells, kernel, coeffs, 0, consts,
                                             dolfinx::fem::IntegralType::cell);
  MatAssemblyBegin(A.mat(), MAT_FINAL_ASSEMBLY);
  MatAssemblyEnd(A.mat(), MAT_FINAL_ASSEMBLY);
  t0.stop();

  {
    // Prepare constants and coefficients
    const auto constants = pack_constants(*a);
    const auto coeffs = pack_coefficients(*a);
    common::Timer t1("~Assemble Matrix DOLINFx/FFCx");
    dolfinx::fem::assemble_matrix(la::petsc::Matrix::set_block_fn(B.mat(), ADD_VALUES), *a,
                                  tcb::make_span(constants),
                                  dolfinx::fem::make_coefficients_span(coeffs), {});
    MatAssemblyBegin(B.mat(), MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(B.mat(), MAT_FINAL_ASSEMBLY);
    t1.stop();
  }

  dolfinx::list_timings(mpi_comm, {dolfinx::TimingType::wall});

  if (!dolfinx_cuas::allclose(A.mat(), B.mat()))
    throw std::runtime_error("Matrices are not the same");

  return 0;
}


#include "point_source_panner.hpp"

#include <Eigen/Dense>
#include <boost/algorithm/clamp.hpp>
#include <boost/make_unique.hpp>
#include "convex_hull.hpp"
#include "ear/bs2051.hpp"
#include "ear/helpers/assert.hpp"
#include "facets.hpp"
#include "geom.hpp"
#include "helpers/eigen_helpers.hpp"

namespace ear {

  RegionHandler::RegionHandler(Eigen::VectorXi outputChannels,
                               Eigen::MatrixXd positions)
      : _outputChannels(outputChannels), _positions(positions){};

  boost::optional<Eigen::VectorXd> RegionHandler::handleRemap(
      Eigen::Vector3d position, int numberOfChannels) const {
    boost::optional<Eigen::VectorXd> pv = handle(position);

    if (pv) {
      Eigen::VectorXd _pv = pv.get();
      Eigen::VectorXd out = Eigen::VectorXd::Zero(numberOfChannels);
      for (int i = 0; i < _pv.size(); ++i) {
        out(_outputChannels(i)) = _pv(i);
      }
      return out;
    }
    return boost::none;
  };

  Eigen::VectorXi RegionHandler::outputChannels() { return _outputChannels; }

  Triplet::Triplet(Eigen::Vector3i outputChannels, Eigen::Matrix3d positions)
      : RegionHandler(outputChannels, positions) {
    _basis = _positions.inverse();
  };

  boost::optional<Eigen::VectorXd> Triplet::handle(
      Eigen::Vector3d position) const {
    Eigen::VectorXd pv = position.transpose() * _basis;
    double epsilon = -1e-11;
    if (pv(0) >= epsilon && pv(1) >= epsilon && pv(2) >= epsilon) {
      pv /= pv.norm();
      return pv.cwiseMax(0.0).cwiseMin(1.0).eval();
    }
    return boost::none;
  }

  VirtualNgon::VirtualNgon(Eigen::VectorXi outputChannels,
                           Eigen::MatrixXd positions,
                           Eigen::Vector3d centrePosition,
                           Eigen::VectorXd centreDownmix)
      : RegionHandler(outputChannels, positions),
        _centrePosition(centrePosition),
        _centreDownmix(centreDownmix) {
    int n = static_cast<int>(_outputChannels.size());

    ear_assert(
        n == _positions.rows(),
        "number of downmix coeffs does not match number of output channels");

    ear_assert(
        n == _centreDownmix.size(),
        "number of downmix coeffs does not match number of output channels");

    Eigen::VectorXi order = ngonVertexOrder(positions);

    for (int i = 0; i < n; ++i) {
      int j = (i + 1) % n;
      Eigen::Matrix3d tripletPositions;
      Eigen::RowVector3d position1 = _positions.row(order(i));
      Eigen::RowVector3d position2 = _positions.row(order(j));
      tripletPositions << position1, position2, _centrePosition.transpose();
      Eigen::Vector3i tripletChannels;
      tripletChannels << order(i), order(j), n;
      _regions.push_back(
          boost::make_unique<Triplet>(tripletChannels, tripletPositions));
    }
  }

  boost::optional<Eigen::VectorXd> VirtualNgon::handle(
      Eigen::Vector3d position) const {
    for (const auto& region : _regions) {
      boost::optional<Eigen::VectorXd> pv = region->handleRemap(
          position, static_cast<int>(_centreDownmix.size() + 1));
      if (pv) {
        // downmix the last channel containing the virtual centre
        // speaker into the real speakers, and renormalise
        Eigen::VectorXd _pv = pv.get();
        _pv = _pv.head(_pv.size() - 1) + _centreDownmix * _pv.tail(1);
        _pv /= _pv.norm();
        return _pv;
      }
    }
    return boost::none;
  }

  QuadRegion::QuadRegion(Eigen::VectorXi outputChannels,
                         Eigen::MatrixXd positions)
      : RegionHandler(outputChannels, positions) {
    _order = ngonVertexOrder(positions);
    Eigen::MatrixXd reorderedPositions = positions(_order, Eigen::all);
    Eigen::MatrixXd reorderedAndShiftedPositions =
        reorderedPositions(Eigen::Vector4i{1, 2, 3, 0}, Eigen::all);
    _polyBasisX = _calcPolyBasis(reorderedPositions);
    _polyBasisY = _calcPolyBasis(reorderedAndShiftedPositions);
  };

  boost::optional<Eigen::VectorXd> QuadRegion::handle(
      Eigen::Vector3d position) const {
    boost::optional<double> x = _pan(position, _polyBasisX);
    boost::optional<double> y = _pan(position, _polyBasisY);

    if (x == boost::none || y == boost::none) {
      return boost::none;
    }

    Eigen::VectorXd pvs = Eigen::Vector4d::Zero();
    pvs(_order) << (1 - x.get()) * (1 - y.get()), x.get() * (1 - y.get()),
        x.get() * y.get(), (1 - x.get()) * y.get();
    if ((pvs.transpose() * _positions) * position <= 0) {
      return boost::none;
    }
    pvs /= pvs.norm();
    return pvs;
  }

  Eigen::Matrix3d QuadRegion::_calcPolyBasis(Eigen::MatrixXd positions) {
    Eigen::Vector3d a = positions.row(0);
    Eigen::Vector3d b = positions.row(1);
    Eigen::Vector3d c = positions.row(2);
    Eigen::Vector3d d = positions.row(3);

    Eigen::Matrix3d polyBasis;
    polyBasis <<  //
        (b - a).cross(c - d),  //
        a.cross(c - d) + (b - a).cross(d),  //
        a.cross(d);

    return polyBasis.transpose();
  }

  /** @brief Calculate the real roots of a quadratic
   *
   * @param a Quadratic term.
   * @param b Linear term.
   * @param c Constant term.
   *
   * @note The equation solved is given by
   *
   *  a x^2 + bx + c = 0
   *
   * @returns The real roots of the quadratic.
   */
  std::vector<double> real_quadratic_roots(double a, double b, double c) {
    double eps = 1e-10;

    if (std::abs(c) < eps) return {0.0};
    if (std::abs(a) < eps) return {-c / b};

    double det = b * b - 4.0 * a * c;
    if (det > eps)
      return {(-b + sqrt(det)) / (2.0 * a), (-b - sqrt(det)) / (2.0 * a)};
    else if (det > -eps)
      return {-b / (2.0 * a)};
    else
      return {};
  }

  boost::optional<double> QuadRegion::_pan(Eigen::Vector3d position,
                                           Eigen::Matrix3d polyBasis) const {
    double epsilon = 1e-10;

    Eigen::Vector3d poly = polyBasis * position;

    for (double root : real_quadratic_roots(poly(0), poly(1), poly(2))) {
      if (-epsilon < root && root < 1.0 + epsilon) {
        return boost::algorithm::clamp(root, 0.0, 1.0);
      }
    }
    return boost::none;
  }

  PolarPointSourcePanner::PolarPointSourcePanner(
      std::vector<std::unique_ptr<RegionHandler>> regions,
      boost::optional<int> numberOfChannels)
      : _regions(std::move(regions)) {
    if (!numberOfChannels) {
      _numberOfOutputChannels = _numberOfRequiredChannels();
    } else {
      _numberOfOutputChannels = numberOfChannels.get();
      ear_assert(_numberOfOutputChannels >= _numberOfRequiredChannels(),
                 "not enough output channels in PolarPointSourcePanner");
    }
  };

  boost::optional<Eigen::VectorXd> PolarPointSourcePanner::handle(
      Eigen::Vector3d position) {
    boost::optional<Eigen::VectorXd> pv;
    for (const auto& region : _regions) {
      pv = region->handleRemap(position, _numberOfOutputChannels);
      if (pv) {
        return pv;
      }
    }
    return boost::none;
  }

  int PolarPointSourcePanner::numberOfOutputChannels() const {
    return _numberOfOutputChannels;
  }

  int PolarPointSourcePanner::_numberOfRequiredChannels() {
    int ret = 0;
    for (const auto& region : _regions) {
      auto maxRegion = region->outputChannels().maxCoeff();
      if (ret < maxRegion) {
        ret = maxRegion;
      }
    }
    return ret + 1;
  }

  PointSourcePannerDownmix::PointSourcePannerDownmix(
      std::shared_ptr<PointSourcePanner> psp, Eigen::MatrixXd downmix)
      : _psp(psp), _downmix(downmix){};

  boost::optional<Eigen::VectorXd> PointSourcePannerDownmix::handle(
      Eigen::Vector3d position) {
    boost::optional<Eigen::VectorXd> pv = _psp->handle(position);
    if (pv) {
      Eigen::VectorXd _pv = pv.get();
      _pv = _downmix.transpose() * _pv;
      _pv /= _pv.norm();
      return _pv;
    }
    return boost::none;
  }

  int PointSourcePannerDownmix::numberOfOutputChannels() const {
    return static_cast<int>(_downmix.cols());
  }

  /** @brief Generate extra loudspeaker positions to fill gaps in layers.
   *
   * @param  layout Original layout without the LFE channels
   *
   * @returns
   *   - list of extra channels (layout.Channel).
   *   - downmix matrix to mix the extra channel outputs to the real channels
   */
  std::pair<std::vector<Channel>, Eigen::MatrixXd> extraPosVerticalNominal(
      Layout layout) {
    std::vector<Channel> extraChannels;
    Eigen::MatrixXd downmix = Eigen::MatrixXd::Identity(
        layout.channels().size(), layout.channels().size());

    Layout midLayerLayout;
    std::copy_if(layout.channels().begin(), layout.channels().end(),
                 std::back_inserter(midLayerLayout.channels()), [](Channel c) {
                   return -10 <= c.polarPositionNominal().elevation &&
                          c.polarPositionNominal().elevation <= 10;
                 });

    auto layers = {std::make_tuple(-30.0, -70.0, -10.0),
                   std::make_tuple(30.0, 10.0, 70.0)};

    double layerNominalElevation, layerLowerBound, layerUpperBound;
    for (const auto& layer : layers) {
      std::tie(layerNominalElevation, layerLowerBound, layerUpperBound) = layer;

      Layout currentLayerLayout;
      std::copy_if(
          layout.channels().begin(), layout.channels().end(),
          std::back_inserter(currentLayerLayout.channels()), [&](Channel c) {
            return layerLowerBound <= c.polarPositionNominal().elevation &&
                   c.polarPositionNominal().elevation <= layerUpperBound;
          });

      // for each loudspeaker in the mid layer that has an azimuth greater
      // than az_limit, add a virtual speaker directly above/below it at the
      // elevation of the current layer, which is downmixed directly to the
      // mid layer loudspeaker. az_limit is set to the range of azimuths in
      // the current layer, with some space added to prevent fast vertical
      // source movements when sources move horizontally. If there are no
      // channels on this layer then a copy of all mid layer speakers is
      // made.
      double azimuthLimit = 0.0;
      double layerRealElevation = 0.0;
      if (currentLayerLayout.channels().size() != 0) {
        double azimuthRange = std::numeric_limits<double>::min();
        for (const auto& channel : currentLayerLayout.channels()) {
          if (azimuthRange < std::abs(channel.polarPositionNominal().azimuth)) {
            azimuthRange = std::abs(channel.polarPositionNominal().azimuth);
          }
        }
        azimuthLimit = azimuthRange + 40.0;
        layerRealElevation =
            std::accumulate(currentLayerLayout.channels().begin(),
                            currentLayerLayout.channels().end(), 0.0,
                            [&](double sum, const Channel& c) -> double {
                              return sum + c.polarPosition().elevation;
                            }) /
            static_cast<double>(currentLayerLayout.channels().size());
      } else {
        layerRealElevation = layerNominalElevation;
      }

      double epsilon = 1e-5;
      for (const auto& midChannel : midLayerLayout.channels()) {
        if (std::abs(midChannel.polarPosition().azimuth) >=
            azimuthLimit - epsilon) {
          extraChannels.push_back(Channel(
              "extra",
              PolarPosition(midChannel.polarPosition().azimuth,
                            layerRealElevation, 1.0),
              PolarPosition(midChannel.polarPositionNominal().azimuth,
                            layerNominalElevation, 1.0)

                  ));
          Eigen::VectorXd downmixRow =
              Eigen::VectorXd::Zero(layout.channels().size());
          auto names = layout.channelNames();
          int midChannelIndex = static_cast<int>(std::distance(
              names.begin(),
              std::find(names.begin(), names.end(), midChannel.name())));
          downmixRow(midChannelIndex) = 1.0;
          downmix.conservativeResize(downmix.rows() + 1, Eigen::NoChange);
          downmix.row(downmix.rows() - 1) = downmixRow;
        }
      }
    }
    return std::make_pair(extraChannels, downmix);
  }

  /** @brief Find the adjacent vertices in a hull to the given vertex.
   *
   * @param  facets (list of sets of ints): Convex hull facets, each item
   * represents a facet, with the contents of the set being its vertex indices.
   * @param  vert (int): Vertex index to find vertices adjacent to.
   *
   * @returns Vertices adjacent to `vert`.
   */
  Facet _adjacent_verts(std::vector<Facet> facets, int vert) {
    std::set<int> ret;
    for (const auto& facetVerts : facets) {
      if (std::find(facetVerts.begin(), facetVerts.end(), vert) !=
          facetVerts.end()) {
        ret.insert(facetVerts.begin(), facetVerts.end());
      }
    }
    ret.erase(vert);
    return ret;
  }

  StereoPannerDownmix::StereoPannerDownmix(Eigen::VectorXi outputChannels,
                                           Eigen::MatrixXd positions)
      : RegionHandler(outputChannels, positions) {
    auto layout = getLayout("0+5+0").withoutLfe();
    _psp = configureFullPolarPanner(layout);
  }

  boost::optional<Eigen::VectorXd> StereoPannerDownmix::handle(
      Eigen::Vector3d position) const {
    Eigen::MatrixXd downmix(2, 5);
    downmix << 1.0, 0.0, std::sqrt(3.0) / 3.0, std::sqrt(0.5), 0.0,  //
        0.0, 1.0, std::sqrt(3.0) / 3.0, 0.0, std::sqrt(0.5);

    // pan with 0+5+0, downmix and power normalise
    boost::optional<Eigen::VectorXd> pv = _psp->handle(position);

    if (pv) {
      Eigen::VectorXd _pv = pv.get();
      Eigen::VectorXd pvDownmix = downmix * _pv;
      pvDownmix /= pvDownmix.norm();

      // vary the output level by the balance between the front and rear
      // loudspeakers; 0dB at the front to -3dB at the back
      double front = _pv.head(3).maxCoeff();
      double back = _pv.tail(2).maxCoeff();

      pvDownmix = pvDownmix * std::pow(0.5, (0.5 * back / (front + back)));
      return pvDownmix;
    }
    return boost::none;
  }

  boost::optional<Eigen::VectorXd> AllocentricPanner::handle(
      Eigen::Vector3d position) {
    return boost::none;
  }

  int AllocentricPanner::numberOfOutputChannels() const { return 0; }

  std::shared_ptr<PointSourcePanner> configureStereoPolarPanner(
      const Layout& layout) {
    auto leftChannel = layout.channelWithName("M+030");
    auto rightChannel = layout.channelWithName("M-030");
    auto channelNames = layout.channelNames();
    auto leftChannelIndex =
        distance(channelNames.begin(),
                 find(channelNames.begin(), channelNames.end(), "M+030"));
    auto rightChannelIndex =
        distance(channelNames.begin(),
                 find(channelNames.begin(), channelNames.end(), "M-030"));

    Eigen::MatrixXd positions(2, 3);
    positions << toCartesianVector3d(leftChannel.polarPosition()).transpose(),
        toCartesianVector3d(rightChannel.polarPosition()).transpose();
    Eigen::Vector2i outputChannels{leftChannelIndex, rightChannelIndex};

    auto panner =
        boost::make_unique<StereoPannerDownmix>(outputChannels, positions);

    std::vector<std::unique_ptr<RegionHandler>> regions;
    regions.push_back(std::move(panner));
    return std::make_shared<PolarPointSourcePanner>(std::move(regions));
  }

  std::tuple<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>,
             std::set<int>, Eigen::MatrixXd>
  getAugmentedLayout(const Layout& layout) {
    // add some extra height speakers that are treated as real speakers until
    // the downmix in PointSourcePannerDownmix
    std::vector<Channel> allChannels(layout.channels());

    std::vector<Channel> extraChannels;
    Eigen::MatrixXd downmix;
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    for (const auto& extraChannel : extraChannels) {
      allChannels.push_back(extraChannel);
    }

    // add some virtual speakers above and below that will be used as the
    // centre speaker in a virtual ngon. No upper speaker is added for
    // layouts with UH+180 as this speaker may actually be directly
    // overhead, which may cause a step in the gains wrt the source
    // position.
    auto channelNames = layout.channelNames();

    std::vector<Eigen::Vector3d> virtualPositions;
    virtualPositions.push_back(Eigen::Vector3d{0.0, 0.0, -1.0});
    if (std::find(channelNames.begin(), channelNames.end(), "T+000") ==
            channelNames.end() &&
        std::find(channelNames.begin(), channelNames.end(), "UH+180") ==
            channelNames.end()) {
      virtualPositions.push_back(Eigen::Vector3d{0.0, 0.0, 1.0});
    }

    std::vector<Eigen::Vector3d> positionsReal;
    std::vector<Eigen::Vector3d> positionsNominal;
    std::set<int> virtualVerts;

    for (const Channel& channel : allChannels) {
      positionsReal.push_back(toNormalisedVector3d(channel.polarPosition()));
      positionsNominal.push_back(
          toNormalisedVector3d(channel.polarPositionNominal()));
    }
    for (const Eigen::Vector3d& pos : virtualPositions) {
      virtualVerts.insert(static_cast<int>(positionsReal.size()));
      positionsReal.push_back(pos);
      positionsNominal.push_back(pos);
    }

    return {positionsReal, positionsNominal, virtualVerts, downmix};
  }

  std::shared_ptr<PointSourcePanner> configureFullPolarPanner(
      const Layout& layout) {
    std::vector<Eigen::Vector3d> positionsReal;
    std::vector<Eigen::Vector3d> positionsNominal;
    std::set<int> virtualVerts;
    Eigen::MatrixXd downmix;
    std::tie(positionsReal, positionsNominal, virtualVerts, downmix) =
        getAugmentedLayout(layout);

    // Facets of the convex hull; each set represents a facet and contains the
    // indices of its corners in positions.
    auto facets_it = FACETS.find(layout.name());
    std::vector<Facet> facets = facets_it != FACETS.end()
                                    ? facets_it->second
                                    : convex_hull(positionsNominal);

    // Turn the facets into regions for the point source panner.
    std::vector<std::unique_ptr<RegionHandler>> regions;

    // Facets adjacent to one of the virtual speakers are turned into virtual
    // ngons, with an equal power downmix from the virtual speaker to the real
    // speakers.
    for (int virtualVert : virtualVerts) {
      Facet realVerts = _adjacent_verts(facets, virtualVert);

      ear_assert(!doIntersect(realVerts.begin(), realVerts.end(),
                              virtualVerts.begin(), virtualVerts.end()),
                 "invalid triangulation");

      std::vector<int> realVertsVec(realVerts.begin(), realVerts.end());
      Eigen::Map<Eigen::VectorXi> outputChannels(realVertsVec.data(),
                                                 realVertsVec.size());
      Eigen::MatrixXd positions(outputChannels.size(), 3);
      int rowIndex = 0;
      for (int vert : realVerts) {
        positions.row(rowIndex) = positionsReal[vert];
        ++rowIndex;
      }
      Eigen::Vector3d centrePosition = positionsReal[virtualVert];
      Eigen::VectorXd centreDownmix(outputChannels.size());
      centreDownmix.fill(1.0 /
                         std::sqrt(static_cast<double>(outputChannels.size())));

      regions.push_back(boost::make_unique<VirtualNgon>(
          outputChannels, positions, centrePosition, centreDownmix));
    }
    // Facets not adjacent to virtual speakers are turned into triplets or
    // quads. In the supported layouts there are never facets with more
    // vertices.
    for (const auto& facetVerts : facets) {
      if (doIntersect(facetVerts.begin(), facetVerts.end(),
                      virtualVerts.begin(), virtualVerts.end())) {
        continue;
      }

      if (facetVerts.size() == 3) {
        std::vector<int> facetVertsVec(facetVerts.begin(), facetVerts.end());
        Eigen::Vector3i outputChannels(facetVertsVec.data());
        Eigen::MatrixXd positions(outputChannels.size(), 3);
        int rowIndex = 0;
        for (int vert : facetVerts) {
          positions.row(rowIndex) = positionsReal[vert];
          ++rowIndex;
        }
        regions.push_back(
            boost::make_unique<Triplet>(outputChannels, positions));
      } else if (facetVerts.size() == 4) {
        std::vector<int> facetVertsVec(facetVerts.begin(), facetVerts.end());
        Eigen::Vector4i outputChannels(facetVertsVec.data());
        Eigen::MatrixXd positions(outputChannels.size(), 3);
        int rowIndex = 0;
        for (int vert : facetVerts) {
          positions.row(rowIndex) = positionsReal[vert];
          ++rowIndex;
        }
        regions.push_back(
            boost::make_unique<QuadRegion>(outputChannels, positions));
      } else {
        throw internal_error(
            "facets with more than 4 vertices are not supported");
      }
    }
    return std::make_shared<PointSourcePannerDownmix>(
        std::make_shared<PolarPointSourcePanner>(std::move(regions)), downmix);
  }

  // Check that screen loudspeakers are within allowed ranges.
  void checkScreenSpeakers(const Layout& layout) {
    for (auto& channel : layout.channels()) {
      if (channel.name() == "M+SC" || channel.name() == "M-SC") {
        double abs_az = std::abs(channel.polarPosition().azimuth);
        if (!((5.0 <= abs_az && abs_az < 25.0) ||
              (35.0 <= abs_az && abs_az < 60.0))) {
          throw invalid_argument(
              "M+SC or M-SC has azimuth not in the allowed ranges of 5 to 25 "
              "and 35 to 60 degrees");
        }

        if (25.0 < abs_az) {
          throw not_implemented(
              "M+SC and M-SC with azimuths wider than 25 degrees are not "
              "currently supported");
        }
      }
    }
  }

  std::shared_ptr<PointSourcePanner> configureAllocentricPanner(
      const Layout& layout) {
    checkScreenSpeakers(layout);
    return std::make_shared<AllocentricPanner>();
  }

  std::shared_ptr<PointSourcePanner> configurePolarPanner(
      const Layout& layout) {
    auto isLfe = layout.isLfe();
    if (find(isLfe.begin(), isLfe.end(), true) != isLfe.end()) {
      throw internal_error("lfe channel passed to point source panner");
    }

    checkScreenSpeakers(layout);

    if (layout.name() == std::string("0+2+0")) {
      return configureStereoPolarPanner(layout);
    } else {
      return configureFullPolarPanner(layout);
    }
  }
}  // namespace ear


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIV_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bs::pack_<bd::floating_<T>, X>
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::ifix_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return saturated_(toint)(a/b);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bs::pack_<bd::floating_<T>, X>
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::iceil_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return iceil(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::ifloor_
                          , bs::pack_<bd::floating_<T>, X>
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::ifloor_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return ifloor(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::iround_
                          , bs::pack_<bd::floating_<T>, X>
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE  bd::as_integer_t<T>  operator()( bd::functor<bs::tag::iround_> const&
                                                      ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return iround(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bs::pack_<bd::floating_<T>, X>
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::inearbyint_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return inearbyint(a/b);
    }
  };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bs::pack_<bd::integer_<T>, X>
                          , bs::pack_<bd::integer_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::ifix_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return  div(fix, a, b);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bs::pack_<bd::integer_<T>, X>
                          , bs::pack_<bd::integer_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::iceil_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return div(ceil, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::ifloor_
                          , bs::pack_<bd::integer_<T>, X>
                          , bs::pack_<bd::integer_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::ifloor_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return  div(floor, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::iround_
                          , bs::pack_<bd::integer_<T>, X>
                          , bs::pack_<bd::integer_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::iround_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return div(round, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bs::pack_<bd::integer_<T>, X>
                          , bs::pack_<bd::integer_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::inearbyint_> const&
                                  ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return  div(nearbyint, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<T>, X>
                          , bs::pack_<bd::arithmetic_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };

} } }

#endif


/**
 * \file PedalToneStackFilter.hxx
 */

#include <boost/math/tools/polynomial.hpp>

#include <ATK/EQ/PedalToneStackFilter.h>

namespace ATK
{
  template<typename DataType>
  SD1ToneCoefficients<DataType>::SD1ToneCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels)
  {
  }

  template<typename DataType>
  void SD1ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    CoeffDataType tempm[2] = {static_cast<CoeffDataType>(-2) * input_sampling_rate, static_cast<CoeffDataType>(2) * input_sampling_rate};
    CoeffDataType tempp[2] = {static_cast<CoeffDataType>(1), static_cast<CoeffDataType>(1)};
    boost::math::tools::polynomial<CoeffDataType> poly1(tempm, 1);
    boost::math::tools::polynomial<CoeffDataType> poly2(tempp, 1);

    boost::math::tools::polynomial<CoeffDataType> b;
    boost::math::tools::polynomial<CoeffDataType> a;

    b += poly2 * poly2;
    b += poly2 * poly1 * (C2*R3+R4*C3+alpha*(1-alpha)*R2*C2+alpha*C2*R4);
    b += poly1 * poly1 * (C3*R4*(R3*C2+alpha*(1-alpha)*R2*C2));

    a += poly2 * poly2;
    a += poly2 * poly1 * (C2*R3+R1*C1+alpha*(1-alpha)*R2*C2+(1-alpha)*C2*R1);
    a += poly1 * poly1 * (C1*R1*(R3*C2+alpha*(1-alpha)*R2*C2));

    for(gsl::index i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(gsl::index i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void SD1ToneCoefficients<DataType_>::set_tone(CoeffDataType alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  typename SD1ToneCoefficients<DataType_>::CoeffDataType SD1ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }

  template<typename DataType>
  TS9ToneCoefficients<DataType>::TS9ToneCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels)
  {
  }

  template<typename DataType>
  void TS9ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    CoeffDataType tempm[2] = {static_cast<CoeffDataType>(-2) * input_sampling_rate, static_cast<CoeffDataType>(2) * input_sampling_rate};
    CoeffDataType tempp[2] = {static_cast<CoeffDataType>(1), static_cast<CoeffDataType>(1)};
    boost::math::tools::polynomial<CoeffDataType> poly1(tempm, 1);
    boost::math::tools::polynomial<CoeffDataType> poly2(tempp, 1);

    boost::math::tools::polynomial<CoeffDataType> b;
    boost::math::tools::polynomial<CoeffDataType> a;

    b += poly2 * poly2 * R2;
    b += poly2 * poly1 * (alpha * C2 * R2 * R3 + alpha * (1-alpha) * C2 * P * R2 + R2 * R4 * C2);

    a += poly2 * poly2 * (R2 + R1);
    a += poly2 * poly1 * ((1-alpha) * C2 * (alpha * P * R2 + R1 * alpha * P + R1 * R2) + R4 * C2 * (R2 + R1) + R1 * C1 * R2);
    a += poly1 * poly1 * (C2 * R4 * C1 * R2 * R1 + (1-alpha) * C2 * R1 * P * C1 * R2);

    for(gsl::index i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(gsl::index i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void TS9ToneCoefficients<DataType_>::set_tone(CoeffDataType alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  typename TS9ToneCoefficients<DataType_>::CoeffDataType TS9ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }
}


// 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
//
//     https://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 "SphereWeights.h"

#include <algorithm>
#include <cfloat>
#include <numeric>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include <igl/harmonic.h>

#include <OpenGP/Image/Image.h>
#include <OpenGP/SphereMesh/helpers.h>

using namespace OpenGP;
using Vec2i = Eigen::Vector2i;
using Vec3i = Eigen::Vector3i;
using Vec4i = Eigen::Vector4i;

void calc_weights(const SphereMesh &smesh, SurfaceMesh &mesh) {

    auto vpoint = smesh.get_vertex_property<Vec4>("v:point");

    auto distribute = [&](Vec3 p, SphereMesh::Edge edge) -> Vec2 {
        Vec4 s0 = vpoint[smesh.vertex(edge, 0)];
        Vec4 s1 = vpoint[smesh.vertex(edge, 1)];

        Vec3 c0 = s0.head<3>();
        Vec4 a = s1 - s0;
        Vec3 d = a.head<3>();
        float l = d.norm();
        Vec3 an = d / l;
        Vec3 d2 = p - c0;
        Vec3 p_proj = d2 - an * d2.dot(an);
        float beta = asin(a(3) / l);
        Vec3 offset = an * p_proj.norm() * tan(beta);
        float t = an.dot(p + p_proj + offset - c0) / l;
        t = fmax(fmin(t, 1), 0);
        return Vec2(1 - t, t);
    };

    int n_pills = smesh.n_edges();
    int n_verts = mesh.n_vertices();

    Eigen::MatrixXf P(n_verts, n_pills);
    P.setZero();

    float lambda = 0.1;

    for (int i = 0; i < n_verts; ++i) {

        auto vert_i = SurfaceMesh::Vertex(i);

        for (int j = 0; j < smesh.n_edges(); ++j) {

            auto edge_j = SphereMesh::Edge(j);

            Vec4 s0 = vpoint[smesh.vertex(edge_j, 0)];
            Vec4 s1 = vpoint[smesh.vertex(edge_j, 1)];

            float sdf;
            pill_project(mesh.position(vert_i), s0, s1, &sdf);

            P(i, j) = 1.f / (1e-6 + std::pow(std::max(sdf, 0.f), 2.f));
        }

        float sum = P.row(i).sum();
        P.row(i) /= sum;
    }

    P *= lambda;

    Eigen::Matrix<double, -1, -1> V(n_verts, 3);
    Eigen::Matrix<int, -1, -1> F(mesh.n_faces(), 3);

    for (int i = 0; i < n_verts; ++i) {
        V.row(i) =
            mesh.position(SurfaceMesh::Vertex(i)).transpose().cast<double>();
    }

    for (int i = 0; i < mesh.n_faces(); ++i) {
        int j = 0;
        for (auto vert : mesh.vertices(SurfaceMesh::Face(i)))
            F(i, j++) = vert.idx();
    }

    Eigen::SparseMatrix<double> Ld(n_verts, n_verts);

    igl::harmonic(V, F, 1, Ld);

    for (int i = 0; i < n_verts; ++i) {
        Ld.coeffRef(i, i) += lambda;
    }

    Eigen::SparseMatrix<float> L = Ld.cast<float>();

    Eigen::SparseLU<Eigen::SparseMatrix<float>> solver(L);

    Eigen::MatrixXf weights(n_verts, n_pills);
    for (int i = 0; i < n_pills; ++i) {
        weights.col(i) = solver.solve(P.col(i));
    }

    auto weights_prop =
        mesh.add_vertex_property<std::vector<float>>("v:skinweight");
    auto bone_ids_prop = mesh.add_vertex_property<std::vector<int>>("v:boneid");

    for (int i = 0; i < n_verts; ++i) {
        Vec3 p = mesh.position(SurfaceMesh::Vertex(i));
        Eigen::VectorXf row = weights.row(i).transpose();
        std::vector<float> per_cap_weights;
        for (int j = 0; j < n_pills; ++j) {
            Vec2 cap_weights = distribute(p, SphereMesh::Edge(j)) * row[j];
            per_cap_weights.push_back(cap_weights[0]);
            per_cap_weights.push_back(cap_weights[1]);
        }
        std::vector<int> inds(2 * n_pills);
        std::iota(inds.begin(), inds.end(), 0);
        std::sort(inds.begin(), inds.end(), [&](int i, int j) {
            return per_cap_weights[i] > per_cap_weights[j];
        });

        std::vector<float> w;
        std::vector<int> wi;
        for (int j = 0; j < inds.size(); ++j) {
            w.push_back(per_cap_weights[inds[j]]);
            wi.push_back(inds[j]);
            if (w.back() / w.front() < 0.01)
                break;
        }

        float sum = std::accumulate(w.begin(), w.end(), 0.f);
        std::transform(w.begin(), w.end(), w.begin(),
                       [sum](float x) { return x / sum; });

        SurfaceMesh::Vertex vert(i);
        weights_prop[vert] = w;
        bone_ids_prop[vert] = wi;

    }

}