#ifndef L_Katyusha_H
#define L_Katyusha_H

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>
#include <math.h>

//This class implements the loopless variance reduced type methods with arbitrary sampling

/*
The optimization problem to solve is:

F(x):=\sum_{i=1}^n 1/n* phi_i(x)+ g(x)
*/

template<typename L, typename D>
class L_Katyusha
{

protected:

  // involved variables

  std::vector<D> proba_vector;

  std::vector<D>  tilde_proba_vector;

  std::vector<D> group_C;

  std::vector<D> index_group_C;

  std::vector<D> maxp_group_C;

  std::vector<D> isolated_I;

  std::vector<D> sump_group_C;
  std::vector<D> theta_S;   //theta_S in the paper

  std::vector<D> Li;  // the Lipchitz constant of phi_i

  std::vector<D> gk;     // the vector g^k in the paper

  std::vector<D> gradient_f_w;  // gradient of f at w
  std::vector<D> gradient_f_x;  // gradient of f at x
  std::vector<D> batch_delta_gradient; //  1/n(G(x^k)-G(w^k))theta_{S_k} I_{S_k} e in the paper

  L c;

  L noverc;

  // auxiliary variables

  L nb_of_iters_per_loop;

  L max_nb_loops;

  D max_p;

  L nsamples;
  L nfeatures;

  D running_time;

  L nb_loops;

  L print_every_N;

  vector<L> batch_i;
  vector<L> my_batch;

  L batch_size;

  L nb_groups;    //number of groups in group sampling

  string uniform;

  D Lf;

  D L2;

  D sumLi;

  D upper_bound;   // upper bound of F(x)-F^*

  D p; // the probability of changing w to x as in the paper
  L tau;  //number of threads on each node/computer

  D mu;

  D scaler;

  L current_nb_iters;

  L nb_iters;

  ofstream samp;

  D theta1;
  D theta2;
  D theta3;
  D eta;  //eta/L in the paper
  D oneovereta;

  D primal_value;

  std::vector<D> x;
  std::vector<D> w;
  std::vector<D> y;
  std::vector<D> z;
  std::vector<D> next_x;

public:

  gsl_rng * rng;
  virtual inline D value_of_phi_i(L) {return D(NULL);}
  virtual inline D value_of_g(){return D(NULL);}
  virtual inline void prox_of_g(D, vector<D> &, vector<D> &, vector<D> &){}   //prox_of_g(L,x, gr, y) computes y=argmin_u{L/2 ||u-(x-gr/L)||^2+g(u)}
  virtual inline void set_auxiliary_v(){}
  virtual inline void update_gradient(){}
  virtual inline void set_Li_Lf(){}
  virtual inline void compute_batch_delta_gradient(){}
  virtual inline void compute_full_gradient(vector<D> &, vector<D> &){}

  L_Katyusha()
  {

  }

  void update_x(){
    for(L i=0; i<nfeatures; i++){
      x[i]= theta1*z[i]+ theta2*w[i]+ theta3*y[i];
    }
  }

  void update_y(){
    for(L i=0; i<nfeatures; i++){
      y[i]=theta1*z[i]+ theta2*w[i]+ theta3*y[i];
    }
  }

    void update_gk(){
      batch_delta_gradient.clear();
      batch_delta_gradient.resize(nfeatures,0);
      compute_batch_delta_gradient();
      for(L i=0;i<nfeatures;i++){
        gk[i]=batch_delta_gradient[i]+gradient_f_w[i];
      }
    }

    void update_z(){
      prox_of_g(oneovereta,z,gk,z);
    }

    void update_w(){
      D yi=gsl_rng_uniform(rng);
      if(yi<=p){
        w=x;
        compute_full_gradient(w, gradient_f_w);
      }
    }

    void compute_upper_bound_of_optimality_gap(){
      compute_full_gradient(x, gradient_f_x);
      prox_of_g(Lf,x,gradient_f_x,next_x);
      D tmp=0;
      for(L i=0;i<nfeatures;i++){
        tmp+=(next_x[i]-x[i])*(next_x[i]-x[i]);
        x[i]=next_x[i];
      }
      upper_bound=tmp*Lf*Lf/mu;
   	}

    void set_rng()
    {
      gsl_rng_env_setup();
      const gsl_rng_type * T;
      T = gsl_rng_default;
      rng = gsl_rng_alloc(T);
      gsl_rng_set(rng,time(NULL));
      //gsl_rng_set(rng, 27432042);

    }

    void set_print_every_N(L i){print_every_N=i;}

    void set_optimal_probability()
    {
      tilde_proba_vector.clear();
      tilde_proba_vector.resize(nsamples,0);
      proba_vector.clear();
      proba_vector.resize(nsamples,0);
      D sum=0;
      for(L i=0; i<nsamples;i++)
      {
        tilde_proba_vector[i]=Li[i];
        sum+=Li[i];
      }
      max_p=0;
      for(L i=0; i<nsamples;i++)
      {
        tilde_proba_vector[i]=tilde_proba_vector[i]/sum;
        proba_vector[i]=1-pow(1-tilde_proba_vector[i],tau);
        if(max_p<tilde_proba_vector[i])
        {
          max_p=tilde_proba_vector[i];
        }
      }
      cout<<"sum="<<sum<<"; proba"<<tilde_proba_vector[0]<<endl;
    }

    void set_group_sampling_probability(){
      proba_vector.clear();
      proba_vector.resize(nsamples,0);
      std::vector<D> q(nsamples);
      D sumq=0;
      cout<<"start setting group sampling probablity"<<endl;
      for(L i=0; i<nsamples;i++)
      {
        q[i]=Li[i];
        sumq+=q[i];
      }
      D maxq=0;
      L nb=0;
      D tmp=0;
      for(L i=0; i<nsamples;i++)
      {
        q[i]=q[i]/sumq*tau;
        if(q[i]>maxq) maxq=q[i];
        if(q[i]>1) {
          nb++; //count the number of elements larger than 1
          tmp+=q[i]-1;
        }
      }
      cout<<"maxq="<<maxq<<endl;
      if(maxq<=1){
        for(L i=0; i<nsamples;i++)
        {
          proba_vector[i]=q[i];
        }
      }else{
        for(L i=0; i<nsamples;i++)
        {
          if(q[i]>1) q[i]=1;
          else {
            D deltaq=min(1-q[i],tmp);
            tmp=tmp-deltaq;
            q[i]+=deltaq;
          }
          proba_vector[i]=q[i];
        }
      }
    }

    void set_L2(L p_mod, L u){
      if(p_mod==0){ //batch sampling mode
        D tmp=0;
        D st;
        for(L i=0;i<nsamples;i++){
          st=Li[i]/tilde_proba_vector[i];
          if (st>tmp) tmp=st;
        }
        L2=tmp/nsamples/tau;
        cout<<"L2="<<L2<<"; "<<tilde_proba_vector[0]<<endl;
      }
      else{ //group sampling mode
        D tmp=0;
        D st;
        for(L i=0;i<nsamples;i++){
          st=Li[i]/proba_vector[i];
          if(isolated_I[i]==1) st=st-Li[i];
          if(st>tmp) tmp=st;
        }
        L2=tmp/nsamples;
      }
      cout<<"set_L2="<<L2<<endl;
    }

    void set_uniform_probability(L p_mod)
    {
      if(p_mod==0) {
        //batch sampling (stochastic process as defined in the paper)
        proba_vector.clear();
        tilde_proba_vector.clear();
        tilde_proba_vector.resize(nsamples,1./nsamples);
        proba_vector.resize(nsamples,1-pow(1-1.0/nsamples,tau));
        max_p=1.0/nsamples;
        cout<<"uniform="<<tilde_proba_vector[0]<<endl;
      }
      else{ //group sampling
        D pi=(tau*c+0.0)/nsamples;
        proba_vector.clear();
        proba_vector.resize(nsamples,pi);
      }
    }

    void sort_p(){
      vector<pair<D,L> >a;
      for (L i = 0 ;i < nsamples ; i++) {
        a.push_back(make_pair(proba_vector[i],i)); // k = value, i = original index
      }
      sort(a.begin(),a.end());
      group_C.clear();
      group_C.resize(nsamples);
      isolated_I.clear();
      isolated_I.resize(nsamples,0);
      D tmp=0;
      D maxpi=0;
      D previous_i=0;
      index_group_C.clear();
      index_group_C.push_back(0);
      maxp_group_C.clear();
      sump_group_C.clear();

      for (L i = 0 ;i < nsamples ; i++){
        group_C[i]=a[nsamples-1-i].second;
        D pi=a[nsamples-1-i].first;
        if(tmp+pi<=1){
          if(pi>maxpi) maxpi=pi;
          tmp+=pi;
        }
        else{
          index_group_C.push_back(i);
          maxp_group_C.push_back(maxpi);
          sump_group_C.push_back(tmp);
          tmp=pi;
          maxpi=pi;
          if(i-previous_i==1) isolated_I[group_C[previous_i]]=1;
          previous_i=i;
        }
      }
      index_group_C.push_back(nsamples);
      maxp_group_C.push_back(maxpi);
      sump_group_C.push_back(tmp);
      nb_groups=sump_group_C.size();
      if(previous_i==nsamples-1) isolated_I[group_C[previous_i]]=1;
      cout<<"size of group="<<nb_groups<<endl;
    }

    inline L sampling(L n)
    {
      L i=(floor)(gsl_rng_uniform(rng)*n);
      D y=gsl_rng_uniform(rng);
      while(y*max_p>tilde_proba_vector[i])
      {
        i=(floor)(gsl_rng_uniform(rng)*n);
        y=gsl_rng_uniform(rng);
      }
      return i;
    }

    void set_theta(){
      theta2=L2/2/max(Lf,L2);
      if(Lf<=L2/p){
        D tmp=mu/L2/p;
        cout<<"L2="<<L2<<endl;
        cout<<"tmp="<<tmp<<endl;
        if(tmp>=1){
          theta1=theta2;
        }
        else{
          theta1=sqrt(tmp)*theta2;
        }
      }else
      theta1=min(sqrt(mu/Lf),p/2);
      theta3=1-theta1-theta2;
      eta=1./(theta1*(Lf+2*max(L2,Lf)));
      oneovereta=theta1*(Lf+2*max(L2,Lf));
      cout<<"Lf="<<Lf<<"; L2="<<L2<<"; theta1="<<theta1<<"; theta2="<<theta2<<"; theta3="<<theta3<<endl;
      cout<<"eta="<<eta<<endl;
    }

    void set_p(D scal_p){
        p=scal_p*tau/(0.0+nsamples);
        cout<<"changing probablity: "<<p<<endl;
    }

    void initialize(vector<D> & x0, D val_mu, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
    {
      cout<<"start initializing loopless Katyusha"<<" u="<<u<<endl;
      c=nb_c;
      noverc=nsamples/c;
      if(nb_tau>noverc) perror("tau should be less than n over c");
      tau=nb_tau;
      nb_of_iters_per_loop=floor(nsamples/(c*(tau+0.0)));
      batch_i.clear();
      batch_i.resize(nsamples,0);

      gk.clear();
      gk.resize(nfeatures,0);
      upper_bound=std::numeric_limits<double>::max();

      /**setup parameters**/
      max_nb_loops=max_nb;
      mu=val_mu;
      cout<<"mu="<<mu<<endl;
      set_rng();

      set_Li_Lf();

      running_time=0;
      /**setup probability**/
      if(p_mod==0)   // batch sampling (stochastic process as defined in the paper)
      {
        if(u==0)   // uniform sampling
        {
          set_uniform_probability(p_mod);
          uniform="uniform";
        }
        else{
          set_optimal_probability();
          uniform="nonuniform";
        }
      }
      else{  //group sampling
        if(u==0){
          set_uniform_probability(p_mod);
          uniform="uniform";
        }else{
          set_group_sampling_probability();
          uniform="nonuniform";
        }
        sort_p();
        cout<<"sort p"<<endl;
      }

      set_L2(p_mod,u);
      scaler=scal_p;
      set_p(scal_p);
      set_thetaS(p_mod);
      cout<<"set thetaS"<<endl;

      x.clear();
      y.clear();
      z.clear();
      w.clear();
      x.resize(nfeatures);
      y.resize(nfeatures);
      z.resize(nfeatures);
      w.resize(nfeatures);
      next_x.clear();
      next_x.resize(nfeatures);

      gradient_f_w.clear();
      gradient_f_w.resize(nfeatures);
      gradient_f_x.clear();
      gradient_f_x.resize(nfeatures);

      for(L i=0;i<nfeatures;i++)
      {
        x[i]=x0[i];
        y[i]= x0[i];
        w[i]=x0[i];
        z[i]=x0[i];
      }

      set_theta();
      cout<<"set stepsizes"<<endl;

      current_nb_iters=0;
      compute_primal_value();
      cout<<"primal value="<<primal_value<<endl;
      cout<<"Initialization  loopless Katyusha is finished!"<<endl;
    }

void compute_primal_value()
{
  D res=0;
  for(L i=0;i<nsamples;i++)
  {
    res+=value_of_phi_i(i);
  }
  D res2=value_of_g();
  primal_value= res/nsamples+ res2;
}

void compute_and_record_result()
{
  if(nb_loops%print_every_N==0){
    compute_primal_value();
    cout<<setprecision(9)<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";  "<<running_time<<" primal value="<<primal_value<<endl;
    samp<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<" "<<running_time<<" "<<primal_value<<endl;
  }
}

void compute_and_record_result2()
{
  if(nb_loops%print_every_N==0){
    compute_primal_value();
    compute_upper_bound_of_optimality_gap();
    cout<<setprecision(9)<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";  "<<running_time<<" primal value="<<primal_value<<" upper bound of F(x)-F^*= "<< upper_bound<< endl;
    samp<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<" "<<upper_bound<<" "<<running_time<<" "<<primal_value<<endl;
  }

}

void batch_sampling()
{
  batch_size=0;
  L i;
  for(L it_t=0;it_t<tau;it_t++)
  {
    i=sampling(nsamples);
    batch_i[batch_size]=i;
    batch_size++;
  }
}

void set_thetaS(L p_mod){
  theta_S.clear();
  theta_S.resize(nsamples,0);
  if(p_mod==0){
    for(L i=0;i<nsamples;i++)
    theta_S[i]=1.0/(tau*tilde_proba_vector[i]);
  }
  else{
    for(L i=0;i<nsamples;i++)
    theta_S[i]=1.0/proba_vector[i];
  }
}

void group_sampling(){
  batch_size=0;
  std::vector<L> sampled_groups;
  for(L i=0;i<nb_groups;i++){
    D y=gsl_rng_uniform(rng);
    if(y<sump_group_C[i])
    {
      sampled_groups.push_back(i);
      batch_size++;
    }
  }
  for(L t=0;t<batch_size;t++){
    L group_i=sampled_groups[t];
    L s1=index_group_C[group_i];
    L s2=index_group_C[group_i+1];
    L i=s1+(floor)(gsl_rng_uniform(rng)*(s2-s1));
    D y=gsl_rng_uniform(rng);
    D maxpi=maxp_group_C[group_i];
    while(y*maxpi>proba_vector[group_C[i]])
    {
      i=s1+(floor)(gsl_rng_uniform(rng)*(s2-s1));
      y=gsl_rng_uniform(rng);
    }
    batch_i[t]=group_C[i];
  }
}

void loopless(vector<D> & x0, string filename, D val_mu, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
{
  initialize(x0, val_mu, max_nb,  nb_tau, nb_c,  u, p_mod, scal_p);
  string sampname="results/L_Katyusha"+filename+uniform;
  if(p_mod==0) sampname=sampname+"_batch";
  else sampname=sampname+"_group";
  string scal_str;
  stringstream  scal_convert;
  scal_convert<<scal_p;
  scal_str=scal_convert.str();
  sampname+=scal_str;
  cout<<"running Loopless Katyusha "<<" ; "<<sampname<<endl;
  samp.open(sampname.c_str());

  nb_loops=0;
  nb_iters=0;

  srand48(27432042);
  //srand(time(NULL));

  D start;
  while(nb_loops<max_nb_loops)
  {
    compute_and_record_result();

    nb_loops++;

    start = std::clock();

    for(L it=0;it<nb_of_iters_per_loop;it++)
    {
      if(p_mod==0)
      batch_sampling();
      else
      group_sampling();

      start = std::clock();
      update_x();
      update_gk();
      update_z();
      update_y();
      update_w();
      update_gradient();
      current_nb_iters++;
      nb_iters++;
      running_time+= ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    }
  }
}

void loopless2(vector<D> & x0, string filename, D val_mu, L max_nb, D epsilon, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
{
  cout<<"max_nb="<<max_nb<<endl;
  initialize(x0, val_mu, max_nb,  nb_tau, nb_c,  u, p_mod, scal_p);
  string sampname="results/L_"+filename+uniform;
  if(p_mod==0) sampname=sampname+"_batch";
  else sampname=sampname+"_group";
  string scal_str;
  stringstream  scal_convert;
  scal_convert<<scal_p;
  scal_str=scal_convert.str();
  sampname+=scal_str;
  cout<<"running Loopless Katyusha 2"<<" ; "<<sampname<<endl;
  samp.open(sampname.c_str());

  nb_loops=0;
  nb_iters=0;

  cout<<setprecision(9)<<"initial: "<< "primal: "<<primal_value << endl;
  samp<<((0.0+nb_iters)*c*tau/(nsamples))<<" "<<running_time<<" "<<primal_value<<endl;
  //srand48(27432042);
  srand(time(NULL));

  D start;
  cout<<"nb_loops="<<nb_loops<<"; max_nb_loops="<<max_nb_loops<<"; upper bound="<<upper_bound<<"; epsilon="<<epsilon<<endl;
  while(nb_loops<max_nb_loops && upper_bound> epsilon)
  {
    compute_and_record_result2();
    start = std::clock();

    nb_loops++;

    //cout<<"before the loop time elapsed="<<duration<<endl;
    start = std::clock();

    //cout<<"nb_of_iters_per_loop="<<nb_of_iters_per_loop<<endl;
    for(L it=0;it<nb_of_iters_per_loop;it++)
    {
      if(p_mod==0)
      batch_sampling();
      else
      group_sampling();

      start = std::clock();
      update_x();
      update_gk();
      update_z();
      update_y();
      update_w();
      update_gradient();
      current_nb_iters++;
      //cout<< "test 1"<< endl;
      //cout<< "test 2"<< endl;
      nb_iters++;
      running_time+= ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    }
    //cout<< "epoch "<< nb_loops<< " finished"<< endl;
    //cout<<"after the loop time elapsed="<<duration<<endl;
  }
}

};

#endif /* MIN_SMOOTH_CONVEX_H */


#include "userFunc.h"
#include <math.h>
#include <gsl/gsl_sf_bessel.h>

/**************************************************************************/
/* This defines userFunc routines for the Pringle (1981) ring test        */
/**************************************************************************/

void
userEOS(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	void *params,
	double *gamma, double *delta) {
  fprintf(stderr,
	  "Warning: userEOS function called but not implemented!\n");
  return;
}

void
userAlpha(const double t, const double dt, const grid *grd,
	  const double *col, const double *pres, const double *eInt,
	  const double *gamma, const double *delta,
	  void *params,
	  double *alpha) {
  /* alpha = nu col/pres vphi / r */

  int i;
  double nu = *((double *) params);

  for (i=0; i<grd->nr; i++)
    alpha[i] = nu * col[i]/pres[i] * grd->vphi_g[i+1] / grd->r_g[i+1];

}

void
userMassSrc(const double t, const double dt, const grid *grd,
	    const double *col, const double *pres, const double *eInt,
	    const double *gamma, const double *delta,
	    void *params,
	    double *massSrc) {
  fprintf(stderr,
	  "Warning: userMassSrc function called but not implemented!\n");
  return;
}

void
userIntEnSrc(const double t, const double dt, const grid *grd,
	     const double *col, const double *pres, const double *eInt,
	     const double *gamma, const double *delta,
	     void *params,
	     double *intEnSrc) {
  fprintf(stderr,
	  "Warning: userIntEnSrc function called but not implemented!\n");
  return;
}

void
userIBC(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	const double *gamma, const double *delta,
	const pres_bc_type ibc_pres, const enth_bc_type ibc_enth,
	void *params,
	double *ibc_pres_val, double *ibc_enth_val) {
  double nu = ((double *) params)[0];
  double r0 = ((double *) params)[1];
  double m0 = ((double *) params)[2];
  double colMin = ((double *) params)[3];
  double pOverCol = ((double *) params)[4];
  double x = grd->r_g[0]/r0;
  double tau = 12.0*nu*t/SQR(r0) + SMALL;
  double sigma0 = m0/(M_PI*SQR(r0));
  double sigma;

  sigma = sigma0/(pow(x, 0.25)*tau) * exp(-SQR(x-1.0)/tau) *
    gsl_sf_bessel_Inu_scaled(0.25, 2*x/tau);
  sigma = (sigma < colMin) ? colMin : sigma;
  *ibc_pres_val = -3.0*M_PI*grd->r_g[0]*nu*grd->vphi_g[0]*sigma;
  *ibc_enth_val = gamma[0]/(gamma[0]-1)*pOverCol;
}

void
userOBC(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	const double *gamma, const double *delta,
	const pres_bc_type obc_pres, const enth_bc_type obc_enth,
	void *params,
	double *obc_pres_val, double *obc_enth_val) {
  double nu = ((double *) params)[0];
  double r0 = ((double *) params)[1];
  double m0 = ((double *) params)[2];
  double colMin = ((double *) params)[3];
  double pOverCol = ((double *) params)[4];
  double x = grd->r_g[grd->nr+1]/r0;
  double tau = 12.0*nu*t/SQR(r0) + SMALL;
  double sigma0 = m0/(M_PI*SQR(r0));
  double sigma;

  sigma = sigma0/(pow(x, 0.25)*tau) * exp(-SQR(x-1.0)/tau) *
    gsl_sf_bessel_Inu_scaled(0.25, 2*x/tau);
  sigma = (sigma < colMin) ? colMin : sigma;
  *obc_pres_val = -3.0*M_PI*grd->r_g[grd->nr+1]*nu*
    grd->vphi_g[grd->nr+1]*sigma;
  *obc_enth_val = gamma[0]/(gamma[0]-1)*pOverCol;
}

void
userPreTimestep(const double t, const double dt,
		const grid *grd, double *col, double *pres,
		double *eInt, double *mBnd, double *eBnd,
		double *mSrc, double *eSrc,
		void *params, const unsigned long nUserOut,
		double *userOut) {
  fprintf(stderr,
	  "Warning: userPreTimestep function called but not implemented!\n");
  return;
}

void
userPostTimestep(const double t, const double dt,
		 const grid *grd, double *col, double *pres,
		 double *eInt, double *mBnd, double *eBnd,
		 double *mSrc, double *eSrc,
		 void *params, const unsigned long nUserOut,
		 double *userOut) {
  fprintf(stderr,
	  "Warning: userPostTimestep function called but not implemented!\n");
  return;
}

void
userCheckRead(
	      FILE *fp, grid *grd, const unsigned long nOut,
	      double *tOut, double *colOut,
	      double *presOut, double *eIntOut, double *mBndOut,
	      double *eBndOut, double *mSrcOut, double *eSrcOut,
	      const unsigned long nUserOut, double *userOut,
	      void *params
	      ) {
  fprintf(stderr,
	  "Warning: userCheckRead function called but not implemented!\n");
  return;
}

void
userCheckWrite(
	      FILE *fp,
	      const grid *grd, const unsigned long nOut,
	      const double *tOut, const double *colOut,
	      const double *presOut, const double *eIntOut,
	      const double *mBndOut, const double *eBndOut,
	      const double *mSrcOut, const double *eSrcOut,
	      const unsigned long nUserOut, const double *userOut,
	      const void *params
	      ) {
  fprintf(stderr,
	  "Warning: userCheckWrite function called but not implemented!\n");
  return;
}


/* Copyright (C) 2015 Atsushi Togo */
/* All rights reserved. */

/* This file is part of phonopy. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions */
/* are met: */

/* * Redistributions of source code must retain the above copyright */
/*   notice, this list of conditions and the following disclaimer. */

/* * Redistributions in binary form must reproduce the above copyright */
/*   notice, this list of conditions and the following disclaimer in */
/*   the documentation and/or other materials provided with the */
/*   distribution. */

/* * Neither the name of the phonopy project nor the names of its */
/*   contributors may be used to endorse or promote products derived */
/*   from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS */
/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE */
/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, */
/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, */
/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; */
/* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER */
/* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT */
/* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN */
/* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE */
/* POSSIBILITY OF SUCH DAMAGE. */

#include <lapacke.h>
#include <phonoc_array.h>
#include <phonoc_utils.h>
#include <phonon4_h/real_to_reciprocal.h>

static void real_to_reciprocal_elements(lapack_complex_double *fc4_rec_elem,
					const double q[12],
					const double *fc4,
					const Darray *shortest_vectors,
					const Iarray *multiplicity,
					const int *p2s,
					const int *s2p,
					const int pi0,
					const int pi1,
					const int pi2,
					const int pi3);

/* fc4_reciprocal[num_patom, num_patom, num_patom, num_patom, 3, 3, 3, 3] */
void real_to_reciprocal4(lapack_complex_double *fc4_reciprocal,
			 const double q[12],
			 const double *fc4,
			 const Darray *shortest_vectors,
			 const Iarray *multiplicity,
			 const int *p2s_map,
			 const int *s2p_map)
{
  int i, j, k, l, num_patom;

  num_patom = multiplicity->dims[1];

  for (i = 0; i < num_patom; i++) {
    for (j = 0; j < num_patom; j++) {
      for (k = 0; k < num_patom; k++) {
	for (l = 0; l < num_patom; l++) {
	  real_to_reciprocal_elements
	    (fc4_reciprocal +
	     i * 81 * num_patom * num_patom * num_patom +
	     j * 81 * num_patom * num_patom +
	     k * 81 * num_patom +
	     l * 81,
	     q,
	     fc4,
	     shortest_vectors,
	     multiplicity,
	     p2s_map,
	     s2p_map,
	     i, j, k, l);
	}
      }
    }
  }
}

static void real_to_reciprocal_elements(lapack_complex_double *fc4_rec_elem,
					const double q[12],
					const double *fc4,
					const Darray *shortest_vectors,
					const Iarray *multiplicity,
					const int *p2s,
					const int *s2p,
					const int pi0,
					const int pi1,
					const int pi2,
					const int pi3)
{
  int i, j, k, l, m, num_satom;
  lapack_complex_double phase_factor, phase_factors[3];
  double fc4_rec_real[81], fc4_rec_imag[81];
  int fc4_elem_address;

  for (i = 0; i < 81; i++) {
    fc4_rec_real[i] = 0;
    fc4_rec_imag[i] = 0;
  }

  num_satom = multiplicity->dims[0];

  i = p2s[pi0];

  for (j = 0; j < num_satom; j++) {
    if (s2p[j] != p2s[pi1]) {
      continue;
    }
    phase_factors[0] =
      get_phase_factor(q, shortest_vectors, multiplicity, pi0, j, 1);

    for (k = 0; k < num_satom; k++) {
      if (s2p[k] != p2s[pi2]) {
	continue;
      }
      phase_factors[1] =
	get_phase_factor(q, shortest_vectors, multiplicity, pi0, k, 2);

      for (l = 0; l < num_satom; l++) {
	if (s2p[l] != p2s[pi3]) {
	  continue;
	}
	phase_factors[2] =
	  get_phase_factor(q, shortest_vectors, multiplicity, pi0, l, 3);

	fc4_elem_address = (i * 81 * num_satom * num_satom * num_satom +
			    j * 81 * num_satom * num_satom +
			    k * 81 * num_satom +
			    l * 81);

	phase_factor = phonoc_complex_prod(phase_factors[0], phase_factors[1]);
	phase_factor = phonoc_complex_prod(phase_factor, phase_factors[2]);
	for (m = 0; m < 81; m++) {
	  fc4_rec_real[m] +=
	    lapack_complex_double_real(phase_factor) * fc4[fc4_elem_address + m];
	  fc4_rec_imag[m] +=
	    lapack_complex_double_imag(phase_factor) * fc4[fc4_elem_address + m];
	}
      }
    }
  }

  for (i = 0; i < 81; i++) {
    fc4_rec_elem[i] =
      lapack_make_complex_double(fc4_rec_real[i], fc4_rec_imag[i]);
  }
}


#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics_double.h>
#include "../include/type.h"
#include "../include/util.h"
#include "../include/myfunc.h"
#include "../include/global.h"
#include "../include/cthreadpool.h"

extern double rho;
extern double cutoff;
extern clock_t dur;

#define ITER_CUTOFF 0.01

/**
 * @brief invoke this function before return from
 *        MLE_marginal_iteration and MLE_marginal_iteration_constrain.
 *
 * @param mle
 * @param sum
 * @param psi1
 * @param psi2
 * @param beta0
 * @param beta1
 * @param var1
 * @param var2
 */
void mle_result_set(mle_result* mle, double sum, gsl_vector* psi1, gsl_vector* psi2,
                    double beta0, double beta1, double var1, double var2) {
    mle->sum = sum;
    mle->params.psi1 = psi1;
    mle->params.psi2 = psi2;
    mle->params.beta0 = beta0;
    mle->params.beta1 = beta1;
    mle->params.var1 = var1;
    mle->params.var2 = var2;
    return;
}

double myfunc_multivar(const double x[], va_list argv) {
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2;

    // TODO  memcpy replaced.
    sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]);
    sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[1]);
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    return sum1 + sum2 + 0.05 * pow(rho, 2) / (1 - pow(rho, 2)) * \
        (pow(gsl_cdf_gaussian_Pinv(x[0], 1), 2) + pow(gsl_cdf_gaussian_Pinv(x[1], 1), 2) - \
         2 * rho * gsl_cdf_gaussian_Pinv(x[0], 1) * gsl_cdf_gaussian_Pinv(x[1], 1));
}

void myfunc_multivar_der(const double x[], double res[], va_list argv) {
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2, tmp = pow(rho,2)/(1-pow(rho,2));
    double ppfx0 = gsl_cdf_gaussian_Pinv(x[0], 1);
    double ppfx1 = gsl_cdf_gaussian_Pinv(x[1], 1);

    sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]);
    sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[1]);
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    // TODO are there any better way to convert cdf to pdf?
    res[0] = sum1 + tmp * 0.1*(ppfx0-rho*ppfx1)/gsl_ran_gaussian_pdf(ppfx0, 1);
    res[1] = sum2 + tmp * 0.1*(ppfx1-rho*ppfx0)/gsl_ran_gaussian_pdf(ppfx1, 1);

    return;
}

double myfunc_1_2(const double x[], va_list argv) {
    int flag = va_arg(argv, int);
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2;

    if (flag == 1) {
        sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]+cutoff);
        sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[0]);
    } else {
        sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]);
        sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[0]+cutoff);
    }
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    return sum1 + sum2 + 0.05 * pow(rho, 2) / (1 - pow(rho, 2)) * \
        (pow(gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1), 2) + pow(gsl_cdf_gaussian_Pinv(x[0], 1), 2) - \
         2 * rho * gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1) * gsl_cdf_gaussian_Pinv(x[0], 1));
}

void myfunc_der_1_2(const double x[], double res[], va_list argv) {
    int flag = va_arg(argv, int);
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2, tmp = pow(rho,2)/(1-pow(rho,2));
    double ppfx0 = gsl_cdf_gaussian_Pinv(x[0], 1);
    double ppfxc = gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1);

    if (flag == 1) {
        sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]+cutoff);
        sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[0]);
    } else {
        sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]);
        sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[0]+cutoff);
    }
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    // TODO are there any better way to convert cdf to pdf?
    res[0] = sum1 + tmp * 0.1*(ppfxc-rho*ppfx0)/gsl_ran_gaussian_pdf(ppfxc, 1);
    res[1] = sum2 + tmp * 0.1*(ppfx0-rho*ppfxc)/gsl_ran_gaussian_pdf(ppfx0, 1);
    res[0] += res[1];

    return;
}

double myfunc_marginal(const double x[], va_list argv) {
    gsl_vector* I = va_arg(argv, gsl_vector*), *S = va_arg(argv, gsl_vector*);
    gsl_vector* psi = va_arg(argv, gsl_vector*);
    double var = va_arg(argv, double), sum = 0;
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int), idx = 0;

    sum = cuscumsum(psi, sum_for_marginal, 7, &idx, x[0], I, S, var, inclu_len, skip_len);

    return sum;
}

void myfunc_marginal_der(const double x[], double res[], va_list argv) {
    gsl_vector* I = va_arg(argv, gsl_vector*), *S = va_arg(argv, gsl_vector*);
    gsl_vector* psi = va_arg(argv, gsl_vector*);
    double var = va_arg(argv, double), sum;
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int), idx = 0;

    sum = cuscumsum(psi, sum_for_marginal_der, 7, &idx, x[0], I, S, var, inclu_len, skip_len);
    res[0] = sum;

    return;
}

double myfunc_marginal_1_2(const double x[], va_list argv) {
    int flag = va_arg(argv, int);
    double beta1, beta2;
    if (flag == 1) {
        beta1 = x[0] + cutoff, beta2 = x[0];
    } else {
        beta1 = x[0], beta2 = x[0] + cutoff;
    }
    // TODO parameter ordering
    return myfunc_marginal(&beta1, argv) + myfunc_marginal(&beta2, argv);
}

void myfunc_marginal_1_2_der(const double x[], double res[], va_list argv) {
    int flag = va_arg(argv, int);
    double beta1, beta2;
    double tmp1, tmp2;
    if (flag == 1) {
        beta1 = x[0] + cutoff, beta2 = x[0];
    } else {
        beta1 = x[0], beta2 = x[0] + cutoff;
    }
    // TODO parameter ordering
    myfunc_marginal_der(&beta1, &tmp1, argv);
    myfunc_marginal_der(&beta2, &tmp2, argv);
    res[0] = tmp1 + tmp2;

    return;
}

double myfunc_individual(const double x[], va_list argv) {
    double I = va_arg(argv, double), S = va_arg(argv, double);
    double beta = va_arg(argv, double), var = va_arg(argv, double);
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    double new_psi = inclu_len * x[0]/(inclu_len * x[0] + skip_len * (1 - x[0]));

    // TODO This change the result.
    return pow((logit(x[0]) - logit(beta)), 2)/(2 * var) -
           (I * log(new_psi) + S * log(1 - new_psi) - log(x[0]) - log(1-x[0]));
    // return -(I * log(new_psi) + S * log(1 - new_psi) -
    //         pow((logit(x[0]) - logit(beta)), 2)/(2 * var) + log(x[0] - x[0]*x[0]));
    // return -(I * log(new_psi) + S * log(1 - new_psi) -
    //         pow((logit(x[0]) - logit(beta)), 2)/(2 * var) + log(x[0]) + log(1-x[0]));
}

void myfunc_individual_der(const double x[], double res[], va_list argv) {
    double I = va_arg(argv, double), S = va_arg(argv, double);
    double beta = va_arg(argv, double), var = va_arg(argv, double);
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    double new_psi = inclu_len * x[0]/(inclu_len * x[0] + skip_len * (1 - x[0]));
    double new_psi_der = inclu_len * skip_len/pow(inclu_len * x[0] + skip_len * (1 - x[0]), 2);

    res[0] = 1/x[0] + S/(1 - new_psi) * new_psi_der + \
             (logit(x[0]) - logit(beta))/(var * x[0] * (1 - x[0])) - (1/(1-x[0]) + I/new_psi * new_psi_der);

    return;
}

#define individual_for_loop() \
    for (idx = 0; idx < psi1->size; ++idx) { \
        x[0] = gsl_vector_get(psi1, idx); \
        cur_sum += l_bfgs_b_wrapper(n, m, x, l, u, nbd, \
                                    myfunc_individual, myfunc_individual_der, \
                                    factr, pgtol, iprint, 15000, 15000, \
                                    6, gsl_vector_get(i1, idx), gsl_vector_get(s1, idx), \
                                    beta0, var1, inclu_len, skip_len); \
        gsl_vector_set(psi1, idx, x[0]); \
    } \
    for (idx = 0; idx < psi2->size; ++idx) { \
        x[0] = gsl_vector_get(psi2, idx); \
        cur_sum += l_bfgs_b_wrapper(n, m, x, l, u, nbd, \
                                    myfunc_individual, myfunc_individual_der, \
                                    factr, pgtol, iprint, 15000, 15000, \
                                    6, gsl_vector_get(i2, idx), gsl_vector_get(s2, idx), \
                                    beta1, var2, inclu_len, skip_len); \
        gsl_vector_set(psi2, idx, x[0]); \
    } \

int MLE_marginal_iteration(gsl_vector* i1, gsl_vector* i2,
                           gsl_vector* s1, gsl_vector* s2,
                           const int inclu_len, const int skip_len,
                           mle_result* mle) {
    gsl_vector* psi1 = gsl_vector_alloc(i1->size); // TODO deallocation.
    gsl_vector* psi2 = gsl_vector_alloc(i2->size);
    vec2psi(psi1, i1, s1, inclu_len, skip_len);
    vec2psi(psi2, i2, s2, inclu_len, skip_len);
    int iter_max = 100, count = 0, i = 0;
    size_t idx = 0;
    double prev_sum = 0, cur_sum = 0;
    double var1 = 10 * gsl_stats_variance(psi1->data, 1, psi1->size) * (psi1->size-1)/(psi1->size);
    double var2 = 10 * gsl_stats_variance(psi2->data, 1, psi2->size) * (psi2->size-1)/(psi2->size);
    double beta0, beta1, iter_cutoff = 1;
    integer n, m = 10, nbd[nmax];
    doublereal x[nmax], l[nmax], u[nmax], factr=1.0e7, pgtol=1.0e-5, iprint=-1;

    if (var1 <= 0.01 || psi1->size == 1) {
        var1 = 0.01;
    }
    if (var2 <= 0.01 || psi2->size == 1) {
        var2 = 0.01;
    }

    // TODO & or && ?
    // According to original python code 'while((iter_cutoff>0.01)&(count<=iter_maxrun)):',
    // it's a bit arithmetic '&'. However,  it should be a logical 'and' in such senario.
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count, n = 2;
        x[0] = gsl_stats_mean(psi1->data, 1, psi1->size);
        x[1] = gsl_stats_mean(psi2->data, 1, psi2->size);
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_multivar, myfunc_multivar_der, factr, pgtol,
                         iprint, 15000, 15000, 4, psi1, psi2, var1, var2);
        beta0 = x[0], beta1 = x[1];
        n = 1, cur_sum = 0;

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }
    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    iter_cutoff = 1, iter_max = 100, count = 0, prev_sum = 0;
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count, n = 1;
        x[0] = beta0;
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_marginal, myfunc_marginal_der,
                         factr, pgtol, iprint, 15000, 15000,
                         6, i1, s1, psi1, var1, inclu_len, skip_len);
        beta0 = x[0];
        x[0] = beta1;
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_marginal, myfunc_marginal_der,
                         factr, pgtol, iprint, 15000, 15000,
                         6, i2, s2, psi2, var2, inclu_len, skip_len);
        beta1 = x[0];

        cur_sum = 0;

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }

    gsl_vector_free(psi1);
    gsl_vector_free(psi2);

    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    mle_result_set(mle, cur_sum, psi1, psi2, beta0, beta1, var1, var2);

    return 0;
}

int MLE_marginal_iteration_constrain(gsl_vector* i1, gsl_vector* i2,
                                     gsl_vector* s1, gsl_vector* s2,
                                     const int inclu_len, const int skip_len,
                                     mle_result* mle) {
    gsl_vector* psi1 = gsl_vector_alloc(i1->size); // TODO deallocation.
    gsl_vector* psi2 = gsl_vector_alloc(i2->size);
    vec2psi(psi1, i1, s1, inclu_len, skip_len);
    vec2psi(psi2, i2, s2, inclu_len, skip_len);
    int iter_max = 100, count = 0, i = 0;
    size_t idx = 0;
    double prev_sum = 0, cur_sum = 0, iter_cutoff = 1;
    double var1 = 10 * gsl_stats_variance(psi1->data, 1, psi1->size) * (psi1->size-1)/(psi1->size);
    double var2 = 10 * gsl_stats_variance(psi2->data, 1, psi2->size) * (psi2->size-1)/(psi2->size);
    double beta0, beta1;
    integer n = 1, m = 10, nbd[nmax];
    doublereal x[nmax], l[nmax], u[nmax], factr=1.0e7, pgtol=1.0e-5, iprint=-1;

    if (var1 <= 0.01 || psi1->size == 1) {
        var1 = 0.01;
    }
    if (var2 <= 0.01 || psi2->size == 1) {
        var2 = 0.01;
    }

    // TODO & or && ?
    // According to original python code 'while((iter_cutoff>0.01)&(count<=iter_maxrun)):',
    // it's a bit arithmetic '&'. However,  it should be a logical 'and' in such senario.
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count;
        x[0] = gsl_stats_mean(psi1->data, 1, psi1->size);
        x[1] = gsl_stats_mean(psi2->data, 1, psi2->size);
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.001;
            u[i] = 0.999-cutoff;
        }
        if (x[0] > x[1]) {
            l_bfgs_b_wrapper(n, m, x+1, l, u, nbd,
                             myfunc_1_2, myfunc_der_1_2, factr, pgtol,
                             iprint, 15000, 15000, 5, 1, psi1, psi2, var1, var2);
            beta1 = fmax(fmin(x[1], 1-cutoff), 0);
            beta0 = beta1 + cutoff;
        } else {
            l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                             myfunc_1_2, myfunc_der_1_2, factr, pgtol,
                             iprint, 15000, 15000, 5, 2, psi1, psi2, var1, var2);
            beta0 = fmax(fmin(x[0], 1-cutoff), 0);
            beta1 = beta0 + cutoff;
        }
        cur_sum = 0;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }
    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    iter_cutoff = 1, iter_max = 100, count = 0, prev_sum = 0;
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.001;
            u[i] = 0.999-cutoff;
        }
        x[0] = beta0;
        x[1] = beta1;
        if (gsl_stats_mean(psi1->data, 1, psi1->size) > gsl_stats_mean(psi2->data, 1, psi2->size)) {
            l_bfgs_b_wrapper(n, m, x+1, l, u, nbd,
                             myfunc_marginal_1_2, myfunc_marginal_1_2_der, factr, pgtol,
                             iprint, 15000, 15000, 13, 1, i1, s1, psi1, var1, inclu_len, skip_len,
                             i2, s2, psi2, var2, inclu_len, skip_len);
            beta1 = fmax(fmin(x[1], 1-cutoff), 0);
            beta0 = beta1 + cutoff;
        } else {
            l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                             myfunc_marginal_1_2, myfunc_marginal_1_2_der, factr, pgtol,
                             iprint, 15000, 15000, 13, 2, i1, s1, psi1, var1, inclu_len, skip_len,
                             i2, s2, psi2, var2, inclu_len, skip_len);
            beta0 = fmax(fmin(x[0], 1-cutoff), 0);
            beta1 = beta0 + cutoff;
        }
        cur_sum = 0;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
        // printf("%.12f, %.12f, %.12f, %.12f\n", beta0, beta1, iter_cutoff, prev_sum);
    }

    gsl_vector_free(psi1);
    gsl_vector_free(psi2);

    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    mle_result_set(mle, cur_sum, psi1, psi2, beta0, beta1, var1, var2);

    return 0;
}

void* thread_wrapper_for_LT(void* arg) {
    double *ret = (double*)malloc(sizeof(double));
    odiff *data = (odiff*)arg;
    *ret = likelihood_test(data->inc1, data->inc2,
                           data->skp1, data->skp2,
                           data->inclu_len, data->skip_len,
                           data->flag, data->id);
    return (void*)ret;
}

void* batch_wrapper_for_LT(void* arg) {
    batch_datum *args = (batch_datum*)arg;
    int batch_size = args->batch_size, i;
    double *ret = (double*)malloc(sizeof(double)*batch_size);
    odiff *datum = (odiff*)*args->datum;
    odiff data;
    for (i = 0; i < batch_size; ++i) {
        data = datum[i];
        ret[i] = likelihood_test(data.inc1, data.inc2,
                                 data.skp1, data.skp2,
                                 data.inclu_len, data.skip_len,
                                 data.flag, data.id);
    }
    return (void*)ret;
}

double likelihood_test(gsl_vector *i1, gsl_vector *i2, gsl_vector *s1, gsl_vector *s2,
                       int inclu_len, int skip_len, int flag, char* id) {
    printf("Testing %s\n", id);
    if (!flag) {
        printf("1 return from: %s\n", id);
        return 1;
    } else {
        mle_result mle;
        MLE_marginal_iteration(i1, i2, s1, s2, inclu_len, skip_len, &mle);
        if (fabs(mle.params.beta0 - mle.params.beta1) <= cutoff) {
            printf("2 return from: %s\n", id);
			return 1;
        } else {
            mle_result mle_constrain;
            MLE_marginal_iteration_constrain(i1, i2, s1, s2, inclu_len, skip_len, &mle_constrain);
            printf("3 return from: %s\n", id);
            return 1 - gsl_cdf_chisq_P(2 * (fabs(mle_constrain.sum - mle.sum)), 1);
        }
    }
}

int vec2psi(gsl_vector* psi, gsl_vector *inc, gsl_vector *skp,
            int inclu_len, int skip_len) {
    size_t idx;
    for(idx = 0; idx < inc->size; ++idx) {
        if (gsl_vector_get(inc, idx) + gsl_vector_get(skp, idx) == 0) {
            gsl_vector_set(psi, idx, 0.5);
        } else {
            gsl_vector_set(psi, idx, gsl_vector_get(inc, idx)/
                    (gsl_vector_get(inc, idx) + inclu_len * gsl_vector_get(skp, idx)/skip_len));
        }
    }
    return 0;
}


// lu2tlub.c
//
//  blocked LU decomposition library for column-major version
//
// Time-stamp: <11/05/13 11:35:56 makino>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#ifndef NOBLAS
#ifdef MKL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#endif

#include <lu2lib.h>

typedef double v2df __attribute__((vector_size(16)));
typedef union {v2df v; double s[2];}v2u;

void timer_init();

#define MAXTHREADS 4

static int findpivot0(int n, double a[][n], int current)
{
    double amax = fabs(a[0][current]);
    int i;
    int p=current;
    BEGIN_TSC;
    for(i=current+1;i<n;i++){
	if (fabs(a[0][i]) > amax){
	    amax = fabs(a[0][i]);
	    p = i;
	}
    }
    END_TSC(t,7);
    return p;
}
static int findpivot(int n, double a[][n], int current)
{
    int p;
    int p2;
    BEGIN_TSC;
    p = cblas_idamax(n-current, a[0]+current, 1)+current;
    //    p2 = findpivot0( n, a, current);
    //    printf("n, current, p, p2 = %d %d %d %d\n",n, current,p, p2);
    END_TSC(t,7);
    return p;
}
static int findpivot_sequentical(int n, double a[][n], int current)
{
    double amax = fabs(a[0][current]);
    int i;
    int p=current;
    for(i=current+1;i<n;i++){
	if (fabs(a[0][i]) > amax){
	    amax = fabs(a[0][i]);
	    p = i;
	}
    }
    return p;
}
static int findpivot_omp(int n, double a[][n], int current)
// factor 2 slower than sequential code even for n=8k....
// on Core i7 920
{
    double amax[MAXTHREADS]={-1.0,-1.0,-1.0,-1.0};
    int p[MAXTHREADS];
    double am;
    int pm;
    int di= (n-current-1)/4;
    int k;
    BEGIN_TSC;
    if (di > 1024){
	//#pragma omp parallel for private(k)
	for (k=0;k<MAXTHREADS; k++){
	    int i;
	    int istart = current+1+k*di;
	    int iend = current+1+(k+1)*di;
	    if (iend > n) iend = n;
	    for(i=istart;i<iend;i++){
		if (fabs(a[0][i]) > amax[k]){
		    amax[k] = fabs(a[0][i]);
		    p[k] = i;
		}
	    }
	}
	pm =p[0];
	am = amax[0];
	for (k=1;k<MAXTHREADS; k++){
	    if(amax[k]>am){
		am = amax[k];
		pm=p[k];
	    }
	}
    }else{
	pm=findpivot_sequentical( n,  a, current);
    }
    END_TSC(t,7);
    return pm;
}

static void swaprows(int n, double a[][n], int row1, int row2,
		     int cstart, int cend)
{
    int j;
    if (row1 != row2){
	for(j=cstart;j<cend;j++){
	    double tmp = a[j][row1];
	    a[j][row1] = a[j][row2];
	    a[j][row2]=tmp;
	}
    }
}

static void scalerow( int n, double a[n+1][n], double scale,
	       int row, int cstart, int cend)
{
    int j;
    for(j=cstart;j<cend;j++) a[j][row]*= scale;
}

static void vsmulandsub(int n, double a[n+1][n], int cr, int cc,
		 int c0,  int r0,int r1)
{
    int j,k;
    double * ar = a[cr];
    k=c0;
    double s = a[c0][cc];
    double *al = a[c0];
    while (r0 & 7){
	al[r0] -= ar[r0]*s;
	r0++;
    }
    while (r1 & 7){
	al[r1-1] -= ar[r1-1]*s;
	r1--;
    }
    v2df * arv = (v2df*) (ar+r0);
    v2df * alv = (v2df*) (al+r0);
    v2df ss = (v2df){s,s};
    //    for(j=r0;j<r1;j++)
    //	al[j] -= ar[j]*s;
    for(j=0;j<(r1-r0)/2;j+=4){
	alv[j] -= arv[j]*ss;
	alv[j+1] -= arv[j+1]*ss;
	alv[j+2] -= arv[j+2]*ss;
	alv[j+3] -= arv[j+3]*ss;
	__builtin_prefetch(alv+j+32,1,3);
	__builtin_prefetch(arv+j+32,0,0);

    }
}

static void vvmulandsub(int n, double a[n+1][n], int cr, int cc,
		 int c0, int c1, int r0,int r1)
{
    int j,k;
    double * ar = a[cr];
#ifdef TIMETEST
    BEGIN_TSC;
#endif
    if (c1-c0 == 1){
	vsmulandsub(n,a, cr, cc,c0, r0,r1);
    }else{
	for (k=c0;k<c1;k++){
	    double s = a[k][cc];
	    double *al = a[k];
	    for(j=r0;j<r1;j++)
		al[j] -= ar[j]*s;
	}
    }
#ifdef TIMETEST
    END_TSC(t,6);
#endif
}
static void mmmulandsub_old(int n, double a[n+1][n], int m0, int m1,
		 int c0, int c1, int r0,int r1)
{
    int j,k,l;
    printf("Enter mmul\n");
#ifndef NOBLAS
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
		 r1-r0, c1-c0, m1-m0, -1.0, &(a[m0][r0]), n,
		 &(a[c0][m0]), n, 1, &(a[c0][r0]), n );
    // example:
    // r0, m0 = i+m,i
    // m0, c0 = i, i+m
    // r0, c0 = i+m, i+m
    //r1-r0 = n-i-m
    // c1-c0 = iend-i-m
    // m1-m0 = m
#else
    for(j=r0;j<r1;j++)
	for (k=c0;k<c1;k++)
	    for (l=m0; l<m1; l++)
		a[k][j] -= a[l][j]*a[k][l];
#endif
}

static void matmul_for_nk4_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    for(l=0;l<4;l+=2){
	for(k=0;k<4;k+=2){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*b[k][l]+ a[l+1][j]*b[k][l+1];
		c[k+1][j] -= a[l][j]*b[k+1][l]+ a[l+1][j]*b[k+1][l+1];
	    }
	}
    }
}
static void matmul_for_nk8_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    double btmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
#pragma omp parallel for private(l,k,j)
    for(l=0;l<8;l+=2){
	for(k=0;k<8;k+=2){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*btmp[k][l]+ a[l+1][j]*btmp[k][l+1];
		c[k+1][j] -= a[l][j]*btmp[k+1][l]+ a[l+1][j]*btmp[k+1][l+1];
	    }
	}
    }

}
static void matmul_for_nk8_2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    double btmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
#pragma omp parallel for private(l,k,j)
    for(k=0;k<8;k+=2){
	for(l=0;l<8;l+=4){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*btmp[k][l]
		    + a[l+1][j]*btmp[k][l+1]
		    + a[l+2][j]*btmp[k][l+2]
		    + a[l+3][j]*btmp[k][l+3];
		c[k+1][j] -= a[l][j]*btmp[k+1][l]
		    + a[l+1][j]*btmp[k+1][l+1]
		    + a[l+2][j]*btmp[k+1][l+2]
		    + a[l+3][j]*btmp[k+1][l+3];
		//		c[k][j] -= a[l][j]*btmp[k][l]+ a[l+1][j]*btmp[k][l+1];
		//		c[k+1][j] -= a[l][j]*btmp[k+1][l]+ a[l+1][j]*btmp[k+1][l+1];
	    }
	}
    }

}
static void matmul_for_nk8_3(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 32;
    double btmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    for(k=0;k<mm;k++){
		atmp[i][k]=a[i][j+k];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=2){
		c[i][j+jj] -= atmp[0][jj]*btmp[i][0]
		    +atmp[1][jj]*btmp[i][1]
		    +atmp[2][jj]*btmp[i][2]
		    +atmp[3][jj]*btmp[i][3]
		    +atmp[4][jj]*btmp[i][4]
		    +atmp[5][jj]*btmp[i][5]
		    +atmp[6][jj]*btmp[i][6]
		    +atmp[7][jj]*btmp[i][7];
		c[i][j+jj+1] -= atmp[0][jj+1]*btmp[i][0]
		    +atmp[1][jj+1]*btmp[i][1]
		    +atmp[2][jj+1]*btmp[i][2]
		    +atmp[3][jj+1]*btmp[i][3]
		    +atmp[4][jj+1]*btmp[i][4]
		    +atmp[5][jj+1]*btmp[i][5]
		    +atmp[6][jj+1]*btmp[i][6]
		    +atmp[7][jj+1]*btmp[i][7];
	    }
	}
    }

}
static void matmul_for_nk8_4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 16;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    dest[0]=src[0];
	    dest[ 1]=src[ 1];
	    dest[ 2]=src[ 2];
	    dest[ 3]=src[ 3];
	    dest[ 4]=src[ 4];
	    dest[ 5]=src[ 5];
	    dest[ 6]=src[ 6];
	    dest[ 7]=src[ 7];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    __builtin_prefetch(c[i]+j+64,1,0);
	    __builtin_prefetch(c[i]+j+80,1,0);
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*b2tmp[i][0]
		    +(*(ap+16))*b2tmp[i][1]
		    +(*(ap+32))*b2tmp[i][2]
		    +(*(ap+48))*b2tmp[i][3]
		    +(*(ap+64))*b2tmp[i][4]
		    +(*(ap+80))*b2tmp[i][5]
		    +(*(ap+96))*b2tmp[i][6]
		    +(*(ap+112))*b2tmp[i][7];
		*cpp -= (*app)*b2tmp[i][0]
		    +(*(app+16))*b2tmp[i][1]
		    +(*(app+32))*b2tmp[i][2]
		    +(*(app+48))*b2tmp[i][3]
		    +(*(app+64))*b2tmp[i][4]
		    +(*(app+80))*b2tmp[i][5]
		    +(*(app+96))*b2tmp[i][6]
		    +(*(app+112))*b2tmp[i][7];
	    }
	}
    }

}
static void matmul_for_nk8_5(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	double atmp[m][mm];
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    dest[0]=src[0];
	    dest[ 1]=src[ 1];
	    dest[ 2]=src[ 2];
	    dest[ 3]=src[ 3];
	    dest[ 4]=src[ 4];
	    dest[ 5]=src[ 5];
	    dest[ 6]=src[ 6];
	    dest[ 7]=src[ 7];
	    dest[ 8]=src[ 8];
	    dest[ 9]=src[ 9];
	    dest[ 10]=src[ 10];
	    dest[ 11]=src[ 11];
	    dest[ 12]=src[ 12];
	    dest[ 13]=src[ 13];
	    dest[ 14]=src[ 14];
	    dest[ 15]=src[ 15];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    __builtin_prefetch(c[i]+j+64,1,0);
	    __builtin_prefetch(c[i]+j+80,1,0);
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*b2tmp[i][0]
		    +(*(ap+16))*b2tmp[i][1]
		    +(*(ap+32))*b2tmp[i][2]
		    +(*(ap+48))*b2tmp[i][3]
		    +(*(ap+64))*b2tmp[i][4]
		    +(*(ap+80))*b2tmp[i][5]
		    +(*(ap+96))*b2tmp[i][6]
		    +(*(ap+112))*b2tmp[i][7];
		*cpp -= (*app)*b2tmp[i][0]
		    +(*(app+16))*b2tmp[i][1]
		    +(*(app+32))*b2tmp[i][2]
		    +(*(app+48))*b2tmp[i][3]
		    +(*(app+64))*b2tmp[i][4]
		    +(*(app+80))*b2tmp[i][5]
		    +(*(app+96))*b2tmp[i][6]
		    +(*(app+112))*b2tmp[i][7];
	    }
	}
    }

}

static void matmul_for_nk8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int m0,m1,m2,m3;
    int s1, s2, s3;
    int ds = (n/64)*16;
    s1 = ds;
    s2 = ds*2;
    s3 = ds*3;
    m0 = ds;
    m1 = ds;
    m2 = ds;
    m3 = n-s3;
    //    fprintf(stderr,"n, s, m = %d %d %d %d %d %d %d %d\n",
    //    n,s1,s2,s3,m0,m1,m2,m3);

#pragma omp parallel
#pragma omp sections
  {
#pragma omp section
      matmul_for_nk8_5(n1,a,n2,b,n3,c,m0);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s1 ,n2,b,n3,((double*)c)+s1,m1);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s2 ,n2,b,n3,((double*)c)+s2,m2);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s3 ,n2,b,n3,((double*)c)+s3,m3);
  }
}

static void matmul_for_nk4_3(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 16;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];
    v2df atmpt[mm/2][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	int jj;

	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    atmpt[ 0][i]=src[0];
	    atmpt[ 1][i]=src[ 1];
	    atmpt[ 2][i]=src[ 2];
	    atmpt[ 3][i]=src[ 3];
	    atmpt[ 4][i]=src[ 4];
	    atmpt[ 5][i]=src[ 5];
	    atmpt[ 6][i]=src[ 6];
	    atmpt[ 7][i]=src[ 7];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    if (jjend+j > n) jjend = n-j;
	    v2df* cp = (v2df*)(&c[i][j]);
	    v2df* ap = (v2df*)(atmpt[0]);
	    v2df* cpp = (v2df*)(&c[i][j+2]);
	    v2df* app = (v2df*)(atmpt[1]);
	    v2df* bp = b2tmp[i];
	    for(jj=0;jj<jjend;jj+=4){
		*cp -= ap[0]*bp[0]
		    +ap[1]*bp[1]
		    +ap[2]*bp[2]
		    +ap[3]*bp[3];
		*cpp -= app[0]*bp[0]
		    +app[1]*bp[1]
		    +app[2]*bp[2]
		    +app[3]*bp[3];
		cp += 2;
		cpp+= 2;
		ap += m*2;
		app+= m*2;
	    }
	}

    }

}

static void matmul_for_nk4_4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    // use of OMP does not speed things up here...
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
#if 0
	    for(k=0;k<mm;k++)atmp[i][k]=a[i][j+k];
#endif
#if 0
	    for(k=0;k<mm/2;k++)dest[k]=src[k];
#endif
	    for(k=0;k<mm/2;k+=2){
		dest[k]=src[k];
		dest[k+1]=src[k+1];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    v2df * bp = b2tmp[i];
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=2){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*bp[0]
		    +(*(ap+16))*bp[1]
		    +(*(ap+32))*bp[2]
		    +(*(ap+48))*bp[3];
#if 0
		*cpp -= (*app)*bp[0]
		    +(*(app+16))*bp[1]
		    +(*(app+32))*bp[2]
		    +(*(app+48))*bp[3];
#endif
	    }
	}
    }

}
static void matmul_for_nk4_1(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
#pragma omp parallel for private(j,i)
    // use of OMP does not speed things up here...
    for(j=0;j<n;j+=mm){
	double atmp[m][mm];
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
#if 0
	    for(k=0;k<mm;k++)atmp[i][k]=a[i][j+k];
#endif
#if 0
	    for(k=0;k<mm/2;k++)dest[k]=src[k];
#endif
	    for(k=0;k<mm/2;k+=4){
		dest[k]=src[k];
		dest[k+1]=src[k+1];
		dest[k+2]=src[k+2];
		dest[k+3]=src[k+3];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    v2df * bp = b2tmp[i];
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*bp[0]
		    +(*(ap+16))*bp[1]
		    +(*(ap+32))*bp[2]
		    +(*(ap+48))*bp[3];
		*cpp -= (*app)*bp[0]
		    +(*(app+16))*bp[1]
		    +(*(app+32))*bp[2]
		    +(*(app+48))*bp[3];
	    }
	}
    }

}
static void matmul_for_nk2_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    for(j=0;j<n;j++){
	    c[0][j] -= a[0][j]*b[0][0]+a[1][j]*b[0][1];
	    c[1][j] -= a[0][j]*b[1][0]+a[1][j]*b[1][1];
    }
}
static void matmul_for_nk2_1(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;

    for(j=0;j<n;j+=2){
	    c[0][j] -= a[0][j]*b[0][0]+a[1][j]*b[0][1];
	    c[1][j] -= a[0][j]*b[1][0]+a[1][j]*b[1][1];
	    c[0][j+1] -= a[0][j+1]*b[0][0]+a[1][j+1]*b[0][1];
	    c[1][j+1] -= a[0][j+1]*b[1][0]+a[1][j+1]*b[1][1];
    }
}
static void matmul_for_nk2_2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;

    register double b00 = b[0][0];
    register double b01 = b[0][1];
    register double b10 = b[1][0];
    register double b11 = b[1][1];
    for(j=0;j<n;j+=2){
	    c[0][j] -= a[0][j]*b00+a[1][j]*b01;
	    c[0][j+1] -= a[0][j+1]*b00+a[1][j+1]*b01;
	    c[1][j] -= a[0][j]*b10+a[1][j]*b11;
	    c[1][j+1] -= a[0][j+1]*b10+a[1][j+1]*b11;
    }

}
static void matmul_for_nk2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    register v2df b00 = (v2df){b[0][0],b[0][0]};
    register v2df b01 = (v2df){b[0][1],b[0][1]};
    register v2df b10 = (v2df){b[1][0],b[1][0]};
    register v2df b11 = (v2df){b[1][1],b[1][1]};
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * c0 = (v2df*) c[0];
    v2df * c1 = (v2df*) c[1];
    int nh = n>>1;
    if (nh & 1){
	j=nh-1;
	c0[j] -= a0[j]*b00+a1[j]*b01;
	c1[j] -= a0[j]*b10+a1[j]*b11;
	nh = nh-1;
    }
    for(j=0;j<nh;j+=2){
	c0[j] -= a0[j]*b00+a1[j]*b01;
	c0[j+1] -= a0[j+1]*b00+a1[j+1]*b01;
	c1[j] -= a0[j]*b10+a1[j]*b11;
	c1[j+1] -= a0[j+1]*b10+a1[j+1]*b11;
	//	__builtin_prefetch((double*)&a0[j+32],0);
	//	__builtin_prefetch((double*)&a1[j+32],0);
	//	__builtin_prefetch((double*)&c0[j+32],1);
	//	__builtin_prefetch((double*)&c1[j+32],1);
	//	asm("prefetcht2 %0"::"m"(a0[j+32]):"memory");
	//	asm("prefetcht2 %0"::"m"(a1[j+32]):"memory");
    }
}

static void matmul_for_nk4_5(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{

    matmul_for_nk2(n1, a, n2, b, n3,c, n);
    matmul_for_nk2(n1, (double(*)[])  (a[2]), n2,
		   (double(*)[])  &(b[0][2]), n3,c, n);
    matmul_for_nk2(n1, a, n2, (double(*)[])  b[2], n3,
		   (double(*)[])  c[2], n);
    matmul_for_nk2(n1, (double(*)[])  &(a[2]), n2,
		   (double(*)[])  &(b[2][2]), n3,
		   (double(*)[])  c[2], n);

}

static void matmul_for_nk4_6(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    register v2df b00 = (v2df){b[0][0],b[0][0]};
    register v2df b01 = (v2df){b[0][1],b[0][1]};
    register v2df b02 = (v2df){b[0][2],b[0][2]};
    register v2df b03 = (v2df){b[0][3],b[0][3]};
    register v2df b10 = (v2df){b[1][0],b[1][0]};
    register v2df b11 = (v2df){b[1][1],b[1][1]};
    register v2df b12 = (v2df){b[1][2],b[1][2]};
    register v2df b13 = (v2df){b[1][3],b[1][3]};
    register v2df b20 = (v2df){b[2][0],b[2][0]};
    register v2df b21 = (v2df){b[2][1],b[2][1]};
    register v2df b22 = (v2df){b[2][2],b[2][2]};
    register v2df b23 = (v2df){b[2][3],b[2][3]};
    register v2df b30 = (v2df){b[3][0],b[3][0]};
    register v2df b31 = (v2df){b[3][1],b[3][1]};
    register v2df b32 = (v2df){b[3][2],b[3][2]};
    register v2df b33 = (v2df){b[3][3],b[3][3]};

    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    v2df * c0 = (v2df*) c[0];
    v2df * c1 = (v2df*) c[1];
    v2df * c2 = (v2df*) c[2];
    v2df * c3 = (v2df*) c[3];
    int nh = n>>1;
    //#pragma omp parallel
    {
	//#pragma omp section
	{
	    int j;
	    //#pragma omp for private (j)
	    for(j=0;j<nh;j++){
		c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
		c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    }
	}
	//#pragma omp section
	{
	    int j;
	    //#pragma omp for private (j)
	    for(j=0;j<nh;j++){
		c2[j] -= a0[j]*b20+a1[j]*b21+a2[j]*b22+a3[j]*b23;
		c3[j] -= a0[j]*b30+a1[j]*b31+a2[j]*b32+a3[j]*b33;
	    }
	}
    }
}
static void matmul_for_nk4_7(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    int j;
    {
	int nh = n>>1;
	v2df * c0 = (v2df*) c[0];
	v2df * c1 = (v2df*) c[1];
	 v2df b00 = (v2df){b[0][0],b[0][0]};
	 v2df b01 = (v2df){b[0][1],b[0][1]};
	 v2df b02 = (v2df){b[0][2],b[0][2]};
	 v2df b03 = (v2df){b[0][3],b[0][3]};
	 v2df b10 = (v2df){b[1][0],b[1][0]};
	 v2df b11 = (v2df){b[1][1],b[1][1]};
	 v2df b12 = (v2df){b[1][2],b[1][2]};
	 v2df b13 = (v2df){b[1][3],b[1][3]};
	if (nh & 1){
	    j=nh-1;
	    c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
	    c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    nh--;
	}
	for(j=0;j<nh;j+=2){
	    c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
	    c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    c0[j+1] -= a0[j+1]*b00+a1[j+1]*b01+a2[j+1]*b02+a3[j+1]*b03;
	    c1[j+1] -= a0[j+1]*b10+a1[j+1]*b11+a2[j+1]*b12+a3[j+1]*b13;
	}
    }
    {
	int nh = n>>1;
	v2df * c2 = (v2df*) c[2];
	v2df * c3 = (v2df*) c[3];
	 v2df b20 = (v2df){b[2][0],b[2][0]};
	 v2df b21 = (v2df){b[2][1],b[2][1]};
	 v2df b22 = (v2df){b[2][2],b[2][2]};
	 v2df b23 = (v2df){b[2][3],b[2][3]};
	register v2df b30 = (v2df){b[3][0],b[3][0]};
	register v2df b31 = (v2df){b[3][1],b[3][1]};
	register v2df b32 = (v2df){b[3][2],b[3][2]};
	register v2df b33 = (v2df){b[3][3],b[3][3]};

	for(j=0;j<nh;j++){
	    c2[j] -= a0[j]*b20+a1[j]*b21+a2[j]*b22+a3[j]*b23;
	    c3[j] -= a0[j]*b30+a1[j]*b31+a2[j]*b32+a3[j]*b33;
	}
    }
}

static void matmul_for_nk4_8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    int nh = n>>1;
    int j;

    int k;
    v2df * cv;
    v2df * cvv;
    register v2df b0;
    register v2df b1;
    register v2df b2;
    register v2df b3;
    register v2df b4;
    register v2df b5;
    register v2df b6;
    register v2df b7;
    for(k=0;k<4;k+=2){
	cv = (v2df*) c[k];
	cvv = (v2df*) c[k+1];
	b0 = (v2df){b[k][0],b[k][0]};
	b1 = (v2df){b[k][1],b[k][1]};
	b2 = (v2df){b[k][2],b[k][2]};
	b3 = (v2df){b[k][3],b[k][3]};
	b4 = (v2df){b[k+1][0],b[k+1][0]};
	b5 = (v2df){b[k+1][1],b[k+1][1]};
	b6 = (v2df){b[k+1][2],b[k+1][2]};
	b7 = (v2df){b[k+1][3],b[k+1][3]};
	for(j=0;j<nh;j++){
	    register v2df aa0 = a0[j];
	    register v2df aa1 = a1[j];
	    register v2df aa2 = a2[j];
	    register v2df aa3 = a3[j];
	    register v2df x = aa0*b0;
	    x+= aa1*b1;
	    x+= aa2*b2;
	    x+= aa3*b3;
	    cv[j]-=x;
	    x = aa0*b4;
	    x+= aa1*b5;
	    x+= aa2*b6;
	    x+= aa3*b7;
	    cvv[j] -= x;
	}
    }
}

static void matmul_for_nk4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int m0,m1,m2,m3;
    int s1, s2, s3;
    int ds = (n/32)*8;
    s1 = ds;
    s2 = ds*2;
    s3 = ds*3;
    m0 = ds;
    m1 = ds;
    m2 = ds;
    m3 = n-s3;
    //    fprintf(stderr,"n, s, m = %d %d %d %d %d %d %d %d\n",
    //    n,s1,s2,s3,m0,m1,m2,m3);

#pragma omp parallel
#pragma omp sections
  {
#pragma omp section
      matmul_for_nk4_7(n1,a,n2,b,n3,c,m0);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s1 ,n2,b,n3,((double*)c)+s1,m1);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s2 ,n2,b,n3,((double*)c)+s2,m2);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s3 ,n2,b,n3,((double*)c)+s3,m3);
  }
}
static void matmul_for_nk8_9(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    matmul_for_nk4(n1, a, n2, b, n3,c, n);
    matmul_for_nk4(n1, (double(*)[])  (a[4]), n2,
		   (double(*)[])  &(b[0][4]), n3,c, n);
    matmul_for_nk4(n1, a, n2, (double(*)[])  b[4], n3,
		   (double(*)[])  c[4], n);
    matmul_for_nk4(n1, (double(*)[])  &(a[4]), n2,
		   (double(*)[])  &(b[4][4]), n3,
		   (double(*)[])  c[4], n);

}
#if 0
static void matmul_for_nk8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i;
    int nb=384;
    for (i=0;i<n;i+=nb){
	int iend = i+nb;
	if (iend > n) iend = n;
	matmul_for_nk8_worker(n1, (double(*)[]) (a[0]+i),
			      n2,  (double(*)[]) (b[0]+i),
			      n3, (double(*)[]) (c[0]+i),
			      iend-i);
    }
}
#endif

static void matmul_for_small_nk_local(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int kk,
			 int n)
{
    // simplest version
    int j,k,l;
    BEGIN_TSC;
    if (kk == 2){
	matmul_for_nk2(n1, a, n2, b, n3,c, n);
	END_TSC(t,16);
	return;
    }
    if (kk == 4){
	matmul_for_nk4(n1, a, n2, b, n3,c, n);
	END_TSC(t,13);
	return;
    }
    if (kk == 8){
	matmul_for_nk8(n1, a, n2, b, n3,c, n);
	END_TSC(t,12);
	return;
    }

    for(j=0;j<n;j++)
	for(k=0;k<m;k++)
	    for(l=0;l<kk;l++)
		c[k][j] -= a[l][j]*b[k][l];
}

static void mmmulandsub(int n, double a[n+1][n], int rshift, int m0, int m1,
		 int c0, int c1, int r0,int r1)
{
    int j,k,l;
#ifdef TIMETEST
    BEGIN_TSC;
#endif
#ifndef NOBLAS
    if ((m1-m0)<=8){
	// =4 is slighly faster than =8 on Ci7
	matmul_for_small_nk_local(n, (double(*)[])  &(a[m0-rshift][r0]),
			      n, (double(*)[])  &(a[c0-rshift][m0]),
			      n, (double(*)[])  &(a[c0-rshift][r0]),
			      c1-c0,m1-m0,r1-r0);
    }else{
	cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
		     r1-r0, c1-c0, m1-m0, -1.0, &(a[m0-rshift][r0]), n,
		     &(a[c0-rshift][m0]), n, 1, &(a[c0-rshift][r0]), n );
    }
    // example:
    // r0, m0 = i+m,i
    // m0, c0 = i, i+m
    // r0, c0 = i+m, i+m
    //r1-r0 = n-i-m
    // c1-c0 = iend-i-m
    // m1-m0 = m
#else
    for(j=r0;j<r1;j++)
	for (k=c0;k<c1;k++)
	    for (l=m0; l<m1; l++)
		a[k-rshift][j] -= a[l-rshift][j]*a[k-rshift][l];
#endif
#ifdef TIMETEST
    END_TSC(t,4);
#endif
}

static int nswap;
static void column_decomposition(int n, double a[][n],  int m, int pv[], int i)
{
    // shift a so that partial array is okay
    int  j, k;
    int ip,ii;
    double ainv;
#ifdef TIMETEST
    BEGIN_TSC;
#endif
    for(ip=0;ip<m;ip++){
	ii=i+ip;
	int p = findpivot(n,a,ii);
	pv[ip]=p;
	swaprows(n,a,p,ii,0,m);
	nswap++;
	// normalize row ii
	ainv = 1.0/a[ip][ii];
	scalerow(n,a,ainv,ii,0,ip);
	scalerow(n,a,ainv,ii,ip+1,m);
	// subtract row ii from all lower rows
	vvmulandsub(n,  a, ip,ii, ip+1, m, ii+1, n);
    }
#ifdef TIMETEST
    END_TSC(t,5);
#endif
}

static void process_right_part(int n,
			double a[n+1][n],
			int m,
			int pv[],
			int i,
			int iend)
{
    int ii;
    // exchange rows
    for(ii=i;ii<i+m;ii++){
	swaprows(n,a,pv[ii-i],ii,m,iend-i);
    }

    // normalize rows
    for(ii=i;ii<i+m;ii++){
	scalerow(n,a,1.0/a[ii-i][ii] ,ii,m,iend-i);
    }
    // subtract rows (within i-i+m-1)
    for(ii=i;ii<i+m;ii++){
	vvmulandsub(n,  a, ii-i,ii,  m, iend-i, ii+1, i+m);
    }

    // subtract rows i-i+m-1 from all lower rows
    mmmulandsub(n, a, i, i,i+m, i+m, iend, i+m, n);
    //    fprintf(stderr,"process_r, end\n");
    //    usleep(1);
}
static void column_decomposition_recursive(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    int  j, k;
    int ip,ii;
    double ainv;
    //    fprintf(stderr,"enter t column recursive %d %d\n", i, m);
    if (m <= 2){
	// perform non-recursive direct decomposition
	column_decomposition(n, a, m, pv,i);
    }else{
	// process the left half by recursion
	//	fprintf(stderr,"call column recursive %d %d\n", i, m);
	column_decomposition_recursive(n, a, m/2, pv,i);
	// process the right half
	//	fprintf(stderr,"call right part %d %d\n", i, m);
	process_right_part(n,a,m/2,pv,i,i+m);
	//	fprintf(stderr,"call right recursive %d %d\n", i, m);
	column_decomposition_recursive(n, a+m/2, m/2, pv+m/2,i+m/2);
	// process the swap of rows for the left half
	//	fprintf(stderr,"call swaprowse %d %d\n", i, m);
	for(ii=i+m/2;ii<i+m;ii++){
	    swaprows(n,a,pv[ii-i],ii,0,m/2);
	}
	//	fprintf(stderr,"call scalerows %d %d\n", i, m);
	// normalize rows
	for(ii=i+m/2;ii<i+m;ii++){
	    scalerow(n,a,1.0/a[ii-i][ii] ,ii,0,m/2);
	}
    }
}

void cm_column_decomposition_recursive(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    column_decomposition_recursive( n, a, m, pv, i);
}
void cm_column_decomposition(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    column_decomposition( n, a, m, pv, i);
}
void cm_process_right_part(int n,
			   double a[n+1][n],
			   int m,
			   int pv[],
			   int i,
			   int iend)
{
    process_right_part(n, a, m, pv, i,iend);
}


#ifndef __BLAS_H__
#define __BLAS_H__

#define sgemm sgemm_
#define dgemm dgemm_
#define ssteqr ssteqr_
#define dsteqr dsteqr_
#define dgemv dgemv_
#define ddot ddot_
#define daxpy daxpy_
#define dscal dscal_
#define dasum dasum_

#ifdef __cplusplus
extern "C"{
#endif
    //#include <cblas.h>
    //#include <clapack.h>

    void sgemm_(const char* TRANSA, const char* TRANSB,
        const int* M, const int* N, const int* K,
        const float* ALPHA, const float* A, const int* LDA,
        const float* B, const int* LDB,
        const float* BETA, float* C, const int* LDC);

    void dgemm_(const char* TRANSA, const char* TRANSB,
        const int* M, const int* N, const int* K,
        const double* ALPHA, const double* A, const int* LDA,
        const double* B, const int* LDB,
        const double* BETA, double* C, const int* LDC);
/*
    void ssteqr_(char *compz, const int *n, float *d, float *e,
        float *z, const int *ldz, float *work, const int *info);

    void dsteqr_(char *compz, const int *n, double *d, double *e,
        double *z, const int *ldz, double *work, const int *info);
 */

     void dgemv_(const char *trans, const int *m, const int *n,
          const double *alpha, const double *a, const int *lda,
          const double *x, const int *incx, const double *beta,
          double *y, const int *incy);

     double ddot_(const int* n, const double* x, const int* incx,
          const double* y, const int* incy);

     void daxpy_(const int* n, const double *alpha, const double* x,
	const int* incx, double* y, const int* incy);

	 void dscal_(const int*n, const double *alpha, const double* x, const int* incx);

	 double dasum_(const int*n, const double *x, const int* incx);

#ifdef __cplusplus
}
#endif

#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <gsl/gsl_math.h>

#include "cosmocalc.h"
#include "haloprofs.h"
#include "weaklens.h"

void test_cosmo(char *path);

int main(int argc, char **argv)
{
  cosmoData.useSmoothTransFunc = 0;
  cosmoData.delta = 200.0;
  char path[4096];

  //init cosmology 2
  cosmoData.cosmoNum = 2;
  cosmoData.OmegaM = 0.28;
  cosmoData.OmegaL = 0.72;
  cosmoData.OmegaB = 0.05;
  cosmoData.OmegaNu = 0.0;
  cosmoData.OmegaK = 0.0;
  cosmoData.h = 0.7;
  cosmoData.w0 = -1.0;
  cosmoData.wa = 0.0;
  cosmoData.SpectralIndex = 0.96;
  cosmoData.Sigma8 = 0.8;

  sprintf(path,"./cosmo2");
  test_cosmo(path);

  //init cosmology 1
  cosmoData.cosmoNum = 1;
  cosmoData.OmegaM = 0.3;
  cosmoData.OmegaL = 0.68;
  cosmoData.OmegaB = 0.05;
  cosmoData.OmegaNu = 0.0;
  cosmoData.OmegaK = 1.0 - cosmoData.OmegaM - cosmoData.OmegaL - cosmoData.OmegaNu;
  cosmoData.h = 0.7;
  cosmoData.w0 = -0.8;
  cosmoData.wa = 0.2;
  cosmoData.SpectralIndex = 0.96;
  cosmoData.Sigma8 = 0.8;

  sprintf(path,"./cosmo1");
  test_cosmo(path);

  return 0;
}

void test_cosmo(char *path)
{
  char fname[4096];
  char fnameo[4096];
  char str[4096];
  FILE *fp;
  FILE *fpout;
  double z;

  sprintf(fname,"%s/codist.dat",path);
  sprintf(fnameo,"%s/codist.dat.matt",path);
  fp = fopen(fname,"r");
  if( fp != NULL)
    {
      fpout = fopen(fnameo,"w");
      while(fgets(str,4096,fp) != NULL)
	{
	  if(str[0] == '#')
	    continue;
	  sscanf(str,"%le %*e\n",&z);

	  fprintf(fpout,"%e %e\n",z,comvdist(1.0/(1.0+z)));
	}
      fclose(fp);
      fclose(fpout);
    }

  sprintf(fname,"%s/Evol_z.dat",path);
  sprintf(fnameo,"%s/Evol_z.dat.matt",path);
  fp = fopen(fname,"r");
  if(fp != NULL)
    {
      fpout = fopen(fnameo,"w");
      while(fgets(str,4096,fp) != NULL)
	{
	  if(str[0] == '#')
	    continue;
	  sscanf(str,"%le %*e\n",&z);

	  //fprintf(stderr,"weff(%f) = %f, h(a) = %f\n",1.0/(1.0+z),weff(1.0/(1.0+z)),hubble_noscale(1.0/(1.0+z)));
	  fprintf(fpout,"%e %e\n",z,hubble_noscale(1.0/(1.0+z)));
	}
      fclose(fp);
      fclose(fpout);
    }
}


/* hka chi-square stat estimation
based on original Hudson, Kreitman, Aguade 1987 implementation.

The parameters of the model are calculated by solving the system of equations numerically.

A. D. Kern  6/2005
*/

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "string.h"
#include "hka.h"
#include "ctype.h"

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>

/* currently the maximum number of loci is at 10^7, and the maximum locus name is 80 characters long */
struct hkaData data[MAXLOCI];
int locusNumber = 0;
int lineageNumber = 1;

int main(int argc, char *argv[]){
  int i = 0;
  gsl_vector  *chiSquares, *expectedPolyA, *expectedD, *devs;
  gsl_vector  *params; //0-locusNumber-1 = 4Nu's, locusNumber = tHat, locusNumber + 1 = fHat
  double x;

  getData(argc, argv);
  if(lineageNumber > 1){
    params = gsl_vector_alloc(locusNumber + 2);
  }
  else{
    params = gsl_vector_alloc(locusNumber + 1);
  }

  //solve the system of equations
  solveSystem(params);

  //calculate chiSquare Stats
  chiSquares =gsl_vector_alloc(locusNumber);
  expectedPolyA = gsl_vector_alloc(locusNumber);
  expectedD = gsl_vector_alloc(locusNumber);
  devs =  gsl_vector_alloc(locusNumber);

  x =  calculateHKA(params, chiSquares, expectedPolyA, expectedD);

  //print some stuff
  if(lineageNumber > 1){
    printf("chiSquared = %f\nt_hat = %f\nf_hat = %f\n",x, gsl_vector_get(params,locusNumber), gsl_vector_get(params,locusNumber + 1));
  }
  else{
    printf("chiSquared = %f\nt_hat = %f\nf_hat = 1.0\n",x, gsl_vector_get(params,locusNumber));
  }
    for(i = 0; i < locusNumber; i++){
      printf("locus %s \ttheta:\t %f\texpSA:\t %f\tobsSA:\t %f\texpD:\t %f\tobsD:\t %f\t chiSquared:\t%f\n",data[i].name,   gsl_vector_get(params,i),gsl_vector_get(expectedPolyA,i), data[i].sa, gsl_vector_get(expectedD,i), data[i].d, gsl_vector_get(chiSquares,i));
  }

  gsl_vector_free(params);
  return(0);
}

void usage(){
  printf("usage:\nhka infile\n");
  exit(1);
}

/* parses the data file and reads in options (currently no options boss) */
void getData(int argc, char *argv[]){
  FILE *infile;
  int  l, na, nb, sa, sb, args, flag, i,lineCount;
  double d;
  char string[81];  //here's the character length limit
  char *sArray[30], line[500];

  if (argc < 2){
    usage();
  }
  else{
    infile = fopen(argv[1],"r");
    if (infile == NULL){
      fprintf(stderr,"Error opening infile!!!\n");
      exit(1);
    }
    flag = 0;
    args = 2;
    while(args < argc){
      switch(argv[args][1]){
      case 'm' : //missing data mode
	flag = 1;
      }
      args++;
    }
    if (flag){
      //missing data mode expect- na, d,lArray, sArray
      while(fgets(line, 100, infile)){
	lineCount = chopByWhite(line,sArray,30);
	//	data[locusNumber].name = (char) sArray[0];
	data[locusNumber].na = atoi(sArray[1]);
	data[locusNumber].d = (double) atof(sArray[2]);
	for(i = 0; i < data[locusNumber].na - 2; i++){
	  data[locusNumber].lArray[i] = atoi(sArray[i+3]);
	  data[locusNumber].sArray[i] = atoi(sArray[i+data[locusNumber].na - 1 + 3]);
	}

	printf("%d\n",data[locusNumber].sArray[2]);
      }
    }
    else{

      /* collect hka data from infile- expect length, na, nb, sa, sb, d */
      while(fscanf(infile, "%80s %d %d %d %d %d %lf", string,  &l, &na, &nb, &sa, &sb, &d) != EOF){
	strcpy(data[locusNumber].name, string);
	data[locusNumber].length = l;
	data[locusNumber].na = na;
	data[locusNumber].nb = nb;
	if (nb > 1){
	  lineageNumber = 2;
	}
	data[locusNumber].sa = (double) sa;
	data[locusNumber].sb = (double) sb;
	data[locusNumber].d = (double) d;
	data[locusNumber].lArray[0] = 666;
	locusNumber++;
      }
    }
    fclose(infile);
  }
}

/* harmSum returns the denominator portion of Watterson's estimator */
double harmSum(int n){
  int i;
  double sum = 0.0;

  for(i = 1; i < n; i++){
    sum += 1.0 /  i;
  }
  return(sum);
}

/* harmSumSquare is for calculating the variance of Watterson's estimator */
double harmSumSquare(int n){
  int i;
  double sum = 0.0;

  for(i = 1; i < n; i++){
    sum += 1.0 / ( i * i);
  }
  return(sum);
}

/*setFunction- this sets up the multiroot function for solving */
int setHKAFunction(const gsl_vector *x, void *p, gsl_vector *f){
  double sumS, trueS, sumD, trueD, yi, trueYi;
  double sumSb, trueSb;
  int i;

  /*set up all the equations. the first are the sum of segSites and sum of divergence respectively */
  if (lineageNumber == 1){
    sumS = yi = sumD = trueS = trueD =  0.0;
    for(i = 0; i < locusNumber; i++){
      sumS += data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i);
      sumD += data[i].length * (gsl_vector_get(x,locusNumber) + 1) * gsl_vector_get(x,i);
      trueS += data[i].sa;
      trueD += data[i].d;
    }
    gsl_vector_set(f,0,fabs(trueS - sumS));
    gsl_vector_set(f,1,fabs(trueD - sumD));
    //Sa+D eqns
    for(i = 0; i < locusNumber - 1; i++){
      trueYi = data[i].sa + data[i].d;
      yi = gsl_vector_get(x,i) *						\
	((data[i].length * gsl_vector_get(x,locusNumber) +		\
	  data[i].length + (data[i].length * harmSum(data[i].na))));
      gsl_vector_set(f,i+2,fabs(trueYi - yi));
    }
  }
  else{
    sumS = sumSb = yi = sumD = trueS = trueSb = trueD =  0.0;
    for(i = 0; i < locusNumber; i++){
      sumS += data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i);
      sumSb += data[i].length * harmSum(data[i].nb) * gsl_vector_get(x,i) * gsl_vector_get(x, locusNumber + 1);
      sumD += data[i].length * (gsl_vector_get(x,locusNumber) + ((1.0 + gsl_vector_get(x,locusNumber+1)) / 2.0)) * gsl_vector_get(x,i);
      trueS += data[i].sa;
      trueSb += data[i].sb;
      trueD += data[i].d;
    }
    gsl_vector_set(f,0,fabs(trueS - sumS));
    gsl_vector_set(f,1,fabs(trueSb - sumSb));
    gsl_vector_set(f,2,fabs(trueD - sumD));
    //Sa+Sb+D eqns
    for(i = 0; i < locusNumber - 1; i++){
      trueYi = data[i].sa + data[i].sb + data[i].d;
      yi = (data[i].length * (gsl_vector_get(x,locusNumber) + ((1.0 + gsl_vector_get(x,locusNumber+1)) / 2.0)) * gsl_vector_get(x,i)) \
	+ ( data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i)) + \
	(data[i].length * harmSum(data[i].nb) * gsl_vector_get(x,i) * gsl_vector_get(x, locusNumber + 1));
      gsl_vector_set(f,i+3,fabs(trueYi - yi));
    }
  }
  return GSL_SUCCESS;
}

int solveSystem(gsl_vector *params){
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  gsl_vector *x;
  int status;
  size_t  iter = 0;
  void *point=NULL;
  size_t n;

  if (lineageNumber == 1){
    n = locusNumber + 1;
    x = gsl_vector_alloc(locusNumber + 1);
  }
  else{
    n = locusNumber + 2;
    x = gsl_vector_alloc(locusNumber + 2);
  }

  gsl_multiroot_function f = {&setHKAFunction, n, point};
  gsl_vector_set_all(x, 1);
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc(T, n);
  gsl_multiroot_fsolver_set(s, &f, x);

  do{
    iter++;
    status = gsl_multiroot_fsolver_iterate(s);
    if(status){
      break;
    }
    status = gsl_multiroot_test_residual(s->f, 1e-7);
  }
  while (status == GSL_CONTINUE && iter < 100);

  gsl_vector_memcpy(params, s->x);
  gsl_vector_free(x);
  gsl_multiroot_fsolver_free (s);

  return 0;
}

/* calculateHKA- this is the main beast that calculates estimates of t_hat, f(the ratio of popn sizes),
 and theta's for each locus. it returns the chi-square statistic, and takes pointers to the other
 relavent quantities (t_hat, etc.). */
double calculateHKA(gsl_vector *params,gsl_vector *chiSquares, gsl_vector *expectedPolyA, gsl_vector *expectedD){
  double  chiSquared, chiSquared_i;
  double expSA, expSB, varSA, varSB, expD, varD;
  double hsA, hsB;
  double devA, devB, devD,fHat;
  int i;

  if(lineageNumber > 1){
    fHat = gsl_vector_get(params,locusNumber+1);
  }
  else{
    fHat = 1.0;
  }
  chiSquared = 0.0;
  for(i=0; i < locusNumber; i++){
    hsA = harmSum(data[i].na);
    hsB = harmSum(data[i].nb);
    //calculate expectations and variances at the ith locus
    expSA = gsl_vector_get(params,i) * data[i].length * hsA;
    varSA = expSA + ((gsl_vector_get(params,i) * data[i].length) * (gsl_vector_get(params,i) * data[i].length) * harmSumSquare(data[i].na));
    expSB =  gsl_vector_get(params,i) * fHat * data[i].length * hsB;
    varSB = expSB + ((gsl_vector_get(params,i) * fHat * data[i].length) * (gsl_vector_get(params,i) * fHat * data[i].length) * harmSumSquare(data[i].nb)); // equals zero if no poly data from speciesB
    expD = (gsl_vector_get(params,i) * data[i].length) * (gsl_vector_get(params,locusNumber) + (0.5 * (1.0 + fHat)));
    varD = expD + (((gsl_vector_get(params,i) * data[i].length) * 0.5 * (1.0 + fHat)) * ((gsl_vector_get(params,i) *  data[i].length) * 0.5 * (1.0 + fHat)));

    //calculate deviations for species_a_theta, species_b_theta, and divergence.
    // then add to chi-square stat
    devA = ((data[i].sa - expSA) *  (data[i].sa - expSA)) / varSA;
    devB = ((data[i].sb - expSB) *  (data[i].sb - expSB)) / varSB;  //careful here will not be number if no poly data for speciesB
    devD = ((data[i].d - expD) * (data[i].d - expD)) / varD;

    //is deviation for species B finite?
    if (isnan(devB)){
      chiSquared_i = devA + devD;
    }
    else{
      chiSquared_i = devA + devB + devD;
    }
    chiSquared += chiSquared_i;
    gsl_vector_set(chiSquares,i,chiSquared_i);
    gsl_vector_set(expectedPolyA, i, expSA);
    gsl_vector_set(expectedD, i, expD);
  }

  return(chiSquared);
}

/* took this out of Jim Kent's tree */
int chopByWhite(char *in, char *outArray[], int outSize)
/* Like chopString, but specialized for white space separators. */
{
int recordCount = 0;
char c;
for (;;)
    {
    if (outArray != NULL && recordCount >= outSize)
	break;

    /* Skip initial separators. */
    while (isspace(*in)) ++in;
    if (*in == 0)
	break;

    /* Store start of word and look for end of word. */
    if (outArray != NULL)
	outArray[recordCount] = in;
    recordCount += 1;
    for (;;)
        {
        if ((c = *in) == 0)
            break;
        if (isspace(c))
            break;
        ++in;
        }
    if (*in == 0)
	break;

    /* Tag end of word with zero. */
    if (outArray != NULL)
	*in = 0;
    /* And skip over the zero. */
    in += 1;
    }
return recordCount;
}


/*
 * C version of Diffusive Nested Sampling (DNest4) by Brendon J. Brewer
 *
 * Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 * Jun 30, 2016
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <gsl/gsl_rng.h>

#include "dnestvars.h"

/* output files */
FILE *fsample, *fsample_info;

/* random number generator */
const gsl_rng_type * dnest_gsl_T;
gsl_rng * dnest_gsl_r;

DNestOptions options;
char options_file[STR_MAX_LENGTH];

// sampler
bool save_to_disk;
double compression;
unsigned int regularisation;

void *particles;
int dnest_size_of_modeltype;
int particle_offset_size, particle_offset_double;

LikelihoodType *log_likelihoods;
unsigned int *level_assignments;

// number account of unaccepted times
unsigned int *account_unaccepts;

int size_levels, size_levels_combine;
Level *levels;
Level *copies_of_levels, *levels_combine;
LikelihoodType *all_above;
unsigned int count_saves, num_saves, num_saves_restart;
int dnest_which_particle_update; // which particle to be updated
int dnest_which_level_update;    // which level to be updated;
unsigned long long int count_mcmc_steps;
LikelihoodType *above;
unsigned int size_above, size_all_above;

double post_logz;
int dnest_num_params;
char dnest_sample_postfix[STR_MAX_LENGTH], dnest_sample_tag[STR_MAX_LENGTH], dnest_sample_dir[STR_MAX_LENGTH];
double *dnest_param_range, *dnest_prior_info;
int *dnest_prior_type;
void *dnest_args;

double *limits, *copies_of_limits;

int dnest_thistask, dnest_totaltask;
int *dnest_perturb_accept;
int dnest_root;

int dnest_flag_restart=0, dnest_flag_postprc=0, dnest_flag_sample_info=0, dnest_flag_limits=0;
double dnest_post_temp=1.0;
char file_restart[STR_MAX_LENGTH], file_save_restart[STR_MAX_LENGTH];

//***********************************************
/*                  functions                  */
double mod(double y, double x);
void wrap_limit(double *x, double min, double max);
int mod_int(int y, int x);
int dnest_cmp(const void *pa, const void *pb);

void options_load(char *optfile, DNestOptions *opts);
void setup(int argc, char** argv, DNestFptrSet *fptrset, int num_params,
           double *param_range, int *prior_type, double *prior_info,
           char *sample_dir, char *optfile, DNestOptions *opts, void *args);
void finalise();

void dnest_run();
void dnest_mcmc_run();
void update_particle(unsigned int which);
void update_level_assignment(unsigned int which);
double log_push(unsigned int which_level);
bool enough_levels(Level *l, int size_l);
void do_bookkeeping();
void save_levels();
void save_particle();
void save_limits();
void kill_lagging_particles();
void renormalise_visits();
void recalculate_log_X();
void dnest_postprocess(double temperature, char *optfile, DNestOptions *opts);
void postprocess(double temperature);
void initialize_output_file();
void close_output_file();
void dnest_save_restart();
void dnest_restart();
void dnest_restart_action(int iflag);
void dnest_accept_action();
void dnest_kill_action(int i, int i_copy);
void dnest_from_prior(void *model);
double dnest_perturb(void *model);
void dnest_print_particle(FILE *fp, const void *model);
void dnest_read_particle(FILE *fp, void *model);
/*=====================================================*/
// users responsible for following functions
void (*print_particle)(FILE *fp, const void *model);
void (*read_particle)(FILE *fp, void *model);
void (*from_prior)(void *model);
double (*log_likelihoods_cal)(const void *model);
double (*log_likelihoods_cal_initial)(const void *model);
double (*log_likelihoods_cal_restart)(const void *model);
double (*perturb)(void *model);
void (*restart_action)(int iflag);
void (*accept_action)();
void (*kill_action)(int i, int i_copy);
/*=====================================================*/