/* The MIT License

   Copyright (c) 2013-2015 Genome Research Ltd.

   Author: Petr Danecek <pd3@sanger.ac.uk>

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.

 */

#include "peakfit.h"
#include <stdio.h>
#include <gsl/gsl_version.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>
#include <htslib/hts.h>
#include <htslib/kstring.h>
#include <assert.h>
#include <math.h>

#define NPARAMS 5

// gauss params: sqrt(scale), center, sigma
typedef struct _peak_t
{
    int fit_mask;
    double params[NPARAMS], ori_params[NPARAMS];    // current and input parameters
    struct { int scan; double min, max, best; } mc[NPARAMS];    // monte-carlo settings and best parameter
    void (*calc_f)  (int nvals, double *xvals, double *yvals, void *args);
    void (*calc_df) (int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args);
    void (*print_func) (struct _peak_t *pk, kstring_t *str);
    void (*convert_get) (struct _peak_t *pk, double *params);
    double (*convert_set) (struct _peak_t *pk, int iparam, double value);
}
peak_t;

struct _peakfit_t
{
    int npeaks, mpeaks, nparams, mparams;
    peak_t *peaks;
    double *params;
    int nvals, mvals;
    double *xvals, *yvals, *vals;
    kstring_t str;
    int verbose, nmc_iter;
};

/*
    Gaussian peak with the center bound in the interval <d,e>:
        yi = scale^2 * exp(-(xi-z)^2/sigma^2)

        dy/dscale  = 2*scale * EXP
        dy/dcenter = -scale^2 * sin(center) * (e-d) * (xi - z) * EXP / sigma^2
        dy/dsigma  = 2*scale^2 * (xi - z)^2 * EXP / sigma^3

    where
        z   = 0.5*(cos(center)+1)*(e-d) + d
        EXP = exp(-(xi-z)^2/sigma^2)
*/
void bounded_gaussian_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;

    int i;
    for (i=0; i<nvals; i++)
    {
        double tmp = (xvals[i] - z)/sigma;
        yvals[i] += scale2 * exp(-tmp*tmp);
    }
}
void bounded_gaussian_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;

    int i;
    for (i=0; i<nvals; i++)
    {
        double EXP = exp(-(xvals[i]-z)*(xvals[i]-z)/sigma/sigma);
        double zi  = xvals[i] - z;
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==1 )  // dcenter
            dfvals[i] -= scale*scale*sin(center)*(e-d)*zi*EXP/sigma/sigma;
        else if ( idf==2 )  // dsigma
            dfvals[i] += 2*scale*scale*zi*zi*EXP/sigma/sigma/sigma;
    }
}
void bounded_gaussian_sprint_func(peak_t *pk, kstring_t *str)
{
    double center = pk->params[1];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;
    ksprintf(str,"%f**2 * exp(-(x-%f)**2/%f**2)",fabs(pk->params[0]),z,fabs(pk->params[2]));
}
double bounded_gaussian_convert_set(peak_t *pk, int iparam, double value)
{
    if ( iparam!=1 ) return value;
    double d = pk->ori_params[3];
    double e = pk->ori_params[4];
    if ( value<d ) value = d;
    else if ( value>e ) value = e;
    return acos(2*(value-d)/(e-d) - 1);
}
void bounded_gaussian_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[2] = fabs(pk->params[2]);
    double center = pk->params[1];
    double d = pk->params[3];
    double e = pk->params[4];
    params[1] = 0.5*(cos(center)+1)*(e-d) + d;
}

void peakfit_add_bounded_gaussian(peakfit_t *pkf, double a, double b, double c, double d, double e, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    assert( d<e );

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = bounded_gaussian_calc_f;
    pk->calc_df     = bounded_gaussian_calc_df;
    pk->print_func  = bounded_gaussian_sprint_func;
    pk->convert_set = bounded_gaussian_convert_set;
    pk->convert_get = bounded_gaussian_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[2] = c;
    pk->ori_params[3] = d;
    pk->ori_params[4] = e;
    pk->ori_params[1] = pk->convert_set(pk, 1, b);
}

/*
    Gaussian peak:
        yi = scale^2 * exp(-(x-center)^2/sigma^2)

        dy/dscale  = 2 * scale * EXP
        dy/dcenter = 2 * scale^2 * (x-center) * EXP / sigma^2
        dy/dsigma  = 2 * scale^2 * (x-center)^2 * EXP / sigma^3

    where
        EXP = exp(-(x-center)^2/sigma^2)
*/
void gaussian_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double tmp = (xvals[i] - center)/sigma;
        yvals[i] += scale2 * exp(-tmp*tmp);
    }
}
void gaussian_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double zi  = xvals[i] - center;
        double EXP = exp(-zi*zi/(sigma*sigma));
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==1 )  // dcenter
            dfvals[i] += 2*scale*scale*zi*EXP/(sigma*sigma);
        else if ( idf==2 )  // dsigma
            dfvals[i] += 2*scale*scale*zi*zi*EXP/(sigma*sigma*sigma);
    }
}
void gaussian_sprint_func(struct _peak_t *pk, kstring_t *str)
{
    ksprintf(str,"%f**2 * exp(-(x-%f)**2/%f**2)",fabs(pk->params[0]),pk->params[1],fabs(pk->params[2]));
}
void gaussian_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[1] = fabs(pk->params[1]);
    params[2] = fabs(pk->params[2]);
}

void peakfit_add_gaussian(peakfit_t *pkf, double a, double b, double c, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = gaussian_calc_f;
    pk->calc_df     = gaussian_calc_df;
    pk->print_func  = gaussian_sprint_func;
    pk->convert_get = gaussian_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[1] = b;
    pk->ori_params[2] = c;
}

/*
    exp peak:
        yi = scale^2 * exp((x-center)/sigma^2)

        dy/dscale  = 2 * scale * EXP
        dy/dcenter = -scale^2  * EXP / sigma^2
        dy/dsigma  = -2 * scale^2 * (x-center) * EXP / sigma^3

    where
        EXP = exp((x-center)/sigma^2)
*/
void exp_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        yvals[i] += scale2 * exp((xvals[i]-center)/sigma/sigma);
    }
}
void exp_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double EXP = exp((xvals[i]-center)/sigma/sigma);
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==2 )  // dsigma
            dfvals[i] -= 2*scale*scale*(xvals[i]-center)*EXP/sigma/sigma/sigma;
    }
}
void exp_sprint_func(struct _peak_t *pk, kstring_t *str)
{
    ksprintf(str,"%f**2 * exp((x-%f)/%f**2)",fabs(pk->params[0]),pk->params[1],fabs(pk->params[2]));
}
void exp_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[1] = fabs(pk->params[1]);
    params[2] = fabs(pk->params[2]);
}

void peakfit_add_exp(peakfit_t *pkf, double a, double b, double c, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    assert( !(fit_mask&2) );

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = exp_calc_f;
    pk->calc_df     = exp_calc_df;
    pk->print_func  = exp_sprint_func;
    pk->convert_get = exp_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[1] = b;
    pk->ori_params[2] = c;
}

void peakfit_set_params(peakfit_t *pkf, int ipk, double *params, int nparams)
{
    peak_t *pk = &pkf->peaks[ipk];
    int i;
    if ( pk->convert_set )
        for (i=0; i<nparams; i++) pk->params[i] = pk->convert_set(pk, i, params[i]);
    else
        for (i=0; i<nparams; i++) pk->params[i] = params[i];
}

void peakfit_get_params(peakfit_t *pkf, int ipk, double *params, int nparams)
{
    peak_t *pk = &pkf->peaks[ipk];
    if ( pk->convert_get ) pk->convert_get(pk, params);
    else
    {
        int i;
        for (i=0; i<nparams; i++) params[i] = pk->params[i];
    }
}

peakfit_t *peakfit_init(void)
{
    return (peakfit_t*)calloc(1,sizeof(peakfit_t));
}

void peakfit_reset(peakfit_t *pkf)
{
    pkf->npeaks = pkf->nparams = 0;
    memset(pkf->peaks,0,sizeof(peak_t)*pkf->mpeaks);
}

void peakfit_destroy(peakfit_t *pkf)
{
    free(pkf->str.s);
    free(pkf->vals);
    free(pkf->params);
    free(pkf->peaks);
    free(pkf);
}

int peakfit_calc_f(const gsl_vector *params, void *data, gsl_vector *yvals)
{
    peakfit_t *pkf = (peakfit_t *) data;

    int i,j;
    for (i=0; i<pkf->nvals; i++)
        pkf->vals[i] = 0;

    int iparam = 0;
    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            pk->params[j] = gsl_vector_get(params,iparam);
            iparam++;
        }
        pk->calc_f(pkf->nvals, pkf->xvals, pkf->vals, pk);
    }

    for (i=0; i<pkf->nvals; i++)
        gsl_vector_set(yvals, i, (pkf->vals[i] - pkf->yvals[i])/0.01);

    return GSL_SUCCESS;
}
int peakfit_calc_df(const gsl_vector *params, void *data, gsl_matrix *jacobian)
{
    peakfit_t *pkf = (peakfit_t *) data;

    int i,j,k,iparam = 0;
    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        int iparam_prev = iparam;
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            pk->params[j] = gsl_vector_get(params,iparam);
            iparam++;
        }
        iparam = iparam_prev;
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            for (k=0; k<pkf->nvals; k++) pkf->vals[k] = 0;
            pk->calc_df(pkf->nvals, pkf->xvals, pkf->yvals, pkf->vals, j, pk);
            for (k=0; k<pkf->nvals; k++) gsl_matrix_set(jacobian, k, iparam, pkf->vals[k]);
            iparam++;
        }
    }
    return GSL_SUCCESS;
}
int peakfit_calc_fdf(const gsl_vector *params, void *data, gsl_vector *yvals, gsl_matrix *jacobian)
{
    peakfit_calc_f(params, data, yvals);
    peakfit_calc_df(params, data, jacobian);
    return GSL_SUCCESS;
}

double peakfit_evaluate(peakfit_t *pkf)
{
    int i;
    for (i=0; i<pkf->nvals; i++)
        pkf->vals[i] = 0;

    for (i=0; i<pkf->npeaks; i++)
        pkf->peaks[i].calc_f(pkf->nvals, pkf->xvals, pkf->vals, &pkf->peaks[i]);

    double sum = 0;
    for (i=0; i<pkf->nvals; i++)
        sum += fabs(pkf->vals[i] - pkf->yvals[i]);

    return sum;
}

const char *peakfit_sprint_func(peakfit_t *pkf)
{
    pkf->str.l = 0;
    int i;
    for (i=0; i<pkf->npeaks; i++)
    {
        if ( i>0 ) kputs(" + ", &pkf->str);
        pkf->peaks[i].print_func(&pkf->peaks[i], &pkf->str);
    }
    return (const char*)pkf->str.s;
}

void peakfit_verbose(peakfit_t *pkf, int level)
{
    pkf->verbose = level;
}

void peakfit_set_mc(peakfit_t *pkf, double xmin, double xmax, int iparam, int niter)
{
    peak_t *pk = &pkf->peaks[ pkf->npeaks-1 ];
    pk->mc[iparam].scan = 1;
    pk->mc[iparam].min  = xmin;
    pk->mc[iparam].max  = xmax;
    pkf->nmc_iter = niter;
}

double peakfit_run(peakfit_t *pkf, int nvals, double *xvals, double *yvals)
{
    srand(0);   // for reproducibility

    pkf->nvals = nvals;
    pkf->xvals = xvals;
    pkf->yvals = yvals;
    hts_expand0(double,pkf->nvals,pkf->mvals,pkf->vals);
    if ( !pkf->nparams ) return peakfit_evaluate(pkf);

    gsl_multifit_function_fdf mfunc;
    mfunc.f   = &peakfit_calc_f;
    mfunc.df  = &peakfit_calc_df;
    mfunc.fdf = &peakfit_calc_fdf;
    mfunc.n   = nvals;
    mfunc.p   = pkf->nparams;
    mfunc.params = pkf;

    const gsl_multifit_fdfsolver_type *solver_type;
    gsl_multifit_fdfsolver *solver;
    solver_type = gsl_multifit_fdfsolver_lmsder;
    solver = gsl_multifit_fdfsolver_alloc(solver_type, nvals, mfunc.p);
    gsl_vector *grad = gsl_vector_alloc(pkf->nparams);

    int imc_iter, i,j, iparam;
    double best_fit = HUGE_VAL;
    for (imc_iter=0; imc_iter<=pkf->nmc_iter; imc_iter++)   // possibly multiple monte-carlo iterations
    {
        // set GSL parameters
        iparam = 0;
        for (i=0; i<pkf->npeaks; i++)
        {
            peak_t *pk = &pkf->peaks[i];
            for (j=0; j<NPARAMS; j++)
            {
                pk->params[j] = pk->ori_params[j];
                if ( pk->mc[j].scan )
                {
                    pk->params[j] = rand()*(pk->mc[j].max - pk->mc[j].min)/RAND_MAX + pk->mc[j].min;
                    if ( pk->convert_set ) pk->params[j] = pk->convert_set(pk, j, pk->params[j]);
                }
                if ( !(pk->fit_mask & (1<<j)) ) continue;
                pkf->params[iparam] = pk->params[j];
                iparam++;
            }
        }

        gsl_vector_view vview = gsl_vector_view_array(pkf->params, mfunc.p);
        gsl_multifit_fdfsolver_set(solver, &mfunc, &vview.vector);

        // iterate until convergence (or lack of it)
        int ret, test1 = 0, test2 = 0, niter = 0, niter_max = 500;
        do
        {
            ret = gsl_multifit_fdfsolver_iterate(solver);
            if ( pkf->verbose >1 )
            {
                fprintf(stderr, "%d: ", niter);
                for (i=0; i<pkf->npeaks; i++)
                {
                    peak_t *pk = &pkf->peaks[i];
                    fprintf(stderr,"\t%f %f %f", pk->params[0],pk->params[1],pk->params[2]);
                }
                fprintf(stderr, "\t.. %s\n", gsl_strerror(ret));
            }
            if ( ret ) break;

#if GSL_MAJOR_VERSION >= 2
            int info;
            test1 = gsl_multifit_fdfsolver_test(solver, 1e-8,1e-8, 0.0, &info);
#else
            gsl_multifit_gradient(solver->J, solver->f, grad);
            test1 = gsl_multifit_test_gradient(grad, 1e-8);
            test2 = gsl_multifit_test_delta(solver->dx, solver->x, 1e-8, 1e-8);
#endif
        }
        while ((test1==GSL_CONTINUE || test2==GSL_CONTINUE) && ++niter<niter_max);
        if ( pkf->verbose >1 )
        {
            fprintf(stderr,"test1=%s\n", gsl_strerror(test1));
            fprintf(stderr,"test2=%s\n", gsl_strerror(test2));
        }

        // recover parameters
        iparam = 0;
        for (i=0; i<pkf->npeaks; i++)
        {
            peak_t *pk = &pkf->peaks[i];
            for (j=0; j<NPARAMS; j++)
            {
                if ( !(pk->fit_mask & (1<<j)) ) continue;
                pk->params[j] = gsl_vector_get(solver->x, iparam++);
            }
        }

        // evaluate fit, update best parameters
        double fit = peakfit_evaluate(pkf);
        if ( fit<best_fit )
        {
            for (i=0; i<pkf->npeaks; i++)
            {
                peak_t *pk = &pkf->peaks[i];
                for (j=0; j<NPARAMS; j++) pk->mc[j].best = pk->params[j];
            }
        }
        if ( fit<best_fit ) best_fit = fit;
    }
    gsl_multifit_fdfsolver_free(solver);
    gsl_vector_free(grad);

    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        for (j=0; j<NPARAMS; j++) pk->params[j] = pk->mc[j].best;
    }
    return best_fit;
}


/*! \file GMatrix.h
 *  \brief this file defines the \ref Math::GMatrix class that is part of the Math namespace
 */

#ifndef GMATRIX_H
#define GMATRIX_H

#include <iostream>
#include <complex>
#include <cstdlib>
#include <cstdio>
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <Precision.h>
#include <Coordinate.h>
#include <Matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

using namespace std;
namespace Math
{

/*! \class Math::GMatrix
    \brief General n x m matrix
    General matrix class that eventually should replace \ref Math::Matrix class.
    will have to adjust error checks for this class and adjust inverse
    and make most calls inline to increase speed.
*/
class GMatrix
{
    private:

    int row,col;
    Double_t *matrix;

    protected:

    ///useful comparison functions which can be used with qsort
    int compare_Double(const void* a,const void* b) {
        Double_t arg1 = *((Double_t*) a);
        Double_t arg2 = *((Double_t*) b);
        if( arg1 < arg2 ) return -1;
        else if( arg1==arg2 ) return 0;
        else return 1;
    }

    public:

    /// \name Constructors & Destructors
    //@{
    /// Constructor (where data can be is initilized) otherwise set to zero
    GMatrix(int r, int c, Double_t *data=NULL)
    {
        if (r*c>0){
            row=r;col=c;
            matrix=new Double_t[row*col];
            if (data!=NULL){
                for (int i = 0; i < row; i++)
                    for (int j = 0; j < col; j++)
                        matrix[i*col+j] = data[i*col+j];
            }
            else{
                for (int i = 0; i < row; i++)
                    for (int j = 0; j < col; j++)
                        matrix[i*col+j] = 0.;
            }
        }
        else {
            row=0;col=0;matrix=NULL;
            printf("Error, net size 0 with row,col=%d,%d\n",row,col);
        }
    }

    /// Constructor based on Coordinate.
    GMatrix(const Coordinate &c)
    {
        row=3;col=1;
        matrix=new Double_t[row*col];
        for (int i = 0; i < row; i++) matrix[i] = c[i];
    }

    /// Constructor Matrix
    GMatrix(const Matrix &m)
    {
        row=3;col=3;
        matrix=new Double_t[row*col];
        for (int i=0; i<row; i++)
            for (int j=0;j<col;j++) matrix[i*col+j] = m(i,j);
    }

    /// Copy Constructor
    GMatrix(const GMatrix &m)
    {
        row=m.row;col=m.col;
        matrix=new Double_t[row*col];
        for (int i = 0; i < row; i++)
            for (int j = 0; j < col; j++)
                matrix[i*col+j] = m.matrix[i*col+j];
    }

    /// Identity Matrix of dimension N
    GMatrix(int N)
    {
        row=N;col=N;
        matrix=new Double_t[row*col];
        for (int i = 0; i < row; i++)
            for (int j = 0; j < col; j++)
                matrix[i*col+j] = (i==j);
    }

    ~GMatrix(){
        if (row>0&&col>0) delete[] matrix;
    }
    //@}

    /// \name Get & Set functions
    //@{
    ///get row dimensions
    int Row()const {return row;}
    ///get col dimensions
    int Col()const {return col;}
    ///get rank
    int Rank()const {return min(row,col);}
    //@}

    /// \name Overloaded Operators
    //@{
    /// How to access a matrix element: M(i, j)
    Double_t& operator () (int i, int j) {
        //if (j<col&&i<row)
        return matrix[j + col * i];
        //else {printf("Error, (i,j)=(%d, %d) exceed matrix dimesions of %d %d \n", i,j,row,col); return 0;}
    }
    const Double_t& operator() (int i, int j) const
    {
        //if (j<col&&i<row)
        return matrix[j + col * i];
        //else {printf("Error, (i,j)=(%d, %d) exceed matrix dimesions of %d %d \n", i,j,row,col); return 0;}
    }

    std::runtime_error incompatible_dimensions_error(const GMatrix &m)
    {
        std::ostringstream os;
        os << "Error, incompatible dimensions: "
           << "(" << row << "," << col << ") + "
           << "(" << m.row << "," << m.col << ")";
        return std::runtime_error(os.str());
    }

    /// Matrix addition
    GMatrix operator + (const GMatrix& m)
    {
        if (row==m.row&&col==m.col){
            Double_t data[row*col];
            for (int i = 0; i < row; i++)
                for (int j = 0; j < col; j++)
                    data[i*col+j] = matrix[i*col+j]+m.matrix[i*col+j];
            return GMatrix(row,col,data);
        }
        throw incompatible_dimensions_error(m);
    }

    /// Matrix subtraction
    GMatrix operator - (const GMatrix& m)
    {
        if (row==m.row&&col==m.col){
            Double_t data[row*col];
            for (int i = 0; i < row; i++)
                for (int j = 0; j < col; j++)
                    data[i*col+j] = matrix[i*col+j]-m.matrix[i*col+j];
            return GMatrix(row,col,data);
        }
        throw incompatible_dimensions_error(m);
    }

    /// Multiply by a scalar: a * M
    friend GMatrix operator * (Double_t a, const GMatrix& m);

    /// Multiply by a scalar: M * a
    GMatrix operator * (Double_t a)
    {
        Double_t data[row*col];
        for (int i = 0; i < row; i++)
            for (int j = 0; j < col; j++)
                data[i*col+j] = matrix[i*col+j]*a;
        return GMatrix(row,col,data);
    }

    /// Multiply by another Gmatrix
    GMatrix operator * (const GMatrix& m)
    {
        if (col==m.row){
            Double_t data[row*m.col];
            for (int i = 0; i < row; i++)
                for (int j = 0; j < m.col; j++){
                    data[i*m.col+j]=0.;
                    for (int k=0;k<col;k++)
                        data[i*m.col+j]+=matrix[i*col+k]*m.matrix[k*m.col+j];
                }
            return GMatrix(row,m.col,data);
        }
        else {
            printf("Error, incompatible dimensions (%d,%d) + (%d,%d)\n",row,col,m.row,m.col);
            return GMatrix(0,0);
        }

    }
    /// Asignment constructor
    GMatrix& operator = (const GMatrix m)
    {
        delete[] matrix;
        row=m.row;
        col=m.col;
        matrix=new Double_t[row*col];
        for (int i = 0; i < row; i++)
            for (int j = 0; j < col; j++)
                matrix[i*col+j] = m.matrix[i*col+j];
        return *this;
    }
    //@}

    /// \name Mathematical Operations
    //@{

    /// Trace the matrix.
    Double_t Trace() const
    {
        Double_t trace=1.0;
        for (int i = 0; i < row; i++)trace*=matrix[i*col+i];
        return trace;
    }

    /// return the Transpose of the matrix.
    GMatrix Transpose() const
    {
        Double_t data[col*row];
        for (int i = 0; i < col; i++)
            for (int j = 0; j < row; j++)
                data[i*row+j]=(*this)(i,j);
        return GMatrix(col,row,data);
    }

    /// Transpose matrix in place (changes the matrix permanently!)
    void TransposeInPlace()
    {
        Double_t data[col*row];
        int temp;
        for (int i = 0; i < col; i++)
            for (int j = 0; j < row; j++)
                data[i*row+j]=(*this)(i,j);
        temp=row;row=col;col=temp;
        for (int i = 0; i < row; i++)
            for (int j = 0; j < col; j++)
                matrix[i*col+j]=data[i*col+j];
    }
    ///Get the diagonals
    GMatrix Diag(){
        GMatrix D(row,col);
        for (int i = 0; i < col; i++) D(i,i)=matrix[i*col+i];
        return D;
    }
    /// Return Pivot matrix P of A, where P*A=B, such that B(i,i)!=0
    GMatrix Pivot() const
    {
        Double_t a[row*col];
        GMatrix P(row,col);
        for (int j=0;j<row;j++) P(j,j)=1;
        if (Trace()==0) {
            for (int j=0;j<row;j++) for (int k=0;k<row;k++) a[j*col+k]=matrix[j*col+k];
            for (int j=0;j<row;j++) if (a[j*col+j]*a[j*col+j]<1e-32) {
                int k=j+1,ipivot;
                if (k==row)k=0;
                ipivot=(P(k,k)==1);
                while (!(a[j*col+k]*a[j*col+k]>1e-32&&ipivot)){
                    k++;
                    if (k==row)k=0;
                    ipivot=(P(k,k)==1);
                }
                P(j,j)=0;P(k,k)=0;
                P(j,k)=1;P(k,j)=1;
                for (int l=0;l<row;l++) {
                    Double_t temp=a[l*col+j];
                    a[l*col+j]=a[l*col+k];
                    a[l*col+k]=temp;
                }
            }
        }
        return P;
    }
    /// Return Pivot matrix P of A, where P*A=B, such that B(i,i)!=0
    GMatrix PivotInPlace()
    {
        GMatrix P(row,col);
        for (int j=0;j<row;j++) P(j,j)=1;
        if (Trace()==0) {
            for (int j=0;j<row;j++) if (matrix[j*col+j]*matrix[j*col+j]<1e-32) {
                int k=j+1,ipivot;
                if (k==row)k=0;
                ipivot=(P(k,k)==1);
                while (!(matrix[j*col+k]*matrix[j*col+k]>1e-32&&ipivot)){
                    k++;
                    if (k==row)k=0;
                    ipivot=(P(k,k)==1);
                }
                P(j,j)=0;P(k,k)=0;
                P(j,k)=1;P(k,j)=1;
                for (int l=0;l<row;l++) {
                    Double_t temp=matrix[l*col+j];
                    matrix[l*col+j]=matrix[l*col+k];
                    matrix[l*col+k]=temp;
                }
            }
        }
        return P;
    }

    /// return a SubMatrix of the matrix from row ra to  row rb and col ca to cb
    GMatrix SubMatrix(int ra, int rb, int ca, int cb) const
    {
        int r=(rb-ra+1), c=(cb-ca+1);
        Double_t data[r*c];
        for (int i = ra; i <= rb; i++)
            for (int j = ca; j <=cb; j++)
                data[(i-ra)*c+(j-ca)]=(*this)(i,j);
        return GMatrix((rb-ra+1),(cb-ca+1),data);
    }

    /// return a submatrix ignore column r, c
    GMatrix ExtractMatrix(int r, int c) const
    {
        Double_t data[(row-1)*(col-1)];
        int tempi=0;
        for (int i = 0; i < row; i++)
        for (int j = 0; j <col; j++)
            if (i!=r&&j!=c) data[tempi++]=(*this)(i,j);
        return GMatrix((row-1),(col-1),data);
    }

    /// Check if matrix square symmetric
    int isSymmetric() const
    {
        if (row!=col) return 0;
        for (int i=0;i<row;i++)
        for (int j=i+1;j<col-1;j++)
            if (((*this)(i,j)-(*this)(j,i))*((*this)(i,j)-(*this)(j,i))/(*this)(i,j)/(*this)(i,j)>1e-8) return 0;
        return 1;
    }
    /// Check if matrix is zero matrix
    int isZero() const
    {
        Double_t sm=0;
        for (int i=0;i<row;i++)
            for (int j=0;j<col;j++)
                sm+=matrix[i*row+j]*matrix[i*row+j];
        if (sm==0.0) return 1;
        else return 0;
    }

    /// Determinant of matrix, for now recursively implemented.
    Double_t Det() const
    {
        Double_t total =0.;
        int tempi;
        if (row==col) {
            if (row==2) return matrix[0]*matrix[3]-matrix[1]*matrix[2];
            else {
                for (int i = 0; i < col; i++) {
                    Double_t data[(row-1)*(row-1)];
                    tempi=0;
                    for (int j=1;j<row;j++)
                        for (int k=0;k<col;k++)
                            if (k!=i) data[tempi++]=(*this)(j,k);
                    total+=matrix[i]*(GMatrix(row-1,col-1,data)).Det();
                }
                return total;
            }
        }
        else {
            printf("not a square matrix %d, %d returning 0\n",row,col);
            return 0.;
        }
    }

    /*! \brief Lower-Upper decompistion

        LU decomposition of matrix stored in such a fashion that
        matrix return is (for 3x3):
        \f[
        \left(\begin{array}{ccc}
            L11 & U12 &U13\\
            L21 & L22 & U23\\
            L31 & L32 & L33
        \end{array}\right)
        \f]
        L is lower triangular matrix, U is upper unit triangular matrix. \n
        Crout's algorithm is used to determine the L and U matrices. System of equations to solve are : \n
        \f[
        \begin{array}{lcl}
            i<j : & l_{i1}*u_{1j}+l_{i2}*u_{2j}+...+l_{ii}*u_{ij}&=a_{ij} \\
            i=j : & l_{i1}*u_{1j}+l_{i2}*u_{2j}+...+l_{ii}*u_{jj}&=a_{ij} \\
            i>j : & l_{i1}*u_{1j}+l_{i2}*u_{2j}+...+l_{ij}*u_{ij}&=a_{ij}
            \end{array}
        \f]
        with condition that \f$ l_{ii}=1 \f$. \n
        If matrix singular and no decomposition, print error and return zero matrix.
        Here matrix is singular if diagonal element is zero. \n
        Must alter this to pivot method so that subroutine reorder matrix if necessary and diagonals
        can be zero.
    */
    GMatrix LUDecomp() const
    {
        if (row!=col) {
            printf("only implemented for square, returning zeros\n");
            return GMatrix(row,col);
        }
        else {
            Double_t A[row*col];
            Int_t i, j, k, p, p_k, p_row, p_col;
            for (j=0;j<row;j++) for (k=0;k<col;k++) A[j*col+k]=matrix[j*col+k];
            for (k = 0, p_k = 0; k < row; p_k += row, k++) {
                for (i = k, p_row = p_k; i < row; p_row += row, i++) {
                    for (p = 0, p_col = 0; p < k; p_col += row, p++)
                        A[p_row + k] -= A[p_row + p] * A[p_col + k];
                }
                if ( A[p_k + k] == 0.0 ) {
                    printf("Singular matrix, returning zero matrix\n");
                    return GMatrix(row,col);
                }
                for (j = k+1; j < row; j++) {
                    for (p = 0, p_col = 0; p < k; p_col += row,  p++)
                        A[p_k + j] -= A[p_k + p] * A[p_col + j];
                    A[p_k + j] /= A[p_k + k];
                }
            }
            return GMatrix(row,col,A);
        }
    }
    /*! \brief Calculate the Inverse of the lower triangular matrix

        This routine calculates the inverse of the lower triangular matrix L.
        The superdiagonal part of the matrix is not addressed.
        The algorithm follows: \n
        Let M be the inverse of L, then \f$ L M = I \f$, \n
        \f$ M[i][i] = 1.0 / L[i][i] \f$ for i = 0, ..., row-1, and \n
        \f$ M[i][j] = -[(L[i][j] M[j][j] + ... + L[i][i-1] M[i-1][j])] / L[i][i], \f$
        for i = 1, ..., row-1, j = 0, ..., i - 1.
    */
    GMatrix Lower_Triangular_Inverse()
    {
        Double_t L[row*col];
        int i, j, k, p_i, p_j, p_k;
        Double_t sum;
        for (j=0;j<row;j++) for (k=0;k<=j;k++) L[j*col+k]=matrix[j*col+k];
        for (j=0;j<row;j++) for (k=j+1;k<col;k++) L[j*col+k]=0;
        //Invert the diagonal elements of the lower triangular matrix L.
        for (k = 0, p_k = 0; k < row; p_k += (row + 1), k++) {
            if (L[p_k] == 0.0) return GMatrix(row,row);//error, returning zero matrix
            else L[p_k] = 1.0 / L[p_k];
        }
        //Invert the remaining lower triangular matrix L row by row.
        for (i = 1, p_i = 0 + row; i < row; i++, p_i += row) {
            for (j = 0, p_j = 0; j < i; p_j += row, j++) {
                sum = 0.0;
                for (k = j, p_k = p_j; k < i; k++, p_k += row)
                    sum += L[p_i + k] * L[p_k + j];
                L[p_i + j] = - L[p_i + i] * sum;
            }
        }
        return GMatrix(row,row,L);
    }
    /*! \brief Calculate the Inverse of the lower triangular matrix

        This routine calculates the inverse of the unit upper triangular matrix U.
        The subdiagonal part of the matrix is not addressed.
        The diagonal is assumed to consist of 1's and is not addressed.
        The algorithm follows: \n
        Let M be the inverse of U, then \f$ U M = I \f$, \n
        \f$ M[i][j] = -( U[i][i+1] M[i+1][j] + ... + U[i][j] M[j][j] ), \f$
        for i = row-2, ... , 0,  j = row-1, ..., i+1.
    */
    GMatrix Unit_Upper_Triangular_Inverse()
    {
        Double_t U[row*col];
        int i, j, k, p_i, p_k;
        for (j=0;j<row;j++) for (k=j+1;k<col;k++) U[j*col+k]=matrix[j*col+k];
        for (j=0;j<row;j++) for (k=0;k<=j;k++) U[j*col+k]=(j==k);
        //         Invert the superdiagonal part of the matrix U row by row where
        //         the diagonal elements are assumed to be 1.0.
        for (i = row - 2, p_i = 0 + row * (row - 2); i >=0; p_i -= row, i-- ) {
            for (j = row - 1; j > i; j--) {
                U[p_i + j] = -U[p_i + j];
                for (k = i + 1, p_k = p_i + row; k < j; p_k += row, k++ )
                    U[p_i + j] -= U[p_i + k] * U[p_k + j];
            }
        }
        return GMatrix(row,row,U);
    }

    /// \brief Adjugate (classical adjoint) of matrix
    /// This is simply the transpose of the cofactor of the matrix
    GMatrix Adjugate() const
    {
        GMatrix cofactorT(row,col);
        for (int i=0;i<row;i++)
            for (int j=0;j<col;j++)
                cofactorT(j,i)=(ExtractMatrix(i,j)).Det()*pow(-1.,i+j);
        return cofactorT;
    }

    /// Inverse of matrix
    GMatrix Inverse() const
    {
        if (row!=col) {
            printf("only implemented for square, returning zeros\n");
            return GMatrix(row,col);
        }
        GMatrix LU(LUDecomp());
        Double_t d=1.0;
        for (int i=0;i<row;i++) d*=LU(i,i);
        if (d  == 0.0) return GMatrix(row, col);
        else {
            GMatrix Uinv=LU.Unit_Upper_Triangular_Inverse();
            GMatrix Linv=LU.Lower_Triangular_Inverse();
            return Uinv*Linv;
        }
    }

    /// Inverse of matrix using a pivot (needed in case diagonals are zero)
    GMatrix InversewithPivot() const
    {
        if (row!=col) {
            printf("only implemented for square, returning zeros\n");
            return GMatrix(row,col);
        }
        GMatrix A(*this);
        GMatrix Pivot(A.PivotInPlace());
        GMatrix LU(A.LUDecomp());
        Double_t d=1.0;
        for (int i=0;i<row;i++) d*=LU(i,i);
        if (d  == 0.0) return GMatrix(row, col);
        else {
            GMatrix Uinv=LU.Unit_Upper_Triangular_Inverse();
            GMatrix Linv=LU.Lower_Triangular_Inverse();
            return Uinv*Linv*Pivot;
        }
    }

    /// helpful rotation definition used in \ref Math::GMatrix::Jacobi (see Numerical recipes)
    inline void ROTATE(GMatrix &m, int i, int j, int k, int l, Double_t sinrot, Double_t tanrot) const {
        Double_t tempa=m(i,j),tempb=m(k,l);
        m(i,j)=tempa-sinrot*(tempb+tempa*tanrot);m(k,l)=tempb+sinrot*(tempa-tempb*tanrot);
    }
    /// Applies jacobi transformations to a copy of a matrix and returns effectively the eigenvalues of the matrix
    GMatrix Jacobi(Double_t tol=1e-2) const
    {
        GMatrix m(*this);
        Double_t eigen[row], b[row], z[row];
        GMatrix Prot(row);//identity
        int nrot=0;
        Double_t crot,srot,trot2,thres,t,theta,sm,temp1,temp2;

        //initialize vectors to diagonal elements of matrix
        for (int i=0;i<row;i++){ eigen[i]=b[i]=m(i,i);z[i]=0.;}

        do {
            //first check to see if off diagonal elements are zero
            // could check to within some precision relative to trace
            sm=0.;
            for (int ip=0;ip<row-1;ip++)
            for (int iq=ip+1;iq<col;iq++)
                sm+=sqrt(m(ip,iq)*m(ip,iq));
            if (sm==0.) return GMatrix(row,1,eigen);
            //set the threshold to be 0 until several rotations have been applied
            nrot<4? thres=tol*sm/(Double_t)(row*row): thres=0.;
            //now start applying Jacobi rotations
            for (int ip=0;ip<row-1;ip++){
                for (int iq=ip+1;iq<col;iq++){
                    //get absolute value of off diagonal element and check if it is small in comparison to
                    //threshold factor of 1/5 is from numerical recipes
                    temp1=sqrt(m(ip,iq)*m(ip,iq))*0.2;
                    //if smaller than tolerance set to 0. after for sweeps and skip the rotation.
                    if (nrot > 4 && (sqrt(eigen[ip]*eigen[ip])+temp1)==(sqrt(eigen[ip]*eigen[ip])+tol)
                        && (sqrt(eigen[iq]*eigen[iq])+temp1)==(sqrt(eigen[iq]*eigen[iq])+tol))
                        m(ip,iq)=0.0;
                    else if (thres<temp1*5.0) {
                        temp2=eigen[iq]-eigen[ip];
                        //find the solution to t^2+2*t*theta-1=0
                        //where theta=(cos^2-sin^2)/(2*s*c)=(m(iq,iq)-m(ip,ip))/(2*m(ip,iq))
                        //note that nr has ad hoc check if theta^2 would overflow the computer
                        /*if ((sqrt(temp2*temp2)+temp1) == sqrt(temp2*temp2)) t=(m(ip,iq))/temp2;
                        else {
                            theta=0.5*temp2/m(ip,iq);
                            t=1.0/(sqrt(theta*theta)+sqrt(1.0+theta*theta));
                            if (theta < 0.0) t = -t;
                        }*/
                        //instead I just determine the value of t
                        theta=0.5*temp2/m(ip,iq);
                        t=1.0/(sqrt(theta*theta)+sqrt(1.0+theta*theta));
                        if (theta < 0.0) t = -t;
                        //calculate cos and sin of rotation angle and tan(rot/2)
                        crot=1.0/sqrt(1.+t*t); srot=t*crot; trot2=srot/(1.0+crot);
                        //not alter values in matrix
                        temp2=t*m(ip,iq);
                        z[ip]-= temp2; z[iq] += temp2;
                        eigen[ip]-= temp2; eigen[iq] += temp2;
                        m(ip,iq)=0.0;
                        //now rotate the ipth values for 0<=j<ip
                        for (int j=0;j<ip;j++) {ROTATE(m,j,ip,j,iq,srot,trot2);}
                        //rotate the values for ip<j<iq
                        for (int j=ip+1;j<iq;j++) {ROTATE(m,ip,j,j,iq,srot,trot2);}
                        //rotate the values for iq<j<=row
                        for (int j=iq+1;j<row;j++) {ROTATE(m,ip,j,iq,j,srot,trot2);}
                        //store the rotations in Prot
                        for (int j=0;j<row;j++) {ROTATE(Prot,j,ip,j,iq,srot,trot2);}
                    }
                }
            }
            //now update vectors used in rotation and reinitialize b
            for (int ip=0;ip<row;ip++) {
                b[ip] += z[ip];
                eigen[ip]=b[ip];
                z[ip]=0.0;
            }
        }while (nrot++<10);
        printf("Too many iterations in routine jacobi, giving zeros\n");
        for (int i=0;i<row;i++) eigen[i]=0.;
        return GMatrix(row,1,eigen);
    }
    /// same as \ref Math::GMatrix::Jacobi above with different arguments and also returns Prot
    void Jacobi(GMatrix &eigenval, GMatrix &Prot, Double_t tol=1e-2) const
    {
        GMatrix m((*this));
        Double_t eigen[row], b[row], z[row];
        for (int i=0;i<row;i++) Prot(i,i)=1.;
        int nrot=0;
        Double_t crot,srot,trot2,thres,t,theta,sm,temp1,temp2;

        //initialize vectors to diagonal elements of matrix
        for (int i=0;i<row;i++){eigen[i]=b[i]=m(i,i);z[i]=0.;}

        do {
            //first check to see if off diagonal elements are zero
            // could check to within some precision relative to trace
            sm=0.;
            for (int ip=0;ip<row-1;ip++)
            for (int iq=ip+1;iq<col;iq++)
                sm+=sqrt(m(ip,iq)*m(ip,iq));
            if (sm==0.) break;
            //set the threshold to be 0 until several rotations have been applied
            nrot<4? thres=tol*sm/(Double_t)(row*row): thres=0.;
            //now start applying Jacobi rotations
            for (int ip=0;ip<row-1;ip++){
                for (int iq=ip+1;iq<col;iq++){
                    //get absolute value of off diagonal element and check if it is small in comparison to
                    //threshold factor of 1/5 is from numerical recipes
                    temp1=sqrt(m(ip,iq)*m(ip,iq))*0.2;
                    //if smaller than tolerance set to 0. after for sweeps and skip the rotation.
                    if (nrot > 4 && (sqrt(eigen[ip]*eigen[ip])+temp1)==(sqrt(eigen[ip]*eigen[ip])+tol)
                        && (sqrt(eigen[iq]*eigen[iq])+temp1)==(sqrt(eigen[iq]*eigen[iq])+tol))
                        m(ip,iq)=0.0;
                    else if (thres<temp1*5.0) {
                        temp2=eigen[iq]-eigen[ip];
                        //find the solution to t^2+2*t*theta-1=0
                        //where theta=(cos^2-sin^2)/(2*s*c)=(m(iq,iq)-m(ip,ip))/(2*m(ip,iq))
                        //note that nr has ad hoc check if theta^2 would overflow the computer
                        /*if ((sqrt(temp2*temp2)+temp1) == sqrt(temp2*temp2)) t=(m(ip,iq))/temp2;
                        else {
                            theta=0.5*temp2/m(ip,iq);
                            t=1.0/(sqrt(theta*theta)+sqrt(1.0+theta*theta));
                            if (theta < 0.0) t = -t;
                        }*/
                        //instead I just determine the value of t
                        theta=0.5*temp2/m(ip,iq);
                        t=1.0/(sqrt(theta*theta)+sqrt(1.0+theta*theta));
                        if (theta < 0.0) t = -t;
                        //calculate cos and sin of rotation angle and tan(rot/2)
                        crot=1.0/sqrt(1.+t*t); srot=t*crot; trot2=srot/(1.0+crot);
                        //not alter values in matrix
                        temp2=t*m(ip,iq);
                        z[ip]-= temp2; z[iq] += temp2;
                        eigen[ip]-= temp2; eigen[iq] += temp2;
                        m(ip,iq)=0.0;
                        //now rotate the ipth values for 0<=j<ip
                        for (int j=0;j<ip;j++) {ROTATE(m,j,ip,j,iq,srot,trot2);}
                        //rotate the values for ip<j<iq
                        for (int j=ip+1;j<iq;j++) {ROTATE(m,ip,j,j,iq,srot,trot2);}
                        //rotate the values for iq<j<=row
                        for (int j=iq+1;j<row;j++) {ROTATE(m,ip,j,iq,j,srot,trot2);}
                        //store the rotations in Prot
                        for (int j=0;j<row;j++) {ROTATE(Prot,j,ip,j,iq,srot,trot2);}
                    }
                }
            }
            //now update vectors used in rotation and reinitialize b
            for (int ip=0;ip<row;ip++) {
                b[ip] += z[ip];
                eigen[ip]=b[ip];
                z[ip]=0.0;
            }
        }while (nrot++<10);
        if (nrot>=10) {
            printf("Too many iterations in routine jacobi, giving zeros\n");
            for (int i=0;i<row;i++) {
                eigenval(i,0)=0.;
                for (int j=0;j<row;j++)
                Prot(i,j)=(i==j);
            }
        }
        for (int i=0;i<row;i++) eigenval(i,0)=eigen[i];
    }

    /*! \brief Calculate eigenvalues of matrix of rank N. Stored as a GMatrix, e1, e2 ...eN.

        The user must know what to expect from this function, whether real eigenvalues
        or complex ones.
        Have to implement the Francis QR algorithm or use successive Jacobi transformations
        to get eigenvalues and eigenvectors (note Jacobi slower than QR)
        but to ensure real eigenvalues, check that the matrix is symmetric
    */
    GMatrix Eigenvalues(Double_t tol=1e-2) const
    {
        if (isSymmetric()==0||isZero()==1) {
            printf("only implemented for nonzero symmetric matrices, returning null matrix\n");
            return GMatrix(0,0);
        }
        else {
            //using Jacobi transformations
            //issue is then must order eigen values
            GMatrix eigen(Jacobi(tol));
            //now order the eigen vaules e0>e1>e2..>eN
            //qsort(eigen.matrix,row,sizeof(Double_t),compare_Double);
            return eigen;
        }
    }

    // Computes the eigenvectors of the matrix, given the eigenvalues.  Sadly, this function will only work
    // properly on symmetric matrices -- i.e., all real eigenvalues.
    //GMatrix Eigenvectors(const Coordinate& e) const;

    /// Calculate eigenvalues \e and eigenvector of matrix of rank N. Stored as a GMatrix, e1, e2 ...eN.
    void Eigenvalvec(GMatrix &eigenval, GMatrix &eigenvector, Double_t tol=1e-2) const
    {
        if (isSymmetric()==0||isZero()==1) {
            printf("only implemented for nonzero symmetric matrices, returning null matrix\n");
            for (int i=0;i<row;i++) {
                eigenval(i,0)=0.;
                for (int j=0;j<row;j++)
                    eigenvector(i,j)=0.;
            }
        }
        else {
            //using Jacobi transformations
            Jacobi(eigenval,eigenvector,tol);
        }
    }

    //@}
};

}
#endif


#include <math.h>
#include <gsl/gsl_linalg.h>
#include "advanceCN.h"
#include "applyBC.h"
#include "userFunc.h"
#include "ppmExtrap.h"
#include "getNextIterate.h"

/**************************************************************************/
/* Time step advance routine with Crank-Nicolson time centering           */
/**************************************************************************/

double
advanceCN(const double t, const double dt, const grid *grd,
	  double *col, double *pres, double *eInt,
	  double *mBnd, double *eBnd, double *mSrc, double *eSrc,
	  const bool eos_func, const double gamma_val,
	  const double delta_val,
	  const bool alpha_func, const double alpha_val,
	  const pres_bc_type ibc_pres, const enth_bc_type ibc_enth,
	  const bool ibc_func, const double ibc_pres_val,
	  const double ibc_enth_val,
	  const pres_bc_type obc_pres, const enth_bc_type obc_enth,
	  const bool obc_func, const double obc_pres_val,
	  const double obc_enth_val,
	  const bool massSrc_func, const double massSrc_val,
	  const bool intEnSrc_func, const double intEnSrc_val,
	  const double errTol, const double dtTol,
	  const unsigned long maxIter, const unsigned long interpOrder,
	  const bool noUpdate, const bool verbose,
	  const wksp *w, void *params, unsigned long *itCount
#ifdef TESTING_MODE
	  , double *resid, unsigned long *rtype,
	  double *advanceTime, double *nextIterTime,
	  double *userTime
#endif
	  ) {

  double pIn, qIn, pOut, qOut;
  double err, errCell;
  double dPres, dCol, dEInt, dtMin;
  double ibc_pres_val1, ibc_enth_val1, obc_pres_val1, obc_enth_val1;
  double h_up, h_up1;
  double mBndTemp[2], eBndTemp[2];
  unsigned long maxErrCell, converged, residType;
  long i;
#if AA_M > 0
  long nHist;
  double residMax = 0.0;
#endif
#ifdef TESTING_MODE
  clock_t start_t, end_t, user_start_t, user_end_t;
  clock_t next_iter_start_t, next_iter_end_t;

  start_t = clock();
#endif

  /****************************************************************/
  /* Step 1: initialize workspace arrays from starting conditions */
  /****************************************************************/

  /* Copy column density and pressure */
  for (i=0; i<grd->nr; i++) {
    w->colNew[i] = col[i];
    w->pres_g[i+1] = w->presNew_g[i+1] = pres[i];
  }
  /* Evaluate EOS parameters and get enthalpy */
  if (eos_func == 1) {
#if AA_M > 0
    nHist = 3*grd->nr;
#endif
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userEOS(t, dt, grd, col, pres, eInt, params,
	    w->gammaLast, w->deltaLast);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t)
      / CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++) {
      w->eIntTmp[i] = w->eIntNew[i] = eInt[i];
      w->gammaNew[i] = w->gammaLast[i];
      w->deltaNew[i] = w->deltaLast[i];
      w->hint_g[i+1] = (pres[i] + eInt[i])/col[i];
    }
  } else {
#if AA_M > 0
    nHist = 2*grd->nr;
#endif
    for (i=0; i<grd->nr; i++) {
      w->gammaLast[i] = w->gammaNew[i] = gamma_val;
      w->deltaLast[i] = w->deltaNew[i] = delta_val;
      w->hint_g[i+1] = gamma_val/(gamma_val-1.0)*pres[i]/col[i];
    }
  }
  /* Get starting viscosity */
  if (alpha_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userAlpha(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	      params, w->alpha_g+1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t)
      / CLOCKS_PER_SEC;
#endif
  } else {
    for (i=1; i<=grd->nr; i++) w->alpha_g[i] = alpha_val;
  }

  /* Store starting mass and energy fluxes through boundary */
  mBndTemp[0] = mBnd[0];
  mBndTemp[1] = mBnd[1];
  eBndTemp[0] = eBnd[0];
  eBndTemp[1] = eBnd[1];

  /* Store starting source function values */
  if (massSrc_func == 1) {
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] = mSrc[i];
      w->eSrc[i] = eSrc[i];
    }
  } else if (intEnSrc_func == 1) {
    for (i=0; i<grd->nr; i++) w->eSrc[i] = eSrc[i];
  }

  /****************************************************************/
  /* Step 2: apply boundary conditions at old time                */
  /****************************************************************/

  /* Set alpha in ghost cells */
  w->alpha_g[0] = w->alpha_g[1];
  w->alpha_g[grd->nr+1] = w->alpha_g[grd->nr];

  /* Get pressure / enthalpy boundary values */
  if (ibc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userIBC(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	    ibc_pres, ibc_enth, params,
	    &ibc_pres_val1, &ibc_enth_val1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
  } else {
    ibc_pres_val1 = ibc_pres_val;
    ibc_enth_val1 = ibc_enth_val;
  }
  if (obc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userOBC(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	    obc_pres, obc_enth, params,
	    &obc_pres_val1, &obc_enth_val1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
  } else {
    obc_pres_val1 = obc_pres_val;
    obc_enth_val1 = obc_enth_val;
  }

  /* Set boundary conditions from user-specified parameters */
  applyBC(grd, w->alpha_g,
	  ibc_pres, ibc_enth, ibc_pres_val1, ibc_enth_val1,
	  obc_pres, obc_enth, obc_pres_val1, obc_enth_val1,
	  w->pres_g, w->hint_g, &pIn, &qIn, &pOut, &qOut);

  /****************************************************************/
  /* Step 3: get mass flux, source terms, and RHS at old time     */
  /****************************************************************/

  /* Mass and energy source terms */
  if (massSrc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userMassSrc(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
		params, w->massSrcLast);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] += 0.5 * dt * w->massSrcLast[i];
      w->eSrc[i] += 0.5 * dt * w->massSrcLast[i] *
	(grd->psiEff_g[i+1] + w->deltaLast[i]*pres[i]/col[i]);
    }
  } else {
    for (i=0; i<grd->nr; i++) w->massSrcLast[i] = massSrc_val;
  }
  if (intEnSrc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userIntEnSrc(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
		 params, w->intEnSrc);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++)
      w->eSrc[i] += 0.5 * dt * w->intEnSrc[i];
  } else {
    for (i=0; i<grd->nr; i++) w->intEnSrc[i] = intEnSrc_val;
  }

  /* Reconstruct internal enthalpies at cell edges */
  if (interpOrder == 1) {
    for (i=0; i<=grd->nr+1; i++)
      w->hintL_g[i] = w->hintR_g[i] = w->hint_g[i];
  } else if (interpOrder == 2) {
    if (grd->linear == 1) {
      for (i=1; i<=grd->nr+1; i++)
	w->hintR_g[i-1] = w->hintL_g[i] =
	  ((grd->r_g[i]-grd->r_h[i-1]) * w->hint_g[i-1] +
	   (grd->r_h[i-1]-grd->r_g[i-1]) * w->hint_g[i]) /
	  (grd->r_g[i] - grd->r_g[i-1]);
    } else {
      for (i=1; i<=grd->nr+1; i++)
	w->hintR_g[i-1] = w->hintL_g[i] =
	  (log(grd->r_g[i]/grd->r_h[i-1]) * w->hint_g[i-1] +
	   log(grd->r_h[i-1]/grd->r_g[i-1]) * w->hint_g[i]) /
	  log(grd->r_g[i]/grd->r_g[i-1]);
    }
    /* Limit the slope */
    for (i=1; i<=grd->nr+1; i++) {
      if (w->hintR_g[i-1]/w->hint_g[i-1]-1.0 > SLOPELIMIT) {
	w->hintR_g[i-1] = w->hint_g[i-1]*(1.0+SLOPELIMIT);
      } else if (w->hintR_g[i-1]/w->hint_g[i-1] < 1.0-SLOPELIMIT) {
	w->hintR_g[i-1] = w->hint_g[i-1]*(1.0-SLOPELIMIT);
      }
      if (w->hintL_g[i]/w->hint_g[i]-1.0 > SLOPELIMIT) {
	w->hintL_g[i] = w->hint_g[i]*(1.0+SLOPELIMIT);
      } else if (w->hintL_g[i]/w->hint_g[i] < 1.0-SLOPELIMIT) {
	w->hintL_g[i] = w->hint_g[i]*(1.0-SLOPELIMIT);
      }
    }
  } else if (interpOrder == 3) {
    ppmExtrap(grd->nr+2, grd->dr_g, w->hint_g, w->hintL_g, w->hintR_g,
	      w->ppmwksp_g);
  }

  /* Compute fluxes at cell edges */
  for (i=0; i<=grd->nr; i++) {

    /* Mass flux; initialize the new mass flux to match the old one,
       as is appropriate for the first guess in the iteration below */
    w->fmLast_h[i] = w->fmNew_h[i] = -grd->g_h[i] *
      (SQR(grd->r_g[i+1])*(1-grd->beta_g[i+1]) *
       w->pres_g[i+1]*w->alpha_g[i+1] -
       SQR(grd->r_g[i])*(1-grd->beta_g[i]) *
       w->pres_g[i]*w->alpha_g[i]);

    /* Enthalpy flux; use upwinding */
    if (w->fmLast_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
    else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
    w->feLast_h[i] = h_up * w->fmLast_h[i];

    /* Torque flux */
    w->ftLast_h[i] = M_PI * grd->r_h[i] * grd->vphi_h[i] *
      (1-grd->beta_h[i]) *
      (w->pres_g[i+1]*w->alpha_g[i+1] +
       w->pres_g[i]*w->alpha_g[i]);
  }

  /* Fill RHS vector */
  for (i=0; i<grd->nr; i++) {
    gsl_vector_set(w->rhs_g, i+1,
		   pres[i] - 0.5*dt/grd->area[i] *
		   (w->gammaLast[i]-1.0) *
		   (w->feLast_h[i+1] - w->feLast_h[i] +
		    w->ftLast_h[i+1] - w->ftLast_h[i] -
		    (grd->psiEff_g[i+1] +
		     w->deltaLast[i+1]*pres[i]/col[i])
		    *(w->fmLast_h[i+1] - w->fmLast_h[i])) +
		   0.5*dt*(w->gammaLast[i]-1) * w->intEnSrc[i]);
  }

  /* Zero out energy source, since below we expect this to
     be the rate from the previous iteration of the loop */
  for (i=0; i<grd->nr; i++) w->intEnSrc[i] = 0.0;

  /****************************************************************/
  /* Step 4: main iteration loop                                  */
  /****************************************************************/

  /* Status message */
  if (verbose)
    printf("advanceCN: starting iteration, dt = %e\n", dt);

  /* Start loop */
  for (*itCount = 1, converged = 0; 1; (*itCount)++) {

    /**************************************************************/
    /* Step 4a: compute enthalpy, alpha, and BC for current guess */
    /**************************************************************/

    /* Get new EOS parameters and cell-center enthalpies */
    if (eos_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userEOS(t+dt, dt, grd, w->colNew, w->presNew_g+1, w->eIntNew,
	      params, w->gammaNew, w->deltaNew);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
      for (i=1; i<=grd->nr; i++)
	w->hint_g[i] = (w->presNew_g[i] + w->eIntNew[i-1]) /
	  w->colNew[i-1];
    } else {
      for (i=1; i<=grd->nr; i++) {
	w->hint_g[i] = gamma_val/(gamma_val-1.0) *
	  w->presNew_g[i]/w->colNew[i-1];
      }
    }

    /* If alpha is non-constant, get new values */
    if (alpha_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userAlpha(t+dt, dt, grd, w->colNew, w->presNew_g+1,
		w->eIntNew, w->gammaNew,
		w->deltaNew, params, w->alpha_g+1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
      w->alpha_g[0] = w->alpha_g[1];
      w->alpha_g[grd->nr+1] = w->alpha_g[grd->nr];
    }

    /* Subtract off energy source term from previous iteration; we
       need to do this here because we're about to update gamma */
    for (i=0; i<grd->nr; i++) {
      gsl_vector_set(w->rhs_g, i+1,
		     gsl_vector_get(w->rhs_g, i+1) -
		     0.5*dt*(w->gammaNew[i]-1)*w->intEnSrc[i]);
    }

    /* Get new pressure / enthalpy boundary values */
    if (ibc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userIBC(t+dt, dt, grd, w->colNew, w->presNew_g+1,
	      w->eIntNew, w->gammaNew, w->deltaNew,
	      ibc_pres, ibc_enth, params,
	      &ibc_pres_val1, &ibc_enth_val1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }
    if (obc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userOBC(t+dt, dt, grd, w->colNew, w->presNew_g+1,
	      w->eIntNew, w->gammaNew, w->deltaNew,
	      obc_pres, obc_enth, params,
	      &obc_pres_val1, &obc_enth_val1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }

    /* Set boundary conditions from user-specified parameters, and
       also set ghost cell elements in RHS matrix */
    applyBC(grd, w->alpha_g,
	    ibc_pres, ibc_enth, ibc_pres_val1, ibc_enth_val1,
	    obc_pres, obc_enth, obc_pres_val1, obc_enth_val1,
	    w->presNew_g, w->hint_g, &pIn, &qIn, &pOut, &qOut);
    gsl_vector_set(w->rhs_g, 0, pIn);
    gsl_vector_set(w->rhs_g, grd->nr+1, pOut);

    /* Get new enthalpies at cell centers and extrapolated to cell edges */
    if (interpOrder == 1) {
      for (i=0; i<=grd->nr+1; i++)
	w->hintL_g[i] = w->hintR_g[i] = w->hint_g[i];
    } else if (interpOrder == 2) {
      if (grd->linear == 1) {
	for (i=1; i<=grd->nr+1; i++)
	  w->hintR_g[i-1] = w->hintL_g[i] =
	    ((grd->r_g[i]-grd->r_h[i-1]) * w->hint_g[i-1] +
	     (grd->r_h[i-1]-grd->r_g[i-1]) * w->hint_g[i]) /
	    (grd->r_g[i] - grd->r_g[i-1]);
      } else {
	for (i=1; i<=grd->nr+1; i++)
	  w->hintR_g[i-1] = w->hintL_g[i] =
	    (log(grd->r_g[i]/grd->r_h[i-1]) * w->hint_g[i-1] +
	     log(grd->r_h[i-1]/grd->r_g[i-1]) * w->hint_g[i]) /
	    log(grd->r_g[i]/grd->r_g[i-1]);
      }
      /* Limit the slope */
      for (i=1; i<=grd->nr+1; i++) {
	if (w->hintR_g[i-1]/w->hint_g[i-1]-1.0 > SLOPELIMIT) {
	  w->hintR_g[i-1] = w->hint_g[i-1]*(1.0+SLOPELIMIT);
	} else if (w->hintR_g[i-1]/w->hint_g[i-1] < 1.0-SLOPELIMIT) {
	  w->hintR_g[i-1] = w->hint_g[i-1]*(1.0-SLOPELIMIT);
	}
	if (w->hintL_g[i]/w->hint_g[i]-1.0 > SLOPELIMIT) {
	  w->hintL_g[i] = w->hint_g[i]*(1.0+SLOPELIMIT);
	} else if (w->hintL_g[i]/w->hint_g[i] < 1.0-SLOPELIMIT) {
	  w->hintL_g[i] = w->hint_g[i]*(1.0-SLOPELIMIT);
	}
      }
    } else if (interpOrder == 3) {
      ppmExtrap(grd->nr+2, grd->dr_g, w->hint_g, w->hintL_g, w->hintR_g,
		w->ppmwksp_g);
    }

    /**************************************************************/
    /* Step 4b: add energy source term to RHS vector for pressure */
    /* solve                                                      */
    /**************************************************************/

    /* If source term can vary, compute its value for this
       iteration */
    if (intEnSrc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userIntEnSrc(t+dt, dt, grd, w->colNew, w->presNew_g+1,
		   w->eIntNew, w->gammaNew, w->deltaNew, params,
		   w->intEnSrc);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }
    for (i=0; i<grd->nr; i++) {
      gsl_vector_set(w->rhs_g, i+1,
		     gsl_vector_get(w->rhs_g, i+1) +
		     0.5*dt*(w->gammaNew[i]-1)*w->intEnSrc[i]);
    }

    /**************************************************************/
    /* Step 4c: construct the tridiagonal matrix                  */
    /**************************************************************/

    /* Upper diagonal */
    gsl_vector_set(w->ud_g, 0, -qIn);
    for (i=1; i<=grd->nr; i++) {
      if (w->fmNew_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->ud_g, i,
		     0.5*dt*w->alpha_g[i+1]*(w->gammaNew[i-1]-1.0)
		     / grd->area[i-1] *
		     (grd->g_h[i]*SQR(grd->r_g[i+1]) *
		      (1-grd->beta_g[i+1]) *
		      (grd->psiEff_g[i]
		       + w->deltaNew[i-1] *
		       w->presNew_g[i]/w->colNew[i-1]
		       - h_up) +
		      M_PI*grd->r_h[i]*grd->vphi_h[i] *
		      (1-grd->beta_h[i])));
    }

    /* Lower diagonal */
    for (i=0; i<grd->nr; i++) {
      if (w->fmNew_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->ld_g, i,
		     0.5*dt*w->alpha_g[i]*(w->gammaNew[i]-1.0)
		     / grd->area[i] *
		     (grd->g_h[i]*SQR(grd->r_g[i]) *
		      (1-grd->beta_g[i]) *
		      (grd->psiEff_g[i+1]
		       + w->deltaNew[i] *
		       w->presNew_g[i+1]/w->colNew[i]
		       - h_up) -
		      M_PI*grd->r_h[i]*grd->vphi_h[i] *
		      (1-grd->beta_h[i])));
    }
    gsl_vector_set(w->ld_g, grd->nr, -qOut);

    /* Diagonal */
    for (i=1; i<=grd->nr; i++) {
      if (w->fmNew_h[i-1] > 0) h_up = w->hintR_g[i-1] + grd->psiEff_h[i-1];
      else h_up = w->hintL_g[i] + grd->psiEff_h[i-1];
      if (w->fmNew_h[i] > 0) h_up1 = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up1 = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->diag_g, i,
		     1.0 + 0.5*dt*w->alpha_g[i]*(w->gammaNew[i-1]-1.0)
		     / grd->area[i-1] *
		     ((grd->g_h[i]*
		       (h_up1 - grd->psiEff_g[i]
			- w->deltaNew[i-1] *
			w->presNew_g[i]/w->colNew[i-1]) +
		       grd->g_h[i-1]*
		       (h_up - grd->psiEff_g[i]
			- w->deltaNew[i-1] *
			w->presNew_g[i]/w->colNew[i-1])) *
		      SQR(grd->r_g[i]) * (1-grd->beta_g[i]) +
		      M_PI*(grd->r_h[i]*grd->vphi_h[i]*
			    (1-grd->beta_h[i]) -
			    grd->r_h[i-1]*grd->vphi_h[i-1]*
			    (1-grd->beta_h[i-1]))));
    }

    /**************************************************************/
    /* Step 4d: solve the matrix equation to get the new pressure */
    /* guess                                                      */
    /**************************************************************/

    if (gsl_linalg_solve_tridiag(w->diag_g, w->ud_g, w->ld_g,
				 w->rhs_g, w->presTmp_g)) {
      /* Non-zero return code means error has occured, probably due to
	 bad inputs data. Bail out, and let the driver try again */
      if (verbose)
	printf("   ...iteration %ld, detected NaN or Inf in tridiagonal inputs, exiting without convergence!\n", *itCount);
#ifdef TESTING_MODE
      end_t = clock();
      *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
      return(-1.0);
    }

    /**************************************************************/
    /* Step 4e: get the new column density guess                  */
    /**************************************************************/

    /* Get new mass fluxes */
    for (i=0; i<=grd->nr; i++)
      w->fmNew_h[i] = -grd->g_h[i] *
	(SQR(grd->r_g[i+1])*
	 (1-grd->beta_g[i+1]) *
	 gsl_vector_get(w->presTmp_g, i+1)*w->alpha_g[i+1] -
	 SQR(grd->r_g[i])*
	 (1-grd->beta_g[i]) *
	 gsl_vector_get(w->presTmp_g, i)*w->alpha_g[i]);

    /* Get source terms at new time */
    if (massSrc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userMassSrc(t+dt, dt, grd, w->colNew, w->presNew_g, w->eIntNew,
		  w->gammaNew, w->deltaNew, params, w->massSrcNew);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    } else {
      for (i=0; i<grd->nr; i++) w->massSrcNew[i] = massSrc_val;
    }

    /* Get new column density guess */
    for (i=0; i<grd->nr; i++) {
      w->colTmp[i] = col[i] - dt/grd->area[i] * 0.5 *
	(w->fmNew_h[i+1] - w->fmNew_h[i] +
	 w->fmLast_h[i+1] - w->fmLast_h[i]) +
	dt * 0.5 * (w->massSrcLast[i] + w->massSrcNew[i]);
    }

    /**************************************************************/
    /* Step 4f: if doing separate internal energy evolution, get  */
    /* new internal energy guess                                  */
    /**************************************************************/

    if (eos_func == 1) {
      for (i=0; i<grd->nr; i++) {
	w->eIntTmp[i] = eInt[i] +
	  0.5 * (1.0/(w->gammaLast[i]-1.0) +
		 1.0/(w->gammaNew[i]-1.0)) *
	  (gsl_vector_get(w->presTmp_g, i+1) - pres[i]) +
	  0.5 *
	  (w->deltaLast[i]*pres[i]/col[i] +
	   w->deltaNew[i]*gsl_vector_get(w->presTmp_g, i+1) /
	   w->colTmp[i]) * (w->colTmp[i]-col[i]);
      }
    }

    /**************************************************************/
    /* Step 4g: check for convergence or loop termination; print  */
    /* status message                                             */
    /**************************************************************/

    /* Compute residuals on pressure */
    err = 0.0;
    maxErrCell = -1;
    residType = 0;
    for (i=0; i<grd->nr; i++) {
      if (!isfinite(gsl_vector_get(w->presTmp_g, i+1))) {
	/* If we've diverged to the point of getting inf's, we won't
	   converge, so bail out */

	if (verbose)
	  printf("   ...iteration %ld, detected NaN or Inf in tridiagonal solution, cell %ld, exiting without convergence!\n", *itCount, i);
#ifdef TESTING_MODE
	end_t = clock();
	*advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
	return(-1.0);
      }
      errCell = (gsl_vector_get(w->presTmp_g, i+1) -
		 w->presNew_g[i+1]) /
	gsl_vector_get(w->presTmp_g, i+1);
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
      }
    }

    /* Compute residuals on column density */
    for (i=0; i<grd->nr; i++) {
      errCell = (w->colTmp[i] - w->colNew[i]) / w->colTmp[i];
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
	residType = 1;
      }
    }

    /* Compute residuals on internal energy */
    for (i=0; i<grd->nr; i++) {
      errCell = (w->eIntTmp[i] - w->eIntNew[i]) / w->eIntTmp[i];
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
	residType = 2;
      }
    }

#ifdef TESTING_MODE
    /* In testing mode, store residuals and residual type */
    resid[*itCount-1] += err;
    rtype[*itCount-1] = residType;
#endif

    /* Print status */
    if (fabs(err) < errTol) converged = 1;
    if (verbose) {
      printf("   ...iteration %ld, max residual = %e in cell %ld",
	     *itCount, err, maxErrCell);
      if (residType==0)
	printf(" pressure\n");
      else if (residType==1)
	printf(" density\n");
      else if (residType==2)
	printf(" internal energy\n");
      if (converged == 1) {
	printf("   ...converged!\n");
      } else if (*itCount == maxIter) {
	printf("   ...reached maximum iterations, exiting without convergence! Max residual = %e in cell %ld\n",
	       err, maxErrCell);
      }
    }

    /* If we have converged, shift temporaries into final arrays, then
       exit the loop */
    if (converged) {
      for (i=0; i<grd->nr; i++) {
	w->colNew[i] = w->colTmp[i];
	w->presNew_g[i+1] = gsl_vector_get(w->presTmp_g, i+1);
	if (eos_func == 1)
	  w->eIntNew[i] = w->eIntTmp[i];
      }
      w->presNew_g[0] = gsl_vector_get(w->presTmp_g, 0);
      w->presNew_g[grd->nr+1] = gsl_vector_get(w->presTmp_g, grd->nr+1);
      break;
    }

    /* If we've exceeded iteration limit, bail out */
    if (*itCount == maxIter) {
#ifdef TESTING_MODE
      end_t = clock();
      *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
      return(-1.0);
    }

    /**************************************************************/
    /* Step 4h: if we're here, we haven't converged yet, so use   */
    /* Anderson acceleration to generate new guesses at solution  */
    /**************************************************************/

#ifdef TESTING_MODE
    next_iter_start_t = clock();
#endif
    getNextIterate(grd, eos_func, w
#if AA_M > 0
		   , *itCount, nHist, &residMax
#endif
		   );
#ifdef TESTING_MODE
    next_iter_end_t = clock();
    *nextIterTime += (double) (next_iter_end_t - next_iter_start_t) /
      CLOCKS_PER_SEC;
#endif

  } /* End main iteration loop */

  /****************************************************************/
  /* Step 5: compute rate of change of pressure and column        */
  /* density; use this to estimate new time step                  */
  /****************************************************************/

  dtMin = LARGE;
  for (i=0; i<grd->nr; i++) {
    dCol = fabs(w->colNew[i] - col[i]) + SMALL;
    dPres = fabs(w->presNew_g[i+1] - pres[i]) + SMALL;
    dtMin = fmin( dtMin, dtTol*dt *
		  fmin(col[i]/dCol, pres[i]/dPres) );
    if (eos_func == 1) {
      dEInt = fabs(w->eIntNew[i] - eInt[i]) + SMALL;
      dtMin = fmin( dtMin, dtTol*dt * eInt[i]/dEInt );
    }
  }
  if (verbose > 0)
    printf("advanceCN: estimated dtNew = %e\n", dtMin);

  /****************************************************************/
  /* Step 6: record the total mass and energy transported across  */
  /* the inner and outer boundaries during this time step.        */
  /****************************************************************/

  /* Mass fluxes */
  mBndTemp[0] += 0.5*dt*(w->fmLast_h[0] + w->fmNew_h[0]);
  mBndTemp[1] += 0.5*dt*(w->fmLast_h[grd->nr] + w->fmNew_h[grd->nr]);

  /* Old time energy fluxes */
  eBndTemp[0] += 0.5*dt*(w->feLast_h[0] + w->ftLast_h[0]);
  eBndTemp[1] += 0.5*dt*(w->feLast_h[grd->nr] + w->ftLast_h[grd->nr]);

  /* New time energy fluxes; need to get enthalpy to evaluate f_E */
  if (w->fmNew_h[0] > 0) h_up = w->hintR_g[0] + grd->psiEff_h[0];
  else h_up = w->hintL_g[1] + grd->psiEff_h[0];
  eBndTemp[0] += 0.5*dt*h_up*w->fmNew_h[0];
  if (w->fmNew_h[grd->nr] > 0)
    h_up1 = w->hintR_g[grd->nr] + grd->psiEff_h[grd->nr];
  else h_up1 = w->hintL_g[grd->nr+1] + grd->psiEff_h[grd->nr];
  eBndTemp[1] += 0.5*dt*h_up1*w->fmNew_h[grd->nr];

  /* New time torque fluxes */
  eBndTemp[0] += 0.5*dt*M_PI*grd->r_h[0]*grd->vphi_h[0]*(1.0-grd->beta_h[0]) *
    (w->presNew_g[1]*w->alpha_g[1] + w->presNew_g[0]*w->alpha_g[0]);
  eBndTemp[1] += 0.5*dt*M_PI*grd->r_h[grd->nr]*grd->vphi_h[grd->nr]
    *(1.0-grd->beta_h[grd->nr]) *
    (w->presNew_g[grd->nr+1]*w->alpha_g[grd->nr+1]
     + w->presNew_g[grd->nr]*w->alpha_g[grd->nr]);

  /****************************************************************/
  /* Step 7: if using source functions, record total mass and     */
  /* energy added to every cell by sources                        */
  /****************************************************************/
;
  if (massSrc_func == 1) {
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] += 0.5 * dt * w->massSrcNew[i];
      w->eSrc[i] += 0.5 * dt * w->massSrcNew[i] *
	(grd->psiEff_g[i+1] +
	 w->deltaNew[i]*w->presNew_g[i+1]/w->colNew[i]);
    }
  }
  if (intEnSrc_func == 1) {
    for (i=0; i<grd->nr; i++)
      w->eSrc[i] += 0.5 * dt * w->intEnSrc[i];
  }

  /****************************************************************/
  /* Step 8: store final values in arrays                         */
  /****************************************************************/

  if (noUpdate==0) {
    for (i=0; i<grd->nr; i++) {
      pres[i] = w->presNew_g[i+1];
      col[i] = w->colNew[i];
    }
    if (eos_func==1)
      for (i=0; i<grd->nr; i++) eInt[i] = w->eIntNew[i];
    mBnd[0] = mBndTemp[0];
    mBnd[1] = mBndTemp[1];
    eBnd[0] = eBndTemp[0];
    eBnd[1] = eBndTemp[1];
    if (massSrc_func == 1) {
      for (i=0; i<grd->nr; i++) {
	mSrc[i] = w->mSrc[i];
	eSrc[i] = w->eSrc[i];
      }
    } else if (intEnSrc_func == 1) {
      for (i=0; i<grd->nr; i++) eSrc[i] = w->eSrc[i];
    }
  }

  /****************************************************************/
  /* Step 9: return new time step                                */
  /****************************************************************/

#ifdef TESTING_MODE
  end_t = clock();
  *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
  return(dtMin);
}


#include <math.h>
#include <gsl/gsl_sf_bessel.h>
#include <galpy_potentials.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
//Double exponential disk potential
double DoubleExponentialDiskPotentialEval(double R,double z, double phi,
					  double t,
					  struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  double amp, alpha;
  int nzeros, glorder;
  if ( R > 6. ) { //Approximate as Keplerian
    nzeros= (int) *(args+4);
    glorder= (int) *(args+5);
    amp= *(args + 6 + 2 * glorder + 4 * (nzeros + 1));
    alpha= *(args + 7 + 2 * glorder + 4 * (nzeros + 1));
    return - *args * amp * pow(R*R+z*z,1.-0.5*alpha) / (alpha - 2.);
  }
  //Get args
  amp= *args++;
  alpha= *args++;
  double beta= *args++;
  double kmaxFac= *args++;
  double kmax= kmaxFac * beta;
  nzeros= (int) *args++;
  glorder= (int) *args++;
  double * glx= args;
  double * glw= args + glorder;
  double * j0zeros= args + 2 * glorder;
  double * dj0zeros= args + 2 * glorder + nzeros + 1;
  //Calculate potential
  double out= 0.;
  double k;
  int ii, jj;
  if ( R < 1. ) kmax= kmax/R;
  for (ii=0; ii < ( nzeros + 1 ); ii++) {
    for (jj=0; jj < glorder; jj++) {
      k= 0.5 * ( *(glx+jj) + 1. ) * *(dj0zeros+ii+1) + *(j0zeros+ii);
      out+= *(glw+jj) * *(dj0zeros+ii+1) * gsl_sf_bessel_J0(k*R)
	* pow(alpha * alpha + k * k,-1.5)
	* (beta * exp(-k * fabs(z) ) - k * exp(-beta * fabs(z) ))
	/ (beta * beta - k * k);
    }
    if ( k > kmax ) break;
  }
  return - amp * 2 * M_PI * alpha * out;
}
double DoubleExponentialDiskPotentialRforce(double R,double z, double phi,
					    double t,
					    struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  double amp, alpha;
  int nzeros,glorder;
  if ( R > 6. ) { //Approximate as Keplerian
    nzeros= (int) *(args+4);
    glorder= (int) *(args+5);
    amp= *(args + 6 + 2 * glorder + 4 * (nzeros + 1));
    alpha= *(args + 7 + 2 * glorder + 4 * (nzeros + 1));
    return - *args * amp * R * pow(R*R+z*z,-0.5*alpha);
  }
  //Get args
  amp= *args++;
  alpha= *args++;
  double beta= *args++;
  double kmaxFac= *args++;
  double kmax= 2. * kmaxFac * beta;
  nzeros= (int) *args++;
  glorder= (int) *args++;
  double * glx= args;
  double * glw= args + glorder;
  double * j1zeros= args + 2 * glorder + 2 * (nzeros + 1);
  double * dj1zeros= args + 2 * glorder + 3 * (nzeros + 1);
  //Calculate potential
  double out= 0.;
  double k;
  int ii, jj;
  if ( R < 1. ) kmax= kmax/R;
  for (ii=0; ii < ( nzeros + 1 ); ii++) {
    for (jj=0; jj < glorder; jj++) {
      k= 0.5 * ( *(glx+jj) + 1. ) * *(dj1zeros+ii+1) + *(j1zeros+ii);
      out+= *(glw+jj) * *(dj1zeros+ii+1) * k * gsl_sf_bessel_J1(k*R)
	* pow(alpha * alpha + k * k,-1.5)
	* (beta * exp(-k * fabs(z) ) - k * exp(-beta * fabs(z) ))
	/ (beta * beta - k * k);
    }
    if ( k > kmax ) break;
  }
  return - amp * 2 * M_PI * alpha * out;
}
double DoubleExponentialDiskPotentialPlanarRforce(double R,double phi,
						  double t,
						  struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  double amp, alpha;
  int nzeros, glorder;
  if ( R > 6. ) { //Approximate as Keplerian
    nzeros= (int) *(args+4);
    glorder= (int) *(args+5);
    amp= *(args + 6 + 2 * glorder + 4 * (nzeros + 1));
    alpha= *(args + 7 + 2 * glorder + 4 * (nzeros + 1));
    return - *args * amp * pow(R,-alpha + 1.);
  }
  //Get args
  amp= *args++;
  alpha= *args++;
  double beta= *args++;
  double kmaxFac= *args++;
  double kmax= 2. * kmaxFac * beta;
  nzeros= (int) *args++;
  glorder= (int) *args++;
  double * glx= args;
  double * glw= args + glorder;
  double * j1zeros= args + 2 * glorder + 2 * (nzeros + 1);
  double * dj1zeros= args + 2 * glorder + 3 * (nzeros + 1);
  //Calculate potential
  double out= 0.;
  double k;
  int ii, jj;
  if ( R < 1. ) kmax= kmax/R;
  for (ii=0; ii < ( nzeros + 1 ); ii++) {
    for (jj=0; jj < glorder; jj++) {
      k= 0.5 * ( *(glx+jj) + 1. ) * *(dj1zeros+ii+1) + *(j1zeros+ii);
      out+= *(glw+jj) * *(dj1zeros+ii+1) * k * gsl_sf_bessel_J1(k*R)
	* pow(alpha * alpha + k * k,-1.5)
	/ (beta + k);
    }
    if ( k > kmax ) break;
  }
  return - amp * 2 * M_PI * alpha * out;
}
double DoubleExponentialDiskPotentialzforce(double R,double z,double phi,
					    double t,
					    struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  double amp, alpha;
  int nzeros, glorder;
  if ( R > 6. ) { //Approximate as Keplerian
    nzeros= (int) *(args+4);
    glorder= (int) *(args+5);
    amp= *(args + 6 + 2 * glorder + 4 * (nzeros + 1));
    alpha= *(args + 7 + 2 * glorder + 4 * (nzeros + 1));
    return - *args * amp * z * pow(R*R+z*z,-0.5*alpha);
  }
  //Get args
  amp= *args++;
  alpha= *args++;
  double beta= *args++;
  double kmaxFac= *args++;
  double kmax= kmaxFac * beta;
  nzeros= (int) *args++;
  glorder= (int) *args++;
  double * glx= args;
  double * glw= args + glorder;
  double * j0zeros= args + 2 * glorder;
  double * dj0zeros= args + 2 * glorder + nzeros + 1;
  //Calculate potential
  double out= 0.;
  double k;
  int ii, jj;
  if ( R < 1. ) kmax= kmax/R;
  for (ii=0; ii < ( nzeros + 1 ); ii++) {
    for (jj=0; jj < glorder; jj++) {
      k= 0.5 * ( *(glx+jj) + 1. ) * *(dj0zeros+ii+1) + *(j0zeros+ii);
      out+= *(glw+jj) * *(dj0zeros+ii+1) * k * gsl_sf_bessel_J0(k*R)
	* pow(alpha * alpha + k * k,-1.5)
	* (exp(-k * fabs(z) ) - exp(-beta * fabs(z) ))
	/ (beta * beta - k * k);
    }
    if ( k > kmax ) break;
  }
  if ( z > 0. )
    return - amp * 2 * M_PI * alpha * beta * out;
  else
    return amp * 2 * M_PI * alpha * beta * out;
}
double DoubleExponentialDiskPotentialDens(double R,double z, double phi,
					  double t,
					  struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double amp= *args++;
  double alpha= *args++;
  double beta= *args;
  // calculate density
  return amp * exp ( - alpha * R - beta * fabs ( z ) );
}


/*
*/

#ifndef MINIMIZER_H
#define MINIMIZER_H

#include <iostream>
#include "Vector.h"
#include <stdexcept>

#ifdef USE_GSL
extern "C" {
#include <gsl/gsl_multimin.h>
}

namespace PsimagLite {

template<typename FunctionType>
typename FunctionType::FieldType myFunction(const gsl_vector *v, void *params)
{
	FunctionType* ft = (FunctionType *)params;
	return ft->operator()(v->data,v->size);
}

template<typename FunctionType>
void myDfunction(const gsl_vector *v,
                 void *params,
                 gsl_vector* df)
{
	FunctionType* ft = (FunctionType *)params;
	ft->df(v->data,v->size,df->data,df->size);
}

template<typename FunctionType>
void myFdFunction(const gsl_vector *v,
                  void *params,
                  double *f,
                  gsl_vector *df)
{
	*f = myFunction<FunctionType>(v, params);
	myDfunction<FunctionType>(v, params, df);
}

template<typename RealType,typename FunctionType>
class Minimizer {

	typedef typename FunctionType::FieldType FieldType;
	typedef typename Vector<FieldType>::Type VectorType;
	typedef Minimizer<RealType,FunctionType> ThisType;

public:

	enum {GSL_SUCCESS=::GSL_SUCCESS, GSL_CONTINUE=::GSL_CONTINUE};

	Minimizer(FunctionType& function,SizeType maxIter, bool verbose = false)
	    : function_(function),
	      maxIter_(maxIter),
	      verbose_(verbose),
	      status_(100),
	      gslT_(gsl_multimin_fminimizer_nmsimplex2),
	      gslS_(gsl_multimin_fminimizer_alloc(gslT_,function_.size())),
	      gslDt_(gsl_multimin_fdfminimizer_conjugate_fr),
	      gslDs_(gsl_multimin_fdfminimizer_alloc (gslDt_, function_.size()))
	{}

	~Minimizer()
	{
		gsl_multimin_fminimizer_free(gslS_);
		gsl_multimin_fdfminimizer_free(gslDs_);
	}

	int simplex(VectorType& minVector,RealType delta=1e-3,RealType tolerance=1e-3)
	{
		gsl_vector *x;
		/* Starting point,  */
		x = gsl_vector_alloc (function_.size());
		for (SizeType i=0;i<minVector.size();i++)
			gsl_vector_set (x, i, minVector[i]);

		gsl_vector *xs;
		xs = gsl_vector_alloc (function_.size());
		for (SizeType i=0;i<minVector.size();i++)
			gsl_vector_set (xs, i, delta);

		gsl_multimin_function func;
		func.f= myFunction<FunctionType>;
		func.n = function_.size();
		func.params = &function_;
		gsl_multimin_fminimizer_set (gslS_, &func, x, xs);

		SizeType iter = 0;
		RealType prevValue = 0;

		for (;iter<maxIter_;iter++) {
			status_ = gsl_multimin_fminimizer_iterate (gslS_);

			if (status_) {
				String gslError(gsl_strerror(status_));
				String msg("Minimizer::simplex(...): GSL Error: ");
				msg += gslError + "\n";
				throw RuntimeError(msg);
			}

			RealType size = gsl_multimin_fminimizer_size(gslS_);
			status_ = gsl_multimin_test_size(size, tolerance);

			if (verbose_) {
				RealType thisValue = function_(gslS_->x->data,func.n);
				RealType diff = fabs(thisValue - prevValue);
				std::cerr<<"simplex: "<<iter<<" "<<thisValue<<" diff= "<<diff;
				std::cerr<<" status= "<<status_<<" size="<<size<<"\n";
				prevValue = thisValue;
			}

			if (status_ == GSL_SUCCESS) break;
		}

		found(minVector,gslS_->x,iter);
		gsl_vector_free (x);
		gsl_vector_free (xs);
		return iter;
	}

	int conjugateGradient(VectorType& minVector,
	                      RealType delta=1e-3,
	                      RealType delta2=1e-3,
	                      RealType tolerance=1e-3,
	                      SizeType saveEvery = 0)
	{
		gsl_vector *x;
		/* Starting point,  */
		x = gsl_vector_alloc (function_.size());
		for (SizeType i=0;i<minVector.size();i++)
			gsl_vector_set (x, i, minVector[i]);

		gsl_multimin_function_fdf func;
		func.f= myFunction<FunctionType>;
		func.df = myDfunction<FunctionType>;
		func.fdf = myFdFunction<FunctionType>;
		func.n = function_.size();
		func.params = &function_;

		gsl_multimin_fdfminimizer_set(gslDs_, &func, x, delta, delta2);

		RealType prevValue = 0;
		SizeType iter = 0;
		for (;iter<maxIter_;iter++) {
			status_ = gsl_multimin_fdfminimizer_iterate(gslDs_);

			if (status_) {
				String gslError(gsl_strerror(status_));
				String msg("Minimizer::conjugateGradient(...): GSL Error: ");
				msg += gslError + "\n";
				throw RuntimeError(msg);
			}

			status_ = gsl_multimin_test_gradient (gslDs_->gradient, tolerance);

			if (verbose_) {
				RealType thisValue = function_(gslDs_->x->data,func.n);
				RealType diff = fabs(thisValue - prevValue);
				std::cerr<<"conjugateGradient: "<<iter<<" "<<thisValue;
				std::cerr<<" diff= "<<diff;
				std::cerr<<" gradientNorm= "<<gradientNorm(gslDs_->x);
				std::cerr<<" status= "<<status_<<"\n";
				prevValue = thisValue;
			}

			if (status_ == GSL_SUCCESS) break;

			if (saveEvery > 0 && iter%saveEvery==0)
				printIntermediate(gslDs_->x, iter);
		}

		found(minVector,gslDs_->x,iter);
		gsl_vector_free (x);
		return iter;
	}

	int status() const { return status_; }

	String statusString() const
	{
		switch (status_) {
		case GSL_SUCCESS:
			return "GSL_SUCCESS";
			break;
		case GSL_CONTINUE:
			return "GSL_CONTINUE";
			break;
		default:
			return "UNKNOWN";
			break;
		}
	}

private:

	void found(VectorType& minVector,gsl_vector* x,SizeType iter)
	{
		for (SizeType i=0;i<minVector.size();i++)
			minVector[i] = gsl_vector_get(x,i);
	}

	void printIntermediate(gsl_vector* x, SizeType iter) const
	{
		std::cerr<<"INTERMEDIATE "<<iter<<"\n";
		std::cerr<<x->size<<"\n";
		for (SizeType i=0;i<x->size;i++)
			std::cerr<<gsl_vector_get(x,i)<<"\n";
	}

	RealType gradientNorm(const gsl_vector *v) const
	{
		gsl_vector* df = gsl_vector_alloc (function_.size());
		myDfunction<FunctionType>(v,&function_,df);
		RealType sum = 0;
		for (int i = 0; i < df->size; ++i) {
			RealType tmp = gsl_vector_get(df,i);
			sum += PsimagLite::conj(tmp) * tmp;
		}

		gsl_vector_free (df);
		return sqrt(sum);
	}

	FunctionType& function_;
	SizeType maxIter_;
	bool verbose_;
	int status_;
	const gsl_multimin_fminimizer_type *gslT_;
	gsl_multimin_fminimizer *gslS_;
	const gsl_multimin_fdfminimizer_type *gslDt_;
	gsl_multimin_fdfminimizer *gslDs_;

}; // class Minimizer
}

#else

namespace PsimagLite {

template<typename RealType,typename FunctionType>
class Minimizer {

	typedef typename FunctionType::FieldType FieldType;
	typedef typename Vector<FieldType>::Type VectorType;

public:

	enum {GSL_SUCCESS=0, GSL_CONTINUE=1};

	Minimizer(FunctionType&,SizeType, bool = false)
	{
		String str("Minimizer needs the gsl\n");
		throw RuntimeError(str);
	}

	int simplex(VectorType&,RealType=1e-3,RealType=1e-3)
	{
		String str("Minimizer needs the gsl\n");
		throw RuntimeError(str);
	}

	int status() const { return 1; }

	String statusString() const
	{
		return "Minimizer needs the gsl";
	}
};

} // namespace PsimagLite
#endif

#endif // MINIMIZER_H


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

///@brief
///@file  DenseMatrixMultiplication.h

#ifndef CORE_DATATYPES_DENSEMATRIXMULTIPLICATION_H
#define CORE_DATATYPES_DENSEMATRIXMULTIPLICATION_H

#if defined(HAVE_CBLAS)
#if defined(__APPLE__)
#include <vecLib/cblas.h>
#else
extern "C"{
#include <cblas.h>
}
#endif
#endif

namespace SCIRun {

template <typename T>
void
DenseMatrixGeneric<T>::mult(const ColumnMatrix& x, ColumnMatrix& b,
                            index_type beg, index_type end,
                            int spVec) const
{
  ASSERTEQ(x.nrows(), this->ncols_);
  ASSERTEQ(b.nrows(), this->nrows_);

  if (beg == -1) beg = 0;
  if (end == -1) end = this->nrows_;

#if defined(HAVE_CBLAS)
  double ALPHA = 1.0;
  double BETA = 0.0;
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, (end-beg),
    1, this->ncols_, ALPHA, dataptr_+(beg*this->ncols_), this->ncols_,
    x.get_data_pointer(), 1, BETA,
    b.get_data_pointer()+beg, 1);
#else

  double* xdata = x.get_data_pointer();
  double* bdata = b.get_data_pointer();

  size_type m8 = (this->ncols_)/8;
  size_type m = (this->ncols_)-m8*8;

  index_type i, j;
  if(!spVec)
  {
    for (i=beg; i<end; i++)
    {
      double sum=0;
      double* row=data[i];
      double* xd=xdata;

      for (j=0; j<m8; j++)
      {
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
        sum+=(*row)*(*xd);
        row++; xd++;
      }

      for (j=0; j<m; j++)
      {
        sum+=(*row)*(*xd);
        row++; xd++;
      }
      (*bdata)=sum;
      bdata++;
    }
  }
  else
  {
    for (i=beg; i<end; i++) b[i]=0;
    for (j=0; j<this->ncols_; j++)
    {
      if (x[j])
        for (i=beg; i<end; i++)
        {
          b[i]+=data[i][j]*x[j];
        }
    }
  }

#endif
}

template <typename T>
void
DenseMatrixGeneric<T>::multiply(ColumnMatrix& x, ColumnMatrix& b) const
{
  index_type i, j;

  double* xdata = x.get_data_pointer();
  double* bdata = b.get_data_pointer();

  size_type m8 = (this->ncols_)/8;
  size_type m = (this->ncols_)-m8*8;

  for (i=0; i<this->nrows_; i++)
  {
    double sum=0;
    double* row = data[i];
    double* xd = xdata;
    for (j=0; j<m8; j++)
    {
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
      sum+=(*row)*(*xd);
      row++; xd++;
    }

    for (j=0; j<m; j++)
    {
      sum+=(*row)*(*xd);
      row++; xd++;
    }

    *bdata=sum;
    bdata++;
  }
}

template <typename T>
void
DenseMatrixGeneric<T>::mult_transpose(const ColumnMatrix& x, ColumnMatrix& b,
                                      index_type beg, index_type end,
                                      int spVec) const
{
  // Compute At*x=b
  ASSERT(x.nrows() == this->nrows_);
  ASSERT(b.nrows() == this->ncols_);
  if (beg == -1) beg = 0;
  if (end == -1) end = this->ncols_;
  index_type i, j;
  if (!spVec)
  {
    for (i=beg; i<end; i++)
    {
      double sum=0;
      for (j=0; j<this->nrows_; j++)
      {
        sum+=data[j][i]*x[j];
      }
      b[i]=sum;
    }
  }
  else
  {
    for (i=beg; i<end; i++) b[i]=0;
    for (j=0; j<this->nrows_; j++)
      if (x[j])
      {
        double *row=data[j];
        for (i=beg; i<end; i++)
          b[i]+=row[i]*x[j];
      }
  }
}

template <typename T>
DenseMatrix*
DenseMatrixGeneric<T>::make_diagonal_from_column(const ColumnMatrix& column, size_type rows, size_type cols)
{
  DenseMatrix* result = zero_matrix(rows, cols);
  for (size_type i = 0; i < column.nrows(); ++i)
    (*result)[i][i] = column[i];

  return result;
}

} // End namespace SCIRun

#endif


/* rng/lecuyer21.c
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This generator is taken from
 *
 * Donald E. Knuth
 * The Art of Computer Programming
 * Volume 2
 * Third Edition
 * Addison-Wesley
 * Page 108
 *
 * This implementation copyright (C) 2001 Brian Gough, Carlo Perassi
 * and (C) 2003 Heiko Bauke.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

#define AAA 40692
#define MMM 2147483399UL
#define QQQ 52774
#define RRR 3791

static inline unsigned long int ran_get (void *vstate);
static double ran_get_double (void *vstate);
static void ran_set (void *state, unsigned long int s);

typedef struct
{
  unsigned long int x;
}
ran_state_t;

static inline unsigned long int
ran_get (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  long int y = state->x;
  long int r = RRR * (y / QQQ);

  y = AAA * (y % QQQ) - r;
  if (y < 0)
    y += MMM;

  state->x = y;

  return state->x;
}

static double
ran_get_double (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  return ran_get (state) / 2147483399.0;
}

static void
ran_set (void *vstate, unsigned long int s)
{
  ran_state_t *state = (ran_state_t *) vstate;

  if ((s%MMM) == 0)
    s = 1;                      /* default seed is 1 */

  state->x = s % MMM;

  return;
}

static const gsl_rng_type ran_type = {
  "lecuyer21",                  /* name */
  MMM-1,                        /* RAND_MAX */
  1,                            /* RAND_MIN */
  sizeof (ran_state_t),
  &ran_set,
  &ran_get,
  &ran_get_double
};

const gsl_rng_type *gsl_rng_lecuyer21 = &ran_type;