// An interface to GSL - hides all the gsl calls behind easier to use functions
// Jason Sanders
#ifndef GSLINTERFACE_H
#define GSLINTERFACE_H

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_permute.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_ellint.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_siman.h>
#include <stdlib.h>
#include <iostream>
//=================================================================================================
// RANDOM NUMBERS //
// random number generators - rand_uniform returns random numbers uniformly distributed in the
// interval [0,1], rand_gaussian returns gaussian distributed random numbers with sd = sigma

class rand_base{
	private:
		const gsl_rng_type * TYPE;
		unsigned long int seed;
    public:
       	gsl_rng * r;
     	rand_base(unsigned long int s){
     	// construct random number generator with seed s
     		seed = s;
     		gsl_rng_env_setup();
     	    TYPE = gsl_rng_default;
       		r = gsl_rng_alloc (TYPE);
       		gsl_rng_set(r, seed);
       		}
       	~rand_base(){gsl_rng_free (r);}
       	void reseed(unsigned long int newseed){
       	// give new seed
       		seed = newseed;
       		gsl_rng_set(r,newseed);
       		}
};

class rand_uniform:public rand_base{
	public:
		rand_uniform(unsigned long int SEED=0):rand_base(SEED){}
		~rand_uniform(){}
		// return uniformly distributed random numbers
		double nextnumber(){return gsl_rng_uniform (r);}
};

class rand_gaussian:public rand_base{
	public:
		double sigma;
		rand_gaussian(double s, unsigned long int SEED=0):rand_base(SEED){sigma = s;}
		~rand_gaussian(){}
		// return gaussian distributed random numbers
		double nextnumber(){return gsl_ran_gaussian (r,sigma);}
		void newsigma(double newsigma){sigma=newsigma;}
};

//=================================================================================================
// ROOT FINDING	  //
// finds root by Brent's method. Constructor initialises function and tolerances, findroot finds
// root in given interval. Function must be of form double(*func)(double,void*)

class root_find{
	private:
		int status;
       	const gsl_root_fsolver_type *T; gsl_root_fsolver *s;
       	gsl_function F;	double xlo,xhi,eps,root,tol;
       	int iter, max_iter;
    public:
    	root_find(double tol,int max_iter)
    		: tol(tol), max_iter(max_iter){
    		iter=0;eps=1.e-8;
       		T = gsl_root_fsolver_brent;
       		s = gsl_root_fsolver_alloc (T);
		}
		~root_find(){gsl_root_fsolver_free (s);}

    	void bracket(){
    		// crude bracketing routine. Expands interval till root found
			if(xhi<xlo){double xt = xlo; xlo=xhi; xhi=xt;}
			double diff = (xhi-xlo)/2.;
			while(GSL_FN_EVAL(&F,xlo)*GSL_FN_EVAL(&F,xhi)>0.){
				xlo-=diff;xhi+=diff;
			}
		}

     	double findroot(double(*func)(double,void *),double xlo1,double xhi1, void *p=NULL){
     		// finds root in interval [xlo1,xhi1]
    		F.function = func;F.params=p;xhi = xhi1; xlo = xlo1;
     	    if(GSL_FN_EVAL(&F,xlo)*GSL_FN_EVAL(&F,xhi)>0.){
     			bracket();
     		}
       		gsl_root_fsolver_set (s, &F, xlo, xhi);
			do{
				iter++;
				status = gsl_root_fsolver_iterate (s);
				root = gsl_root_fsolver_root (s);
				xlo = gsl_root_fsolver_x_lower (s);
				xhi = gsl_root_fsolver_x_upper (s);
				status = gsl_root_test_interval (xlo, xhi, tol, eps);
			  }
			while (status == GSL_CONTINUE && iter < max_iter);
       		return root;
		}

};

//=================================================================================================
// INTEGRATION //
// Simple 1d numerical integration using adaptive Gauss-Kronrod which can deal with singularities
// constructor takes function and tolerances and integrate integrates over specified region.
// integrand function must be of the form double(*func)(double,void*)
class integrator{
	private:
		gsl_integration_workspace *w;
		void *p;
		double result,err,eps;
		gsl_function F;
		size_t neval;
	public:
		integrator(double eps): eps(eps){
			w= gsl_integration_workspace_alloc (1000);
			F.params = &p;
       		}
		~integrator(){gsl_integration_workspace_free (w);}
		double integrate(double(*func)(double,void*),double xa, double xb){
		    F.function = func;
			gsl_integration_qags (&F, xa, xb, 0, eps, 1000,w, &result, &err);
			//gsl_integration_qng(&F, xa, xb, 0, eps, &result, &err, &neval);
			return result;
			}
		double error(){return err;}
};

inline double integrate(double(*func)(double,void*),double xa, double xb, double eps){
	double result,err; size_t neval;void *p;gsl_function F;F.function = func;F.params = &p;
	gsl_integration_qng(&F, xa, xb, 0, eps, &result, &err, &neval);
	return result;
}

class MCintegrator{
	private:
		gsl_monte_vegas_state *s;
		const gsl_rng_type *T;
 		gsl_rng *r;
 		size_t dim;
 	public:
 		MCintegrator(size_t Dim){
 			dim=Dim;
 			gsl_rng_env_setup ();
  			T = gsl_rng_default;
  			r = gsl_rng_alloc (T);
 			s=gsl_monte_vegas_alloc(Dim);
 		}
 		~MCintegrator(){
 			gsl_monte_vegas_free(s);
 			gsl_rng_free(r);
 		}
 		double integrate(double(*func)(double*,size_t,void*),double *xlow, double *xhigh,
 		size_t calls, double *err, int burnin=10000){
 			gsl_monte_function G = { func, dim, 0 }; double res;
 			if(burnin)gsl_monte_vegas_integrate(&G,xlow,xhigh,dim,burnin,r,s,&res,err);
 			gsl_monte_vegas_integrate(&G,xlow,xhigh,dim,calls,r,s,&res,err);
 			return res;
 		}
};

//=================================================================================================
// 1D INTERPOLATION //
// Interpolation using cubic splines

class interpolator{
	private:
		gsl_interp_accel *acc;
		gsl_spline *spline;
	public:
		interpolator(double *x, double *y, int n){
			acc = gsl_interp_accel_alloc();
			spline = gsl_spline_alloc(gsl_interp_cspline,n);
			gsl_spline_init (spline, x, y, n);
		}
		~interpolator(){
			gsl_spline_free (spline);
         	gsl_interp_accel_free (acc);
        }
        double interpolate(double xi){
        	return gsl_spline_eval (spline, xi, acc);
        }
        double derivative(double xi){
        	return gsl_spline_eval_deriv(spline, xi, acc);
        }
        void new_arrays(double *x, double *y,int n){
        	spline = gsl_spline_alloc(gsl_interp_cspline,n);
        	gsl_spline_init (spline, x, y, n);
        }
};

//=================================================================================================
// SORTING //
// sorting algorithm
// sort2 sorts first argument and then applies the sorted permutation to second list
class sorter{
	private:
		const gsl_rng_type * T;
       	gsl_rng * r;
    public:
    	sorter(){
    		gsl_rng_env_setup();
            T = gsl_rng_default;
     		r = gsl_rng_alloc (T);
     		}
     	~sorter(){gsl_rng_free (r);}
     	void sort(double *data, int n){
     		gsl_sort(data,1,n);
     	}
     	void sort2(double *data, int n, double *data2){
     		size_t p[n];
     		gsl_sort_index(p,data,1,n);
     		gsl_permute(p,data2,1,n);
     	}

};

//=================================================================================================
// ODE SOLVER //
// Simple ODE integrator using Runge-Kutta Dormand-Prince 8 adaptive stepping
// dy_i/dt = f_i(t) where int (*f)(double t, const double y, double f, void *params)

class ode{
	private:
		const gsl_odeiv2_step_type * T;
		gsl_odeiv2_step * s;
		gsl_odeiv2_control * c;
		gsl_odeiv2_evolve * e;
  		gsl_odeiv2_system sys;
		int N;double h,eps;
		double direction;
	public:
		ode(int (*derivs)(double,const double *,double*,void*),int N,double eps, void *params=NULL):N(N),eps(eps){
			sys.function=derivs;sys.jacobian=NULL;sys.dimension=N;sys.params=params;

			T = gsl_odeiv2_step_rk8pd;s = gsl_odeiv2_step_alloc (T, N);
			c = gsl_odeiv2_control_y_new (eps, 0.0);e = gsl_odeiv2_evolve_alloc (N);
			gsl_ieee_env_setup();
		}
		~ode(){
			gsl_odeiv2_evolve_free(e);
  			gsl_odeiv2_control_free(c);
  			gsl_odeiv2_step_free(s);
  		}
		void step(double tstart, double tfinish, double *y, double step){
			h = step;
			direction = (h>0?1.:-1.);
			while ((tfinish-tstart)*direction>0){
      			int status = gsl_odeiv2_evolve_apply (e, c, s, &sys, &tstart, tfinish, &h, y);
      			if (status != GSL_SUCCESS)break;
    		}
		}
};

//=================================================================================================
// MINIMISER //
// finds a minimum of a function of the form double(*func)(const gsl_vector *v, void *params)
// using a downhill simplex algorithm. Setup minimiser with initial guesses and required tolerance
// with constructor and then minimise with minimise().

class minimiser{
	private:
		const gsl_multimin_fminimizer_type *T; ;
		gsl_multimin_fminimizer *s;
		gsl_vector *ss, *x;
		gsl_multimin_function minex_func;
		size_t iter; int status,N_params; double size;
		double eps;
	public:
		minimiser(double(*func)(const gsl_vector *v, void *params),double *parameters,
		 int N, double *sizes, double eps):N_params(N), eps(eps){

			T = gsl_multimin_fminimizer_nmsimplex2rand;
			ss = gsl_vector_alloc (N_params);x = gsl_vector_alloc (N_params);
			for(int i=0;i<N_params;i++){
				gsl_vector_set (x, i, parameters[i]);gsl_vector_set(ss,i,sizes[i]);}

			minex_func.n = N; minex_func.f = func; minex_func.params = NULL;
			s = gsl_multimin_fminimizer_alloc (T, N_params);
			gsl_multimin_fminimizer_set (s, &minex_func, x, ss);
			status = 0; iter = 0;

		}

		~minimiser(){
			gsl_vector_free(x);
			gsl_vector_free(ss);
			gsl_multimin_fminimizer_free (s);
		}

		double minimise(double *results,unsigned int maxiter,bool vocal){
			do
			  {
				iter++; status = gsl_multimin_fminimizer_iterate(s);
				if(status)break;
				size = gsl_multimin_fminimizer_size (s);
				status = gsl_multimin_test_size (size, eps);
				if(vocal){	std::cout<<iter<<" ";
							for(int i=0; i<N_params;i++)std::cout<<gsl_vector_get(s->x,i)<<" ";
							std::cout<<s->fval<<" "<<size<<std::endl;
							}
			}
			while (status == GSL_CONTINUE && iter < maxiter);
			for(int i=0;i<N_params;i++){results[i] = gsl_vector_get(s->x,i);}
			return s->fval;
		}
};

class minimiser1D{
	private:
		const gsl_min_fminimizer_type *T; ;
		gsl_min_fminimizer *s;
		size_t iter; int status; double size;
		double m, a, b, eps;
	public:
		minimiser1D(double(*func)(double, void *params), double m, double a, double b, double eps)
			:m(m), a(a), b(b), eps(eps){

			gsl_function F;F.function = func;F.params = 0;
			T = gsl_min_fminimizer_brent;
			s = gsl_min_fminimizer_alloc (T);
			gsl_min_fminimizer_set (s, &F, m, a, b);
			status = 0; iter = 0;
		}
		~minimiser1D(){
			gsl_min_fminimizer_free (s);
		}
		double minimise(unsigned int maxiter){
			do
			  {
				iter++;
				status = gsl_min_fminimizer_iterate(s);
				m = gsl_min_fminimizer_x_minimum (s);
           		a = gsl_min_fminimizer_x_lower (s);
           		b = gsl_min_fminimizer_x_upper (s);
				status = gsl_min_test_interval (a, b, eps, 0.0);
			}
			while (status == GSL_CONTINUE && iter < maxiter);
			return m;
		}
};
/*
double Distance(void *xp, void *yp){
       double x = *((double *) xp);
       double y = *((double *) yp);
       return fabs(x - y);
}

void Step(const gsl_rng * r, void *xp, double step_size){
    double old_x = *((double *) xp);
    double new_x;

    double u = gsl_rng_uniform(r);
    new_x = u * 2 * step_size - step_size + old_x;

    memcpy(xp, &new_x, sizeof(new_x));
}

void Print(void *xp){
    printf ("%12g", *((double *) xp));
}

class sim_anneal{
	private:
		const gsl_rng_type * T;
    	gsl_rng * r;
    	int N_TRIES, ITER_FIXED_T;
    	double STEP_SIZE, K, T_INITIAL, MU_T, T_MIN;
    	gsl_siman_params_t params;
    public:
    	sim_anneal(int N_TRIES, int ITER_FIXED_T, double STEP_SIZE):
    	N_TRIES(N_TRIES), ITER_FIXED_T(ITER_FIXED_T),STEP_SIZE(STEP_SIZE){
    		gsl_rng_env_setup();
            T = gsl_rng_default;
     	  	r = gsl_rng_alloc(T);
     	  	//params[0]=N_TRIES;params[1]=ITER_FIZED_T;params[2]=STEP_SIZE;
     	  	//K=1.; params[3]=K; T_INITIAL=0.008; params[4]=T_INITIAL;
     	  	//MU_T=1.003; params[5]=MU_T; T_MIN=2.0e-6; params[6]=T_MIN;
     	  	gsl_siman_params_t params
       = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
          K, T_INITIAL, MU_T, T_MIN};
    	}
    	~sim_anneal(){
    		gsl_rng_free(r);
    	}
    	double minimise(double(*func)(void *xp), double x){
    		double x_initial=x;
    		gsl_siman_solve(r, &x_initial, &func, Step, Distance, Print,
                       		NULL, NULL, NULL,
                       		sizeof(double), params);
    		return x_initial;
    	}
};*/

//=================================================================================================
// SPECIAL FUNCTIONS //
inline double erf(double x){return gsl_sf_erf (x);}
inline double erfc(double x){return gsl_sf_erfc (x);}
inline double besselI(double x, int n){return gsl_sf_bessel_In (n,x);}
inline double besselJ(double x, int n){return gsl_sf_bessel_Jn (n,x);}
inline double gamma(double x){return gsl_sf_gamma (x);}
inline double ellint_first(double phi, double k){ return gsl_sf_ellint_F(phi,k,1e-15);}
// F(\phi,k) = \int_0^\phi \d t \, \frac{1}{\sqrt{1-k^2\sin^2 t}}
inline double ellint_second(double phi, double k){ return gsl_sf_ellint_E(phi,k,1e-15);}
// E(\phi,k) = \int_0^\phi \d t \, \sqrt{1-k^2\sin^2 t}
inline double ellint_third(double phi, double k, double n){ return gsl_sf_ellint_P(phi,k,n,1e-15);}
// \Pi(\phi,k,n) = \int_0^\phi \d t \, \frac{1}{(1+n\sin^2 t)\sqrt{1-k^2\sin^2 t}}
#endif


/* examples/C/ssmfe/precond_core.f90 */
/* Laplacian on a square grid (using SPRAL_SSMFE_CORE routines) */
#include "spral.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <cblas.h>

/* Header that implements Laplacian and preconditioners */
#include "laplace2d.h"

int main(void) {
   const int ngrid = 20;         /* grid points along each side */
   const int n = ngrid*ngrid;    /* problem size */
   const int nep = 5;            /* eigenpairs wanted */
   const int m = 3;              /* dimension of the iterated subspace */
   const double tol = 1.e-6;     /* eigenvector tolerance */

   int state = SPRAL_RANDOM_INITIAL_SEED; /* PRNG state */

   int ind[m];                   /* permutation index */
   double lambda[n];             /* eigenvalues */
   double X[n][n];               /* eigenvectors */
   /* Work arrays */
   double lmd[m];
   double rr[3][2*m][2*m];
   double W[7][m][n];
   double U[m][n];

   /* Derived types */
   struct spral_ssmfe_rcid rci;              /* reverse communication data */
   struct spral_ssmfe_core_options options;  /* options */
   void *keep;                               /* private data */
   struct spral_ssmfe_inform inform;         /* information */

   /* Initialize options to default values */
   spral_ssmfe_core_default_options(&options);

   /* Initialize W to lin indep vectors by randomizing */
   for(int i=0; i<n; i++)
   for(int j=0; j<m; j++)
      W[0][j][i] = spral_random_real(&state, true);

   int ncon = 0;        /* number of converged eigenpairs */
   rci.job = 0; keep = NULL;
   while(true) { /* reverse communication loop */
      spral_ssmfe_double(&rci, 0, nep, 0, m, lmd, &rr[0][0][0], ind,
         &keep, &options, &inform);
      switch ( rci.job ) {
      case 1:
         apply_laplacian(
            ngrid, ngrid, rci.nx, &W[rci.kx][rci.jx][0], &W[rci.ky][rci.jy][0]
            );
         break;
      case 2:
         apply_gauss_seidel_step (
            ngrid, ngrid, rci.nx, &W[rci.kx][rci.jx][0], &W[rci.ky][rci.jy][0]
            );
         break;
      case 4:
         for(int j=0; j<m; j++) {
            if ( inform.converged[j] != 0 ) continue;
            if ( inform.err_X[j] > 0 && inform.err_X[j] < tol )
               inform.converged[j] = 1;
         }
         break;
      case 5:
         if ( rci.i < 0 ) continue;
         for(int k=0; k<rci.nx; k++) {
           int j = ncon + k;
           lambda[j] = lmd[rci.jx + k];
           cblas_dcopy( n, &W[0][rci.jx+k][0], 1, &X[j][0], 1 );
         }
         ncon += rci.nx;
         if ( ncon >= nep || inform.iteration > 300 ) goto finished;
         break;
      case 11:
         if ( rci.i == 0 ) {
            if ( rci.kx != rci.ky || rci.jx > rci.jy ) {
               cblas_dcopy(n*rci.nx, &W[rci.kx][rci.jx][0], 1, &W[rci.ky][rci.jy][0], 1);
            } else if ( rci.jx < rci.jy ) {
               for(int j=rci.nx-1; j>=0; j--)
                  cblas_dcopy(n, &W[rci.kx][rci.jx+j][0], 1, &W[rci.ky][rci.jy+j][0], 1);
            }
         } else {
            for(int i=0; i<n; i++) {
               for(int j=0; j<rci.nx; j++)
                  U[j][i] = W[rci.kx][ind[j]][i];
               for(int j=0; j<rci.nx; j++)
                  W[rci.kx][j][i] = U[j][i];
               if(rci.ky != rci.kx) {
                  for(int j=0; j<rci.nx; j++)
                    U[j][i] = W[rci.ky][ind[j]][i];
                  for(int j=0; j<rci.nx; j++)
                    W[rci.ky][j][i] = U[j][i];
               }
            }
         }
         break;
      case 12:
         for(int i=0; i<rci.nx; i++)
           rr[rci.k][rci.j+i][rci.i+i] =
             cblas_ddot(n, &W[rci.kx][rci.jx+i][0], 1, &W[rci.ky][rci.jy+i][0], 1);
         break;
      case 13:
         for(int i=0; i<rci.nx; i++) {
            if( rci.kx == rci.ky ) {
               double s = cblas_dnrm2(n, &W[rci.kx][rci.jx+i][0], 1);
               if( s > 0 )
                  cblas_dscal(n, 1/s, &W[rci.kx][rci.jx+i][0], 1);
            } else {
               double s = sqrt(fabs(cblas_ddot(
                  n, &W[rci.kx][rci.jx+i][0], 1, &W[rci.ky][rci.jy+i][0], 1)
                  ));
               if ( s > 0 ) {
                  cblas_dscal(n, 1/s, &W[rci.kx][rci.jx+i][0], 1);
                  cblas_dscal(n, 1/s, &W[rci.ky][rci.jy+i][0], 1);
               } else {
                  for(int j=0; j<n; j++)
                     W[rci.ky][rci.jy+i][j] = 0.0;
               }
            }
         }
         break;
      case 14:
         for(int i=0; i<rci.nx; i++) {
           double s = -rr[rci.k][rci.j+i][rci.i+i];
           cblas_daxpy(n, s, &W[rci.kx][rci.jx+i][0], 1, &W[rci.ky][rci.jy+i][0], 1);
         }
         break;
      case 15:
         if ( rci.nx > 0 && rci.ny > 0 )
            cblas_dgemm(
               CblasColMajor, CblasTrans, CblasNoTrans, rci.nx, rci.ny, n,
               rci.alpha, &W[rci.kx][rci.jx][0], n, &W[rci.ky][rci.jy][0], n,
               rci.beta, &rr[rci.k][rci.j][rci.i], 2*m
               );
         break;
      case 16: // Fall through to 17
      case 17:
         if( rci.ny < 1 ) continue;
         if( rci.nx < 1 ) {
            if( rci.job == 17 ) continue;
            if( rci.beta == 1.0 ) continue;
            for(int j=rci.jy; j<rci.jy+rci.ny; j++)
               cblas_dscal(n, rci.beta, &W[rci.ky][j][0], 1);
            continue;
         }
         if( rci.job == 17 ) {
            cblas_dgemm(
               CblasColMajor, CblasNoTrans, CblasNoTrans, n, rci.ny, rci.nx,
               1.0, &W[rci.kx][rci.jx][0], n, &rr[rci.k][rci.j][rci.i], 2*m,
               0.0, &W[rci.ky][rci.jy][0], n
               );
            cblas_dcopy(n*rci.ny, &W[rci.ky][rci.jy][0], 1, &W[rci.kx][rci.jx][0], 1);
         } else {
            cblas_dgemm(
               CblasColMajor, CblasNoTrans, CblasNoTrans, n, rci.ny, rci.nx,
               rci.alpha, &W[rci.kx][rci.jx][0], n, &rr[rci.k][rci.j][rci.i],
               2*m, rci.beta, &W[rci.ky][rci.jy][0], n
               );
         }
         break;
      case 21: // Fall through to 22
      case 22:
         if( ncon > 0 ) {
            cblas_dgemm(
               CblasColMajor, CblasTrans, CblasNoTrans, ncon, rci.nx, n,
               1.0, &X[0][0], n, &W[rci.ky][rci.jy][0], n, 0.0, &U[0][0], n
               );
            cblas_dgemm(
               CblasColMajor, CblasNoTrans, CblasNoTrans, n, rci.nx, ncon,
               -1.0, &X[0][0], n, &U[0][0], n, 1.0, &W[rci.kx][rci.jx][0], n
               );
         }
         break;
      default:
         goto finished;
      }
   }
finished:
   if(inform.flag != 0) printf("inform.flag = %d\n", inform.flag);
   printf("%3d eigenpairs converged in %d iterations\n", ncon, inform.iteration);
   for(int i=0; i<ncon; i++)
      printf(" lambda[%1d] = %13.7e\n", i, lambda[i]);
   spral_ssmfe_core_free(&keep, &inform);

   /* Success */
   return 0;
}


#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_gegenbauer.h>
#include "scf.h"

/* Given the coefficients stored mat_cos and mat_sin, this code does
 * the summation to compute the potential and accelerations
 *
 * scf_potential() is the full 3D treatment for the host halo
 *
 * scf_potential_spherical() is the spherically symmetric treatment
 * for the subhalos. This spherical code skips the loops over l,m
 * so it's simpler.
 */

float scf_potential(float* a, double* mat_cos, double* mat_sin, int orbit_nmax, int orbit_lmax, float x, float y, float z)
{
    double x2 = (double)x * (double)x;
    double y2 = (double)y * (double)y;
    double r = sqrt(x2+y2+(double)z*(double)z);
    double r_xy = sqrt(x2+y2);
    double phi = atan2((double)y, (double)x);

    double sin_theta = r_xy / r;
    double sin_phi = (double)y / r_xy;
    double cos_theta = (double)z / r;
    double cos_phi = (double)x / r_xy;

    double cos_theta_for_Plm = cos_theta;

#ifdef USE_GSL_GEGENBAUER_HOST
    double xi = (r-1.0)/(r+1.0);
#endif

    double a_r = 0.0;
    double a_t = 0.0;
    double a_p = 0.0;

    int n,l,m;

    float C_lm = 0.0f;
    float D_lm = 0.0f;
    float E_lm = 0.0f;
    float F_lm = 0.0f;

    float this_Phi_nl = 0.f;
    float this_dPhi_nl_dr = 0.f;

#ifdef USE_GSL_GEGENBAUER_HOST
    double* Cna_2l_32_array = (double*)malloc((orbit_nmax+1)*sizeof(double));
    double* Cna_2l_52_array = (double*)malloc(orbit_nmax*sizeof(double));
#endif

    double this_P_lm = 0.0;
    double this_dP_lm = 0.0;
    double* P_l;
    double* dP_l;
    int P_size;

    float cos_mphi;
    float sin_mphi;

    float pot = 0.f;
    a[0] = 0.f;
    a[1] = 0.f;
    a[2] = 0.f;

    for(m=0; m<=orbit_lmax; m++)
    {
        cos_mphi = cos(m*phi);
        sin_mphi = sin(m*phi);

        P_size = orbit_lmax - m + 1;
        P_l = (double*)malloc(P_size*sizeof(double));
        dP_l = (double*)malloc(P_size*sizeof(double));

        gsl_sf_legendre_Plm_deriv_array(orbit_lmax, m, cos_theta_for_Plm, P_l, dP_l);

        for(l=m; l<=orbit_lmax; l++)
        {
            C_lm = 0.f;
            D_lm = 0.f;
            E_lm = 0.f;
            F_lm = 0.f;

#ifdef USE_GSL_GEGENBAUER_HOST
            gsl_sf_gegenpoly_array(orbit_nmax, 2*l+1.5, xi, Cna_2l_32_array);
            if(orbit_nmax > 0)
                gsl_sf_gegenpoly_array(orbit_nmax-1, 2*l+2.5, xi, Cna_2l_52_array);
#endif

            for(n=0; n<=orbit_nmax; n++)
            {
#ifdef USE_GSL_GEGENBAUER_HOST
                this_Phi_nl = Phi_nl(Cna_2l_32_array[n], l, r);
                if(n>0)
                    this_dPhi_nl_dr = dPhi_nl_dr(Cna_2l_52_array[n-1], this_Phi_nl, n, l, r);
                else
                    this_dPhi_nl_dr = dPhi_nl_dr(1e99, this_Phi_nl, n, l, r);
#else
                this_Phi_nl = Phi_nl(n,l,r);
                this_dPhi_nl_dr = dPhi_nl_dr(this_Phi_nl, n, l, r);
#endif

                C_lm += mat_cos[ind(n,l,m)] * this_Phi_nl;
                D_lm += mat_sin[ind(n,l,m)] * this_Phi_nl;

                E_lm += mat_cos[ind(n,l,m)] * this_dPhi_nl_dr;
                F_lm += mat_sin[ind(n,l,m)] * this_dPhi_nl_dr;
            }

            this_P_lm = P_l[l-m];
            this_dP_lm = dP_l[l-m] * -sin_theta;

            pot += this_P_lm * (C_lm*cos_mphi + D_lm*sin_mphi);

            a_r += this_P_lm * (E_lm*cos_mphi + F_lm*sin_mphi);
            a_t += this_dP_lm * (C_lm*cos_mphi + D_lm*sin_mphi);
            a_p += m * this_P_lm * (D_lm*cos_mphi - C_lm*sin_mphi);

        } // l loop

        free(P_l);
        free(dP_l);

    } // m loop

    a_r = -a_r;
    a_t = -a_t/r;
    a_p = -a_p/r_xy;

    a[0] = a_r*sin_theta*cos_phi + a_t*cos_theta*cos_phi - a_p*sin_phi;
    a[1] = a_r*sin_theta*sin_phi + a_t*cos_theta*sin_phi + a_p*cos_phi;
    a[2] = a_r*cos_theta - a_t*sin_theta;

    a[0] *= G;
    a[1] *= G;
    a[2] *= G;
    pot *= G;

#ifdef USE_GSL_GEGENBAUER_HOST
    free(Cna_2l_32_array);
    free(Cna_2l_52_array);
#endif

    return pot;

}

float scf_potential_spherical(float* a, float* mat_cos, int orbit_nmax, float x, float y, float z)
{
    float x2 = x*x;
    float y2 = y*y;

    float r = sqrt(x2+y2+z*z);
    float r_xy = sqrt(x2+y2);

#ifdef USE_GSL_GEGENBAUER_SUBHALO
    float xi = (r-1.0f)/(r+1.0f);
#endif

    float sin_theta = r_xy / r;
    float sin_phi = y / r_xy;
    float cos_theta = z / r;
    float cos_phi = x / r_xy;

    float this_Phi_n0 = 0.f;
    float this_dPhi_n0_dr = 0.f;

#ifdef USE_GSL_GEGENBAUER_SUBHALO
    double* Cna_32_array = (double*)malloc((orbit_nmax+1)*sizeof(double));
    double* Cna_52_array = (double*)malloc(orbit_nmax*sizeof(double));
    gsl_sf_gegenpoly_array(orbit_nmax, 1.5, xi, Cna_32_array);
    gsl_sf_gegenpoly_array(orbit_nmax-1, 2.5, xi, Cna_52_array);
#endif

    float C_lm = 0.0f;
    float E_lm = 0.0f;

    int n;
    for(n=0; n<=orbit_nmax; n++)
    {
#ifdef USE_GSL_GEGENBAUER_SUBHALO
        this_Phi_n0 = Phi_n0(Cna_32_array[n], r);
        if(n>0)
            this_dPhi_n0_dr = dPhi_n0_dr(Cna_52_array[n-1], this_Phi_n0, n, r);
        else
            this_dPhi_n0_dr = dPhi_n0_dr(1e99, this_Phi_n0, n, r);
#else
        this_Phi_n0 = Phi_n0(n,r);
        this_dPhi_n0_dr = dPhi_n0_dr(this_Phi_n0, n, r);
#endif

        C_lm += mat_cos[n] * this_Phi_n0;

        E_lm += mat_cos[n] * this_dPhi_n0_dr;
    }

    float pot = C_lm;

    float a_r = -E_lm;

    a[0] = G * a_r*sin_theta*cos_phi;
    a[1] = G * a_r*sin_theta*sin_phi;
    a[2] = G * a_r*cos_theta;

    pot = G * pot;

#ifdef USE_GSL_GEGENBAUER_SUBHALO
    free(Cna_32_array);
    free(Cna_52_array);
#endif

    return pot;

}


/*
 * dgl.c -- Loesung einer DGL mit der GNU scientific library
 *
 * (c) 2016 Prof Dr Andreas Mueller, Hochschule Rapperswil
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

static long	evalcounter = 0;

int	f(double x, const double y[], double f[], void *params) {
	evalcounter++;
	double	omega = *(double *)params;
	f[0] = y[1];
	f[1] = -y[0] + sin(omega * x);
	return GSL_SUCCESS;
}

static inline double	sqr(double x) { return x * x; }

int	main(int argc, char *argv[]) {
	double	omega = 0.1;
	gsl_odeiv2_system	sys = { f, NULL, 2, &omega };
	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&sys, gsl_odeiv2_step_rkf45,
			1e-6, 1e-6, 0.0);
	double	x = 0.0;
	double	y[2] = { 0.0, 0.0 };
	long	lastcounter = evalcounter;
	for (int i = 1; i <= 10000; i++) {
		double	xnext = i;
		int	status = gsl_odeiv2_driver_apply(driver, &x, xnext, y);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "error: return value = %d\n", status);
		}
		double yexakt = (omega * sin(x) - sin(omega * x))
					/ (sqr(omega) - 1);
		double delta = y[0] - yexakt;
		long	evaldensity = evalcounter - lastcounter;
		lastcounter = evalcounter;
		if ((i < 10) ||
			((i < 100) && (0 == i % 10)) ||
			((i < 1000) && (0 == i % 100)) ||
			((i < 10000) && (0 == i % 1000)) ||
			((i < 100000) && (0 == i % 10000))) {
			printf("%5.0f& %12.8f& %12.8f& %12.8f&%ld\\\\\n",
				 x,    y[0],  yexakt, delta, evaldensity);
		}
	}
	return EXIT_SUCCESS;
}


#include "AFS.h"
#include "cs.h"
#include "AFS_ctmc.h"
#include "sparseExp.h"
#include "adkCSparse.h"
#include "adkGSL.h"
//#include "cuba.h"
#include <gsl/gsl_blas.h>

void fillExpectedAFS_unnorm(afsStateSpace *S, double *visitMat ,gsl_vector *rates, gsl_matrix *expAFS);

//fills a sparse transition matrix for Continuous Time model
struct cs_di_sparse *fillTransitionMatrix_ctmc(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2){
	int i, j,k;
	int N = S->nstates;
	double  c0r[N],c1r[N],m0r[N],m1r[N],totR[N],rowSums[N], tmp;;

	struct cs_di_sparse *triplet;

	//now allocate new cs_sparse obj
	triplet = cs_spalloc(N, N, *nzCount + N , 1, 1); //alloc sparse mat with extra space for nonzero identity mats
	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1) / theta1 ;
		c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1) / theta2;
		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];
		rowSums[i] = 0.0;
	}

	for(k=0;k<*nzCount;k++){
		i = dim1[k];
		j= dim2[k];
		switch(moveType[k]){
			case 0: //coal pop0
			tmp = c0r[i] / totR[i] * topol[k];
			cs_entry(triplet,i,j,tmp);
			rowSums[i] += tmp;
			break;
			case 1: //coal pop1
			tmp = c1r[i] / totR[i] * topol[k];
			cs_entry(triplet,i,j,tmp);
			rowSums[i] += tmp;
			break;
			case 2: //mig pop0
			tmp = m0r[i] / totR[i] * topol[k];
			cs_entry(triplet,i,j,tmp);
			rowSums[i] += tmp;
			break;
			case 3: //mig pop1
			tmp = m1r[i] / totR[i] * topol[k];
			cs_entry(triplet,i,j,tmp);
			rowSums[i] += tmp;
			break;
		}
	}
	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
	 		cs_entry(triplet,k,k,0-rowSums[k]);
			*nzCount += 1;
		}
	}
//	cs_dropzeros(triplet);
//	tmat = cs_compress(triplet);
//	cs_spfree(triplet);
	return(triplet);

}

//this only fills a new set of arrays
void fillTransitionMatrixArray_ctmc(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2,double *newArray,
	int *newDim1, int *newDim2){
	int i, j,k, newnz;
	int N = S->nstates;
	double  c0r[N],c1r[N],m0r[N],m1r[N],totR[N],rowSums[N], tmp;;

	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1) / theta1 ;
		c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1) / theta2;
		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];
		rowSums[i] = 0.0;
	}
	newnz=0;
	for(k=0;k<*nzCount;++k){
		i = dim1[k];
		j= dim2[k];
		switch(moveType[k]){
			case 0: //coal pop0
			tmp = c0r[i] / totR[i] * topol[k];
			newArray[k]=tmp;
			newDim1[k] = i+1;
			newDim2[k] = j+1;
			rowSums[i] += tmp;
			newnz++;
			break;
			case 1: //coal pop1
			tmp = c1r[i] / totR[i] * topol[k];
			newArray[k]=tmp;
			newDim1[k] = i+1;
			newDim2[k] = j+1;
			rowSums[i] += tmp;
			newnz++;
			break;
			case 2: //mig pop0
			tmp = m0r[i] / totR[i] * topol[k];
			newArray[k]=tmp;
			newDim1[k] = i+1;
			newDim2[k] = j+1;
			rowSums[i] += tmp;
			newnz++;
			break;
			case 3: //mig pop1
			tmp = m1r[i] / totR[i] * topol[k];
			newArray[k]=tmp;
			newDim1[k] = i+1;
			newDim2[k] = j+1;
			rowSums[i] += tmp;
			newnz++;
			break;
		}
	}

	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
			newArray[newnz]=0-rowSums[k];
			newDim1[newnz] = k+1;
			newDim2[newnz] = k+1;
			newnz++;
		}
	}

	//sanity check
//	for(k=0;k<newnz;k++){
//		printf("row: %d col: %d val: %g\n",newDim1[k],newDim2[k],newArray[k]);
//	}
	*nzCount = newnz;
}

//Functions for jointly computing CTMC transitions and embedded chain

//this function returns pointer to embedded Discrete MC trans Mat in sparse form, while filling rates vector and filling
// arrays for CTMC representation
struct cs_di_sparse * fillTransitionMatrixArray_embed_ctmc(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2,double *newArray,
	int *newDim1, int *newDim2, gsl_vector *rates){
	int i, j,k, newnz, abcount=0;
	int N = S->nstates,abFlag;
	double  c0r[N],c1r[N],m0r[N],m1r[N],totR[N],rowSums[N], tmp;;
	struct cs_di_sparse *triplet, *tmat;

	gsl_vector_set_zero(rates);
	//allocate new cs_sparse obj
	triplet = cs_spalloc(N, N, *nzCount + N , 1, 1); //alloc sparse mat with extra space for nonzero identity mats

	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1)  / theta1 ;
	//	printf("co check:%f\n",c0r[i]);
		if(theta2 == 0)
			c1r[i] = 0.0;
		else
			c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1)  / theta2;
		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];

		rowSums[i] = 0.0;
		if(S->states[i]->nalleles > 1  && c0r[i] >= 0 && c1r[i] >= 0 && totR[i] != 0.0)
			gsl_vector_set(rates,i,1.0/totR[i]);
		else
			gsl_vector_set(rates,i,0);
	}

	newnz=0;
	for(k=0;k<*nzCount;k++){
		i = dim1[k];
		j= dim2[k];
//		printf("i: %d, j:%d, movetype: %d\n",i,j,moveType[k]);
		if(S->states[i]->nalleles ==1 || S->states[j]->nalleles ==1 )abFlag=1;
	//	if(S->states[i]->nalleles ==1 )abFlag=1;
		else abFlag = 0;
		switch(moveType[k]){
			case 0: //coal pop0
			if(totR[i]==0)tmp=0;
			else tmp = c0r[i] / totR[i] * topol[k];
			newArray[k]=tmp* totR[i];
			newDim1[k] = i;
			newDim2[k] = j;
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 1: //coal pop1
			if(totR[i]==0)tmp=0;
			else tmp = c1r[i] / totR[i] * topol[k];
			newArray[k]=tmp* totR[i];
			newDim1[k] = i;
			newDim2[k] = j;
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 2: //mig pop0
			if(totR[i]==0)tmp=0;
			else tmp = m0r[i] / totR[i] * topol[k];
			newArray[k]=tmp* totR[i];
			newDim1[k] = i;
			newDim2[k] = j;
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 3: //mig pop1
			if(totR[i]==0)tmp=0;
			else tmp = m1r[i] / totR[i] * topol[k];
			newArray[k]=tmp * totR[i];
			newDim1[k] = i;
			newDim2[k] = j;
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case -1:
			abcount+=1;
			break;

		}
	}
	newnz=*nzCount ;
	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
			newArray[newnz]=0.0-rowSums[k];
			newDim1[newnz] = k;
			newDim2[newnz] = k;
			newnz++;
		}
	}

	tmat = cs_compress(triplet);
	cs_spfree(triplet);
	*nzCount = newnz;
	return(tmat);
}

//fills up an AFS matrix using expected times and a precalculated visitMat
void fillExpectedAFS_ctmc(afsStateSpace *S, double *visitMat, gsl_matrix *expAFS){
	int  j, k, l, m1, m2, count;

	double tmpRes;

	m1=expAFS->size1;
	m2=expAFS->size2;

	gsl_matrix_set_zero(expAFS);

	for(j=0;j<S->nstates;j++){
		if(S->states[j]->nalleles != 1){
			for(k=0;k<m1;k++){
			for(l=0;l<m2;l++){
				count = gsl_matrix_int_get(S->states[j]->popMats[0],k,l) +  gsl_matrix_int_get(S->states[j]->popMats[1],k,l);
				tmpRes = gsl_matrix_get(expAFS,k,l) + (count * visitMat[j]);
				gsl_matrix_set(expAFS,k,l, tmpRes);
				}
			}
		}
	}
//	tmpRes = matrixSumDouble(expAFS);
//	gsl_matrix_scale(expAFS, 1.0 / tmpRes);
}

//fills up an AFS matrix using expected times and a precalculated visitMat
void fillExpectedAFS_unnorm(afsStateSpace *S, double *visitMat ,gsl_vector *rates, gsl_matrix *expAFS){
	int  j, k, l, m1, m2, count;

	double tmpRes;

	m1=expAFS->size1;
	m2=expAFS->size2;

	gsl_matrix_set_zero(expAFS);
	for(j=0;j<S->nstates;j++){
		for(k=0;k<m1;k++){
			for(l=0;l<m2;l++){
				count = gsl_matrix_int_get(S->states[j]->popMats[0],k,l) +  gsl_matrix_int_get(S->states[j]->popMats[1],k,l);
				tmpRes = gsl_matrix_get(expAFS,k,l) + (count * gsl_vector_get(rates,j) * visitMat[j]);
//				printf("tmpRes[%d][%d]: %f rates[j]: %f visits[j]: %f count: %d\n",tmpRes,k,l,gsl_vector_get(rates,j),visitMat[j],count);
				gsl_matrix_set(expAFS,k,l, tmpRes);
			}
		}
	}
}

//fills up an AFS matrix using expected times and a precalculated visitMat
void fillExpectedAFS_unnorm_mat(afsStateSpace *S, double **visitMat , double *initialStateProbs, gsl_vector *rates, gsl_matrix *expAFS){
	int  j, k, l,i, m1, m2, count;

	double tmpRes,sum;

	m1=expAFS->size1;
	m2=expAFS->size2;

	gsl_matrix_set_zero(expAFS);
	for(j=0;j<S->nstates;j++){
		for(k=0;k<m1;k++){
			for(l=0;l<m2;l++){
				count = gsl_matrix_int_get(S->states[j]->popMats[0],k,l) +  gsl_matrix_int_get(S->states[j]->popMats[1],k,l);
				sum=0.0;
				for(i=0;i<S->nstates;i++){
					sum += visitMat[i][j] * initialStateProbs[i];
				}
				tmpRes	= gsl_matrix_get(expAFS,k,l) + (count * gsl_vector_get(rates,j) * sum);
			//	printf("tmpRes[%d][%d]: %f rates[j]: %f visits[j]: %f count: %d\n",k,l,tmpRes,gsl_vector_get(rates,j),sum,count);
				gsl_matrix_set(expAFS,k,l, tmpRes);
			}
		}
	}
}

/// I believe the below is now completely unused
//fillLogAFS -- uses cxsparse library for inversion -- will replace above
// void fillLogAFS(void * p){
// 	struct im_lik_params * params = (struct im_lik_params *) p;
// 	struct expoMatObj anExpoMatObj;
// 	int i,j, N = params->stateSpace->nstates;
//  	cs *spMat, *mt, *ident, *eye, *tmpMat ,*tmpMat2;
// 	double timeV, sum, thetaA, theta2, mig1, mig2;
// 	int Na;
// 	gsl_vector *tmpStates;
//
//
// 	//initialize some vectors
// 	gsl_vector_set_zero(params->rates);
// 	tmpStates = gsl_vector_alloc(N);
//
// 	//For straight MLE the paramVector takes the form [N2,NA,m1,m2,t]
// 	theta2 = gsl_vector_get(params->paramVector,0);
// 	thetaA = gsl_vector_get(params->paramVector,1);
// 	mig1 = gsl_vector_get(params->paramVector,2);
// 	mig2 = gsl_vector_get(params->paramVector,3);
// 	timeV = gsl_vector_get(params->paramVector,4);
// //	printf("params-> %f %f %f %f %f\n",theta2,thetaA,mig1,mig2,timeV);
// 	//fill transMat
// 	tmpMat= fillTransitionMatrixArray_embed_ctmc(params->stateSpace, params->topA, params->moveA,
// 	 	&(params->nnz),params->dim1, params->dim2,1.0, theta2, mig1, mig2,
// 	 	params->new, params->newdim1, params->newdim2,params->rates);
//
// 	//using CSparse
// 	// S = (I-P)^-1
//
// 	//add negative ident
// 	ident = cs_spalloc(N,N,N,1,1);
// 	for(i=0;i<N;i++) cs_entry(ident,i,i,1);
// 	eye = cs_compress(ident);
// 	spMat = cs_add(eye,tmpMat,1.0,-1.0);
// 	mt = cs_transpose(spMat,1);
//
// 	////Compute Entire Inverse Mat
// 	for(j=0;j<N;j++){
// 		//create unit array for solve
// 		for(i=0; i<N; i++)params->b[i] = 0.0;
// 		params->b[j]=1.0;
// 		cs_lusol(0, mt, params->b, 1e-12);
// 		for(i=0; i<N; i++){
// 			params->invMat[j][i]=params->b[i];
// 			gsl_matrix_set(params->invMatGSL,j,i,params->b[i]);
// 		}
// 		printf("inv mat row %d complete\n",j);
// 	}
//
// 	//Get Island Time 0-INF Unnormal
// 	gsl_matrix_set_zero(params->expAFS);
// 	for(i=0; i<N; i++)params->b[i] = params->invMat[0][i];
// 	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS);
// 	// printf("////////////////////Island Time 0 - INF unnormalized\n");
// 	// for(i=0;i< (params->expAFS->size1) ;i++){
// 	// 	for(j=0;j< (params->expAFS->size2);j++){
// 	// 		printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
// 	// 	}
// 	// 	printf("\n");
// 	// }
//
// 	//Matrix Exponentiation to get state vector at time t
// 	//
//
// 	//push indices
// 	for(i=0;i<params->nnz;i++){
// 		params->expodim1[i]=params->newdim1[i]+1;
// 		params->expodim2[i]=params->newdim2[i]+1;
//
// 	}
// 	//reset expoInit
// 	for(i = 0;i<N;i++){
// 		params->expoInit[i]=0.0;
// 		params->expoResult[i]=0.0;
// 	}
// 	params->expoInit[0] = 1.0;
//
// 	//sanity check ///////
// //	for(i=0;i<params->nnz;i++){
// //		printf("a[%d]=%f expodim1=%d expodim2=%d \n", i,params->anExpoMatObj->a[i],params->anExpoMatObj->ai[i],params->anExpoMatObj->aj[i]);
// //	}
// //	printf("rank=%d nnz=%d\n", params->anExpoMatObj->rank, params->anExpoMatObj->nnz);
//
// //	for(i=0;i<N;i++)printf("params->anExpoMatObj->resultSpace[%d]=%f\n",i,params->anExpoMatObj->resultSpace[i]);
// //	for(i=0;i<N;i++)printf("params->anExpoMatObj->initVec[%d]=%f\n",i,params->anExpoMatObj->initVec[i]);
//
// 	anExpoMatObj.ai = params->expodim1;
// 	anExpoMatObj.aj = params->expodim2;
// 	anExpoMatObj.a = params->new;
// 	anExpoMatObj.initVec = params->expoInit;
// 	anExpoMatObj.resultSpace = params->expoResult;
// //	sparse_exponential_single_row(N, params->nnz, anExpoMatObj.ai, anExpoMatObj.aj,
// //		anExpoMatObj.a, timeV,anExpoMatObj.initVec, params->expoResult, 0);
// 	//state vector at time t stored in b[i]
// 	for(i=0; i<N; i++){
// 		params->b[i] = params->expoResult[i];
// //		printf("params->expoResult[%d]=%f\n",i,params->expoResult[i] );
// 		gsl_vector_set(tmpStates,i,params->expoResult[i]);
// 	}
//
// 	gsl_blas_dgemv(CblasTrans, 1.0, params->invMatGSL,tmpStates, 0.0, params->resVec);
//
//
// 	for(i=0; i<N; i++){
// 		params->b[i] = gsl_vector_get(params->resVec,i);
// 	}
// 	gsl_matrix_set_zero(params->expAFS2);
// 	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS2);
//
// 	//now subtract older AFS (t_t - t_INF) from total AFS (t_0 - t_INF)
// 	/////////////////
// 	///////
// //	printf("////////////////////Island time0-t=%f unnormalized\n",timeV);
//
// 	gsl_matrix_sub(params->expAFS,params->expAFS2);
// 	// for(i=0;i< (params->expAFS->size1) ;i++){
// 	// 	for(j=0;j< (params->expAFS->size2);j++){
// 	// 		printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
// 	// 	}
// 	// 	printf("\n");
// 	// }
// 	// printf("////////////////////\n");
//
//
// 	//mapping of states already complete when params initialized
// 	//state vector at time t stored in st[] for largerStateSpace; use reverseMap
// 	for(i=0;i<N;i++)params->st[i]=0.0;
// 	for(i=0;i<N;i++)params->st[params->map[i]]+=params->expoResult[i];
// 	for(i=0; i<params->reducedStateSpace->nstates; i++){
// 		gsl_vector_set(params->ancStateVec,i,params->st[params->reverseMap[i]]);
// 	}
// 	Na = params->reducedStateSpace->nstates;
// 	//fill up ancestral transition matrix
// 	gsl_vector_set_zero(params->rates);
// 	tmpMat2= fillTransitionMatrixArray_embed_ctmc(params->reducedStateSpace, params->topA2, params->moveA2, &(params->nnzA),
// 		params->dim1A, params->dim2A,thetaA, 0, 0, 0,params->new, params->newdim1, params->newdim2,params->rates);
//
// 	ident = cs_spalloc(Na,Na,Na,1,1);
// 	for(i=0;i<Na;i++) cs_entry(ident,i,i,1);
// 	eye = cs_compress(ident);
// 	spMat = cs_add(eye,tmpMat2,1.0,-1.0);
// 	mt = cs_transpose(spMat,1);
//
// //	gsl_matrix_free(invMatGSL);
// //	invMatGSLA = gsl_matrix_calloc(Na,Na);
//
// 	for(j=0;j<Na;j++){
// 		//create unit array for solve
// 		for(i=0; i<Na; i++)params->b[i] = 0.0;
// 		params->b[j]=1.0;
// 		cs_lusol(0, mt, params->b, 1e-12);
// 		for(i=0; i<Na; i++){
// 			gsl_matrix_set(params->invMatGSLA,j,i,params->b[i]);
// 		}
// 	}
//
// 	//get contribution starting at st
// 	gsl_blas_dgemv(CblasTrans, 1.0, params->invMatGSLA, params->ancStateVec, 0.0, params->ancResVec);
// 	for(i=0; i<Na; i++)params->b[i] = gsl_vector_get(params->ancResVec,i);
// 	gsl_matrix_set_zero(params->expAFS2);
// 	fillExpectedAFS_unnorm(params->reducedStateSpace, params->b,params->rates,params->expAFS2);
//
// 	////////////////////normalized IM AFS
// 	gsl_matrix_add(params->expAFS,params->expAFS2);
// 	sum = matrixSumDouble(params->expAFS);
// 	gsl_matrix_scale(params->expAFS, 1.0 / sum);
//
//
// 	///////////// Replace AFS with log(AFS)
// 	// for(i=0;i<params->expAFS->size1;i++){
// 	// 	for(j=0;j<params->expAFS->size2;j++){
// 	// 		gsl_matrix_set(params->expAFS,i,j,log(gsl_matrix_get(params->expAFS,i,j)));
// 	// 	}
// 	// }
//
// 	//clean up
// 	cs_spfree(spMat);
// 	cs_spfree(mt);
// 	cs_spfree(ident);
// 	cs_spfree(eye);
// 	cs_spfree(tmpMat);
// 	cs_spfree(tmpMat2);
// 	gsl_vector_free(tmpStates);
// }

//////////////////// Uniformization Algorithm for Exponentiation
/// P. 413 from Stewart Book
void uniformizExpSparseMat(int N, int nnz, double *vals, int *dim1, int *dim2, double tSoln, double *result){
	int i, k, ii;
	double cosi, sigma, nu, gamma, eps,y[N],tmpY[N],scale;
	struct cs_di_sparse *tmpMat, *ctmc, *ident, *eye, *new;

	//stuff values into more convenient matrix
	//uniformization of matrix
	tmpMat = cs_spalloc(N,N,N,1,1);
	ident = cs_spalloc(N,N,N,1,1);
	for(i=0;i<nnz;i++)cs_entry(tmpMat,dim1[i],dim2[i],vals[i]);
	ctmc=cs_compress(tmpMat);
	gamma = cs_max(ctmc);
	printf("gamma=%f\n------------------\n",gamma);
	cs_print_adk(ctmc);
	cs_scale(ctmc,1.0/gamma);
	cs_print_adk(ctmc);

	for(i=0;i<N;i++) cs_entry(ident,i,i,1);
	eye = cs_compress(ident);
	new = cs_add(eye,ctmc,1.0,1.0);
	cs_print_adk(new);

	//tmpMat now holds the subjugated dtmc

	//step one of uniformization is to compute K, number of terms in the summation
	eps=0.001;
	k=0; cosi=1.0; sigma=1.0; nu = (1-eps)/exp(-1.0*gamma*tSoln);
	while(sigma < nu){
		k+=1;
		cosi *= (gamma * tSoln)/k;
		sigma += cosi;
	}
	printf("here is k: %d\n",k);

	//initialize result;
	for(i=1;i<N;i++){
		result[i]=0.0;
		y[i]=0.0;
		tmpY[i]=0.0;
	}
	result[0]=1.0;y[0]=1.0;tmpY[0]=0.0;
	printf("initialization done\n");
	for(ii=0;ii<N;ii++) printf("tmpY[%d]=%f\n",ii,tmpY[ii]);
	printf("---------------------\n");
	for(ii=0;ii<N;ii++)printf("y[%d]=%f\n",ii,y[ii]);
	printf("---------------------\n");

	//now approximate pi(t)
	int testVal;
	for(i=1;i<=k;i++){
		printf("k loop k=%d\n",i);
		testVal=cs_gaxpy(new, y, tmpY); //tmpY now contains the answer
		printf("test of gaxpy: %d\n",testVal);
		for(ii=0;ii<N;ii++) printf("tmpY[%d]=%f\n",ii,tmpY[ii]);
		printf("---------------------\n");
		for(ii=0;ii<N;ii++)printf("y[%d]=%f\n",ii,y[ii]);
		printf("---------------------\n");
		cs_print_adk(tmpMat);
		printf("---------------------\n");

		for(ii=0;ii<N;ii++){
	//		printf("tmpY[%d]=%f\n",ii,tmpY[ii]);
	//		printf("y[%d]=%f\n",ii,y[ii]);
			y[ii]=tmpY[ii] * ((gamma*tSoln)/i);
			result[ii]+=y[ii];
			tmpY[ii]=0;
		}

	}
	//final scaling
	scale = exp(-1.0*gamma*tSoln);
	for(i=0;i<N;i++){
		result[i] *= scale;
		printf("result[%d]=%f\n",i,result[i]);
	}
}


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

#include <nlopt.h>

typedef struct {
    int N;
    double *x, *y;              /* length N; */
} lorentzdata;

static double sqr(double x)
{
    return x * x;
}

static int count = 0;

static double lorentzerr(int n, const double *p, double *grad, void *data)
{
    lorentzdata *d = (lorentzdata *) data;
    int N = d->N;
    const double *xs = d->x;
    const double *ys = d->y;
    double val = 0;
    int i, j;

    for (i = 0; i < N; ++i) {
        double x = xs[i], y = ys[i];
        double lorsum = 0;

        for (j = 0; j < n; j += 3) {
            double A = p[j + 0];
            double w = p[j + 1];
            double G = p[j + 2];
            double lor = A / (sqr(x - w) + G * G);

            lorsum += lor;
        }

        val += sqr(y - lorsum);

        if (grad)
            for (j = 0; j < n; j += 3) {
                double A = p[j + 0];
                double w = p[j + 1];
                double G = p[j + 2];
                double deninv = 1.0 / (sqr(x - w) + G * G);

                grad[j + 0] += -2 * (y - lorsum) * deninv;
                grad[j + 1] += 4 * A * (w - x) * (y - lorsum) * sqr(deninv);
                grad[j + 2] += 4 * A * G * (y - lorsum) * sqr(deninv);
            }
    }
    ++count;
    // printf("%d: f(%g,%g,%g) = %g\n", count, p[0],p[1],p[2], val);
    return val;
}

extern double nlopt_urand(double a, double b);

int main(void)
{
    lorentzdata d;
    int i;
    double A = 1, w = 0, G = 1, noise = 0.01;
    double lb[3] = { -HUGE_VAL, -HUGE_VAL, 0 };
    double ub[3] = { HUGE_VAL, HUGE_VAL, HUGE_VAL };
    double p[3] = { 0, 1, 2 }, minf;

    nlopt_srand_time();

    d.N = 200;
    d.x = (double *) malloc(sizeof(double) * d.N * 2);
    d.y = d.x + d.N;
    for (i = 0; i < d.N; ++i) {
        d.x[i] = nlopt_urand(-0.5, 0.5) * 8 * G + w;
        d.y[i] = 2 * noise * nlopt_urand(-0.5, 0.5) + A / (sqr(d.x[i] - w) + G * G);
    }

    nlopt_minimize(NLOPT_LN_NEWUOA_BOUND, 3, lorentzerr, &d, lb, ub, p, &minf, -HUGE_VAL, 0, 0, 1e-6, NULL, 0, 0);

    printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0], p[1], p[2]);

    count = 0;
    nlopt_minimize(NLOPT_LN_COBYLA, 3, lorentzerr, &d, lb, ub, p, &minf, -HUGE_VAL, 0, 0, 1e-6, NULL, 0, 0);

    printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0], p[1], p[2]);

    count = 0;
    nlopt_minimize(NLOPT_LN_NELDERMEAD, 3, lorentzerr, &d, lb, ub, p, &minf, -HUGE_VAL, 0, 0, 1e-6, NULL, 0, 0);

    printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0], p[1], p[2]);

    count = 0;
    nlopt_minimize(NLOPT_LN_SBPLX, 3, lorentzerr, &d, lb, ub, p, &minf, -HUGE_VAL, 0, 0, 1e-6, NULL, 0, 0);

    printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0], p[1], p[2]);

    return 0;
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_poly.h>

/*
 * This module implements a 2D subspace trust region subproblem method,
 * as outlined in
 *
 * [1] G. A. Shultz, R. B. Schnabel, and R. H. Byrd
 *     A Family of Trust-Region-Based Algorithms for Unconstrained
 *     Minimization with Strong Global Convergence Properties,
 *     SIAM Journal on Numerical Analysis 1985 22:1, 47-67
 *
 * [2] R. H. Byrd, R. B. Schnabel, G. A. Shultz,
 *     Approximate solution of the trust region problem by
 *     minimization over two-dimensional subspaces,
 *     Mathematical Programming, January 1988, Volume 40,
 *     Issue 1, pp 247-263
 *
 * The idea is to solve:
 *
 * min_{dx} g^T dx + 1/2 dx^T B dx
 *
 * with constraints:
 *
 * ||D dx|| <= delta
 * dx \in span{dx_sd, dx_gn}
 *
 * where B is the Hessian matrix, B = J^T J
 *
 * The steps are as follows:
 *
 * 1. preloop:
 *    a. Compute Gauss-Newton and steepest descent vectors,
 *       dx_gn, dx_sd
 *    b. Compute an orthonormal basis for span(D dx_sd, D dx_gn) by
 *       constructing W = [ D dx_sd, D dx_gn ] and performing a QR
 *       decomposition of W. The 2 columns of the Q matrix
 *       will then span the column space of W. W should have rank 2
 *       unless D*dx_sd and D*dx_gn are parallel, in which case it will
 *       have rank 1.
 *    c. Precompute various quantities needed for the step calculation
 *
 * 2. step:
 *    a. If the Gauss-Newton step is inside the trust region, use it
 *    b. if W has rank 1, we cannot form a 2D subspace, so in this case
 *       follow the steepest descent direction to the trust region boundary
 *       and use that as the step.
 *    c. In the full rank 2 case, if the GN point is outside the trust region,
 *       then the minimizer of the objective function lies on the trust
 *       region boundary. Therefore the minimization problem becomes:
 *
 *       min_{dx} g^T dx + 1/2 dx^T B dx, with ||dx|| = delta, dx = Q * x
 *
 *       where x is a 2-vector to be determined and the columns of Q are
 *       the orthonormal basis vectors of the subspace. Note the equality
 *       constraint now instead of <=. In terms of the new variable x,
 *       the minimization problem becomes:
 *
 *       min_x subg^T x + 1/2 x^T subB x, with ||Q*x|| = ||x|| = delta
 *
 *       where:
 *         subg = Q^T g   (2-by-1)
 *         subB = Q^T B Q (2-by-2)
 *
 *       This equality constrained 2D minimization problem can be solved
 *       with a Lagrangian multiplier, which results in a 4th degree polynomial
 *       equation to be solved. The equation is:
 *
 *         lambda^4  1
 *       + lambda^3  2 tr(B)
 *       + lambda^2  (tr(B)^2 + 2 det(B) - g^T g / delta^2)
 *       + lambda^1  (2 det(B) tr(B) - 2 g^T adj(B)^T g / delta^2)
 *       + lambda^0  (det(B)^2 - g^T adj(B)^T adj(B) g / delta^2)
 *
 *       where adj(B) is the adjugate matrix of B.
 *
 *       We then check each of the 4 solutions for lambda to determine which
 *       lambda results in the smallest objective function value. This x
 *       is then used to construct the final step: dx = Q*x
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  gsl_vector *dx_gn;         /* Gauss-Newton step, size p */
  gsl_vector *dx_sd;         /* steepest descent step, size p */
  double norm_Dgn;           /* || D dx_gn || */
  double norm_Dsd;           /* || D dx_sd || */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */
  gsl_matrix *W;             /* orthonormal basis for 2D subspace, p-by-2 */
  gsl_matrix *JQ;            /* J * Q, n-by-p */
  gsl_vector *tau;           /* Householder scalars */
  gsl_vector *subg;          /* subspace gradient = W^T g, 2-by-1 */
  gsl_matrix *subB;          /* subspace Hessian = W^T B W, 2-by-2 */
  gsl_permutation *perm;     /* permutation matrix */

  double trB;                /* Tr(subB) */
  double detB;               /* det(subB) */
  double normg;              /* || subg || */
  double term0;              /* g^T adj(B)^T adj(B) g */
  double term1;              /* g^T adj(B)^T g */

  size_t rank;               /* rank of [ dx_sd, dx_gn ] matrix */

  gsl_poly_complex_workspace *poly_p;

  /* tunable parameters */
  gsl_multifit_nlinear_parameters params;
} subspace2D_state_t;

#include "common.c"

static void * subspace2D_alloc (const void * params, const size_t n, const size_t p);
static void subspace2D_free(void *vstate);
static int subspace2D_init(const void *vtrust_state, void *vstate);
static int subspace2D_preloop(const void * vtrust_state, void * vstate);
static int subspace2D_step(const void * vtrust_state, const double delta,
                           gsl_vector * dx, void * vstate);
static int subspace2D_preduction(const void * vtrust_state, const gsl_vector * dx,
                                 double * pred, void * vstate);
static int subspace2D_solution(const double lambda, gsl_vector * x,
                               subspace2D_state_t * state);
static double subspace2D_objective(const gsl_vector * x, subspace2D_state_t * state);
static int subspace2D_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx);
static int subspace2D_calc_sd(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx,
                              subspace2D_state_t * state);

static void *
subspace2D_alloc (const void * params, const size_t n, const size_t p)
{
  const gsl_multifit_nlinear_parameters *par = (const gsl_multifit_nlinear_parameters *) params;
  subspace2D_state_t *state;

  state = calloc(1, sizeof(subspace2D_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate subspace2D state", GSL_ENOMEM);
    }

  state->dx_gn = gsl_vector_alloc(p);
  if (state->dx_gn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for dx_gn", GSL_ENOMEM);
    }

  state->dx_sd = gsl_vector_alloc(p);
  if (state->dx_sd == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for dx_sd", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->W = gsl_matrix_alloc(p, 2);
  if (state->W == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for W", GSL_ENOMEM);
    }

  state->JQ = gsl_matrix_alloc(n, p);
  if (state->JQ == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for JQ", GSL_ENOMEM);
    }

  state->tau = gsl_vector_alloc(2);
  if (state->tau == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for tau", GSL_ENOMEM);
    }

  state->subg = gsl_vector_alloc(2);
  if (state->subg == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for subg", GSL_ENOMEM);
    }

  state->subB = gsl_matrix_alloc(2, 2);
  if (state->subB == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for subB", GSL_ENOMEM);
    }

  state->perm = gsl_permutation_alloc(2);
  if (state->perm == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for perm", GSL_ENOMEM);
    }

  state->poly_p = gsl_poly_complex_workspace_alloc(5);
  if (state->poly_p == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for poly workspace", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;
  state->rank = 0;
  state->params = *par;

  return state;
}

static void
subspace2D_free(void *vstate)
{
  subspace2D_state_t *state = (subspace2D_state_t *) vstate;

  if (state->dx_gn)
    gsl_vector_free(state->dx_gn);

  if (state->dx_sd)
    gsl_vector_free(state->dx_sd);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->workn)
    gsl_vector_free(state->workn);

  if (state->W)
    gsl_matrix_free(state->W);

  if (state->JQ)
    gsl_matrix_free(state->JQ);

  if (state->tau)
    gsl_vector_free(state->tau);

  if (state->subg)
    gsl_vector_free(state->subg);

  if (state->subB)
    gsl_matrix_free(state->subB);

  if (state->perm)
    gsl_permutation_free(state->perm);

  if (state->poly_p)
    gsl_poly_complex_workspace_free(state->poly_p);

  free(state);
}

/*
subspace2D_init()
  Initialize subspace2D solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
subspace2D_init(const void *vtrust_state, void *vstate)
{
  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

/*
subspace2D_preloop()
  Initialize subspace2D method prior to iteration loop.
This involves computing the Gauss-Newton step and
steepest descent step

Notes: on output,
1) state->dx_gn contains Gauss-Newton step
2) state->dx_sd contains steepest descent step
3) state->rank contains the rank([dx_sd, dx_gn])
4) if full rank subspace (rank = 2), then:
   state->trB = Tr(subB)
   state->detB = det(subB)
   state->normg = || subg ||
*/

static int
subspace2D_preloop(const void * vtrust_state, void * vstate)
{
  int status;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  subspace2D_state_t *state = (subspace2D_state_t *) vstate;
  gsl_vector_view v;
  double work_data[2];
  gsl_vector_view work = gsl_vector_view_array(work_data, 2);
  int signum;

  /* calculate Gauss-Newton step */
  status = subspace2D_calc_gn(trust_state, state->dx_gn);
  if (status)
    return status;

  /* now calculate the steepest descent step */
  status = subspace2D_calc_sd(trust_state, state->dx_sd, state);
  if (status)
    return status;

  /* store norms */
  state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
  state->norm_Dsd = scaled_enorm(trust_state->diag, state->dx_sd);

  /*
   * now compute orthonormal basis for span(D dx_sd, D dx_gn) using
   * QR decomposition; set W = [ D dx_sd, D dx_gn ] and normalize each
   * column to unit magnitude. Then the Q matrix will form a basis for Col(W)
   */

  v = gsl_matrix_column(state->W, 0);
  gsl_vector_memcpy(&v.vector, state->dx_sd);
  gsl_vector_mul(&v.vector, trust_state->diag);
  if (state->norm_Dsd != 0)
    gsl_vector_scale(&v.vector, 1.0 / state->norm_Dsd);

  v = gsl_matrix_column(state->W, 1);
  gsl_vector_memcpy(&v.vector, state->dx_gn);
  gsl_vector_mul(&v.vector, trust_state->diag);
  if (state->norm_Dgn != 0)
    gsl_vector_scale(&v.vector, 1.0 / state->norm_Dgn);

  /* use a rank revealing QR decomposition in case dx_sd and dx_gn
   * are parallel */
  gsl_linalg_QRPT_decomp(state->W, state->tau, state->perm, &signum, &work.vector);

  /* check for parallel dx_sd, dx_gn, in which case rank will be 1 */
  state->rank = gsl_linalg_QRPT_rank(state->W, -1.0);

  if (state->rank == 2)
    {
      /*
       * full rank subspace, compute:
       * subg = Q^T D^{-1} g
       * subB = Q^T D^{-1} B D^{-1} Q where B = J^T J
       */
      const size_t p = state->p;
      size_t i;
      gsl_matrix_view JQ = gsl_matrix_submatrix(state->JQ, 0, 0, state->n, GSL_MIN(2, p));
      double B00, B10, B11, g0, g1;

      /* compute subg */
      gsl_vector_memcpy(state->workp, trust_state->g);
      gsl_vector_div(state->workp, trust_state->diag);
      gsl_linalg_QR_QTvec(state->W, state->tau, state->workp);

      g0 = gsl_vector_get(state->workp, 0);
      g1 = gsl_vector_get(state->workp, 1);

      gsl_vector_set(state->subg, 0, g0);
      gsl_vector_set(state->subg, 1, g1);

      /* compute subB */

      /* compute J D^{-1} */
      gsl_matrix_memcpy(state->JQ, trust_state->J);

      for (i = 0; i < p; ++i)
        {
          gsl_vector_view c = gsl_matrix_column(state->JQ, i);
          double di = gsl_vector_get(trust_state->diag, i);
          gsl_vector_scale(&c.vector, 1.0 / di);
        }

      /* compute J D^{-1} Q */
      gsl_linalg_QR_matQ(state->W, state->tau, state->JQ);

      /* compute subB = Q^T D^{-1} J^T J D^{-1} Q */
      gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &JQ.matrix, 0.0, state->subB);

      B00 = gsl_matrix_get(state->subB, 0, 0);
      B10 = gsl_matrix_get(state->subB, 1, 0);
      B11 = gsl_matrix_get(state->subB, 1, 1);

      state->trB = B00 + B11;
      state->detB = B00*B11 - B10*B10;
      state->normg = gsl_blas_dnrm2(state->subg);

      /* g^T adj(B)^T adj(B) g */
      state->term0 = (B10*B10 + B11*B11)*g0*g0 -
                     2*B10*(B00 + B11)*g0*g1 +
                     (B00*B00 + B10*B10)*g1*g1;

      /* g^T adj(B)^T g */
      state->term1 = B11 * g0 * g0 + g1 * (B00*g1 - 2*B10*g0);

    }

  return GSL_SUCCESS;
}

/*
subspace2D_step()
  Calculate a new step with 2D subspace method. Based on [1]. We
seek a vector dx in span{dx_gn, dx_sd} which minimizes the model
function subject to ||dx|| <= delta
*/

static int
subspace2D_step(const void * vtrust_state, const double delta,
                gsl_vector * dx, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  subspace2D_state_t *state = (subspace2D_state_t *) vstate;

  if (state->norm_Dgn <= delta)
    {
      /* Gauss-Newton step is inside trust region, use it as final step
       * since it is the global minimizer of the quadratic model function */
      gsl_vector_memcpy(dx, state->dx_gn);
    }
  else if (state->rank < 2)
    {
      /* rank of [dx_sd, dx_gn] is 1, meaning dx_sd and dx_gn
       * are parallel so we can't form a 2D subspace. Follow the steepest
       * descent direction to the trust region boundary as our step */
      gsl_vector_memcpy(dx, state->dx_sd);
      gsl_vector_scale(dx, delta / state->norm_Dsd);
    }
  else
    {
      int status;
      const double delta_sq = delta * delta;
      double u = state->normg / delta;
      double a[5];
      double z[8];

#if 1
      a[0] = state->detB * state->detB - state->term0 / delta_sq;
      a[1] = 2 * state->detB * state->trB - 2 * state->term1 / delta_sq;
      a[2] = state->trB * state->trB + 2 * state->detB - u * u;
      a[3] = 2 * state->trB;
      a[4] = 1.0;
#else
      double TrB_D = state->trB * delta;
      double detB_D = state->detB * delta;
      double normg_sq = state->normg * state->normg;

      a[0] = detB_D * detB_D - state->term0;
      a[1] = 2 * state->detB * state->trB * delta_sq - 2 * state->term1;
      a[2] = TrB_D * TrB_D + 2 * state->detB * delta_sq - normg_sq;
      a[3] = 2 * state->trB * delta_sq;
      a[4] = delta_sq;
#endif

      status = gsl_poly_complex_solve(a, 5, state->poly_p, z);
      if (status == GSL_SUCCESS)
        {
          size_t i;
          double min = 0.0;
          int mini = -1;
          double x_data[2];
          gsl_vector_view x = gsl_vector_view_array(x_data, 2);

          /*
           * loop through all four values of the Lagrange multiplier
           * lambda. For each lambda, evaluate the objective function
           * with Re(lambda) to determine which lambda minimizes the
           * function
           */
          for (i = 0; i < 4; ++i)
            {
              double cost, normx;

              /*fprintf(stderr, "root: %.12e + %.12e i\n",
                      z[2*i], z[2*i+1]);*/

              status = subspace2D_solution(z[2*i], &x.vector, state);
              if (status != GSL_SUCCESS)
                continue; /* singular matrix system */

              /* ensure ||x|| = delta */

              normx = gsl_blas_dnrm2(&x.vector);
              if (normx == 0.0)
                continue;

              gsl_vector_scale(&x.vector, delta / normx);

              /* evaluate objective function to determine minimizer */
              cost = subspace2D_objective(&x.vector, state);
              if (mini < 0 || cost < min)
                {
                  mini = (int) i;
                  min = cost;
                }
            }

          if (mini < 0)
            {
              /* did not find minimizer - should not get here */
              return GSL_FAILURE;
            }
          else
            {
              /* compute x which minimizes objective function */
              subspace2D_solution(z[2*mini], &x.vector, state);

              /* dx = Q * x */
              gsl_vector_set_zero(dx);
              gsl_vector_set(dx, 0, gsl_vector_get(&x.vector, 0));
              gsl_vector_set(dx, 1, gsl_vector_get(&x.vector, 1));
              gsl_linalg_QR_Qvec(state->W, state->tau, dx);

              /* compute final dx by multiplying by D^{-1} */
              gsl_vector_div(dx, trust_state->diag);
            }
        }
      else
        {
          GSL_ERROR ("gsl_poly_complex_solve failed", status);
        }
    }

  return GSL_SUCCESS;
}

static int
subspace2D_preduction(const void * vtrust_state, const gsl_vector * dx,
                  double * pred, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  subspace2D_state_t *state = (subspace2D_state_t *) vstate;

  *pred = quadratic_preduction(trust_state->f, trust_state->J, dx, state->workn);

  return GSL_SUCCESS;
}

/* solve 2D subspace problem: (B + lambda*I) x = -g */
static int
subspace2D_solution(const double lambda, gsl_vector * x,
                    subspace2D_state_t * state)
{
  int status = GSL_SUCCESS;
  double C_data[4];
  gsl_matrix_view C = gsl_matrix_view_array(C_data, 2, 2);
  double B00 = gsl_matrix_get(state->subB, 0, 0);
  double B10 = gsl_matrix_get(state->subB, 1, 0);
  double B11 = gsl_matrix_get(state->subB, 1, 1);

  /* construct C = B + lambda*I */
  gsl_matrix_set(&C.matrix, 0, 0, B00 + lambda);
  gsl_matrix_set(&C.matrix, 1, 0, B10);
  gsl_matrix_set(&C.matrix, 0, 1, B10);
  gsl_matrix_set(&C.matrix, 1, 1, B11 + lambda);

  /* use modified Cholesky in case C is not positive definite */
  gsl_linalg_mcholesky_decomp(&C.matrix, state->perm, NULL);
  gsl_linalg_mcholesky_solve(&C.matrix, state->perm, state->subg, x);

  gsl_vector_scale(x, -1.0);

  return status;
}

/* evaluate 2D objective function: f(x) = g^T x + 1/2 x^T B x */
static double
subspace2D_objective(const gsl_vector * x, subspace2D_state_t * state)
{
  double f;
  double y_data[2];
  gsl_vector_view y = gsl_vector_view_array(y_data, 2);

  /* compute: y = g + 1/2 B x */
  gsl_vector_memcpy(&y.vector, state->subg);
  gsl_blas_dsymv(CblasLower, 0.5, state->subB, x, 1.0, &y.vector);

  /* compute: f = x^T ( g + 1/2 B x ) */
  gsl_blas_ddot(x, &y.vector, &f);

  return f;
}

/*
subspace2D_calc_gn()
  Calculate Gauss-Newton step which satisfies:

J dx_gn = -f

Inputs: trust_state - trust state variables
        dx          - (output) Gauss-Newton step

Return: success/error
*/

static int
subspace2D_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx)
{
  int status;
  const gsl_multifit_nlinear_parameters *params = trust_state->params;

  /* initialize linear least squares solver */
  status = (params->solver->init)(trust_state, trust_state->solver_state);
  if (status)
    return status;

  /* prepare the linear solver to compute Gauss-Newton step */
  status = (params->solver->presolve)(0.0, trust_state, trust_state->solver_state);
  if (status)
    return status;

  /* solve: J dx_gn = -f for Gauss-Newton step */
  status = (params->solver->solve)(trust_state->f,
                                   dx,
                                   trust_state,
                                   trust_state->solver_state);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/*
subspace2D_calc_sd()
  Calculate steepest descent step,

dx_sd = - || D^{-1} g ||^2 / || J D^{-2} g ||^2 D^{-2} g

Inputs: trust_state - trust state variables
        dx          - (output) steepest descent vector
        state       - workspace

Return: success/error
*/

static int
subspace2D_calc_sd(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx,
                   subspace2D_state_t * state)
{
  double norm_Dinvg;   /* || D^{-1} g || */
  double norm_JDinv2g; /* || J D^{-2} g || */
  double alpha;        /* || D^{-1} g ||^2 / || J D^{-2} g ||^2 */
  double u;

  /* compute workp = D^{-1} g and its norm */
  gsl_vector_memcpy(state->workp, trust_state->g);
  gsl_vector_div(state->workp, trust_state->diag);
  norm_Dinvg = gsl_blas_dnrm2(state->workp);

  /* compute workp = D^{-2} g */
  gsl_vector_div(state->workp, trust_state->diag);

  /* compute: workn = J D^{-2} g */
  gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, state->workp, 0.0, state->workn);
  norm_JDinv2g = gsl_blas_dnrm2(state->workn);

  u = norm_Dinvg / norm_JDinv2g;
  alpha = u * u;

  /* dx_sd = -alpha D^{-2} g */
  gsl_vector_memcpy(dx, state->workp);
  gsl_vector_scale(dx, -alpha);

  return GSL_SUCCESS;
}

static const gsl_multifit_nlinear_trs subspace2D_type =
{
  "2D-subspace",
  subspace2D_alloc,
  subspace2D_init,
  subspace2D_preloop,
  subspace2D_step,
  subspace2D_preduction,
  subspace2D_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_subspace2D = &subspace2D_type;


/*
F(4,3)
cc -static -o test_cblas_open main43.c -I /opt/OpenBLAS/include/ -L/opt/OpenBLAS/lib -lopenblas -lpthread -lgfortran
	->For each cube of kernel
		-> for each tile (a tile is a cube d x d x channel)
			-> for each channel
				-> Apply the winograd algorithm
			-> sum the result of the previous channel to the result of the next channell
		-> write the tile in a single output layer
	->Add the output layer in che cube layer and change the kernel cube
*/

#include <cblas.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <string.h>

#define MAX_ELEMENTS 100
#define FILENAMELEN 10
#define DDIMENSION(m,r) (m+r-1)
#define OUTPUTDIMENSION(m,w,d) (m * (w / d))

typedef float * Matrix;
typedef Matrix * Cube;
typedef Cube * Hypercube;

void productABAt(float result[], float A[], float B[], int rowsA, int colsB, int colsA);
void twoDcorrelation(float C[], float g[], float D[], int m, int r, int d);
Matrix elaborateTile(Cube tile, Cube g, int m, int r, int d, int channel);
Cube fillTheTile(Cube image, int starth, int startw, int w, int d, int channel);
void saveOutputTile(Matrix output, Matrix tileOutput, int starth, int startw, int w, int d, int m);
Matrix elaborateKernel(Cube image, Cube g, int m, int r, int channel, int w, int h, int d);

/*START FUNCTION TO CREATE VARIABLES*/
	Cube generateCube(int w, int h, int ch);
	Matrix generateMatrix(int w, int h);
	Hypercube readKernels(int w, int h, int ch, int k, char fileName[]);
	Cube readInput(int w, int h, int ch, char fileName[]);

	void printCube(Cube c, int w, int channel);
/*END FUNCTION TO CREARE VARIABLES*/

/*START FUNCTIONS TO READ THE FILES*/
	void readFile(float result[], char fileName[]);
	void generateNameAT(char* at, int m, int r);
	void generateNameG(char* g, int m, int r);
	void generateNameBT(char* g, int m, int r);
/* END FUNCRIONS TO READ THE FILES*/

static Matrix A;	//Parameters that are used to make the lowest operation.
static Matrix B;	//
static Matrix G;	//

void main(int argc, char *argv[])
{

	int m = atoi(argv[1]);			//dimension of the output tile
	int r = atoi(argv[2]);			//dimension of the kernel rxr
	int channel = atoi(argv[3]);	//dimension of the channel
	int k = atoi(argv[4]);			//number of kernel
	int w = atoi(argv[5]);			//width of the image -> multiple of d
	int h = atoi(argv[6]);			//height of the image -> multiple of d

	char * inputFilename = argv[7];
	char * kernelFilename = argv[8];

	int d = DDIMENSION(m,r);

	char fnameAT[FILENAMELEN];
	char fnameG[FILENAMELEN];
	char fnameBT[FILENAMELEN];

	generateNameAT(fnameAT, m,r);
	generateNameG(fnameG, m,r);
	generateNameBT(fnameBT, m,r);

	A = generateMatrix(m, d);
	B = generateMatrix(d, d);
	G = generateMatrix(r, d);

	readFile(A, fnameAT);
	readFile(B, fnameBT);
	readFile(G, fnameG);

	Cube image = readInput(w, h, channel, inputFilename);
	Hypercube kernel = readKernels(r, r, channel, k, kernelFilename);

	printf("IMAGE\n");
	printCube(image, w, channel);

	printf("\nKERNELS\n");
	for(int i = 0; i < k; i++)
	{
		printf("Kernel number %d\n", i);
		printCube(kernel[i], r, channel);
	}
	printf("\n------------------\n");
	Cube output = (Cube)malloc(k * sizeof(Matrix));	//k is the number of filters

	//printf("START THE COMPUTATION - SLEEP FOR 3 SECONDS\n");
 	//sleep(3);
	/*
	 *	START COMPUTATION.
	 */
	clock_t start = clock();
	for(int i = 0; i < k; i ++)
		output[i] = elaborateKernel(image, kernel[i], m, r, channel, w, h, d);

	clock_t end = clock();
	/*
	 *	END COMPUTATION.
	 */

	printf("OUTPUT:\n");
	printCube(output, OUTPUTDIMENSION(m,w,d), k);

	float seconds = (float)(end - start) / CLOCKS_PER_SEC;

	printf("\nTIME IN SECONDS TO DO F(%d,%d) = %f\n",m,r,seconds);

}

void printCube(Cube c, int w, int channel)
{
	for(int i = 0; i < channel; i++)
	{
		printf("channel %d:\n",i);
		for(int j = 0; j < w * w; j ++)
		{
			printf("%.2f ", c[i][j]);
			if((j+1) % w == 0) printf("\n");
		}
	}
}

Matrix elaborateKernel(Cube image, Cube g, int m, int r, int channel, int w, int h, int d)
{
	Matrix output = generateMatrix(OUTPUTDIMENSION(m,w,d), OUTPUTDIMENSION(m,w,d));

	for(int i = 0; i < h; i = i + d)
	{
		for (int j = 0; j < w; j = j + d)
		{
			Cube tile = fillTheTile(image, i, j, w, d, channel);

			Matrix tileOutput = elaborateTile(tile, g, m, r, d, channel);
			saveOutputTile(output, tileOutput, i, j, w, d, m);

			free(tileOutput);
			free(tile);
		}
	}
	return output;
}

/*
	Write the output tile on the output image.
*/

void saveOutputTile(Matrix output, Matrix tileOutput, int starth, int startw, int w, int d, int m)
{
		int outputWidth = OUTPUTDIMENSION(m,w,d);
		int offsetw = (startw / d) * m;
		int offseth = (starth / d) * m;
		for(int y = 0; y < m; y ++)
			for(int x = 0; x < m; x++)
				output[(x + offsetw) + (outputWidth * (y + offseth))] = tileOutput[x + (m * y)];

}

/*
	This function takes an image, it divides the image based on starth and startw and it returns a tile.
		- image  -> the all image
		- starth -> height coordinate of the image where to start to make the tile
		- startw -> width coordinate of the image where to start to make the tile
		- w -> total width of the image
		- d -> dimension of the tile -> d x d
		- channel -> number of channels of the image
	RESULT
		- A cube d x d x channell that represent a single tile.
*/
Cube fillTheTile(Cube image, int starth, int startw, int w, int d, int channel)
{
	Cube tile = generateCube(d, d, channel);
	for(int c = 0; c < channel; c++)
		for(int y = 0; y < d; y ++)
			for(int x = 0; x < d; x++)
				tile[c][x + (d * y)] = image[c][(startw + x) + (w * (y + starth))];
	return tile;
}

/*
	This function is used to make the combination with a tile and a kernel.
		- tile has the dimension d x d and has 'channel' channels
		- g is the kernel, it has the dimension of r x r and has 'channel' channels
		- m -> dimension of the output
		- r -> dimension of the kernel
		- d -> dimension of the tile
		- channell -> number of channels
	RESULT
		- it returns a matrix calculated by an element wise sum of the result of the function twoDcorrelation
			calculated for each channel.
*/
Matrix elaborateTile(Cube tile, Cube g, int m, int r, int d, int channel)
{
	Matrix temp = generateMatrix(d,d);
	Matrix tileOutputBig = generateMatrix(d,d);
	Matrix tileOutput = generateMatrix(m,m);

	for(int i = 0; i < m * m; i++)
			tileOutput[i] = 0;

	for(int i = 0; i < channel; i++)
	{
	 	twoDcorrelation(temp, g[i], tile[i], m, r, d);
		for(int j = 0 ; j < d * d; j++)
			tileOutputBig[j] = tileOutputBig[j] + temp[j];

	}
	productABAt(tileOutput, A, tileOutputBig, m, d, d);
	free(temp);
	free(tileOutputBig);

	return tileOutput;
}

Cube generateCube(int w, int h, int ch)
{
	Cube cube = (Cube)malloc(ch * sizeof(Matrix));
	for(int i = 0; i < ch; i++)
		cube[i] = generateMatrix(w,h);

	return cube;
}

Matrix generateMatrix(int w, int h)
{
	return (Matrix)malloc(w * h * sizeof(float));
}

/*
	Calculate a single channel for a single kernel for a single tile
	Input Parameter:
		- result -> matrix of the result
		- g -> the considered kernel
		- D -> the considered tile
		- m -> Dimension of the output
		- r -> dimension of the kernel
		- d -> dimension of the tile (m + r - 1)
*/
void twoDcorrelation(float C[], float g[], float D[], int m, int r, int d)
{

	float GgG[MAX_ELEMENTS];
	float BDB[MAX_ELEMENTS];
	int i=0;

	productABAt(GgG, G, g, d, r, r);
	productABAt(BDB, B, D, d, d, d);

 	for(i=0;i<d*d;i++)
		C[i] = GgG[i] * BDB[i];
  	//productABAt(C, A, GgG, m, d, d);
}

/*
 *   Make the operation R = ABA**T
 *   Parameters:
 *		- Matrix Result
 *		- Matrix A and B
 *		- Number of rows of A -> rowsA
 *		- Number of cols of B -> colsB
 *		- Number of cols of A that is equal to the numer of rows of B -> colsA
 *  Result
 *		- Matrix result -> that is an rowsA x rowsA matrix
 */
void productABAt(float result[], float A[], float B[], int rowsA, int colsB, int colsA)
{
	float C[MAX_ELEMENTS];

	cblas_sgemm(CblasRowMajor,	//Modo in cui è salvata la matrice, legge i numeri riga per riga
		CblasNoTrans,	//Non fare trasposta
		CblasNoTrans, 	//Non fare trasposta
		rowsA, 		//numero righe matrice A
		colsB, 		//numero colonne matrice B
		colsA, 		//numero colonne A & numero righe B
		1, 		//moltiplicatore prima matrice
		A, 		//Matrice A
		colsA, 		//numero colonne matrice A
		B, 		//Matrice B
		colsB, 		//numero colonne matrice B
		0, 		//Moltiplicatore matrice C
		C,		//matrice C
		colsB);		//numero righe matrice C

	//C has rowsA rows and colsB cols
	//the result has rowsA rows and rowsA cols

	cblas_sgemm(CblasRowMajor,
		CblasNoTrans,
		CblasTrans,
		rowsA,	//numero righe matrice C
		rowsA,
		colsB,
		1,
		C,
		colsB,
		A,
		colsA,
		0,
		result,
		rowsA);
}

Cube readInput(int w, int h, int ch, char fileName[])
{

    FILE *f;
    f = fopen(fileName, "r");
    if(f==NULL)
	{
    	printf("I can not read the file %s\n", fileName);
		exit(0);
	}

    int fi = 0;
    int linesize = w*h;
    Cube matrix = (Cube)malloc(ch * sizeof(Matrix));

    for(int i = 0; i < ch; i++)
	{
        matrix[i] = (Matrix)malloc(linesize * sizeof(float));
        while(!feof(f)&&fi<linesize)
		{
            fscanf(f, "%f", &matrix[i][fi]);
            fi++;
        }
        fi=0;
    }
    fclose(f);
    return matrix;

}

Hypercube readKernels(int w, int h, int ch, int k, char fileName[])
{

    FILE *f;
    f = fopen(fileName, "r");
    if(f==NULL)
    {
		printf("I can not read the file %s\n", fileName);
		exit(0);
	}
    int fi = 0;
    int linesize = w*h;
    int kernelsize = linesize*ch;
    Hypercube matrix = (Hypercube)malloc(k * sizeof(Cube));

    for(int i = 0; i < k; i++)
	{
        matrix[i] = (Cube)malloc(kernelsize * sizeof(Matrix));
        for(int j=0; j< ch; j++)
		{
            matrix[i][j] = (Matrix)malloc(linesize * sizeof(float));
            while(!feof(f) && fi<linesize)
			{
                   fscanf(f, "%f", &matrix[i][j][fi]);
                    fi++;
            }
            fi=0;
        }
    }
    fclose(f);
    return matrix;
}

void readFile(float result[], char fileName[])
{
	FILE *f;
	f = fopen(fileName, "r");
	if(f == NULL)
	{
		printf("I can not read the file %s\n", fileName);
		exit(0);
	}
	int i = 0;
	while(!feof(f))
	{
		fscanf(f, "%f", &result[i]);
		i++;
	}
	fclose(f);
}

/* FILENAMES FUNCTIONS */
void generateNameAT(char* at, int m, int r){
    at[0] = 'A';
    at[1] = 'T';
    at[2] = m + '0';
    at[3] = 'x';
    at[4] = r + '0';
    at[5] = '.';
    at[6] = 't';
    at[7] = 'x';
    at[8] = 't';
    at[9] = '\0';
}

void generateNameG(char* g, int m, int r){
    g[0] = 'G';
    g[1] = m + '0';
    g[2] = 'x';
    g[3] = r + '0';
    g[4] = '.';
    g[5] = 't';
    g[6] = 'x';
    g[7] = 't';
    g[8] = '\0';
}

void generateNameBT(char* g, int m, int r){
    g[0] = 'B';
    g[1] = 'T';
    g[2] = m + '0';
    g[3] = 'x';
    g[4] = r + '0';
    g[5] = '.';
    g[6] = 't';
    g[7] = 'x';
    g[8] = 't';
    g[9] = '\0';
}


/* cdf/fdistinv.c
 *
 * Copyright (C) 2002 Przemyslaw Sliwa and Jason H. Stover.
 * Copyright (C) 2006 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

#include <config.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "error.h"

double
gsl_cdf_fdist_Pinv (const double P, const double nu1, const double nu2)
{
  double result;
  double y;

  if (P < 0.0)
    {
      CDF_ERROR ("P < 0.0", GSL_EDOM);
    }
  if (P > 1.0)
    {
      CDF_ERROR ("P > 1.0", GSL_EDOM);
    }
  if (nu1 < 1.0)
    {
      CDF_ERROR ("nu1 < 1", GSL_EDOM);
    }
  if (nu2 < 1.0)
    {
      CDF_ERROR ("nu2 < 1", GSL_EDOM);
    }

  if (P < 0.5)
    {
      y = gsl_cdf_beta_Pinv (P, nu1 / 2.0, nu2 / 2.0);

      result = nu2 * y / (nu1 * (1.0 - y));
    }
  else
    {
      y = gsl_cdf_beta_Qinv (P, nu2 / 2.0, nu1 / 2.0);

      result = nu2 * (1 - y) / (nu1 * y);
    }

  return result;
}

double
gsl_cdf_fdist_Qinv (const double Q, const double nu1, const double nu2)
{
  double result;
  double y;

  if (Q < 0.0)
    {
      CDF_ERROR ("Q < 0.0", GSL_EDOM);
    }
  if (Q > 1.0)
    {
      CDF_ERROR ("Q > 1.0", GSL_EDOM);
    }
  if (nu1 < 1.0)
    {
      CDF_ERROR ("nu1 < 1", GSL_EDOM);
    }
  if (nu2 < 1.0)
    {
      CDF_ERROR ("nu2 < 1", GSL_EDOM);
    }

  if (Q > 0.5)
    {
      y = gsl_cdf_beta_Qinv (Q, nu1 / 2.0, nu2 / 2.0);

      result = nu2 * y / (nu1 * (1.0 - y));
    }
  else
    {
      y = gsl_cdf_beta_Pinv (Q, nu2 / 2.0, nu1 / 2.0);

      result = nu2 * (1 - y) / (nu1 * y);
    }

  return result;
}


#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_poly.h>
#include "userFunc.h"

#define G        6.67384e-8    /* 2010 CODATA value in CGS units */
#define ALPHAMIN 1.0e-3        /* Minimum alpha allowed for numerical reasons */

/**************************************************************************/
/* This defines userFunc routines for the Krumholz & Burkert (2010) GI    */
/* disk problem.                                                          */
/**************************************************************************/

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

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

  gsl_poly_complex_workspace *wksp;
  double coef[6], sol[10];
  double omega, kappa, T1, sigma, kcrit, J, Q, d, tfast, tgrowth;
  int i, j;
  const int m=2;
  gsl_complex nu2, nu;
  double alphacoef = ((double *) params)[4];

  /* Allocate workspace */
  wksp = gsl_poly_complex_workspace_alloc(6);

  /* Store the coefficiencts of the dispersion relation polynomial
     that don't change */
  coef[2] = -8.0;
  coef[3] = coef[4] = 0.0;

  /* Loop over cells */
  for (i=0; i<grd->nr; i++) {

    /* Handle negative column densities, which may arise if the time
       step overshoots. We just need to make sure the code doesn't
       barf here, as the negative column densities will be fixed by
       the iterative solver. */
    if (col[i] <= 0.0) {
      alpha[i] = ALPHAMIN;
      continue;
    }

    /* The various quantities needed for the stability analysis */
    sigma = sqrt(pres[i]/col[i]);
    omega = grd->vphi_g[i+1]/grd->r_g[i+1];
    kappa = sqrt(2.0*(1.0+grd->beta_g[i+1]))*omega;
    T1 = -(2.0*m*omega/(kappa*grd->r_g[i+1])) *
      (2.0*m*omega/(kappa*grd->r_g[i+1])) * (grd->beta_g[i+1]-1);
    kcrit = kappa*kappa / (2.0*M_PI*G*col[i]);
    Q = kappa*sigma / (M_PI*G*col[i]);
    if (Q < 0)
      printf("Q < 0! kappa = %f, sigma = %f, col = %f\n", kappa, sigma, col[i]);
    J = sqrt(T1)/kcrit;

    /* Coefficients of the disperison relation polynomial */
    coef[0] = -Q*Q*Q*Q;
    coef[1] = 6.0*Q*Q;
    coef[5] = 16.0*J*J;

    /* Find roots, giving minima and maxima of D */
    gsl_poly_complex_solve(coef, 6, wksp, sol);

    /* For each root, compute the growth time of the instability */
    tfast = 1.0e30;
    for (j=0; j<5; j++) {

      /* Skip roots with negative real part, or imaginary part that is
	 greater than roundoff */
      if ((sol[2*j] < 0) || (fabs(sol[2*j+1]) > 1.0e-6)) continue;

      /* Compute D */
      d = (Q*Q/(4.0*sol[2*j]*sol[2*j]) - 1.0/sol[2*j]) *
	(Q*Q/(4.0*sol[2*j]*sol[2*j]) - 1.0/sol[2*j] -
	 4.0*J*J*sol[2*j]*sol[2*j]);

      /* Ignore stable roots */
      if (d > 0) continue;

      /* Get frequency and growth time */
      GSL_SET_COMPLEX(&nu2,
		      1.0 + 0.5*(Q*Q/(4.0*sol[2*j]*sol[2*j]) - 1.0/sol[2*j]),
		      0.5 * sqrt(-d));
      nu = gsl_complex_sqrt(nu2);
      tgrowth = 1.0 / (GSL_IMAG(nu)*kappa) / (2.0*M_PI/omega);

      /* Store minimum growth time */
      if (tgrowth < tfast) tfast = tgrowth;
    }

    /* Now check for gravitational instability */
    if (Q < 1) {
      GSL_SET_COMPLEX(&nu, 0.0, sqrt(1.0/(Q*Q)-1.0));
      tgrowth = 1.0 / (GSL_IMAG(nu)*kappa) / (2.0*M_PI/omega);
      if (tgrowth < tfast) tfast = tgrowth;
    }

    /* Compute alpha based on the fastest growing mode timescale */
    alpha[i] = alphacoef * exp(-tfast+1.0);
    if (alpha[i] > 1.0) alpha[i] = 1.0;
    if (alpha[i] < ALPHAMIN) alpha[i] = ALPHAMIN;
  }

  /* Free workspace */
  gsl_poly_complex_workspace_free(wksp);
}

void
userMassSrc(const double t, const double dt, const grid *grd,
	    const double *col, const double *pres, const double *eInt,
	    const double *gamma, const double *delta,
	    void *params,
	    double *massSrc) {
  /* Estimate Mdot by
     dSigma/dt = - eta_ML Sigma_SFR
               = - eta_ML eps_ff Sigma_g/t_ff,
  */

  int i;
  double shapefac = ((double *) params)[2];
  double zetad = ((double *) params)[3];
  double etaML = ((double *) params)[5];
  double epsffmax = ((double *) params)[6];
  double rhostar, a, b, c, h;
  double rhog, tff, alphavir, epsff;

  for (i=0; i<grd->nr; i++) {
    /* Stellar density */
    rhostar = shapefac * SQR(grd->vphi_g[i+1]) *
      (1.0+2.0*grd->beta_g[i+1]) /
      (4.0*M_PI*G*SQR(grd->r_g[i+1]));
    /* Get scale height from OML model */
    if (rhostar > 0.0) {
      a = 2.0*M_PI*zetad*G*rhostar*col[i];
      b = M_PI/2.0*G*SQR(col[i]);
      c = -pres[i];
      h = (-b + sqrt(b*b-4.0*a*c))/(2.0*a);
    } else {
      h = pres[i] / (M_PI/2.0*G*SQR(col[i]));
    }
    /* Gas density and free-fall time */
    rhog = col[i]/(2*h);
    tff = sqrt(3*M_PI/(32*G*rhog));
    /* Virial ratio */
    alphavir = pres[i]/(M_PI/2.0*G*col[i]*col[i]*h);
    /* epsff */
    if (alphavir <= 1.0) epsff = epsffmax;
    else epsff = epsffmax*exp(-alphavir);
    /* Mdot */
    massSrc[i] = -(1.0+etaML) * epsff * col[i] / tff;
  }
}

void
userIntEnSrc(const double t, const double dt, const grid *grd,
	     const double *col, const double *pres, const double *eInt,
	     const double *gamma, const double *delta,
	     void *params,
	     double *intEnSrc) {
  /* Cooling rate = eta Sigma sigma^2 Omega = eta P vphi/r */
  int i;
  double eta = ((double *) params)[0];
  double sigmath = ((double *) params)[1];
  double shapefac = ((double *) params)[2];
  double zetad = ((double *) params)[3];
  double sigma2, sigmaNT, rhostar, a, b, c, h;

  for (i=0; i<grd->nr; i++) {
    /* Gas velocity dispersion */
    sigma2 = pres[i]/col[i];
    if (sigma2 > SQR(sigmath)) {
      sigmaNT = sqrt(sigma2 - SQR(sigmath));
      /* Stellar density */
      rhostar = shapefac * SQR(grd->vphi_g[i+1]) *
	(1.0+2.0*grd->beta_g[i+1]) /
	(4.0*M_PI*G*SQR(grd->r_g[i+1]));
      /* Get scale height from OML model */
      if (rhostar > 0.0) {
	a = 2.0*M_PI*zetad*G*rhostar*col[i];
	b = M_PI/2.0*G*SQR(col[i]);
	c = -pres[i];
	h = (-b + sqrt(b*b-4.0*a*c))/(2.0*a);
      } else {
	h = pres[i] / (M_PI/2.0*G*SQR(col[i]));
      }
      /* Cooling rate = eta Sigma sigmaNT^2 / (h/sigmaNT) */
      intEnSrc[i] = -eta * col[i] * SQR(sigmaNT) / (h/sigmaNT);
    } else {
      intEnSrc[i] = 0.0;
    }
  }
}

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

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

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

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

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

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


#include <stdio.h>
#include <gsl/gsl_blas.h>

int main()
{
  double a[] = { 0.11, 0.12, 0.13,
                0.21, 0.22, 0.23 };

  double b[] = { 1011, 1012,
                 1021, 1022,
                 1031, 1032 };

  double c[] = { 0.00, 0.00,
                 0.00, 0.00 };

  gsl_matrix_view A = gsl_matrix_view_array(a, 2, 3);
  gsl_matrix_view B = gsl_matrix_view_array(b, 3, 2);
  gsl_matrix_view C = gsl_matrix_view_array(c, 2, 2);

  gsl_matrix *D = gsl_matrix_alloc(2, 3);
  int i, j;
  for (i=1; i<3; i++)
    for (j=1; j<4; j++)
      gsl_matrix_set (D, i-1, j-1, ((float) i)/10 + ((float) j)/100);

  for (i=0; i<2; i++)
    for (j=0; j<3; j++)
      printf("D(%d,%d) = %f\n", i, j, gsl_matrix_get(D, i, j));

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                 1.0, D, &B.matrix,
                 0.0, &C.matrix);

  printf ("[ %g, %g\n", c[0], c[1]);
  printf ("  %g, %g ]\n", c[2], c[3]);

  return 0;
}