#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>
typedef struct{
  double g;//gravity
  double M;//mass of the car
  double m;//mass of the load
  double r;//length of the rope
  double C;//damping coefficient
  double k;//spring constang

  double F;//input force
  double T;//tension to the rope
  //  double FF;//Fictitious force

  double x,x0;//x position of the load
  double y,y0;//y position of the load
  double X;//X position of the car
  double a;//angle (theta)

  double dx,dx0;
  double dy,dy0;
  double dX,dX0;
  double dr;
  double da,da0;
  double dXmax;
  double ddXmax;
  double Fmax;

  double ddx;
  double ddy;
  double ddX;
  double ddr;
  double dda;
  double h;
  int nh;
  double _h;
  int _dim;
  double *_y;
  double *_y_err;
  double *_dydt_in;
  double *_dydt_out;
  const gsl_odeiv_step_type *_T;
  gsl_odeiv_step *_s;
  gsl_odeiv_system _sys;
  double _t;
} CRANE;

#define dim_crane 4
int cranefunc (double t, const double y[], double f[], void *params)
{
  CRANE *c = (CRANE *)params;

#ifdef ICONIP2014 //wrong dynamics
  f[0] = y[1];
  f[1] = -c->k*(y[0]-y[2])/c->m - c->C*(y[1]-y[3])/c->m;//iconip2014
  f[2] = y[3];
  f[3] = (c->k*(y[0]-y[2]) + c->F)/c->M;//iconip2014
//  f[0] = y[1];
//  f[1] = -c->k*(y[0]-y[2])/c->m - c->C*(y[1]-y[3])/c->m;
//  f[2] = y[3];
//  f[3] = (c->k*(y[0]-y[2]) + c->C*(y[1]-y[3] + c->F)/c->M;
#else //corrected dynamics
  f[0] = y[1];
  f[1] = (-c->k*(y[0]-y[2]) - c->C*(y[1]-y[3])       )/c->m;//iconip2014
  f[2] = y[3];
  f[3] = ( c->k*(y[0]-y[2]) + c->C*(y[1]-y[3]) + c->F)/c->M;//iconip2014
#endif

//  f[0] = y[1];
//  f[1] = -c->k*y[0]/c->m - c->C*y[1]/c->m - c->ddX;
//  f[2] = y[3];
//  f[3] = (c->k*y[0] + c->F)/c->M;
//
//  f[0] = y[1];
//  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
//  f[2] = y[3];
//  f[3] = (c->F+c->T*sin(y[0]))/c->M;
  return GSL_SUCCESS;
}
////////////////
#define square(x) ((x)*(x))
CRANE crane;
int initialize()
{
  crane.g=9.8;
  crane.M=100;
  crane.m=20;//10;//20
  crane.r=5;//5;
  crane.C=10;//5;
  crane.k=15;//
  crane.dr=crane.ddr=0;
  crane.x0=0;
  crane.y0=crane.r;
  crane.dx0=crane.dy0=crane.da0=0;
  crane.dXmax=1.;//??ddXmax=(dXmax-dX)/dt;//??
  crane.ddXmax=0.2;//??ddXmax=(dXmax-dX)/dt;//??
  crane.Fmax=30.0;//check//  crane.Fmax=30;//check
#ifdef CRANESUB
  crane.h=AP_tS;
  crane.m=_crane_m;
  crane.r=_crane_r;
  crane.C=_crane_C;
  crane.k=_crane_k;//
  crane.dXmax=_crane_dXmax;
  if(_AP_umax>0) AP_u_max=crane.ddXmax=_AP_umax;
  else AP_u_max=crane.ddXmax;
  AP_u_min=-AP_u_max;
  //  starttime=0;
  //  totaltime=50;
  rr=AP_r=_AP_r;//10
  rr_kyoyou=_rr_kyoyou;
  //  p=(double *)malloc(buffsize*sizeof(double));
  C_MODE=11;
  //  iteration=_iteration;
#else
  crane.h=0.01;
  //  crane.h=0.0001;
#endif
  crane.nh=10;
  crane._h=crane.h/crane.nh;//0.001
  crane._dim=4;
  if(crane._y==NULL){
    crane._y=(double*)malloc(crane._dim*sizeof(double));
    crane._y_err=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_in=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_out=(double*)malloc(crane._dim*sizeof(double));
  }
  crane._T= gsl_odeiv_step_rk4;
  crane._s= gsl_odeiv_step_alloc (crane._T, crane._dim);
  crane._sys= (gsl_odeiv_system){cranefunc, NULL, crane._dim, &crane};
  //  crane._sys[0]=cranefunc;
  //  crane._sys[1]=NULL;
  //  crane._sys[2]=crane._dim;
  //  crane._sys[3]=&crane;
  //initialize variables
  crane._t=crane._y[0]=crane._y[1]=crane._y[2]=crane._y[3]=0;
  //  crane.FF=0;
  int i;for(i=0;i<crane._dim;i++) crane._dydt_in[i]=0;
  GSL_ODEIV_FN_EVAL(&(crane._sys), crane._t, crane._y, crane._dydt_in);
//  crane.a  =crane._y[0];
//  crane.da =crane._y[1];
//  crane.X  =crane._y[2];
//  crane.dX =crane._y[3];
//  crane.x  =crane.X+crane.r * sin(crane.a); //output y
//  crane.y  =crane.r*cos(crane.a);
//  crane.dXmax=1.;//??ddXmax=(dXmax-dX)/dt;//??
//  crane.ddXmax=0.2;//??ddXmax=(dXmax-dX)/dt;//??
  return(0);
}
#ifndef CRANESUB
char *fn="crane3io.dat";
FILE *fp;
#endif//#ifndef CRANESUB

//double plant(double uu)
double plant(double uu)
{
  if(uu>crane.ddXmax) uu=crane.ddXmax;
  else if(uu<-crane.ddXmax) uu=-crane.ddXmax;

  int n;
  for(n=0;n<crane.nh;n++){
    crane.ddX=uu;//crane.ddX=(uu<crane.ddXmax)?uu:crane.ddXmax;
    if(crane.dX>=crane.dXmax && crane.ddX>0) crane.ddX=0;
    else if(crane.dX<=-crane.dXmax && crane.ddX<0) crane.ddX=0;

    int status = gsl_odeiv_step_apply (crane._s, crane._t, crane._h,
				       crane._y, crane._y_err,
				       crane._dydt_in,
				       crane._dydt_out,
				       &crane._sys);
    if (status != GSL_SUCCESS) {fprintf(stderr,"### failure of plant calc.\n"); break;}

    int i;for(i=0;i<dim_crane;i++) crane._dydt_in[i]=crane._dydt_out[i];
    crane._t+=crane._h;
    crane.x  =crane._y[0];
    crane.dx =crane._y[1];
    crane.X  =crane._y[2];
    crane.dX =crane._y[3];
    //    crane.x  =crane.X+crane.a; //output y
    //    crane.dx=(crane.x-crane.x0)/crane._h;
    //crane.x  =crane.X+crane.r * sin(crane.a); //output y
    //crane.y  =crane.r*cos(crane.a);
    //    crane.dx=(crane.x-crane.x0)/crane._h;
    //    crane.dy=(crane.y-crane.y0)/crane._h;
    crane.ddx=(crane.dx-crane.dx0)/crane._h;
    //    crane.ddX=(crane.dX-crane.dX0)/crane._h;
    //    crane.ddy=(crane.dy-crane.dy0)/crane._h;
    //    crane.T=crane.m*sqrt(square(crane.ddx)+square(crane.ddy-crane.g));
    crane.F=crane.M*crane.ddX-crane.k*(crane.x-crane.X)-crane.C*(crane.dx-crane.dX);;
    //    crane.F=crane.M*crane.ddX-crane.k*(crane.x-crane.X);
    //    crane.F=crane.M*crane.ddX-crane.T*sin(crane.a);
    //    crane.dda=(crane.da-crane.da0)/crane._h;
    crane.x0=crane.x;//?
    //    crane.y0=crane.y;
    crane.dx0=crane.dx;//?
    //    crane.dy0=crane.dy;
    //    crane.da0=crane.da;
    //    crane.FF=crane.ddX;
    crane.dX0=crane.dX;
  }
#ifndef CRANESUB
  fprintf(fp,"%.7e %.7e %.7e", crane._t,crane._y[0],crane._y[1]);//crane.x,crane.dx
  fprintf(fp," %.7e %.7e %.7e" ,crane.X,crane.dX,crane.ddX);
  fprintf(fp," %.7e %.7e %.7e" ,crane.x,crane.dx,crane.ddx);
  fprintf(fp," %.7e %.7e %.7e" ,crane.y,crane.dy,crane.ddy);
  fprintf(fp," %.7e %.7e" ,crane.T,crane.F);
  fprintf(fp,"\n");
#endif //#ifndef CRANESUB
  return(crane.x);//crane.x in [0,AP_y=10?] ==> rt in [0,1]
}
#ifndef CRANESUB
int main(int argc,char **argv)
{
  ////////////////////////////////////////////////////
  /// method 1                                     ///
  /// input ddX                                    ///
  /// output a,x,y,F                               ///
  ////////////////////////////////////////////////////
  //  double h = 0.001;//,hh=0.1; h = 0.0001;//,hh=0.1;
  double T=5;
  double t0=0;     //stationary
  double t1=T+t0;//accelerate(speed up)
  double t2=T+t1;//free run
  double t3=T+t2;//decelerate(slow down)
  double t4=t0+50;   //free run

  initialize();
  int M=(t4/crane.h)+1;
  double *ddX=(double*)malloc(sizeof(double)*M);
  //  double *F  =(double*)malloc(sizeof(double)*M);
  double Vmax=1;//dX/dt=1[m/s]
  //C:10;K:15;M:100;m:10;TS:0.5;/*数式処理の場合、この行を実行しない.m=10,40,70,100にして行う*/
  /*  crane.M=100;
  crane.C=10;
  crane.k=15;
  crane.m=10;*/
  int i;
  for(i=1;i<argc;i++){
    if(strncmp(argv[i],"m:",2)==0){
      sscanf(&argv[i][2],"%lf",&crane.m);
      //      fprintf(stderr,"#m=%g",crane.m);
    }
  }
  fprintf(stderr,"#C:%g K:%g M:%g m:%g\n",crane.C,crane.k,crane.M,crane.m);

  double TS=0.5;
  double umax=10;
  int n;
  double t;
  ////////////////////////////////////////////////////
  ///   set ddX                                    ///
  ////////////////////////////////////////////////////
  int n0 =t0/crane.h+0.5, n4=t4/crane.h+0.5;
  for(t=0;t<t4;t+=crane.h){
    double u;
    n=t/crane.h;
    if(t<=t0) u=0; //zero
    else if(t<=t1) u=umax*0.8; //accelerate
    else if(t<=t2) u=0;            //free move
    else if(t<=t3) u=-umax*0.8;//decelerate
    else u=0;                      //free move
    ddX[n]=u;//??
    //    ddX[n]=u*crane.h;//??
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  fp=fopen(fn,"w");
  fprintf(fp,"#%.7e %d %d %.7e %.7e %.7e %.7e #h,n0,n4,M,n,r\n",crane.h,n0,n4,crane.M,crane.m,crane.r,crane.C);
  //  double dx0=0,dy0=0,da0=0;
  //  for(n=0;n<n4;){
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(ddX[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}
#endif //#ifndef SUB


#ifndef LADMM_H
#define LADMM_H

#include "Matrix.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 "cmd_line.h"

//This class implements the method LADMM

/*
The optimization problem to solve is:

min \sum_i f^i(A_ix)+ \sum_i h^i(y^i)+ g(x)
s.t. Mx= y
Assumption 1: For each i, f_i is smooth, g(x) is seperable
*/

template<typename L, typename D>
class LADMM
{
private:

  std::vector<D> Ax;
  std::vector<D> old_Ax;

  std::vector<D> Mx;
  std::vector<D> old_Mx;

  std::vector<D> Mty;
  std::vector<D> old_Mty;

  std::vector<D> Mtlambda;
  std::vector<D> old_Mtlambda;

  std::vector<D> gradient;

  std::vector<D> MtMx;

protected:
  Matrix<L, D> data_A;

  Matrix<L, D> data_M;

  std::vector<D> x;

  std::vector<D> old_x;

  std::vector<D> y;

  std::vector<D> old_y;

  std::vector<D> lambda;

  std::vector<D> old_lambda;

  D beta;

  L m_1;

  L m_2;

  L m_3;

  D rho;

  D tau;

  D sigma;

  D L_phi;

  D function_value;

  D infeas;

  L print_every_N_ADMM;

  D running_time_ADMM;

  L nb_outer_iters;

  ofstream samp_ADMM;

public:
  D lambda_f;

  D mu_g;

  D lambda1;

  D lambda2;

  D L_h;

  virtual inline D value_of_f_j(D, L){return D(NULL);}
  virtual inline D value_of_h_j(D, L){return D(NULL);}
  virtual inline D gradient_of_f_j(D, L){return D(NULL);}
  virtual inline D prox_of_h_j(D,D, L){return D(NULL);}
  virtual inline D value_of_g_j(D, L){return D(NULL);}
  virtual inline D prox_of_g_j(D, D, L){return D(NULL);}
  virtual inline void set_matrix_M(){}
  virtual inline void set_matrix_A(){}

/*
  LADMM(const char* matrix_file, const char* matrix_file2)
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(),data_A(matrix_file), data_M(matrix_file2)
  {
  	this->matrix_merge(data_A,data_M);
    this->gamma=1;
  }
 */
  inline void set_L_phi(){
  	if (data_A.nsamples== 0){
  		L_phi= 0;
	  }
	else{
  		L_phi= compute_lambda_max_A(10);
  	}
  }

  D compute_lambda_max_A(L K){

    std::vector<D> bk(data_A.nfeatures);
    for (L j=0;j<data_A.nfeatures;j++)
    {
      bk[j]=1;
    }
    std::vector<D> yk(data_A.nsamples);
    D normk;
    D tmp;
    for(L kk=0;kk<K;kk++){
      for (L i=0;i<data_A.nsamples;i++){
        tmp=0;
        for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];	k++)
        {
          L j=data_A.row_idx[k];
          tmp+=data_A.A[k]*bk[j];
        }
        yk[i]=tmp;
      }
      normk=0;
      for (L j=0;j<data_A.nfeatures;j++){
        bk[j]=0;
        for (L k = data_A.ptr_t[j]; k < data_A.ptr_t[j + 1];	k++)
        {
          L i=data_A.col_idx[k];
          bk[j]+=data_A.A_t[k]*yk[i]*lambda_f;
        }
        normk+=bk[j]*bk[j];
      }
      normk=sqrt(normk);
      for (L j=0;j<data_A.nfeatures;j++)
      {bk[j]=bk[j]/normk; }
    }
    cout<<endl;
    D res=0;
    normk=0;
    for (L i=0;i<data_A.nsamples;i++){
      tmp=0;
      for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];	k++)
      {
        L j=data_A.row_idx[k];
        tmp+=data_A.A[k]*bk[j];
      }
      yk[i]=tmp;
      normk+=yk[i]*yk[i];
    }
    std::vector<D> bk2(data_A.nfeatures);
    for (L j=0;j<data_A.nfeatures;j++){
      bk2[j]=0;
      for (L k = data_A.ptr_t[j]; k < data_A.ptr_t[j + 1];	k++)
      {
        L i=data_A.col_idx[k];
        bk2[j]+=data_A.A_t[k]*yk[i]*lambda_f;
      }
    }
    for (L j=0;j<data_A.nfeatures;j++)
    res+=bk2[j]*bk[j];
    return res;
  }

D compute_lambda_max_M(L K){

    std::vector<D> bk(data_M.nfeatures);
    for (L j=0;j<data_M.nfeatures- 1;j++)
    {
      bk[j]=1;
    }
    bk[data_M.nfeatures- 1]= 2;
    std::vector<D> yk(data_M.nsamples);
    D normk;
    D tmp;
    for(L kk=0;kk<K;kk++){
      for (L i=0;i<data_M.nsamples;i++){
        tmp=0;
        for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];	k++)
        {
          L j=data_M.row_idx[k];
          tmp+=data_M.A[k]*bk[j];
        }
        yk[i]=tmp;
      }
      normk=0;
      for (L j=0;j<data_M.nfeatures;j++){
        bk[j]=0;
        for (L k = data_M.ptr_t[j]; k < data_M.ptr_t[j + 1];	k++)
        {
          L i=data_M.col_idx[k];
          bk[j]+=data_M.A_t[k]*yk[i];
        }
        normk+=bk[j]*bk[j];
      }
      normk=sqrt(normk);
      for (L j=0;j<data_M.nfeatures;j++)
      {bk[j]=bk[j]/normk; }
    }
    D res=0;
    normk=0;
    for (L i=0;i<data_M.nsamples;i++){
      tmp=0;
      for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];	k++)
      {
        L j=data_M.row_idx[k];
        tmp+=data_M.A[k]*bk[j];
      }
      yk[i]=tmp;
      normk+=yk[i]*yk[i];
    }
    std::vector<D> bk2(data_M.nfeatures);
    for (L j=0;j<data_M.nfeatures;j++){
      bk2[j]=0;
      for (L k = data_M.ptr_t[j]; k < data_M.ptr_t[j + 1];	k++)
      {
        L i=data_M.col_idx[k];
        bk2[j]+=data_M.A_t[k]*yk[i];
      }
    }
    for (L j=0;j<data_M.nfeatures;j++)
    res+=bk2[j]*bk[j];
    return res;
  }

  L get_nb_features(){
    return data_A.get_d();
  }

 /* inline D get_lambda1(){return lambda1;}
  inline D get_lambda2(){return lambda2;}
*/

  void update_x(){
    for (L i= 0; i< m_1; i++){
    	gradient[i]= 0;
    	for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
    	{
      		L j=data_A.col_idx[k];
      		gradient[i]+= data_A.A_t[k]*gradient_of_f_j(Ax[j],j);
    	}
    	MtMx[i]= 0;
    	for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    	{
      		L j=data_M.col_idx[k];
      		MtMx[i]+= data_M.A_t[k]*Mx[j];
    	}
    	x[i]= prox_of_g_j(x[i]- (Mtlambda[i]+ gradient[i]+ beta*MtMx[i]- beta*Mty[i])/(beta*tau+ L_phi), beta*tau+ L_phi, i);
	}
	for (L i= 0; i< m_2; i++){
		Ax[i]= compute_AiTx(i);
	}
	for (L i= 0; i< m_3; i++){
		Mx[i]= compute_MiTx(i);
	}
  }

  void update_y(){
  	for (L i=0; i< m_3; i++){
  		y[i]= prox_of_h_j(y[i]+ (lambda[i]+ beta*Mx[i]- beta*y[i])/(beta*sigma), beta*sigma, i);
	  }
	for (L i=0; i< m_1; i++){
		Mty[i]= compute_MTiTy(i);
	}
  }

  D compute_AiTx(L i){
    D res=0;
    for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];k++)
    {
      L j=data_A.row_idx[k];
      res+= data_A.A[k]*x[j];
    }
    return res;
  }

  D compute_MiTx(L i){
    D res=0;
    for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];k++)
    {
      L j=data_M.row_idx[k];
      res+= data_M.A[k]*x[j];
    }
    return res;
  }

  D compute_MTiTy(L i){
    D res=0;
    for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    {
      L j=data_M.col_idx[k];
      res+= data_M.A_t[k]*y[j];
    }
    return res;
  }

  D compute_MTiTlambda(L i){
    D res=0;
    for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    {
      L j=data_M.col_idx[k];
      res+= data_M.A_t[k]*lambda[j];
    }
    return res;
  }

  void compute_function_value(){
    D res= 0;
    for (L i= 0; i< data_A.nfeatures; i++){
      res+= value_of_g_j(x[i],i);
    }
    for (L i= 0; i< data_M.nsamples; i++){
      res+= value_of_h_j(Mx[i],i);
    }
    for (L i= 0; i<data_A.nsamples; i++){
      res+= value_of_f_j(Ax[i],i);
    }
    function_value= res;
  }

  void compute_infeasibility(){
  	D res = 0;
  	for (L i= 0; i< data_M.nsamples; i++){
      res+= (Mx[i]- data_M.b[i])*(Mx[i]- data_M.b[i]);
    }
    infeas= sqrt(res);
  }

void update_lambda(){
  for(L j=0;j<m_3;j++)
  {
    lambda[j]= lambda[j]+ beta*(Mx[j]- y[j]);
  }
  for (L i=0; i< m_1; i++){
		Mtlambda[i]= compute_MTiTlambda(i);
	}
}

void Initialize(D beta_0, D val_rho, vector<D> & x0,vector<D> & y0, vector<D> & lambda0){
  cout<<"start initializing"<<endl;
  set_matrix_M();
  set_matrix_A();

  m_1=data_A.get_d();
  m_2=data_A.get_n();
  m_3=data_M.get_n();
  cout<<"m_1="<<m_1<<endl;
  cout<<"m_2="<<m_2<<endl;
  cout<<"m_3="<<m_3<<endl;

  beta=beta_0;
  //tau= data_A.nsamples;

  tau= 1.02*compute_lambda_max_M(10);
  sigma= 1;

  rho=val_rho;

  x.resize(m_1,0);
  old_x.resize(m_1,0);
  y.resize(m_3,0);
  old_y.resize(m_3,0);
  lambda.resize(m_3,0);
  old_lambda.resize(m_3,0);
  for(L i=0;i<m_1;i++){
    x[i]=x0[i];
  }
  for(L j=0;j<m_3;j++){
    y[j]=y0[j];
  }
  for(L j=0;j<m_3;j++){
    lambda[j]=lambda0[j];
  }

  Ax.clear();
  Ax.resize(m_2,0);
  Mx.clear();
  Mx.resize(m_3,0);
  Mty.clear();
  Mty.resize(m_1,0);
  Mtlambda.clear();
  Mtlambda.resize(m_1,0);
  gradient.clear();
  gradient.resize(m_1,0);
  MtMx.clear();
  MtMx.resize(m_1,0);
  L_phi= 0;
  set_L_phi();
  cout<< "L_phi= "<< L_phi << " beta= "<< beta<< " rho= "<< rho<< " tau= "<< tau<< " sigma= "<< sigma<<endl;
}

void reset_everything(){
  beta*=rho;

}

inline void compute_and_record_res(){
  if(nb_outer_iters%print_every_N_ADMM==0){
    compute_function_value();
    compute_infeasibility();
    cout<<setprecision(9)<<"Iteration: "<<nb_outer_iters<<"; time="<<running_time_ADMM<< "; function value="<<function_value<< "; infeasibility="<< infeas<< endl;
    samp_ADMM<<setprecision(9)<<nb_outer_iters<<" "<<running_time_ADMM<<" "<< function_value<<" "<< infeas<< endl;
  }
}

void ADMM_solve_with_Linear(D beta_0, D val_rho,vector<D> & x0,vector<D> & y0, vector<D> & lambda0, L max_nb_outer, L p_N_1, string filename1, D time){
  Initialize(beta_0,val_rho, x0, y0, lambda0);
  nb_outer_iters=0;
  //string sampname2= ALGparam.data_dir +"/results/L_Katyusha_"+filename2;
  //filename1= ALGparam.data_dir +"/results/ADMM_"+filename1;
  filename1= "results/ADMM_"+filename1;
  samp_ADMM.open(filename1.c_str());
  running_time_ADMM=0;
  print_every_N_ADMM=p_N_1;
  compute_and_record_res();
  D start;
  D res_x, res_y, res_l;
  /*
  for(L i=0;i<m_3;i++){
    old_lambda[i]=lambda[i];
  }
  */
  while(nb_outer_iters<max_nb_outer){
    //rescale();
    for(L i=0;i<m_1;i++){
      old_x[i]=x[i];
    }
    for(L i=0;i<m_3;i++){
      old_y[i]=y[i];
    }
    for(L i=0;i<m_3;i++){
      old_lambda[i]=lambda[i];
    }
    start = std::clock();
    update_x();
    update_y();
    update_lambda();
    nb_outer_iters++;
    running_time_ADMM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    compute_and_record_res();
    /*
    res_x= 0;
    for(L i=0;i<m_1;i++){
      res_x+= (old_x[i]- x[i])*(old_x[i]- x[i]);
    }
    res_y= 0;
    for(L i=0;i<m_3;i++){
      res_y= (old_y[i]- y[i])*(old_y[i]- y[i]);
    }
    res_l= 0;
    for(L i=0;i<m_3;i++){
      res_l= (old_lambda[i]- lambda[i])*(old_lambda[i]- lambda[i]);
    }
    cout<< "res_x= "<< res_x<< " res_y= "<< res_y<< " res_l= "<< res_l<< endl;
    system("pause");
    */
    start = std::clock();
    reset_everything();
    running_time_ADMM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    if (running_time_ADMM> time){
      break;
    }
  }

}
};

#endif /* MIN_SMOOTH_CONVEX_H */


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

/* Runge-Kutta 4th order, Classical */

/* Author:  G. Jungman
 */

/* Reference: Abramowitz & Stegun, section 25.5. equation 25.5.10

   Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
   L.R., Computer methods for ordinary differential and
   differential-algebraic equations, SIAM, Philadelphia, 1998.
*/

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

#include "odeiv_util.h"

typedef struct
{
  double *k;
  double *k1;
  double *y0;
  double *ytmp;
  double *y_onestep;
}
rk4_state_t;

static void *
rk4_alloc (size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) malloc (sizeof (rk4_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk4_state", GSL_ENOMEM);
    }

  state->k = (double *) malloc (dim * sizeof (double));

  if (state->k == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k", GSL_ENOMEM);
    }

  state->k1 = (double *) malloc (dim * sizeof (double));

  if (state->k1 == 0)
    {
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->ytmp);
      free (state->y0);
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rk4_step (double *y, const rk4_state_t *state,
	  const double h, const double t, const size_t dim,
	  const gsl_odeiv_system *sys)
{
  /* Makes a Runge-Kutta 4th order advance with step size h. */

  /* initial values of variables y. */
  const double *y0 = state->y0;

  /* work space */
  double *ytmp = state->ytmp;

  /* Runge-Kutta coefficients. Contains values of coefficient k1
     in the beginning
  */
  double *k = state->k;

  size_t i;

  /* k1 step */

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + h * k[i];
    }

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
    }

  return GSL_SUCCESS;
}

static int
rk4_apply (void *vstate,
           size_t dim,
           double t,
           double h,
           double y[],
           double yerr[],
           const double dydt_in[],
           double dydt_out[],
           const gsl_odeiv_system * sys)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  size_t i;

  double *const k = state->k;
  double *const k1 = state->k1;
  double *const y0 = state->y0;
  double *const y_onestep = state->y_onestep;

  DBL_MEMCPY (y0, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

  /* Error estimation is done by step doubling procedure */

  /* Save first point derivatives*/

  DBL_MEMCPY (k1, k, dim);

  /* First traverse h with one step (save to y_onestep) */

  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = rk4_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Then with two steps with half step length (save to y) */

  DBL_MEMCPY (k, k1, dim);

  {
    int s = rk4_step (y, state, h/2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, y0, dim);
	return s;
    }
  }

  /* Update before second step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, y0, dim);
	return s;
      }
  }

  /* Save original y0 to k1 for possible failures */
  DBL_MEMCPY (k1, y0, dim);

  /* Update y0 for second step */
  DBL_MEMCPY (y0, y, dim);

  {
    int s = rk4_step (y, state, h/2.0, t + h/2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, k1, dim);
	return s;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL) {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, k1, dim);
	return s;
      }
  }

  /* Error estimation

     yerr = C * 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)

     constant C is approximately 8.0 to ensure 90% of samples lie within
     the error (assuming a gaussian distribution with prior p(sigma)=1/sigma.)

  */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 15.0;
    }

  return GSL_SUCCESS;
}

static int
rk4_reset (void *vstate, size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->k1, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y_onestep, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4_order (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4_free (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  free (state->k);
  free (state->k1);
  free (state->y0);
  free (state->ytmp);
  free (state->y_onestep);
  free (state);
}

static const gsl_odeiv_step_type rk4_type = { "rk4",    /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk4_alloc,
  &rk4_apply,
  &rk4_reset,
  &rk4_order,
  &rk4_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;


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

/* Gear 1. This is the implicit Euler a.k.a backward Euler method. */

/* Author:  G. Jungman
 */

/* Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
   L.R., Computer methods for ordinary differential and
   differential-algebraic equations, SIAM, Philadelphia, 1998.
   The method is also described in eg. this reference.
*/

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

#include "odeiv_util.h"

typedef struct
{
  double *k;
  double *y0;
  double *y0_orig;
  double *y_onestep;
}
gear1_state_t;

static void *
gear1_alloc (size_t dim)
{
  gear1_state_t *state = (gear1_state_t *) malloc (sizeof (gear1_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for gear1_state", GSL_ENOMEM);
    }

  state->k = (double *) malloc (dim * sizeof (double));

  if (state->k == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->y0_orig = (double *) malloc (dim * sizeof (double));

  if (state->y0_orig == 0)
    {
      free (state->y0);
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->y0_orig);
      free (state->y0);
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  return state;
}

static int
gear1_step (double *y, gear1_state_t *state,
	    const double h, const double t,
	    const size_t dim, const gsl_odeiv_system *sys)
{
  /* Makes an implicit Euler advance with step size h.
     y0 is the initial values of variables y.

     The implicit matrix equations to solve are:

     k = y0 + h * f(t + h, k)

     y = y0 + h * f(t + h, k)
  */

  const int iter_steps = 3;
  int nu;
  size_t i;
  double *y0 = state->y0;
  double *k = state->k;

  /* Iterative solution of k = y0 + h * f(t + h, k)

     Note: This method does not check for convergence of the
     iterative solution!
  */

  for (nu = 0; nu < iter_steps; nu++)
    {
      int s = GSL_ODEIV_FN_EVAL(sys, t + h, y, k);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}

      for (i=0; i<dim; i++)
	{
	  y[i] = y0[i] + h * k[i];
	}
    }

  return GSL_SUCCESS;
}

static int
gear1_apply(void * vstate,
            size_t dim,
            double t,
            double h,
            double y[],
            double yerr[],
            const double dydt_in[],
            double dydt_out[],
            const gsl_odeiv_system * sys)
{
  gear1_state_t *state = (gear1_state_t *) vstate;

  size_t i;

  double *y0 = state->y0;
  double *y0_orig = state->y0_orig;
  double *y_onestep = state->y_onestep;

  /* initialization */
  DBL_MEMCPY(y0, y, dim);

  /* Save initial values for possible failures */
  DBL_MEMCPY (y0_orig, y, dim);

  /* First traverse h with one step (save to y_onestep) */
  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = gear1_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

 /* Then with two steps with half step length (save to y) */
  {
    int s = gear1_step (y, state, h / 2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        /* Restore original y vector */
        DBL_MEMCPY (y, y0_orig, dim);
        return s;
      }
  }

  DBL_MEMCPY (y0, y, dim);

  {
    int s = gear1_step (y, state, h / 2.0, t + h / 2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
        /* Restore original y vector */
        DBL_MEMCPY (y, y0_orig, dim);
        return s;
      }
  }

  /* Cleanup update */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          /* Restore original y vector */
          DBL_MEMCPY (y, y0_orig, dim);
          return s;
        }
    }

  /* Error estimation */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]);
    }

  return GSL_SUCCESS;
}

static int
gear1_reset (void *vstate, size_t dim)
{
  gear1_state_t *state = (gear1_state_t *) vstate;

  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y0_orig, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->k, dim);
  return GSL_SUCCESS;
}

static unsigned int
gear1_order (void *vstate)
{
  gear1_state_t *state = (gear1_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 1;
}

static void
gear1_free (void *vstate)
{
  gear1_state_t *state = (gear1_state_t *) vstate;
  free (state->y_onestep);
  free (state->y0_orig);
  free (state->y0);
  free (state->k);
  free (state);
}

static const gsl_odeiv_step_type gear1_type = { "gear1",        /* name */
  0,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &gear1_alloc,
  &gear1_apply,
  &gear1_reset,
  &gear1_order,
  &gear1_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_gear1 = &gear1_type;


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gbpInterpolate.h>

void init_interpolate(double *x, double *y, size_t n, const gsl_interp_type *T, interp_info **interp) {
    size_t i;
    (*interp)    = (interp_info *)malloc(sizeof(interp_info));
    (*interp)->n = n;
    (*interp)->x = (double *)malloc(sizeof(double) * n);
    (*interp)->y = (double *)malloc(sizeof(double) * n);
    (*interp)->T = T;
    // If the x-array is not in ascending order, switch it
    if(x[0] < x[n - 1]) {
        for(i = 0; i < n; i++) {
            (*interp)->x[i] = x[i];
            (*interp)->y[i] = y[i];
        }
    } else {
        for(i = 0; i < n; i++) {
            (*interp)->x[i] = x[n - 1 - i];
            (*interp)->y[i] = y[n - 1 - i];
        }
    }
    (*interp)->accel  = gsl_interp_accel_alloc();
    (*interp)->interp = gsl_interp_alloc(T, n);
    gsl_interp_init((*interp)->interp, (const double *)(*interp)->x, (const double *)(*interp)->y, (*interp)->n);
}


/*
  NAME:
     read_IC
  PURPOSE:
     read the initial conditions file
  CALLING SEQUENCE:
     read_IC(char ICfilename[])
  INPUT:
     ICfilename   - initial conditions filename
  OUTPUT:
     sets the options and the initial conditions
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <stdbool.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <proj_gauss_mixtures.h>
#include <proj_gauss_main.h>

bool read_IC(char ICfilename[]){

  FILE *ICfile;
  if ( (ICfile= fopen(ICfilename,"r")) == NULL){
    printf ("Opening the initial conditions file failed...\n");
    return false;
  }
  char line[100];
  printf("Reading the options section of the initial conditions file...\n");
  while (fgets(line,100,ICfile) != NULL){
    if (line[0] != '#'){
      if (line[0] == '\n') break;
      if (parse_option(line) == false){
	printf("One of the lines in the options section of the initial conditions file is corrupted\n");
	printf("Please check the initial conditions file and try again\n");
	return false;
      }
    }
  }

  printf("Successfully read the options\n");

  printf("Reading the initial model parameters...\n");

  //Read first block, establish the dimension d of the modeled quantities
  int countd=0;
  double *mmtemp = (double *) malloc (1000000 * sizeof (double));
  while (fgets(line,100,ICfile) != NULL){
    if (line[0] != '#'){
      if (line[0] == '\n') break;
      *(mmtemp++) = atof(line);
      ++countd;
    }
  }
  //now determine d
  d = (int) (-3 + sqrt(9 + 8 * (countd-1)))/2 ;
  dV = (int) (d*(d+1)/2);

  //allocate the alpha, mm and VV matrices
  int kk;
  for (kk=0; kk != K; ++kk){
    gaussians->mm = gsl_vector_alloc (d);
    gaussians->VV = gsl_matrix_alloc (d,d);;
    ++gaussians;
  }
  gaussians -= K;

  //first map the mmtemp values on the right alpha, mm, VV
  mmtemp -= countd;
  (*gaussians).alpha = *(mmtemp++);
  int dd;
  for (dd=0; dd != d; ++dd)
    gsl_vector_set(gaussians->mm,dd,*(mmtemp++));
  int dd1,dd2;
  for (dd1=0; dd1 != d; ++dd1)
    gsl_matrix_set(gaussians->VV,dd1,dd1,*(mmtemp++));
  for (dd1=0; dd1 != d-1; ++dd1)
    for (dd2=dd1+1; dd2 != d; ++dd2){
      gsl_matrix_set(gaussians->VV,dd1,dd2,*mmtemp);
      gsl_matrix_set(gaussians->VV,dd2,dd1,*mmtemp);
      mmtemp++;
    }

  ++gaussians;

  //reallocate mmtemp
  mmtemp -= countd;
  mmtemp = (double *) realloc (mmtemp,countd * sizeof (double) );
  if (mmtemp == NULL){
    printf("Error reallocating memory\n");
    printf("Returning\n");
    return false;
  }

  //Then read the rest of the Gaussians.
  for (kk=1; kk != K; ++kk){
    while (fgets(line,100,ICfile) != NULL){
      if (line[0] != '#'){
	if (line[0] == '\n') break;
	*(mmtemp++) = atof(line);
      }
    }
    mmtemp -=countd;
    (*gaussians).alpha = *(mmtemp++);
    for (dd=0; dd != d; ++dd)
      gsl_vector_set(gaussians->mm,dd,*(mmtemp++));
    for (dd1=0; dd1 != d; ++dd1)
      gsl_matrix_set(gaussians->VV,dd1,dd1,*(mmtemp++));
    for (dd1=0; dd1 != d-1; ++dd1)
      for (dd2=dd1+1; dd2 != d; ++dd2){
	gsl_matrix_set(gaussians->VV,dd1,dd2,*mmtemp);
	gsl_matrix_set(gaussians->VV,dd2,dd1,*mmtemp);
	mmtemp++;
      }
    ++gaussians;
    mmtemp -= countd;
  }
  gaussians -= K;

  free(mmtemp);

  fclose(ICfile);

  printf("Successfully read initial model parameters from the initial conditions file\n");

  //Print options
  printf("\nThe options are set to:\n");
  printf("K\t\t=\t");
  printf("%i",K);
  printf("\n");
  printf("maxiter\t\t=\t");
  printf("%lli",maxiter);
  printf("\n");
  printf("tol\t\t=\t");
  printf("%f",tol);
  printf("\n");
  printf("splitnmerge\t=\t");
  printf("%i",splitnmerge);
  printf("\n");
  printf("likeonly\t=\t");
  printf("%i",likeonly);
  printf("\n");
  printf("w\t\t=\t");
  printf("%f",w);
  printf("\n");
  printf("fixamp\t\t=\t");
  int ii;
  for (ii=0; ii != K; ++ii){
    printf("%i",*fixampP);
    if (ii < K-1) printf("\t");
    fixampP++;
  }
  fixampP -= K;
  printf("\n");
  printf("fixmean\t\t=\t");
  for (ii=0; ii != K; ++ii){
    printf("%i",*fixmeanP);
    if (ii < K-1) printf("\t");
    fixmeanP++;
  }
  fixmeanP -= K;
  printf("\n");
  printf("fixcovar\t=\t");
  for (ii=0; ii != K; ++ii){
    printf("%i",*fixcovarP);
    if (ii < K-1) printf("\t");
    fixcovarP++;
  }
  fixcovarP -= K;
  printf("\n");

  //Print the initial model parameters
  printf("\nInitial model parameters used:\n\n");
  for (kk=0; kk != K; ++kk){
    printf("Gaussian ");
    printf("%i",kk);
    printf("\n");
    printf("amp\t=\t");
    printf("%f",(*gaussians).alpha);
    printf("\n");
    printf("mean\t=\t");
    for (dd=0; dd != d; ++dd){
      printf("%f",gsl_vector_get(gaussians->mm,dd));
      if (dd < d-1) printf("\t");
    }
    printf("\n");
    printf("covar\t=\t");
    for (dd1=0; dd1 != d; ++dd1)
      printf("%f\t",gsl_matrix_get(gaussians->VV,dd1,dd1));
    for (dd1=0; dd1 != d-1; ++dd1)
      for (dd2=dd1+1; dd2 != d; ++dd2){
	printf("%f\t",gsl_matrix_get(gaussians->VV,dd1,dd2));
      }
    ++gaussians;
    printf("\n\n");
  }
  gaussians -= K;

  return true;

}


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

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"
#include "viaio/option.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>

/* two-pass formula, correct for round-off error */
void VNormalize(gsl_vector *vec)
{
  double ave,var,sd,nx,s,u,tiny=1.0e-6;
  size_t j,nt = vec->size;

  nx = (double)nt;
  ave = 0;
  for (j=0; j<nt; j++) ave += vec->data[j];
  ave /= nx;

  var = u = 0;
  for (j=0; j<nt; j++) {
    s = vec->data[j]-ave;
    u   += s;
    var += s*s;
  }
  var=(var-u*u/nx)/(nx-1);
  sd = sqrt(var);
  if (sd < tiny) {
    for (j=0; j<nt; j++) vec->data[j] = 0;
  }
  else {
    for (j=0; j<nt; j++) {
      u = vec->data[j];
      vec->data[j] = (u-ave)/sd;
    }
  }
}

/* convert to ranks for spearman correlation */
void GetRank(gsl_vector *vec,gsl_permutation *perm,gsl_permutation *rank)
{
  size_t i;
  size_t n = vec->size;
  gsl_sort_vector_index (perm, vec);
  gsl_permutation_inverse (rank, perm);
  for (i=0; i<n; i++) vec->data[i] = (float)rank->data[i];
}

size_t NumVoxels(VImage src)
{
  int b,r,c;
  size_t n=0;
  for (b=0; b<VImageNBands(src); b++) {
    for (r=0; r<VImageNRows(src); r++) {
      for (c=0; c<VImageNColumns(src); c++) {
	if (VGetPixel(src,b,r,c) > 0.01) n++;
      }
    }
  }
  return n;
}

gsl_matrix_float *DataMatrix(VImage *src,int nslices,VImage roi,int metric)
{
  int b,r,c;
  float u=0;
  size_t nt = VImageNBands(src[0]);
  size_t nvox = NumVoxels(roi);
  gsl_matrix_float *X = gsl_matrix_float_calloc(nvox,nt);
  if (X==NULL) VError(" error allocating data matrix, size: %lu x %lu\n",nvox,nt);

  gsl_vector *vec = gsl_vector_calloc(nt);
  gsl_permutation *perm = NULL;
  gsl_permutation *rank = NULL;

  if (metric == 1) {  /* only needed for spearman correlation */
    perm = gsl_permutation_alloc(nt);
    rank = gsl_permutation_alloc(nt);
  }

  size_t i=0,j=0;
  for (b=0; b<VImageNBands(roi); b++) {
    for (r=0; r<VImageNRows(roi); r++) {
      for (c=0; c<VImageNColumns(roi); c++) {

	if (VGetPixel(roi,b,r,c) < 0.01) continue;

	for (j=0; j<nt; j++) {
	  u = (float) VGetPixel(src[b],j,r,c);
	  vec->data[j] = u;
	}

	if (metric == 0) {  /* linear correlation */
	  VNormalize(vec);
	}
	else if (metric == 1) {  /* convert to ranks for Spearman corr */
	  GetRank(vec,perm,rank);
	}
	else {
	  VError(" unknown metric %d",metric);
	}

	for (j=0; j<nt; j++) {
	  gsl_matrix_float_set(X,i,j,(float)vec->data[j]);
	}

	i++;
      }
    }
  }

  gsl_vector_free(vec);
  if (metric==1) {
    gsl_permutation_free (perm);
    gsl_permutation_free (rank);
  }
  return X;
}

/* new voxel map */
VImage VoxelMap(VImage roi)
{
  size_t nvox = NumVoxels(roi);
  VImage map = VCreateImage(1,4,nvox,VShortRepn);
  if (map == NULL) VError(" error allocating addr map");
  VFillImage(map,VAllBands,0);
  int nslices = VImageNBands(roi);
  int nrows = VImageNRows(roi);
  int ncols = VImageNColumns(roi);
  VSetAttr(VImageAttrList(map),"nvoxels",NULL,VLongRepn,(VLong)nvox);
  VSetAttr(VImageAttrList(map),"nslices",NULL,VLongRepn,(VLong)nslices);
  VSetAttr(VImageAttrList(map),"nrows",NULL,VLongRepn,(VLong)nrows);
  VSetAttr(VImageAttrList(map),"ncols",NULL,VLongRepn,(VLong)ncols);
  VPixel(map,0,3,0,VShort) = nslices;
  VPixel(map,0,3,1,VShort) = nrows;
  VPixel(map,0,3,2,VShort) = ncols;

  int b,r,c;
  size_t i = 0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {

	float u = VGetPixel(roi,b,r,c);
	if (u < 0.1) continue;
	VPixel(map,0,0,i,VShort) = b;
	VPixel(map,0,1,i,VShort) = r;
	VPixel(map,0,2,i,VShort) = c;
	i++;
      }
    }
  }
  return map;
}


/*
 Copyright (C) 2006-2008 Luis Matos, A. Castro

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2, or (at your option)
 any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 02110-1301, USA.

 $Id: poisson_cutoffs.c 13812 2015-04-09 01:25:06Z dstrubbe $
*/

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_errno.h>
#include <assert.h>
#include <config.h>

#define M_EULER_MASCHERONI 0.5772156649015328606065120

/*
This file contains the definition of functions used by Fortran module
poisson_cutoff_m. There are four functions to be defined:

c_poisson_cutoff_3d_1d_finite: For the cutoff of 3D/0D cases, in which however
                               we want the region of the cutoff to be a cylinder.
c_poisson_cutoff_2d_0d:

c_poisson_cutoff_2d_1d:

*/

/************************************************************************/
/************************************************************************/
/* C_POISSON_CUTOFF_3D_1D_FINITE */
/************************************************************************/
/************************************************************************/

struct parameters
{
  double kx;
  double kr;
  double x0;
  double r0;
  int index;
};

/* the index is the type of integral. This is necessary because depending of the value of k the
   the integral needs to be performed in different ways */
#define PFC_F_0   0
#define PFC_F_KX  1
#define PFC_F_KR  2

static const double tol = 1e-7;
static double zero = 1e-100;

static double f(double w, void *p)
{
  struct parameters *params = (struct parameters *)p;
  const double  kx = (params->kx);
  double  kr = (params->kr);
  double  x0 = (params->x0);
  double  r0 = (params->r0);
  int index  = (params->index);
  double result;

  if (w == 0) w = zero;

  if (fabs(kx - w) > tol){
    result = sin((kx - w)*x0)/(kx - w)*
      sqrt(pow(w, 2.0*index)*pow(r0, 2.0*index))/(kr*kr + w*w)
      *gsl_sf_bessel_Kn(index, r0*fabs(w));
  }
  else{
    result = x0*sqrt(pow(w, 2.0*index))/(kr*kr + w*w)*gsl_sf_bessel_Kn(index, r0*fabs(w));
  }
  return result;
}

static double f_kx_zero(double w, void *p)
{
  struct parameters *params = (struct parameters *)p;
  double  kr = (params->kr);
  double  x0 = (params->x0);
  double result;

  if (w == 0.0) w = zero;

  result = 2.0*M_PI*w*gsl_sf_bessel_J0(kr*w)*
    (log(x0 + sqrt(w*w + x0*x0)) - log(-x0 + sqrt(w*w + x0*x0)));
  return result;
}

static double f_kr_zero(double w, void *p)
{
  struct parameters *params = (struct parameters *)p;
  double  kx = (params->kx);
  double  r0 = (params->r0);
  double result;

  result = 4.0*M_PI*cos(kx*w)*(sqrt(r0*r0 + w*w) - fabs(w));
  return result;
}

static double int_aux(int index, double kx, double kr, double x0, double r0, int which_int)
{
  /*variables*/
  double result, error;
  double xinf = 100.0/r0;
  struct parameters params;

  /*pass the given parameters to program*/
  const size_t wrk_size = 5000;
  /* I believe that 1e-6 is over-killing, a substantial gain in time
     is obtained by setting this threshold to 1e-3 */
  /* const double epsilon_abs = 1e-6; */
  /* const double epsilon_rel = 1e-6; */
  const double epsilon_abs = 1e-3;
  const double epsilon_rel = 1e-3;

  /*creating the workspace*/
  gsl_integration_workspace * ws = gsl_integration_workspace_alloc (wrk_size);

  /*create the function*/
  gsl_function F;

  assert(which_int == PFC_F_0 || which_int == PFC_F_KR || which_int == PFC_F_KX);

  params.kx = kx;
  params.kr = kr;
  params.x0 = x0;
  params.r0 = r0;
  params.index = index;

  F.params = &params;

  /*select the integration method*/
  switch(which_int){
  case PFC_F_0:
    F.function = &f;
    gsl_integration_qag(&F, -xinf, xinf, epsilon_abs, epsilon_rel,
			wrk_size, 3, ws, &result, &error);
    break;
  case PFC_F_KX:
    F.function = &f_kx_zero;
    gsl_integration_qag (&F, 0.0, r0, epsilon_abs, epsilon_rel,
			 wrk_size, 3, ws, &result, &error);
    break;
  case PFC_F_KR:
    F.function = &f_kr_zero;
    gsl_integration_qag (&F, 0.0, x0, epsilon_abs, epsilon_rel,
			 wrk_size, 3, ws, &result, &error);
    break;
  }

  /*free the integration workspace*/
  gsl_integration_workspace_free (ws);

  /*return*/
  return result;
}

double poisson_finite_cylinder(double kx, double kr, double x0,double r0) {
  double result;

  if ((kx>=tol) & (kr>=tol)) {
    result = 4.0*kr*r0*gsl_sf_bessel_Jn(1, kr*r0)*int_aux(0, kx, kr, x0, r0, PFC_F_0)
      - 4.0*gsl_sf_bessel_Jn(0, kr*r0)*int_aux(1, kx, kr, x0, r0, PFC_F_0)
      + 4.0*M_PI/(kx*kx + kr*kr)*(1.0 + exp(-kr*x0)*(kx/kr*sin(kx*x0) - cos(kx*x0)));
  }
  else if ((kx < tol) & (kr >= tol)){
    result = int_aux(0, kx, kr, x0, r0, PFC_F_KX);
  }
  else if ((kx >= tol) & (kr < tol)){
    result = int_aux(0, kx, kr, x0, r0, PFC_F_KR);
  }
  else
    result = -2.0*M_PI*( log(r0/(x0 + sqrt(r0*r0 + x0*x0)))
			 *r0*r0 + x0*(x0 - sqrt(r0*r0 + x0*x0)));

  /* the 1/(4pi) factor is due to the factor on the poissonsolver3d (end)*/
  return result/(4.0*M_PI);
}

/* --------------------- Interface to Fortran ---------------------- */
double FC_FUNC_(c_poisson_cutoff_3d_1d_finite, C_POISSON_CUTOFF_3D_1D_FINITE)
  (double *gx, double *gperp, double *xsize, double *rsize)
{
  return poisson_finite_cylinder(*gx, *gperp, *xsize, *rsize);
}

/************************************************************************/
/************************************************************************/
/* C_POISSON_CUTOFF_2D_0D */
/************************************************************************/
/************************************************************************/
static double bessel_J0(double w, void *p)
{
  return gsl_sf_bessel_J0(w);
}

/* --------------------- Interface to Fortran ---------------------- */
double FC_FUNC_(c_poisson_cutoff_2d_0d, C_POISSON_CUTOFF_2D_0D)
  (double *x, double *y)
{
  double result, error;
  const size_t wrk_size = 500;
  const double epsilon_abs = 1e-3;
  const double epsilon_rel = 1e-3;

  gsl_integration_workspace * ws = gsl_integration_workspace_alloc (wrk_size);
  gsl_function F;

  F.function = &bessel_J0;
  gsl_integration_qag(&F, *x, *y, epsilon_abs, epsilon_rel,
  			wrk_size, 3, ws, &result, &error);
  gsl_integration_workspace_free(ws);

  return result;
}

/************************************************************************/
/************************************************************************/
/* INTCOSLOG */
/************************************************************************/
/************************************************************************/
/* Int_0^mu dy cos(a*y) log(abs(b*y)) =
   (1/a) * (log(|b*mu|)*sin(a*mu) - Si(a*mu) )    */
double FC_FUNC(intcoslog, INTCOSLOG)(double *mu, double *a, double *b)
{
  if(fabs(*a)>0.0){
    return (1.0/(*a)) * (log((*b)*(*mu))*sin((*a)*(*mu)) - gsl_sf_Si((*a)*(*mu)) );
  }else{
    return (*mu)*(log((*mu)*(*b)) - 1.0);
  }
}

/************************************************************************/
/************************************************************************/
/* C_POISSON_CUTOFF_2D_1D */
/************************************************************************/
/************************************************************************/
struct parameters_2d_1d
{
  double gx;
  double gy;
  double rc;
};

static double cutoff_2d_1d(double w, void *p)
{
  double k0arg, k0;
  struct parameters_2d_1d *params = (struct parameters_2d_1d *)p;
  double  gx = (params->gx);
  double  gy = (params->gy);

  k0arg = fabs(w*gx);
  if(k0arg < 0.05){
    k0 = - (log(k0arg/2) + M_EULER_MASCHERONI);
  }
  else if(k0arg < 50.0 ){
    k0 = gsl_sf_bessel_K0(k0arg);
  }else{
    k0 = sqrt(2.0*M_PI/k0arg)*exp(-k0arg);
  }
  return 4.0*cos(w*gy)*k0;
}

/* --------------------- Interface to Fortran ---------------------- */
double FC_FUNC_(c_poisson_cutoff_2d_1d, C_POISSON_CUTOFF_2D_1D)
     (double *gy, double *gx, double *rc)
{
  double result, error, res, b;
  struct parameters_2d_1d params;
  const size_t wrk_size = 5000;
  const double epsilon_abs = 1e-3;
  const double epsilon_rel = 1e-3;

  double mu;
  gsl_integration_workspace * ws = gsl_integration_workspace_alloc (wrk_size);
  gsl_function F;

  mu = 0.1/(*gx);
  b  = fabs((*gx))/2.0;

  params.gx = *gx;
  params.gy = *gy;
  params.rc = *rc;

  F.function = &cutoff_2d_1d;
  F.params = &params;

  res = -4.0 * FC_FUNC(intcoslog, INTCOSLOG)(&mu, gy, &b);

  if( fabs(*gy) > 0.0) {
    res = res - (4.0 * M_EULER_MASCHERONI / (*gy)) * sin( (*gy)*mu );
  }else{
    res = res - 4.0 * M_EULER_MASCHERONI * mu;
  }

  gsl_integration_qag(&F, mu, (*rc), epsilon_abs, epsilon_rel,
  			wrk_size, 3, ws, &result, &error);
  res = res + result;

  gsl_integration_workspace_free (ws);
  return res;
}

/************************************************************************/
/************************************************************************/
/* C_POISSON_CUTOFF_1D_0D */
/************************************************************************/
/************************************************************************/

struct parameters_1d_0d
{
  double g;
  double a;
};

static double cutoff_1d_0d(double w, void *p)
{
  struct parameters_1d_0d *params = (struct parameters_1d_0d *)p;
  double g = (params->g);
  double a = (params->a);
  return 2.0*(cos(w)/sqrt(w*w+a*a*g*g));
}

/* --------------------- Interface to Fortran ---------------------- */
double FC_FUNC_(c_poisson_cutoff_1d_0d, C_POISSON_CUTOFF_1D_0D)
     (double *g, double *a, double *rc)
{
  double result, error;
  struct parameters_1d_0d params;
  const size_t wrk_size = 5000;
  const double epsilon_abs = 1e-3;
  const double epsilon_rel = 1e-3;
  int status;
  gsl_integration_workspace * ws;
  gsl_function F;

  if((*g) <= 0.0){return 2.0*asinh((*rc)/(*a));};
  if((*g)*(*rc) > 100.0*M_PI){return 2.0*gsl_sf_bessel_K0((*a)*(*g));};

  gsl_set_error_handler_off();

  ws = gsl_integration_workspace_alloc (wrk_size);

  params.g = *g;
  params.a = *a;

  F.function = &cutoff_1d_0d;
  F.params = &params;

  status = gsl_integration_qag(&F, 0.0, (*rc)*(*g), epsilon_abs, epsilon_rel,
    wrk_size, 3, ws, &result, &error);

  gsl_integration_workspace_free (ws);

  if(status){
    return 0.0;
  }else{
    return result;
  }

}


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

#include <config.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "initialise.c"
#include "append.c"
#include "qelg.c"

int
gsl_integration_qawf (gsl_function * f,
		      const double a,
		      const double epsabs,
		      const size_t limit,
		      gsl_integration_workspace * workspace,
		      gsl_integration_workspace * cycle_workspace,
		      gsl_integration_qawo_table * wf,
		      double *result, double *abserr)
{
  double area, errsum;
  double res_ext, err_ext;
  double correc, total_error = 0.0, truncation_error;

  size_t ktmin = 0;
  size_t iteration = 0;

  struct extrapolation_table table;

  double cycle;
  double omega = wf->omega;

  const double p = 0.9;
  double factor = 1;
  double initial_eps, eps;
  int error_type = 0;

  /* Initialize results */

  initialise (workspace, a, a);

  *result = 0;
  *abserr = 0;

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  /* Test on accuracy */

  if (epsabs <= 0)
    {
      GSL_ERROR ("absolute tolerance epsabs must be positive", GSL_EBADTOL) ;
    }

  if (omega == 0.0)
    {
      if (wf->sine == GSL_INTEG_SINE)
	{
	  /* The function sin(w x) f(x) is always zero for w = 0 */

	  *result = 0;
	  *abserr = 0;

	  return GSL_SUCCESS;
	}
      else
	{
	  /* The function cos(w x) f(x) is always f(x) for w = 0 */

	  int status = gsl_integration_qagiu (f, a, epsabs, 0.0,
					      cycle_workspace->limit,
					      cycle_workspace,
					      result, abserr);
	  return status;
	}
    }

  if (epsabs > GSL_DBL_MIN / (1 - p))
    {
      eps = epsabs * (1 - p);
    }
  else
    {
      eps = epsabs;
    }

  initial_eps = eps;

  area = 0;
  errsum = 0;

  res_ext = 0;
  err_ext = GSL_DBL_MAX;
  correc = 0;

  cycle = (2 * floor (fabs (omega)) + 1) * M_PI / fabs (omega);

  gsl_integration_qawo_table_set_length (wf, cycle);

  initialise_table (&table);

  for (iteration = 0; iteration < limit; iteration++)
    {
      double area1, error1, reseps, erreps;

      double a1 = a + iteration * cycle;
      double b1 = a1 + cycle;

      double epsabs1 = eps * factor;

      int status = gsl_integration_qawo (f, a1, epsabs1, 0.0, limit,
					 cycle_workspace, wf,
					 &area1, &error1);

      append_interval (workspace, a1, b1, area1, error1);

      factor *= p;

      area = area + area1;
      errsum = errsum + error1;

      /* estimate the truncation error as 50 times the final term */

      truncation_error = 50 * fabs (area1);

      total_error = errsum + truncation_error;

      if (total_error < epsabs && iteration > 4)
	{
	  goto compute_result;
	}

      if (error1 > correc)
	{
	  correc = error1;
	}

      if (status)
	{
	  eps = GSL_MAX_DBL (initial_eps, correc * (1.0 - p));
	}

      if (status && total_error < 10 * correc && iteration > 3)
	{
	  goto compute_result;
	}

      append_table (&table, area);

      if (table.n < 2)
	{
	  continue;
	}

      qelg (&table, &reseps, &erreps);

      ktmin++;

      if (ktmin >= 15 && err_ext < 0.001 * total_error)
	{
	  error_type = 4;
	}

      if (erreps < err_ext)
	{
	  ktmin = 0;
	  err_ext = erreps;
	  res_ext = reseps;

	  if (err_ext + 10 * correc <= epsabs)
	    break;
	  if (err_ext <= epsabs && 10 * correc >= epsabs)
	    break;
	}

    }

  if (iteration == limit)
    error_type = 1;

  if (err_ext == GSL_DBL_MAX)
    goto compute_result;

  err_ext = err_ext + 10 * correc;

  *result = res_ext;
  *abserr = err_ext;

  if (error_type == 0)
    {
      return GSL_SUCCESS ;
    }

  if (res_ext != 0.0 && area != 0.0)
    {
      if (err_ext / fabs (res_ext) > errsum / fabs (area))
	goto compute_result;
    }
  else if (err_ext > errsum)
    {
      goto compute_result;
    }
  else if (area == 0.0)
    {
      goto return_error;
    }

  if (error_type == 4)
    {
      err_ext = err_ext + truncation_error;
    }

  goto return_error;

compute_result:

  *result = area;
  *abserr = total_error;

return_error:

  if (error_type > 2)
    error_type--;

  if (error_type == 0)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 1)
    {
      GSL_ERROR ("number of iterations was insufficient", GSL_EMAXITER);
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("cannot reach tolerance because of roundoff error",
		 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
		 GSL_ESING);
    }
  else if (error_type == 4)
    {
      GSL_ERROR ("roundoff error detected in the extrapolation table",
		 GSL_EROUND);
    }
  else if (error_type == 5)
    {
      GSL_ERROR ("integral is divergent, or slowly convergent",
		 GSL_EDIVERGE);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }

}


//This does CELL (~soma) stage of LSTM (long short-term memory) model.
//This requires each neuron to have 4 input time series, Xc, Xi, Xf, Xo,
//where Xc is the usual (or "cellular") input and Xi, Xf, Xo the inputs for the input, forget, output gates.
//Xc, Xi, Xf, Xo are the output of separate linear IN stages (weights and baises).

//For dim=0, C[:,t] = tanh{Xc[:,t] + Uc*Y[:,t-1]} \n";
//           I[:,t] = sig{Xi[:,t] + Ui*Y[:,t-1]} \n";
//           F[:,t] = sig{Xf[:,t] + Uf*Y[:,t-1]} \n";
//           O[:,t] = sig{Xo[:,t] + Uo*Y[:,t-1]} \n";
//           H[:,t] = I[:,t].*C[:,t] + F[:,t].*H[:,t-1] \n";
//           Y[:,t] = O[:,t].*tanh{H[:,t]} \n";
//with sizes Xc, Xi, Xf, Xo: N x T \n";
//           Uc, Ui, Uf, Uo: N x N \n";
//           Y             : N x T \n";
//
//For dim=1, C[t,:] = tanh{Xc[t,:] + Y[t-1,:]*Uc} \n";
//           I[t,:] = sig{Xi[t,:] + Y[t-1,:]*Ui} \n";
//           F[t,:] = sig{Xf[t,:] + Y[t-1,:]*Uf} \n";
//           O[t,:] = sig{Xo[t,:] + Y[t-1,:]*Uo} \n";
//           H[t,:] = I[t,:].*C[t,:] + F[t,:].*H[t-1,:] \n";
//           Y[t,:] = O[t,:].*tanh{H[t,:]} \n";
//with sizes Xc, Xi, Xf, Xo: T x N \n";
//           Uc, Ui, Uf, Uo: N x N \n";
//           Y             : T x N \n";
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//I is the input gate, F is the forget gate, O is the output gate,
//C is the "cell input activation vector",
//H is an intermediate (hidden) vector (sometimes called the "cell state vector"),
//Uc, Ui, Uf, Uo are NxN matrices, and Y is the final output (sometimes called the "hidden state vector").

//Note that, the neurons of a layer are independent only if Uc, Ui, Uf, Uo are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

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

#ifdef I
    #undef I
#endif

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int lstm4_s (float *Y, const float *Xc, const float *Xi, const float *Xf, const float *Xo, const float *Uc, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm4_d (double *Y, const double *Xc, const double *Xi, const double *Xf, const double *Xo, const double *Uc, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm4_inplace_s (float *Xc, const float *Xi, const float *Xf, const float *Xo, const float *Uc, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm4_inplace_d (double *Xc, const double *Xi, const double *Xf, const double *Xo, const double *Uc, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm4_s (float *Y, const float *Xc, const float *Xi, const float *Xf, const float *Xo, const float *Uc, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    size_t nT, tN;

    float *C, *I, *F, *O, *H;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_s: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        //C[0] = tanhf(Xc[0]);
        //I[0] = 1.0f / (1.0f+expf(-Xi[0]));
        //F[0] = 1.0f / (1.0f+expf(-Xf[0]));
        //O[0] = 1.0f / (1.0f+expf(-Xo[0]));
        //H[0] = I[0] * C[0];
        //Y[0] = O[0] * tanhf(H[0]);
        H[0] = tanhf(Xc[0]) / (1.0f+expf(-Xi[0]));
        Y[0] = tanhf(H[0]) / (1.0f+expf(-Xo[0]));
        for (size_t t=1; t<T; ++t)
        {
            //C[0] = tanhf(Xc[t]+Uc[0]*Y[t-1]);
            //I[0] = 1.0f / (1.0f+expf(-Xi[t]-Ui[0]*Y[t-1]));
            //F[0] = 1.0f / (1.0f+expf(-Xf[t]-Uf[0]*Y[t-1]));
            //O[0] = 1.0f / (1.0f+expf(-Xo[t]-Uo[0]*Y[t-1]));
            //H[0] = I[0]*C[0] + F[0]*H[0];
            //Y[t] = O[0] * tanhf(H[0]);
            H[0] = tanhf(Xc[t]+Uc[0]*Y[t-1])/(1.0f+expf(-Xi[t]-Ui[0]*Y[t-1])) + H[0]/(1.0f+expf(-Xf[t]-Uf[0]*Y[t-1]));
            Y[t] = tanhf(H[0]) / (1.0f+expf(-Xo[t]-Uo[0]*Y[t-1]));
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                //C[n] = tanhf(Xc[n]);
                //I[n] = 1.0f / (1.0f+expf(-Xi[n]));
                //F[n] = 1.0f / (1.0f+expf(-Xf[n]));
                //O[n] = 1.0f / (1.0f+expf(-Xo[n]));
                //H[n] = I[n] * C[n];
                //Y[n] = O[n] * tanhf(H[n]);
                H[n] = tanhf(Xc[n]) / (1.0f+expf(-Xi[n]));
                Y[n] = tanhf(H[n]) / (1.0f+expf(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xc[tN],1,C,1); cblas_scopy((int)N,&Xi[tN],1,I,1);
                cblas_scopy((int)N,&Xf[tN],1,F,1); cblas_scopy((int)N,&Xo[tN],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Y[tN-N],1,o,C,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Y[tN-N],1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Y[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    //C[n] = tanhf(C[n]);
                    //I[n] = 1.0f / (1.0f+expf(-I[n]));
                    //F[n] = 1.0f / (1.0f+expf(-F[n]));
                    //O[n] = 1.0f / (1.0f+expf(-O[n]));
                    //H[n] = I[n]*C[n] + F[n]*H[n];
                    //Y[tN+n] = O[n] * tanhf(H[n]);
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Y[tN+n] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                //C[n] = tanhf(Xc[nT])
                //I[n] = 1.0f / (1.0f+expf(-Xi[nT]));
                //F[n] = 1.0f / (1.0f+expf(-Xf[nT]));
                //O[n] = 1.0f / (1.0f+expf(-Xo[nT]));
                //H[n] = I[n] * C[n];
                //Y[nT] = O[n] * tanhf(H[n]);
                H[n] = tanhf(Xc[nT]) / (1.0f+expf(-Xi[nT]));
                Y[nT] = tanhf(H[n]) / (1.0f+expf(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xc[t],(int)T,C,1); cblas_scopy((int)N,&Xi[t],(int)T,I,1);
                cblas_scopy((int)N,&Xf[t],(int)T,F,1); cblas_scopy((int)N,&Xo[t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Y[t-1],(int)T,o,C,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Y[t-1],(int)T,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Y[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    //C[n] = tanhf(C[n]);
                    //I[n] = 1.0f / (1.0f+expf(-I[n]));
                    //F[n] = 1.0f / (1.0f+expf(-F[n]));
                    //O[n] = 1.0f / (1.0f+expf(-O[n]));
                    //H[n] = I[n]*C[n] + F[n]*H[n];
                    //Y[t+n*T] = O[n] * tanhf(H[n]);
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Y[t+n*T] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanhf(Xc[nT]) / (1.0f+expf(-Xi[nT]));
                Y[nT] = tanhf(H[n]) / (1.0f+expf(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xc[t],(int)T,C,1); cblas_scopy((int)N,&Xi[t],(int)T,I,1);
                cblas_scopy((int)N,&Xf[t],(int)T,F,1); cblas_scopy((int)N,&Xo[t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Y[t-1],(int)T,o,C,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Y[t-1],(int)T,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Y[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Y[t+n*T] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanhf(Xc[n]) / (1.0f+expf(-Xi[n]));
                Y[n] = tanhf(H[n]) / (1.0f+expf(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xc[tN],1,C,1); cblas_scopy((int)N,&Xi[tN],1,I,1);
                cblas_scopy((int)N,&Xf[tN],1,F,1); cblas_scopy((int)N,&Xo[tN],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Y[tN-N],1,o,C,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Y[tN-N],1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Y[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Y[tN+n] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm4_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm4_d (double *Y, const double *Xc, const double *Xi, const double *Xf, const double *Xo, const double *Uc, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    size_t nT, tN;

    double *C, *I, *F, *O, *H;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_d: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        H[0] = tanh(Xc[0]) / (1.0+exp(-Xi[0]));
        Y[0] = tanh(H[0]) / (1.0+exp(-Xo[0]));
        for (size_t t=1; t<T; ++t)
        {
            H[0] = tanh(Xc[t]+Uc[0]*Y[t-1])/(1.0+exp(-Xi[t]-Ui[0]*Y[t-1])) + H[0]/(1.0+exp(-Xf[t]-Uf[0]*Y[t-1]));
            Y[t] = tanh(H[0]) / (1.0+exp(-Xo[t]-Uo[0]*Y[t-1]));
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanh(Xc[n]) / (1.0+exp(-Xi[n]));
                Y[n] = tanh(H[n]) / (1.0+exp(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xc[tN],1,C,1); cblas_dcopy((int)N,&Xi[tN],1,I,1);
                cblas_dcopy((int)N,&Xf[tN],1,F,1); cblas_dcopy((int)N,&Xo[tN],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Y[tN-N],1,o,C,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Y[tN-N],1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Y[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Y[tN+n] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanh(Xc[nT]) / (1.0+exp(-Xi[nT]));
                Y[nT] = tanh(H[n]) / (1.0+exp(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xc[t],(int)T,C,1); cblas_dcopy((int)N,&Xi[t],(int)T,I,1);
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1); cblas_dcopy((int)N,&Xo[t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Y[t-1],(int)T,o,C,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Y[t-1],(int)T,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Y[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Y[t+n*T] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanh(Xc[nT]) / (1.0+exp(-Xi[nT]));
                Y[nT] = tanh(H[n]) / (1.0+exp(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xc[t],(int)T,C,1); cblas_dcopy((int)N,&Xi[t],(int)T,I,1);
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1); cblas_dcopy((int)N,&Xo[t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Y[t-1],(int)T,o,C,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Y[t-1],(int)T,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Y[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Y[t+n*T] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanh(Xc[n]) / (1.0+exp(-Xi[n]));
                Y[n] = tanh(H[n]) / (1.0+exp(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xc[tN],1,C,1); cblas_dcopy((int)N,&Xi[tN],1,I,1);
                cblas_dcopy((int)N,&Xf[tN],1,F,1); cblas_dcopy((int)N,&Xo[tN],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Y[tN-N],1,o,C,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Y[tN-N],1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Y[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Y[tN+n] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm4_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm4_inplace_s (float *Xc, const float *Xi, const float *Xf, const float *Xo, const float *Uc, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    size_t nT, tN;

    float *C, *I, *F, *O, *H;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm4_inplace_s: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        H[0] = tanhf(Xc[0]) / (1.0f+expf(-Xi[0]));
        Xc[0] = tanhf(H[0]) / (1.0f+expf(-Xo[0]));
        for (size_t t=1; t<T; ++t)
        {
            H[0] = tanhf(Xc[t]+Uc[0]*Xc[t-1])/(1.0f+expf(-Xi[t]-Ui[0]*Xc[t-1])) + H[0]/(1.0f+expf(-Xf[t]-Uf[0]*Xc[t-1]));
            Xc[t] = tanhf(H[0]) / (1.0f+expf(-Xo[t]-Uo[0]*Xc[t-1]));
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanhf(Xc[n]) / (1.0f+expf(-Xi[n]));
                Xc[n] = tanhf(H[n]) / (1.0f+expf(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xc[tN],1,C,1); cblas_scopy((int)N,&Xi[tN],1,I,1);
                cblas_scopy((int)N,&Xf[tN],1,F,1); cblas_scopy((int)N,&Xo[tN],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[tN-N],1,o,C,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[tN-N],1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[tN-N],1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Xc[tN+n] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanhf(Xc[nT]) / (1.0f+expf(-Xi[nT]));
                Xc[nT] = tanhf(H[n]) / (1.0f+expf(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xc[t],(int)T,C,1); cblas_scopy((int)N,&Xi[t],(int)T,I,1);
                cblas_scopy((int)N,&Xf[t],(int)T,F,1); cblas_scopy((int)N,&Xo[t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[t-1],(int)T,o,C,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[t-1],(int)T,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[t-1],(int)T,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Xc[t+n*T] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanhf(Xc[nT]) / (1.0f+expf(-Xi[nT]));
                Xc[nT] = tanhf(H[n]) / (1.0f+expf(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xc[t],(int)T,C,1); cblas_scopy((int)N,&Xi[t],(int)T,I,1);
                cblas_scopy((int)N,&Xf[t],(int)T,F,1); cblas_scopy((int)N,&Xo[t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[t-1],(int)T,o,C,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[t-1],(int)T,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[t-1],(int)T,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Xc[t+n*T] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanhf(Xc[n]) / (1.0f+expf(-Xi[n]));
                Xc[n] = tanhf(H[n]) / (1.0f+expf(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xc[tN],1,C,1); cblas_scopy((int)N,&Xi[tN],1,I,1);
                cblas_scopy((int)N,&Xf[tN],1,F,1); cblas_scopy((int)N,&Xo[tN],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[tN-N],1,o,C,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[tN-N],1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[tN-N],1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanhf(C[n])/(1.0f+expf(-I[n])) + H[n]/(1.0f+expf(-F[n]));
                    Xc[tN+n] = tanhf(H[n]) / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm4_inplace_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm4_inplace_d (double *Xc, const double *Xi, const double *Xf, const double *Xo, const double *Uc, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    size_t nT, tN;

    double *C, *I, *F, *O, *H;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm4_inplace_d: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        H[0] = tanh(Xc[0]) / (1.0+exp(-Xi[0]));
        Xc[0] = tanh(H[0]) / (1.0+exp(-Xo[0]));
        for (size_t t=1; t<T; ++t)
        {
            H[0] = tanh(Xc[t]+Uc[0]*Xc[t-1])/(1.0+exp(-Xi[t]-Ui[0]*Xc[t-1])) + H[0]/(1.0+exp(-Xf[t]-Uf[0]*Xc[t-1]));
            Xc[t] = tanh(H[0]) / (1.0+exp(-Xo[t]-Uo[0]*Xc[t-1]));
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanh(Xc[n]) / (1.0+exp(-Xi[n]));
                Xc[n] = tanh(H[n]) / (1.0+exp(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xc[tN],1,C,1); cblas_dcopy((int)N,&Xi[tN],1,I,1);
                cblas_dcopy((int)N,&Xf[tN],1,F,1); cblas_dcopy((int)N,&Xo[tN],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[tN-N],1,o,C,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[tN-N],1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[tN-N],1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Xc[tN+n] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanh(Xc[nT]) / (1.0+exp(-Xi[nT]));
                Xc[nT] = tanh(H[n]) / (1.0+exp(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xc[t],(int)T,C,1); cblas_dcopy((int)N,&Xi[t],(int)T,I,1);
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1); cblas_dcopy((int)N,&Xo[t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[t-1],(int)T,o,C,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[t-1],(int)T,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[t-1],(int)T,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Xc[t+n*T] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                H[n] = tanh(Xc[nT]) / (1.0+exp(-Xi[nT]));
                Xc[nT] = tanh(H[n]) / (1.0+exp(-Xo[nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xc[t],(int)T,C,1); cblas_dcopy((int)N,&Xi[t],(int)T,I,1);
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1); cblas_dcopy((int)N,&Xo[t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[t-1],(int)T,o,C,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[t-1],(int)T,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[t-1],(int)T,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[t-1],(int)T,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Xc[t+n*T] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                H[n] = tanh(Xc[n]) / (1.0+exp(-Xi[n]));
                Xc[n] = tanh(H[n]) / (1.0+exp(-Xo[n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xc[tN],1,C,1); cblas_dcopy((int)N,&Xi[tN],1,I,1);
                cblas_dcopy((int)N,&Xf[tN],1,F,1); cblas_dcopy((int)N,&Xo[tN],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uc,(int)N,&Xc[tN-N],1,o,C,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,&Xc[tN-N],1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Xc[tN-N],1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,&Xc[tN-N],1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    H[n] = tanh(C[n])/(1.0+exp(-I[n])) + H[n]/(1.0+exp(-F[n]));
                    Xc[tN+n] = tanh(H[n]) / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm4_inplace_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#ifndef BAYESIAN_MERGE_H
#define BAYESIAN_MERGE_H

#include <vector>
#include <multi/GaussianDistribution.h>
#include <gsl/gsl_linalg.h>

using namespace std;

namespace openworld {
  class BayesianMerge {
  public:
    // modifies means in-place to assure maximum likelihood
    static void mergeGaussianHierarchy(vector<GaussianDistribution*> lows, GaussianDistribution* high) {
      vector<double> A, b;
      for (unsigned ii = 0; ii < lows.size(); ii++) {
        for (unsigned jj = 0; jj < lows.size(); jj++) {
          if (ii == jj)
            A.push_back(1 / lows[ii]->getVariance() + 1 / high->getVariance());
          else
            A.push_back(1 / high->getVariance());
        }

        b.push_back(lows[ii]->getMean() / lows[ii]->getVariance() + high->getMean() / high->getVariance());
      }

      gsl_matrix_view Amat = gsl_matrix_view_array(A.data(), lows.size(), lows.size());
      gsl_vector_view bmat = gsl_vector_view_array(b.data(), lows.size());
      gsl_vector* x = gsl_vector_alloc(lows.size());

      int s;

      gsl_permutation* p = gsl_permutation_alloc(lows.size());

      gsl_linalg_LU_decomp(&Amat.matrix, p, &s);
      gsl_linalg_LU_solve(&Amat.matrix, p, &bmat.vector, x);

      double meansum = 0;
      for (unsigned ii = 0; ii < lows.size(); ii++) {
        meansum += gsl_vector_get(x, ii);
        lows[ii]->setMean(gsl_vector_get(x, ii));
      }

      high->setMean(meansum);

      gsl_permutation_free(p);
      gsl_vector_free(x);
    }
  };
}

#endif


/**
 *
 * @file core_dpotrf.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:44:45 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_double
 *
 *  CORE_dpotrf - Computes the Cholesky factorization of a symmetric positive definite
 *  (or Hermitian positive definite in the complex case) matrix A.
 *  The factorization has the form
 *
 *    \f[ A = \{_{L\times L^H, if uplo = PlasmaLower}^{U^H\times U, if uplo = PlasmaUpper} \f]
 *
 *  where U is an upper triangular matrix and L is a lower triangular matrix.
 *
 *******************************************************************************
 *
 * @param[in] uplo
 *          = PlasmaUpper: Upper triangle of A is stored;
 *          = PlasmaLower: Lower triangle of A is stored.
 *
 * @param[in] N
 *          The order of the matrix A. N >= 0.
 *

 * @param[in,out] A
 *          On entry, the symmetric positive definite (or Hermitian) matrix A.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular part of A
 *          contains the upper triangular part of the matrix A, and the strictly
 *          lower triangular part of A is not referenced.
 *          If UPLO = 'L', the leading N-by-N lower triangular part of A
 *          contains the lower triangular part of the matrix A, and the strictly
 *          upper triangular part of A is not referenced.
 *          On exit, if return value = 0, the factor U or L from the Cholesky
 *          factorization A = U**T*U or A = L*L**T.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,N).
 *
 * @param[out] info
 *          - 0 on successful exit
 *          - <0 if -i, the i-th argument had an illegal value
 *          - >0 if i, the leading minor of order i of A is not positive
 *               definite, so the factorization could not be completed, and the
 *               solution has not been computed.
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_dpotrf = PCORE_dpotrf
#define CORE_dpotrf PCORE_dpotrf
#endif
void CORE_dpotrf(PLASMA_enum uplo, int N, double *A, int LDA, int *info)
{
    *info = LAPACKE_dpotrf_work(
        LAPACK_COL_MAJOR,
        lapack_const(uplo),
        N, A, LDA );
}