#include <petsc.h>
#include "flow.h"
#include "mesh.h"

#define DIM 2                   /* Geometric dimension */
#define NFIELDS 3
static const FlowFieldDescriptor PhysicsFields[] = {{"Density", "Den",1},{"Momentum", "Momentum",DIM},{"Energy", "Energy",1},{NULL,0}};
typedef enum {EULER_PAR_GAMMA,EULER_PAR_RHOR,EULER_PAR_AMACH,EULER_PAR_ITANA,EULER_PAR_SIZE} EulerParamIdx;

typedef struct {
    PetscReal rho;
    PetscReal rhoU[DIM];
    PetscReal rhoE;
} EulerNode;
typedef union {
    EulerNode eulernode;
    PetscReal vals[DIM+2];
} EulerNodeUnion;

typedef struct {
    PetscReal gamma;
    PetscReal rhoL;
    PetscReal rhoR;
    PetscReal uL;
    PetscReal uR;
    PetscReal pL;
    PetscReal pR;
    PetscReal maxTime;
    PetscReal length;
} Setup;

typedef struct {
    PetscReal pstar,ustar,rhostarL,astarL,SL,SHL,STL,rhostarR,astarR,SR,SHR,STR,gamm1,gamp1;
} StarState;

typedef struct {
    StarState starState;
    Setup setup;
    PetscReal cfl;
} ProblemSetup;

typedef struct{
    PetscReal f;
    PetscReal fprm;
} PressureFunction;

static PressureFunction f_and_fprm_rarefaction(PetscReal pstar, PetscReal pLR, PetscReal aLR, PetscReal gam, PetscReal gamm1,PetscReal gamp1) {
    // compute value of pressure function for rarefaction
    PressureFunction function;
    function.f = ((2. * aLR) / gamm1) * (pow(pstar / pLR, 0.5 * gamm1 / gam) - 1.);
    function.fprm = (aLR / pLR / gam) * pow(pstar / pLR, -0.5 * gamp1 / gam);
    return function;
}

static PressureFunction f_and_fprm_shock(PetscReal pstar, PetscReal pLR, PetscReal rhoLR, PetscReal gam, PetscReal gamm1, PetscReal gamp1){
    // compute value of pressure function for shock
    PetscReal A = 2./gamp1/rhoLR;
    PetscReal B = gamm1*pLR/gamp1;
    PetscReal sqrtterm =  PetscSqrtReal(A/(pstar+B));
    PressureFunction function;
    function.f = (pstar-pLR)*sqrtterm;
    function.fprm = sqrtterm*(1.-0.5*(pstar-pLR)/(B+pstar));
    return function;
}

#define EPS 1.e-6
#define MAXIT 100
static PetscErrorCode DetermineStarState(const Setup* setup, StarState* starState){
    // compute the speed of sound
    PetscReal aL = PetscSqrtReal(setup->gamma*setup->pL/setup->rhoL);
    PetscReal aR = PetscSqrtReal(setup->gamma*setup->pR/setup->rhoR);

    //first guess pstar based on two-rarefacation approximation
    starState->pstar = aL+aR - 0.5*(setup->gamma-1.)*(setup->uR-setup->uL);
    starState->pstar  = starState->pstar  / (aL/pow(setup->pL,0.5*(setup->gamma-1.)/setup->gamma) + aR/pow(setup->pR,0.5*(setup->gamma-1.)/setup->gamma) );
    starState->pstar = pow(starState->pstar,2.*setup->gamma/(setup->gamma-1.));
    starState->gamm1 = setup->gamma-1.;
    starState->gamp1 = setup->gamma+1.;

    PressureFunction fL;
    if (starState->pstar <= setup->pL){
        fL = f_and_fprm_rarefaction(starState->pstar, setup->pL,aL,setup->gamma,starState->gamm1,starState->gamp1);
    }else{
        fL = f_and_fprm_shock(starState->pstar,setup->pL,setup->rhoL,setup->gamma,starState->gamm1,starState->gamp1);
    }

    PressureFunction fR;
    if (starState->pstar <= setup->pR) {
        fR = f_and_fprm_rarefaction(starState->pstar, setup->pR, aR, setup->gamma, starState->gamm1, starState->gamp1);
    }else {
        fR = f_and_fprm_shock(starState->pstar, setup->pR, setup->rhoR, setup->gamma, starState->gamm1, starState->gamp1);
    }
    PetscReal delu = setup->uR-setup->uL;

    // iterate using Newton-Rapson
    if ((fL.f+fR.f+delu)> EPS) {
        // iterate using Newton-Rapson
        for(PetscInt it =0; it < MAXIT+4; it++){
            PetscReal pold = starState->pstar;
            starState->pstar = pold - (fL.f+fR.f+delu)/(fL.fprm+fR.fprm);

            if(starState->pstar < 0){
                starState->pstar = EPS;
            }

            if(2.0*PetscAbsReal(starState->pstar - pold)/(starState->pstar + pold) < EPS){
                break;
            }else{
                if(starState->pstar < setup->pL){
                    fL = f_and_fprm_rarefaction(starState->pstar, setup->pL,aL,setup->gamma,starState->gamm1,starState->gamp1);
                }else{
                    fL = f_and_fprm_shock(starState->pstar,setup->pL,setup->rhoL,setup->gamma,starState->gamm1,starState->gamp1);
                }
                if (starState->pstar<=setup->pR) {
                    fR = f_and_fprm_rarefaction(starState->pstar, setup->pR, aR, setup->gamma, starState->gamm1, starState->gamp1);
                }else {
                    fR = f_and_fprm_shock(starState->pstar, setup->pR, setup->rhoR, setup->gamma, starState->gamm1, starState->gamp1);
                }
            }

            if (it>MAXIT){
                SETERRQ(PETSC_COMM_WORLD,1,"error in Riemann.find_pstar - did not converage for pstar" );
            }
        }
    }

    // determine rest of star state
    starState->ustar = 0.5*(setup->uL+setup->uR+fR.f-fL.f);

    // left star state
    PetscReal pratio = starState->pstar/setup->pL;
    if (starState->pstar<=setup->pL) {  // rarefaction
        starState->rhostarL = setup->rhoL * PetscPowReal(pratio, 1. / setup->gamma);
        starState->astarL = aL * PetscPowReal(pratio, 0.5 * starState->gamm1 / setup->gamma);
        starState->SHL = setup->uL - aL;
        starState->STL = starState->ustar - starState->astarL;
    }else {  // #shock
        starState->rhostarL = setup->rhoL * (pratio + starState->gamm1 / starState->gamp1) / (starState->gamm1 * pratio / starState->gamp1 + 1.);
        starState->SL = setup->uL - aL * PetscSqrtReal(0.5 * starState->gamp1 / setup->gamma * pratio + 0.5 * starState->gamm1 / setup->gamma);
    }

    // right star state
    pratio = starState->pstar/setup->pR;
    if (starState->pstar<=setup->pR) {  // # rarefaction
        starState->rhostarR = setup->rhoR * PetscPowReal(pratio, 1. / setup->gamma);
        starState->astarR = aR * PetscPowReal(pratio, 0.5 * starState->gamm1 / setup->gamma);
        starState-> SHR = setup->uR + aR;
        starState->STR = starState->ustar + starState->astarR;
    }else {  // shock
        starState->rhostarR = setup->rhoR * (pratio + starState->gamm1 / starState->gamp1) / (starState->gamm1 * pratio / starState->gamp1 + 1.);
        starState->SR = setup->uR + aR * PetscSqrtReal(0.5 * starState->gamp1 / setup->gamma * pratio + 0.5 * starState->gamm1 / setup->gamma);
    }
    return 0;
}

static void SetExactSolutionAtPoint(PetscInt dim, PetscReal xDt, const Setup* setup, const StarState* starState, EulerNode* uu){
    PetscReal p;
    // compute the speed of sound
    PetscReal aL = PetscSqrtReal(setup->gamma*setup->pL/setup->rhoL);
    PetscReal aR = PetscSqrtReal(setup->gamma*setup->pR/setup->rhoR);

    for(PetscInt i =0; i < dim; i++){
        uu->rhoU[i] = 0.0;
    }

    if (xDt <= starState->ustar) {  //# left of contact surface
        if (starState->pstar <= setup->pL) {  // # rarefaction
            if (xDt <= starState->SHL) {
                uu->rho = setup->rhoL;
                p = setup->pL;
                uu->rhoU[0] = setup->uL*uu->rho;
            }else if (xDt <=starState->STL) {  //#SHL < x / t < STL
                PetscReal tmp = 2. / starState->gamp1 + (starState->gamm1 / starState->gamp1 / aL) * (setup->uL - xDt);
                uu->rho = setup->rhoL * pow(tmp, 2. / starState->gamm1);
                uu->rhoU[0] = uu->rho * (2. / starState->gamp1) * (aL + 0.5 * starState->gamm1 * setup->uL + xDt);
                p = setup->pL * pow(tmp, 2. * setup->gamma / starState->gamm1);
            }else {  //# STL < x/t < u*
                uu->rho = starState->rhostarL;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }else{  //# shock
            if (xDt<= starState->SL) {  // # xDt < SL
                uu->rho = setup->rhoL;
                p = setup->pL;
                uu->rhoU[0] = uu->rho * setup->uL;
            }else {  //# SL < xDt < ustar
                uu->rho = starState->rhostarL;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }
    }else{//# right of contact surface
        if (starState->pstar<=setup->pR) {  //# rarefaction
            if (xDt>= starState->SHR) {
                uu->rho = setup->rhoR;
                p = setup->pR;
                uu->rhoU[0] = uu->rho * setup->uR;
            }else if (xDt >= starState->STR) {  // # SHR < x/t < SHR
                PetscReal tmp = 2./starState->gamp1 - (starState->gamm1/starState->gamp1/aR)*(setup->uR-xDt);
                uu->rho = setup->rhoR*PetscPowReal(tmp,2./starState->gamm1);
                uu->rhoU[0] = uu->rho * (2./starState->gamp1)*(-aR + 0.5*starState->gamm1*setup->uR+xDt);
                p = setup->pR*PetscPowReal(tmp,2.*setup->gamma/starState->gamm1);
            }else{ //# u* < x/t < STR
                uu->rho = starState->rhostarR;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }else {//# shock
            if (xDt>= starState->SR) {  // # xDt > SR
                uu->rho = setup->rhoR;
                p = setup->pR;
                uu->rhoU[0] = uu->rho * setup->uR;
            }else {//#ustar < xDt < SR
                uu->rho = starState->rhostarR;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }
    }
    PetscReal e =  p/starState->gamm1/uu->rho;
    PetscReal E = e + 0.5*(uu->rhoU[0]/uu->rho)*(uu->rhoU[0]/uu->rho);
    uu->rhoE = uu->rho*E;
}

static PetscErrorCode SetExactSolutionRho(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);

    u[0] = uu.rho;
    return 0;
}

static PetscErrorCode SetExactSolutionRhoU(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);
    u[0] = uu.rhoU[0];
    u[1] = uu.rhoU[1];
    return 0;
}

static PetscErrorCode SetExactSolutionRhoE(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);
    u[0] = uu.rhoE;
    return 0;
}

static PetscErrorCode PrintVector(DM dm, Vec v, PetscInt step, const char * fileName){
    Vec                cellgeom;
    PetscErrorCode ierr = DMPlexGetGeometryFVM(dm, NULL, &cellgeom, NULL);CHKERRQ(ierr);
    PetscInt cStart, cEnd;
        ierr = DMPlexGetSimplexOrBoxCells(dm, 0, &cStart, &cEnd);CHKERRQ(ierr);
    DM dmCell;
    ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
    const PetscScalar *cgeom;
    ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    const PetscScalar      *x;
    ierr = VecGetArrayRead(v, &x);CHKERRQ(ierr);
    // print the header for each file
    char filename[100];
    ierr = PetscSNPrintf(filename,sizeof(filename),"%s.%d.txt",fileName, step);CHKERRQ(ierr);

    PetscInt rank = 0;
    PetscInt size;
    ierr = MPI_Comm_rank(PetscObjectComm(dm), &rank);CHKERRMPI(ierr);
    ierr = MPI_Comm_size(PetscObjectComm(dm), &size);CHKERRMPI(ierr);

    for(PetscInt r =0; r < size; r++ ) {
        if(r == rank) {
            FILE *fptr;
            if(r == 0){
                fptr = fopen(filename, "w");
                fprintf(fptr, "x rho u e\n");
            }else{
                fptr = fopen(filename, "a");
            }
            for (PetscInt c = cStart; c < cEnd; ++c) {
                PetscFVCellGeom *cg;
                const EulerNode *xc;

                ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);
                CHKERRQ(ierr);
                ierr = DMPlexPointGlobalFieldRead(dm, c, 0, x, &xc);
                CHKERRQ(ierr);
                if(xc) {// must be real cell and not ghost
                    PetscReal u0 = xc->rhoU[0] / xc->rho;
                    fprintf(fptr, "%f %f %f %f\n", cg->centroid[0], xc->rho, u0, (xc->rhoE / xc->rho) - 0.5 * u0 * u0);
                }
            }

            fclose(fptr);
        }
        MPI_Barrier(PetscObjectComm(dm));
    }
    ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(v, &x);CHKERRQ(ierr);
    return 0;
}

static PetscErrorCode MonitorError(TS ts, PetscInt step, PetscReal time, Vec u, void *ctx) {
    PetscFunctionBeginUser;
    PetscErrorCode     ierr;

    // Get the DM
    DM dm;
    ierr = TSGetDM(ts, &dm);CHKERRQ(ierr);

    // Create a copy of the u vector
    Vec e;
    ierr = DMCreateGlobalVector(dm, &e);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject)e, "exact");CHKERRQ(ierr);

    // Set the values
    PetscErrorCode     (*func[3]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {{SetExactSolutionRho, SetExactSolutionRhoU, SetExactSolutionRhoE}};
    void* ctxs[3] ={ctx, ctx, ctx};
    ierr    = DMProjectFunction(dm,time,func,ctxs,INSERT_ALL_VALUES,e);CHKERRQ(ierr);

    // just print to a file for now
    ierr = PrintVector(dm, e, step, "exact");CHKERRQ(ierr);
    ierr = PrintVector(dm, u, step, "solution");CHKERRQ(ierr);

    PetscPrintf(PETSC_COMM_WORLD, "TS %d: %f\n", step, time);

    DMRestoreGlobalVector(dm, &e);
    PetscFunctionReturn(0);
}

static PetscErrorCode PhysicsBoundary_Euler_Mirror(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup* prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = xI->rho;
    xG->rhoE = xI->rhoE;
    xG->rhoU[0] = xI->rhoU[0];
    xG->rhoU[1] = xI->rhoU[1];

    PetscFunctionReturn(0);
}

/* PetscReal* => EulerNode* conversion */
static PetscErrorCode PhysicsBoundary_Euler_Left(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup* prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = prob->setup.rhoL;
    PetscReal eT = prob->setup.rhoL*((prob->setup.pL /(prob->setup.gamma -1) / prob->setup.rhoL) + 0.5 * prob->setup.uL * prob->setup.uL);
    xG->rhoE = eT;
    xG->rhoU[0] = prob->setup.rhoL * prob->setup.uL;
    xG->rhoU[1] = 0.0;

    PetscFunctionReturn(0);
}

/* PetscReal* => EulerNode* conversion */
static PetscErrorCode PhysicsBoundary_Euler_Right(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup*        prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = prob->setup.rhoR;
    PetscReal eT = prob->setup.rhoR*((prob->setup.pR /(prob->setup.gamma -1)/ prob->setup.rhoR) + 0.5 * prob->setup.uR * prob->setup.uR);
    xG->rhoE = eT;
    xG->rhoU[0] = prob->setup.rhoR * prob->setup.uR;
    xG->rhoU[1] = 0.0;

    PetscFunctionReturn(0);
}

static PetscErrorCode  ComputeTimeStep(TS ts){
    DM dm;
    PetscErrorCode ierr = TSGetDM(ts, &dm);CHKERRQ(ierr);
    Vec v;
    TSGetSolution(ts, &v);
    ProblemSetup *problem;
    ierr = DMGetApplicationContext(dm, &problem);CHKERRQ(ierr);

    Vec                cellgeom;
    ierr = DMPlexGetGeometryFVM(dm, NULL, &cellgeom, NULL);CHKERRQ(ierr);
    PetscInt cStart, cEnd;
    ierr = DMPlexGetSimplexOrBoxCells(dm, 0, &cStart, &cEnd);CHKERRQ(ierr);
    DM dmCell;
    ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
    const PetscScalar *cgeom;
    ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    const PetscScalar      *x;
    ierr = VecGetArrayRead(v, &x);CHKERRQ(ierr);

    PetscReal dtMin = 1.0;

    for (PetscInt c = cStart; c < cEnd; ++c) {
        PetscFVCellGeom       *cg;
        const EulerNode           *xc;

        ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);CHKERRQ(ierr);
        ierr = DMPlexPointGlobalFieldRead(dm, c, 0, x, &xc);CHKERRQ(ierr);

        if(xc) {  // must be real cell and not ghost

            PetscReal rho = xc->rho;
            PetscReal u = xc->rhoU[0] / rho;
            PetscReal e = (xc->rhoE / rho) - 0.5 * u * u;
            PetscReal p = (problem->setup.gamma - 1) * rho * e;

            PetscReal a = PetscSqrtReal(problem->setup.gamma * p / rho);
            PetscReal dt = problem->cfl * cg->volume / (a + PetscAbsReal(u));
            dtMin = PetscMin(dtMin, dt);
        }
    }
    PetscInt rank;
    MPI_Comm_rank(PetscObjectComm(ts), &rank);
    printf("dtMin(%d): %f\n", rank,dtMin );

    PetscReal dtMinGlobal;
    ierr = MPI_Allreduce(&dtMin, &dtMinGlobal, 1,MPIU_REAL, MPI_MIN, PetscObjectComm(ts));

    PetscPrintf(PetscObjectComm(ts), "TimeStep: %f\n", dtMinGlobal);
//    ierr = TSSetTimeStep(ts, dtMinGlobal);CHKERRQ(ierr);

    ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(v, &x);CHKERRQ(ierr);
    return 0;
}

static void ComputeFluxRho(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

        flux[0] = rho * u * PetscSignReal(n[0]);
        printf("flux qp[%f]: %f  n:%f rL:%f rR:%f\n ", qp[0], flux[0], n[0], currentValues.rhoL, currentValues.rhoR);
//        flux->rhoU[0] = (rho * u * u + p)* PetscSignReal(n[0]);
//        flux->rhoU[1] = 0.0;
//        PetscReal et = e + 0.5 * u * u;
//        flux->rhoE = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f,%f\n", qp[0], qp[1], flux[0], n[0], n[1]);mm

    }else{
        flux[0] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

static void ComputeFluxU(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

//        flux[0] = (rho * u) * PetscSignReal(n[0]);
        flux[0] = (rho * u * u + p)* PetscSignReal(n[0]);
        flux[1] = 0.0;
//        PetscReal et = e + 0.5 * u * u;
//        flux->rhoE = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f %f %f\n", qp[0], qp[1], flux->rho, flux->rhoU[0], flux->rhoE, n[0]);

    }else{
        flux[0] = 0.0;
        flux[1] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

static void ComputeFluxE(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

//        flux[0] = (rho * u) * PetscSignReal(n[0]);
//        flux[0] = (rho * u * u + p)* PetscSignReal(n[0]);
//        flux[1] = 0.0;
        PetscReal et = e + 0.5 * u * u;
        flux[0] = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f %f %f\n", qp[0], qp[1], flux->rho, flux->rhoU[0], flux->rhoE, n[0]);

    }else{
        flux[0] = 0.0;
//        flux[1] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

int main(int argc, char **argv)
{
    PetscErrorCode ierr;
    // create the mesh
    // setup the ts
    DM dm;                 /* problem definition */
    TS ts;                 /* timestepper */

    // initialize petsc and mpi
    PetscInitialize(&argc, &argv, NULL, "HELP");

    // Setup the problem
    ProblemSetup problem;

    // case 1 - Sod problem
    problem.setup.rhoL=1.0;
    problem.setup.uL=0.0;
    problem.setup.pL=1.0;
    problem.setup.rhoR=0.125;
    problem.setup.uR=0.0;
    problem.setup.pR=0.1;
    problem.setup.maxTime = 0.25;
    problem.setup.length = 1;
    problem.setup.gamma = 1.4;
    problem.cfl = .5;

    // case 2 - 123 problem - expansion left and expansion right
//    problem.setup.rhoL=1.0;
//    problem.setup.uL=-2.0;
//    problem.setup.pL=0.4;
//    problem.setup.rhoR=1.0;
//    problem.setup.uR=2.0;
//    problem.setup.pR=0.4;
//    problem.setup.maxTime = 0.15;
//    problem.setup.length = 1;
//    problem.setup.gamma = 1.4;
//    problem.cfl = 0.4;

    ierr = TSCreate(PETSC_COMM_WORLD, &ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetType(ts, TSEULER);CHKERRQ(ierr);

    //PetscErrorCode DMPlexCreateBoxMesh(MPI_Comm comm, PetscInt dim, PetscBool simplex, const PetscInt faces[], const PetscReal lower[], const PetscReal upper[], const DMBoundaryType periodicity[], PetscBool interpolate, DM *dm)
    PetscReal start[] = {0.0, 0.0};
    PetscReal end[] = {problem.setup.length, 1};
    PetscInt nx[] = {100, 1};
    DMBoundaryType bcType[] = {DM_BOUNDARY_NONE, DM_BOUNDARY_NONE};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, DIM, PETSC_FALSE, nx, start, end, bcType, PETSC_TRUE, &dm);CHKERRQ(ierr);

    {
        DM dmDist;

//        ierr = DMSetBasicAdjacency(dm, PETSC_TRUE, PETSC_FALSE);CHKERRQ(ierr);
        ierr = DMPlexDistribute(dm, 1, NULL, &dmDist);CHKERRQ(ierr);
        if (dmDist) {
            ierr = DMDestroy(&dm);CHKERRQ(ierr);
            dm   = dmDist;
        }
    }

    ierr = DMSetFromOptions(dm);CHKERRQ(ierr);

    {
        DM gdm;
        ierr = DMPlexConstructGhostCells(dm, NULL, NULL, &gdm);CHKERRQ(ierr);
        ierr = DMDestroy(&dm);CHKERRQ(ierr);
        dm   = gdm;
    }

//    DMLabel label;
//    ierr = DMGetLabel(dm, "marker", &label );CHKERRQ(ierr);
//    ierr = DMLabelView(label, PETSC_VIEWER_STDOUT_WORLD);;CHKERRQ(ierr);

    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetDM(ts, dm);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetExactFinalTime(ts, TS_EXACTFINALTIME_MATCHSTEP);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // setup the FV

    {// Setup the fields
        PetscInt f, dof;
        for (f=0,dof=0; f < 3; f++) {
            PetscInt newDof = PhysicsFields[f].components;

//            if (newDof == 1) {
                PetscFV           fvm;
                ierr = PetscFVCreate(PETSC_COMM_WORLD, &fvm);CHKERRQ(ierr);

                ierr = PetscFVSetFromOptions(fvm);CHKERRQ(ierr);
                ierr = PetscFVSetNumComponents(fvm, newDof);CHKERRQ(ierr);
                ierr = PetscFVSetSpatialDimension(fvm, DIM);CHKERRQ(ierr);
                ierr = PetscObjectSetName((PetscObject) fvm,PhysicsFields[f].fieldName);CHKERRQ(ierr);

                /* FV is now structured with one field having all physics as components */
                ierr = DMAddField(dm, NULL, (PetscObject) fvm);CHKERRQ(ierr);
//            }
//            else {
//                PetscInt j;
//
//                for (j = 0; j < newDof; j++) {
//                    char     compName[256]  = "Unknown";
//
//                    ierr = PetscSNPrintf(compName,sizeof(compName),"%s_%d",PhysicsFields[f].fieldName,j);CHKERRQ(ierr);
//                    ierr = PetscFVSetComponentName(fvm,dof+j,compName);CHKERRQ(ierr);
//                }
//            }
//            dof += newDof;
        }
    }

    // Compute the star state
    ierr = DetermineStarState(&problem.setup, &problem.starState);CHKERRQ(ierr);

    PetscDS           prob;
    ierr = DMCreateDS(dm);CHKERRQ(ierr);
    ierr = DMGetDS(dm, &prob);CHKERRQ(ierr);
    ierr = DMSetApplicationContext(dm, &problem);CHKERRQ(ierr);

    //TODO: Add flux
    ierr = PetscDSSetRiemannSolver(prob, 0,ComputeFluxRho);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 0, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetRiemannSolver(prob, 1,ComputeFluxU);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 1, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetRiemannSolver(prob, 2,ComputeFluxE);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 2, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetFromOptions(prob);CHKERRQ(ierr);
    //TODO: Apply boundary

    // setup the solution vector, this olds everything
    Vec X;
    ierr = DMCreateGlobalVector(dm, &X);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject) X, "solution");CHKERRQ(ierr);

//    ierr = DMTSSetBoundaryLocal(dm, DMPlexTSComputeBoundary, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetIFunctionLocal(dm, DMPlexTSComputeIFunctionFEM, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetIJacobianLocal(dm, DMPlexTSComputeIJacobianFEM, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetRHSFunctionLocal(dm, DMPlexTSComputeRHSFunctionFVM, NULL);CHKERRQ(ierr);//TODO: This is were we set the RHS function

    const PetscInt idsLeft[]= {4};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall left", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Left, NULL, 1, idsLeft, &problem);CHKERRQ(ierr);
    const PetscInt idsRight[]= {2};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall right", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Right, NULL, 1, idsRight, &problem);CHKERRQ(ierr);

    const PetscInt mirror[]= {1, 3};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "mirrorWall", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Mirror, NULL, 2, mirror, &problem);CHKERRQ(ierr);

//    ierr = DMTSSetBoundaryLocal(dm, DMPlexTSComputeBoundary, NULL);CHKERRQ(ierr);
    ierr = DMTSSetRHSFunctionLocal(dm, DMPlexTSComputeRHSFunctionFVM, NULL);CHKERRQ(ierr);//TODO: This is were we set the RHS function
    ierr = TSSetMaxTime(ts,problem.setup.maxTime);CHKERRQ(ierr);
    ierr = TSMonitorSet(ts, MonitorError, &problem, NULL);CHKERRQ(ierr);
    ierr = TSSetTimeStep(ts, 0.0008);CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
    ierr = TSSetPostStep(ts, ComputeTimeStep);CHKERRQ(ierr);

    // set the initial conditions
    PetscErrorCode     (*func[3]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {SetExactSolutionRho, SetExactSolutionRhoU, SetExactSolutionRhoE};
    void* ctxs[3] ={&problem, &problem, &problem};
    ierr    = DMProjectFunction(dm,0.0,func,ctxs,INSERT_ALL_VALUES,X);CHKERRQ(ierr);

//    PetscInt rank, size;
//    MPI_Comm_rank(PetscObjectComm(dm), &rank);
//    MPI_Comm_size(PetscObjectComm(dm), &size);
//    for(int r =0; r < size; r++) {
//        if(r == rank) {
//            printf("Rank: %d\n", r);
//            ierr = DMView(dm, PETSC_VIEWER_STDOUT_SELF);
//            CHKERRQ(ierr);
////            ierr = DMViewFromOptions(dm, NULL, "-dm_view");
////            CHKERRQ(ierr);
//        }
//        MPI_Barrier(PetscObjectComm(dm));
//    }
//    TSSetMaxSteps(ts, 1);
    ierr = TSSolve(ts,X);CHKERRQ(ierr);
//    ierr = TSGetSolveTime(ts,&ftime);CHKERRQ(ierr);
//    ierr = TSGetStepNumber(ts,&nsteps);
    //    CHKERRQ(ierr);

    return PetscFinalize();

}


/*
** use median of trials
**
** G.Lohmann, May 2016
*/
#include <viaio/Vlib.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_statistics.h>

#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>

extern void VNormalize(float *data,int nt,VBoolean stddev);
extern void GetRank(float *data,gsl_vector *v,gsl_permutation *perm,gsl_permutation *rank);

void GetMedian(gsl_matrix_float **X1,gsl_matrix_float **X2,int *table,int n,gsl_matrix_float *SNR,int metric)
{
  long j,nvox=X1[0]->size1;
  long k,s,nt=X1[0]->size2;

  double *data = (double *) VCalloc(n,sizeof(double));
  double median = 0;

  gsl_vector *vec = NULL;
  gsl_permutation *perm = NULL;
  gsl_permutation *rank = NULL;
  if (metric == 1) {
    vec = gsl_vector_calloc(nt);
    perm = gsl_permutation_alloc(nt);
    rank = gsl_permutation_alloc(nt);
  }

  /* for each voxel ... */
  for (j=0; j<nvox; j++) {

    for (k=0; k<nt; k++) {
      for (s=0; s<n; s++) {
	if (table[s] == 0)
	  data[s] = (double)gsl_matrix_float_get(X1[s],j,k);
	else
	  data[s] = (double)gsl_matrix_float_get(X2[s],j,k);
      }
      gsl_sort(data,1,n);
      median = gsl_stats_median_from_sorted_data (data,1,(size_t) n);
      gsl_matrix_float_set(SNR,j,k,(float)median);
    }
    if (metric == 0) {  /* pearson correlation */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      VNormalize(qq,nt,TRUE);
    }
    if (metric == 1) {  /* spearman ranks      */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      GetRank(qq,vec,perm,rank);
    }
  }
  VFree(data);

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


//
//  hemm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/6/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _hemm_h
#define _hemm_h

/// @file hemm.h Performs general Hermitian complex matrix-matrix multiplication.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief  Performs general Hermitian complex matrix-matrix multiplication.
   ///
   /// For complex matrices B and C, Hermitian matrix A, and complex scalars alpha and beta,
   ///
   ///         C := alpha*A*B + beta*C  or   C := alpha*B*A + beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then C := alpha*A*B + beta*C,
   ///        if side = 'R' or 'r' then C := alpha*B*A + beta*C.
   /// @param uplo Specifies whether the upper or lower triangular part of the Hermitian matrix A
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular,
   ///        if uplo = 'L' or 'l' then A is lower triangular.
   /// @param m Specifies the number of rows of the matrices B and C.  m>=0
   /// @param n Specifies the number of columns of the matrices B and C.  n>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex Hermitian matrix A.  If side = 'L' or 'l', then A is m-by-m;
   /// if side = 'R' or 'r', then A is n-by-n.
   /// @param ldA Column length of the matrix A.  If side = 'L' or 'l', ldA>=m.  If side = 'R' or 'r', ldA>=n.
   /// @param B Pointer to complex m-by-n matrix B.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex m-by-n matrix C.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int HEMM(char side, char uplo, int_t m, int_t n, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::conj;
      using std::real;
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,k;
      complex<real_t> *a,*b,*c,*at,*bt;
      complex<real_t> s,t;

      side=toupper(side);
      uplo=toupper(uplo);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;
      else if((m==0)||(n==0)||((alpha==zero)&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if(side=='L')
      {
         if(uplo=='U')
         {
            b=B;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<m;i++)
               {
                  t=alpha*b[i];
                  s=zero;
                  for(k=0;k<i;k++)
                  {
                     c[k]+=t*a[k];
                     s+=b[k]*conj(a[k]);
                  }
                  c[i]=beta*c[i]+t*real(a[i])+alpha*s;
                  a+=ldA;
               }
               b+=ldB;
               c+=ldC;
            }
         }
         else
         {
            b=B;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+m*ldA;
               for(i=m-1;i>=0;i--)
               {
                  a-=ldA;
                  t=alpha*b[i];
                  s=zero;
                  for(k=i+1;k<m;k++)
                  {
                     c[k]+=t*a[k];
                     s+=b[k]*conj(a[k]);
                  }
                  c[i]=beta*c[i]+t*real(a[i])+alpha*s;
               }
               b+=ldB;
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            a=A;
            c=C;
            b=B;
            for(j=0;j<n;j++)
            {
               t=alpha*real(a[j]);
               for(i=0;i<m;i++)
                  c[i]=c[i]*beta+t*b[i];
               bt=B;
               for(k=0;k<j;k++)
               {
                  t=alpha*a[k];
                  for(i=0;i<m;i++)
                     c[i]+=bt[i]*t;
                  bt+=ldB;
               }
               at=A+(j+1)*ldA;
               bt=B+(j+1)*ldB;
               for(k=j+1;k<n;k++)
               {
                  t=alpha*conj(at[j]);
                  for(i=0;i<m;i++)
                     c[i]+=t*bt[i];
                  at+=ldA;
                  bt+=ldB;
               }
               a+=ldA;
               b+=ldB;
               c+=ldC;
            }
         }
         else
         {
            a=A;
            c=C;
            b=B;
            for(j=0;j<n;j++)
            {
               t=alpha*real(a[j]);
               for(i=0;i<m;i++)
                  c[i]=c[i]*beta+t*b[i];
               bt=B;
               at=A;
               for(k=0;k<j;k++)
               {
                  t=alpha*conj(at[j]);
                  for(i=0;i<m;i++)
                     c[i]+=bt[i]*t;
                  at+=ldA;
                  bt+=ldB;
               }
               bt=B+(j+1)*ldB;
               for(k=j+1;k<n;k++)
               {
                  t=alpha*at[k];
                  for(i=0;i<m;i++)
                     c[i]+=t*bt[i];
                  bt+=ldB;
               }
               a+=ldA;
               b+=ldB;
               c+=ldC;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int HEMM<float>(char side, char uplo, int_t m, int_t n, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB, complex<float> beta, complex<float> *C, int_t ldC)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_chemm(CblasColMajor,Side,Uplo,m,n,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

   template <> int HEMM<double>(char side, char uplo, int_t m, int_t n, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB, complex<double> beta, complex<double> *C, int_t ldC)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zhemm(CblasColMajor,Side,Uplo,m,n,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }
#endif
}
#endif


#include <sstream>
#include <iostream>
#include <string>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_rng.h>
#include <iomanip>
#include <complex>
#include <cmath>
#include <random>

using namespace std::complex_literals;

// pretty print a gsl vector.
std::string ppVector(const gsl_vector *v) {
    std::ostringstream str;
    int r;

    str << "(";
    for (r = 0; r < (int) v->size; r++) {
        str << gsl_vector_get(v, r);
        if (r < v->size - 1) {
            str << ",";
        }
    }
    str << ")";
    return str.str();
}

void
vectorOpsCheck() {
    fprintf(stdout, "\n Vector Operations ..............................................\n");
    int N = 3;

    gsl_vector *v1 = gsl_vector_alloc(N);
    gsl_vector *v2 = gsl_vector_alloc(N);
    gsl_vector *v3 = gsl_vector_alloc(N);
    gsl_vector *v4 = gsl_vector_alloc(N);
    gsl_vector *v5 = gsl_vector_alloc(N);

    gsl_vector_set_all(v1, 1.0);
    gsl_vector_set_zero(v2);

    gsl_vector_set(v2, 1, 1.0);

    gsl_vector_set(v3, 0, 0.1);
    gsl_vector_set(v3, 1, 0.6);
    gsl_vector_set(v3, 2, 0.3);

    gsl_vector_set(v4, 0, 1.3);
    gsl_vector_set(v4, 1, 0.0);
    gsl_vector_set(v4, 2, 0.9);

    gsl_vector_set(v5, 0, 0.8);
    gsl_vector_set(v5, 1, 1.2);
    gsl_vector_set(v5, 2, 1.6);

    fprintf(stdout, "v1 = %s\n", ppVector(v1).c_str());
    fprintf(stdout, "v2 = %s\n", ppVector(v2).c_str());
    gsl_vector_add(v1, v2);
    fprintf(stdout, "v1 + v2 = %s\n", ppVector(v1).c_str());
    fprintf(stdout, "\n");

    fprintf(stdout, "v1 = %s\n", ppVector(v3).c_str());
    fprintf(stdout, "v2 = %s\n", ppVector(v5).c_str());
    gsl_vector_sub(v3, v5);
    fprintf(stdout, "v1 - v2 = %s\n", ppVector(v3).c_str());
    fprintf(stdout, "\n");

    fprintf(stdout, "v1 = %s\n", ppVector(v2).c_str());
    fprintf(stdout, "v2 = %s\n", ppVector(v4).c_str());
    gsl_vector_mul(v2, v4);
    fprintf(stdout, "v1 dot v2 = %s\n", ppVector(v2).c_str());
    fprintf(stdout, "\n");

    fprintf(stdout, "v1 = %s\n",  ppVector(v5).c_str());
    fprintf(stdout, "norm(v1) = %f\n", gsl_blas_dnrm2(v5));
    fprintf(stdout, "\n");

    gsl_vector_free(v1);
    gsl_vector_free(v2);
    gsl_vector_free(v3);
    gsl_vector_free(v4);
    gsl_vector_free(v5);

    fprintf(stdout, "\n");
}

void
complexNumOpsCheck() {
    fprintf(stdout, "\n Complex Number Operations ..............................................\n");

    std::complex<double> c1 = 1.1 + 2.0i;
    std::complex<double> c2 = 2.3 - 0.4i;
    fprintf(stdout, "let c1 = Complex(real: %1.1f, imag: %1.1f)\n", real(c1), imag(c1));
    fprintf(stdout, "let c2 = Complex(real: %1.1f, imag: %1.1f)\n", real(c2), imag(c2));
    fprintf(stdout, "XCTAssertEqual(c1+c2, c2+c1)\n");
    fprintf(stdout, "XCTAssertEqual(c2*c1, c2*c1)\n");

    fprintf(stdout, "XCTAssertEqual(c1+c2, Complex(real:%1.1f, imag: %1.1f))\n", real(c1+c2), imag(c1+c2));
    fprintf(stdout, "XCTAssertEqual(c1-c2, Complex(real:%1.1f, imag: %1.1f))\n", real(c1-c2), imag(c1-c2));
    fprintf(stdout, "XCTAssertEqual(c1*c2, Complex(real:%1.1f, imag: %1.1f))\n", real(c1*c2), imag(c1*c2));
    fprintf(stdout, "XCTAssertEqual(c1/c2, Complex(real:%1.6f, imag: %1.6f))\n", real(c1/c2), imag(c1/c2));
    fprintf(stdout, "XCTAssertEqual(c2/c1, Complex(real:%1.6f, imag: %1.5f))\n", real(c2/c1), imag(c2/c1));

    fprintf(stdout, "XCTAssertEqual(c1.conjugate(), Complex(real:%1.1f, imag: %1.1f))\n", real(conj(c1)), imag(conj(c1)));
    fprintf(stdout, "XCTAssertEqual(c1.modulus(), %1.14f\n", norm(c1));
    fprintf(stdout, "XCTAssertEqual(c1.norm(), %1.14f\n", norm(c1));
    fprintf(stdout, "XCTAssertEqualWithAccuracy(c1.arg(), %1.15f, accuracy:0.0001)\n", arg(c1));

    fprintf(stdout, "XCTAssertEqual(c2.conjugate(), Complex(real:%1.1f, imag: %1.1f))\n", real(conj(c2)), imag(conj(c2)));
    fprintf(stdout, "XCTAssertEqual(c2.modulus(), %1.14f\n", norm(c2));
    fprintf(stdout, "XCTAssertEqual(c2.norm(), %1.14f\n", norm(c2));
    fprintf(stdout, "XCTAssertEqualWithAccuracy(c2.arg(), %1.15f, accuracy:0.0001)\n", arg(c2));

    fprintf(stdout, "\n");
}

void
printComplexMathChecks(std::complex<double> c1) {

     fprintf(stdout, "var c1 = Complex(real: %1.1f, imag: %1.1f)\n", real(c1), imag(c1));
    fprintf(stdout, "let cosc1 = cos(value: c1)\n");
    fprintf(stdout, "let sinc1 = sin(value: c1)\n");
    fprintf(stdout, "let tanc1 = tan(value: c1)\n");
    fprintf(stdout, "XCTAssertEqual(cosc1.real(), %1.15f)\n", real(cos(c1)));
    fprintf(stdout, "XCTAssertEqual(cosc1.imag(), %1.15f)\n", imag(cos(c1)));
    fprintf(stdout, "XCTAssertEqual(sinc1.real(), %1.15f)\n", real(sin(c1)));
    fprintf(stdout, "XCTAssertEqual(sinc1.imag(), %1.15f)\n", imag(sin(c1)));
    fprintf(stdout, "XCTAssertEqual(tanc1.real(), %1.15f)\n", real(tan(c1)));
    fprintf(stdout, "XCTAssertEqual(tanc1.imag(), %1.15f)\n", imag(tan(c1)));

    fprintf(stdout, "let coshc1 = cosh(value: c1)\n");
    fprintf(stdout, "let sinhc1 = sinh(value: c1)\n");
    fprintf(stdout, "let tanhc1 = tanh(value: c1)\n");
    fprintf(stdout, "XCTAssertEqual(coshc1.real(), %1.15f)\n", real(cosh(c1)));
    fprintf(stdout, "XCTAssertEqual(coshc1.imag(), %1.15f)\n", imag(cosh(c1)));
    fprintf(stdout, "XCTAssertEqual(sinhc1.real(), %1.15f)\n", real(sinh(c1)));
    fprintf(stdout, "XCTAssertEqual(sinhc1.imag(), %1.15f)\n", imag(sinh(c1)));
    fprintf(stdout, "XCTAssertEqual(tanhc1.real(), %1.15f)\n", real(tanh(c1)));
    fprintf(stdout, "XCTAssertEqual(tanhc1.imag(), %1.15f)\n", imag(tanh(c1)));

    fprintf(stdout, "let acosc1 = acos(value: c1)\n");
    fprintf(stdout, "let asinc1 = asin(value: c1)\n");
    fprintf(stdout, "let atanc1 = atan(value: c1)\n");
    fprintf(stdout, "XCTAssertEqual(acosc1.real(), %1.15f)\n", real(acos(c1)));
    fprintf(stdout, "XCTAssertEqual(acosc1.imag(), %1.15f)\n", imag(acos(c1)));
    fprintf(stdout, "XCTAssertEqual(asinc1.real(), %1.15f)\n", real(asin(c1)));
    fprintf(stdout, "XCTAssertEqual(asinc1.imag(), %1.15f)\n", imag(asin(c1)));
    fprintf(stdout, "XCTAssertEqual(atanc1.real(), %1.15f)\n", real(atan(c1)));
    fprintf(stdout, "XCTAssertEqual(atanc1.imag(), %1.15f)\n", imag(atan(c1)));

    fprintf(stdout, "let acoshc1 = acosh(value: c1)\n");
    fprintf(stdout, "let asinhc1 = asinh(value: c1)\n");
    fprintf(stdout, "let atanhc1 = atanh(value: c1)\n");
    fprintf(stdout, "XCTAssertEqual(acoshc1.real(), %1.15f)\n", real(acosh(c1)));
    fprintf(stdout, "XCTAssertEqual(acoshc1.imag(), %1.15f)\n", imag(acosh(c1)));
    fprintf(stdout, "XCTAssertEqual(asinhc1.real(), %1.15f)\n", real(asinh(c1)));
    fprintf(stdout, "XCTAssertEqual(asinhc1.imag(), %1.15f)\n", imag(asinh(c1)));
    fprintf(stdout, "XCTAssertEqual(atanhc1.real(), %1.15f)\n", real(atanh(c1)));
    fprintf(stdout, "XCTAssertEqual(atanhc1.imag(), %1.15f)\n", imag(atanh(c1)));

    fprintf(stdout, "let expc1 = exp(value: c1)\n");
    fprintf(stdout, "let logc1 = log(value: c1)\n");
    fprintf(stdout, "let log10c1 = log10(value: c1)\n");
    fprintf(stdout, "let sqrtc1 = sqrt(value: c1)\n");
    fprintf(stdout, "XCTAssertEqual(expc1.real(), %1.15f)\n", real(exp(c1)));
    fprintf(stdout, "XCTAssertEqual(expc1.imag(), %1.15f)\n", imag(exp(c1)));
    fprintf(stdout, "XCTAssertEqual(logc1.real(), %1.15f)\n", real(log(c1)));
    fprintf(stdout, "XCTAssertEqual(logc1.imag(), %1.15f)\n", imag(log(c1)));
    fprintf(stdout, "XCTAssertEqual(log10c1.real(), %1.15f)\n", real(log10(c1)));
    fprintf(stdout, "XCTAssertEqual(log10c1.imag(), %1.15f)\n", imag(log10(c1)));
    fprintf(stdout, "XCTAssertEqual(sqrtc1.real(), %1.15f)\n", real(sqrt(c1)));
    fprintf(stdout, "XCTAssertEqual(sqrtc1.imag(), %1.15f)\n", imag(sqrt(c1)));
}

void
complexMathCheck() {

    fprintf(stdout, "\n Complex Number Trig Operations ..............................................\n");

    std::complex<double> c1 = 1.1 + 2.0i;
    printComplexMathChecks(c1);

    c1 = 2.3 - 0.4i;
    printComplexMathChecks(c1);

    fprintf(stdout, "\n");
}

void
rngCheck() {

    fprintf(stdout, "\n Random Number Generator Tests ..............................................\n");
    fprintf(stdout, "\n     Mersenne Twister 64 bit ..............................................\n");

    int n = 10;

    std::mt19937_64 generator (12345678);
    std::uniform_real_distribution<double> dis(0.0, 1.0);
    fprintf(stdout, "[%f",  dis(generator));
    for (int i = 1; i < n; i++) {
        fprintf(stdout, ", %f",  dis(generator));
    }
    fprintf(stdout, "]");

    fprintf(stdout, "\n");
}

int
main(void) {
    complexNumOpsCheck();
    vectorOpsCheck();
    rngCheck();
    complexMathCheck();

    return 0;
}


/* specfunc/bessel_amp_phase.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

/* Author:  G. Jungman */

#include <config.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>
#include "bessel_amp_phase.h"

/* chebyshev expansions for amplitude and phase
   functions used in bessel evaluations

   These are the same for J0,Y0 and for J1,Y1, so
   they sit outside those functions.
*/

static double bm0_data[21] = {
   0.09284961637381644,
  -0.00142987707403484,
   0.00002830579271257,
  -0.00000143300611424,
   0.00000012028628046,
  -0.00000001397113013,
   0.00000000204076188,
  -0.00000000035399669,
   0.00000000007024759,
  -0.00000000001554107,
   0.00000000000376226,
  -0.00000000000098282,
   0.00000000000027408,
  -0.00000000000008091,
   0.00000000000002511,
  -0.00000000000000814,
   0.00000000000000275,
  -0.00000000000000096,
   0.00000000000000034,
  -0.00000000000000012,
   0.00000000000000004
};
const cheb_series _gsl_sf_bessel_amp_phase_bm0_cs = {
  bm0_data,
  20,
  -1, 1,
  10
};

static double bth0_data[24] = {
  -0.24639163774300119,
   0.001737098307508963,
  -0.000062183633402968,
   0.000004368050165742,
  -0.000000456093019869,
   0.000000062197400101,
  -0.000000010300442889,
   0.000000001979526776,
  -0.000000000428198396,
   0.000000000102035840,
  -0.000000000026363898,
   0.000000000007297935,
  -0.000000000002144188,
   0.000000000000663693,
  -0.000000000000215126,
   0.000000000000072659,
  -0.000000000000025465,
   0.000000000000009229,
  -0.000000000000003448,
   0.000000000000001325,
  -0.000000000000000522,
   0.000000000000000210,
  -0.000000000000000087,
   0.000000000000000036
};
const cheb_series _gsl_sf_bessel_amp_phase_bth0_cs = {
  bth0_data,
  23,
  -1, 1,
  12
};

static double bm1_data[21] = {
   0.1047362510931285,
   0.00442443893702345,
  -0.00005661639504035,
   0.00000231349417339,
  -0.00000017377182007,
   0.00000001893209930,
  -0.00000000265416023,
   0.00000000044740209,
  -0.00000000008691795,
   0.00000000001891492,
  -0.00000000000451884,
   0.00000000000116765,
  -0.00000000000032265,
   0.00000000000009450,
  -0.00000000000002913,
   0.00000000000000939,
  -0.00000000000000315,
   0.00000000000000109,
  -0.00000000000000039,
   0.00000000000000014,
  -0.00000000000000005,
};
const cheb_series _gsl_sf_bessel_amp_phase_bm1_cs = {
  bm1_data,
  20,
  -1, 1,
  10
};

static double bth1_data[24] = {
   0.74060141026313850,
  -0.004571755659637690,
   0.000119818510964326,
  -0.000006964561891648,
   0.000000655495621447,
  -0.000000084066228945,
   0.000000013376886564,
  -0.000000002499565654,
   0.000000000529495100,
  -0.000000000124135944,
   0.000000000031656485,
  -0.000000000008668640,
   0.000000000002523758,
  -0.000000000000775085,
   0.000000000000249527,
  -0.000000000000083773,
   0.000000000000029205,
  -0.000000000000010534,
   0.000000000000003919,
  -0.000000000000001500,
   0.000000000000000589,
  -0.000000000000000237,
   0.000000000000000097,
  -0.000000000000000040,
};
const cheb_series _gsl_sf_bessel_amp_phase_bth1_cs = {
  bth1_data,
  23,
  -1, 1,
  12
};

int
gsl_sf_bessel_asymp_Mnu_e(const double nu, const double x, double * result)
{
  const double r  = 2.0*nu/x;
  const double r2 = r*r;
  const double x2 = x*x;
  const double term1 = (r2-1.0/x2)/8.0;
  const double term2 = (r2-1.0/x2)*(r2-9.0/x2)*3.0/128.0;
  const double Mnu2_c = 2.0/(M_PI) * (1.0 + term1 + term2);
  *result = sqrt(Mnu2_c)/sqrt(x); /* will never underflow this way */
  return GSL_SUCCESS;
}

int
gsl_sf_bessel_asymp_thetanu_corr_e(const double nu, const double x, double * result)
{
  const double r  = 2.0*nu/x;
  const double r2 = r*r;
  const double x2 = x*x;
  const double term1 = x*(r2 - 1.0/x2)/8.0;
  const double term2 = x*(r2 - 1.0/x2)*(r2 - 25.0/x2)/384.0;
  *result = (-0.25*M_PI + term1 + term2);
  return GSL_SUCCESS;
}


#ifndef GUARD_QDWF_H
#define GUARD_QDWF_H

#include "Numerics/Spline3D/Config.h"
#include "Numerics/Spline3D/uGrid1D.h"
#include "Numerics/Spline3D/CubicSpline.h"
#include "Optimize/VarList.h"
#include <gsl/gsl_sf_airy.h>
#include <math.h>
#include <iostream>

using namespace std;

class QDwf
{

  std::vector<double> X;
  std::vector<double> sigx;
  std::vector<double> a;

  double sigy;//var
  double Y0;//var

  double z0;//var
  double Ez;//var

  double VB;
  double zB;
  double kappa;
  double EL;
  double psiB;

  double onethird;

  double fac1,fac2;

  uGrid1D* m_grid;
  CubicSpline* m_spline;

  gsl_mode_t mode;

  int funcz(double z)
  {
    double denom = gsl_sf_airy_Ai(z,mode);
    double numer = fac2*gsl_sf_airy_Ai_deriv(z,mode);
    double xsign = numer-denom*kappa;
    if(xsign < 0 )
    {
      return -1;
    }
    else
    {
      return 1;
    }
  }

  inline double zprime(double z)
  {
    return fac2*(z-z0-EL/Ez);
  }

public:

  typedef double value_type;

  QDwf()
  {
    VB = 24.535;
    zB = 9.1854;
    //    z0 = 10.6858;
    //    Ez = 2.2;
    Ez = 2.04866;
    z0 = 10.8049;
    //    sigxr = 0.08;
    //    xr = 80.72;
    sigy = 0.107;
    Y0 = 25.3;
    /// set the x parameters
    int nxp = 1;
    a.resize(nxp);
    sigx.resize(nxp);
    X.resize(nxp);
    a[0] = 1.00000;
    sigx[0] = 0.08;
    X[0] = 80.628;
    //    a[1] = 1.00e-4; sigx[1] = 0.08;  X[1] = 62;
    /*
    a[0] = 1.00000; sigx[0] = 0.08; X[0] = 80.628;
    a[1] = 0.08260; sigx[1] = 0.08; X[1] = 76.000;
    a[2] = 2.17e-4; sigx[2] = 0.04; X[2] = 67.000;
    */
    /// optimised parameters
    /*
    a[0] = 1.00000; sigx[0] = 0.092403; X[0] = 80.569;
    a[1] = 0.071703; sigx[1] = 0.18445; X[1] = 76.064;
    a[2] = 2.86e-3; sigx[2] = 0.052224; X[2] = 67.000;
    sigy = 0.11112;
    Y0 = 25.167;
    */
    onethird = 1.0/3.0;
    reset();
    cout << "Airy Solved: " << EL << '\t' << kappa << '\t'
         << psiB << endl;
    /*
    /// create and initialise the uniform one-dimensional uGrid1D
    if( !m_grid ) m_grid = new uGrid1D;
    double zstart = zB; double zend = 16.00; int npts = 1000;
    m_grid->init(zstart,zend,npts);

    /// create and initialise the CubicSpline
    if(!m_spline) m_spline = new CubicSpline(m_grid);
    for(int i = 0; i < npts; i++){
      double z = m_grid->m_coord[i]; double zz = zprime(z);
      //(*m_spline)(i) = gsl_sf_airy_Ai(zz,mode);//evaluate(z);
      m_spline->F(i)[0] = gsl_sf_airy_Ai(zz,mode);
      m_spline->F(i)[1] = fac2*gsl_sf_airy_Ai_deriv(zz,mode);
    }
    */
  }

  template<class T1>
  void put(xmlNodePtr cur, VarRegistry<T1>& vlist)
  {
    //    vlist.add("C_sigxr",&sigxr,1);
    //    vlist.add("C_xr",&xr,1);
    vlist.add("C_X",&X[0],1);
    vlist.add("C_sigx",&sigx[0],1);
    vlist.add("C_a",&a[0],1);
    vlist.add("C_sigy",&sigy,1);
    vlist.add("C_Y0",&Y0,1);
    vlist.add("C_Ez",&Ez,1);
    vlist.add("C_z0",&z0,1);
  }

  void reset();

  inline void set_point(const posvec_t& r) {}

  inline double evaluate(const posvec_t& r,
                         posvec_t& gradf,
                         double& lapf)
  {
    int nsize = sigx.size();
    /// \f$\Psi_{x}\f$ sum of Gaussians
    double psix = 0.0,gfx=0.0,lfx=0.0;
    for(int i = 0; i < nsize; i++)
    {
      double x = r[0] - X[i];
      double sx = sigx[i] * x;
      double sxx = sx * x;
      double phix = a[i] * exp(-sxx);
      psix += phix;
      gfx += -2 * sx * phix;
      lfx += -2 * sigx[i] * ( 1.0 - 2.0 * sxx ) * phix;
    }
    /// \f$\Psi_{y}\f$ single Gaussian
    double y = r[1] - Y0;
    double sy = sigy * y;
    double syy = sy * y;
    double psiy = exp(-syy);
    double gfy = - 2 * sy * psiy;
    double lfy = - 2 * sigy * ( 1.0 - 2.0 * syy ) * psiy;
    /// \f$\Psi_{z}\f$ AiryAi with exponential tail
    double z = r[2];
    double psiz,gfz,lfz;
    if( z <= zB )
    {
      psiz = psiB * exp ( kappa * ( z - zB ) );
      gfz = kappa * psiz;
      lfz = kappa * gfz;
    }
    else
    {
      psiz = m_spline->evaluate(z,gfz,lfz);
    }
    double psi = psix * psiy * psiz;
    gradf[0] = gfx * psiy * psiz;
    gradf[1] = psix * gfy * psiz;
    gradf[2] = psix * gfy * psiz;
    lapf = lfx * psiy * psiz + psix * lfy * psiz + psix * psiy * lfz;
    return psi;
  }

};
#endif


#pragma once

/*** mathematics.h:

     Mathematical and statistical functions. ***/

/** Avoid including this file twice. **/

#ifndef MATHEMATICS_H
#define MATHEMATICS_H

/** Libraries. **/

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

/** Constants. **/

#define MATH_PI 3.1415926535897932385 /* Pi. */
#define MATH_2PI 6.2831853071795864770 /* 2 pi. */
#define MATH_PI_2 1.57079632679489661925 /* Pi / 2. */

/* Coordinates / axes. */

#define MATH_X 0 /* Castesian. */
#define MATH_Y 1
#define MATH_Z 2
#define MATH_RADIUS 0 /* Spherical. */
#define MATH_POLAR 1
#define MATH_AZIMUTH 2
#define MATH_MU 1 /* Pseudospherical (mu = cos(polar) replacing polar). */

/** Variables. **/

/* Random numbers. */

gsl_rng *RandomNumberGenerator; /* Random number generator. */
unsigned RandomSeed; /* Random seed. */
unsigned SortComponent = 0; /* Component used for sorting vectors. */

/** Function prototypes. **/

int SortAscending(const void *, const void *);
int SortIndexesAscending(const void *, const void *);
double ScalarProduct(double *, double *, unsigned);
double Norm(double *, unsigned);
double NormSquared(double *, unsigned);
double AngleCosine(double *, double *);

void SphericalToCartesian(double *, double *);
void PseudoSphericalToCartesian(double *, double *);
void CartesianToSpherical(double *, double *);
void CartesianToPseudoSpherical(double *, double *);
void Rotate(double *, double *, unsigned, double);
unsigned GetRandomSeed();
int InitialiseRandom(const gsl_rng_type *);
int FinaliseRandom();
double MetropolisHastings(double *, double *, double *, double *, unsigned, unsigned, unsigned, unsigned, double *, double *, double *,
                          void (*)(double *, double *, double *, double *, double *, unsigned), double (*)(double *, double *, double *, double *, double *, unsigned),
                          double (*)(double *));
double ParallelTempering(double *, double *, double *, double *, unsigned, unsigned, unsigned, unsigned, double *, double *, double *,
                         void (*)(double *, double *, double *, double *, double *, unsigned), double (*)(double *, double *, double *, double *, double *, unsigned),
                         double (*)(double *), unsigned, double *, double);
void TanWeights(double *, double *, double *, double *, unsigned, unsigned, unsigned, double *, double *, double *,
                double (*)(double *, double *, double *, double *, double *, unsigned), unsigned);
double TanNormalisation(double *, unsigned, double *);
double WeightAndSum(double *, double *, unsigned);
void UniformTransition(double *, double *, double *, double *, double *, unsigned);
double UniformTransitionPDF(double *, double *, double *, double *, double *, unsigned);
double PowerLawDeviate(double, double, double);
void IsotropicDirection(double, double *);
void StandardGaussianDeviates(double *);
void GaussianDeviates(double *, double, double);
complex ComplexGaussianDeviates(complex, complex);

/** SortAscending.

    - Function designed to be an argument of qsort().
    - Sorts the elements of a vector in ascending order. The vector elements are reorganised. **/

int SortAscending(const void *x, const void *y)
{
  double difference;

  difference = *((double *)x) - *((double *)y);
  if (difference > 0.0)
    return 1;
  else if (difference < 0.0)
    return -1;
  else
    return 0;
}

/** SortIndexesAscending.

    - Function designed to be an argument of qsort().
    - Sorts the indexes of a vector, in such a way that its elements are arranged in ascending order. The vector remains unchanged.
    - 'SortComponent' indicates the component used to sort, in the case that the vector elements have more than one dimension. **/

int SortIndexesAscending(const void *x, const void *y)
{
  double difference;

  difference = *(*(double **)x + SortComponent) - *(*(double **)y + SortComponent);
  if (difference > 0.0)
    return 1;
  else if (difference < 0.0)
    return -1;
  else
    return 0;
}

/** ScalarProduct.

    - Computes the scalar product between two vectors. **/

double ScalarProduct(double *vector_1, double *vector_2, unsigned vector_size)
{
  unsigned i;
  double product;

  /* Compute components product and sum. */

  product = 0.0;
  for (i = 0; i < vector_size; i++)
    product += vector_1[i] * vector_2[i];
  return product;
}

/** Norm.

    - Computes the norm of a vector. **/

double Norm(double *vector, unsigned vector_size)
{
  unsigned i;
  double *component_sq, max_component_sq, norm;

  /* Generate an auxiliary vector of component squares, in ascending order. */

  component_sq = malloc(vector_size * sizeof(double));
  for (i = 0; i < vector_size; i++)
    component_sq[i] = vector[i] * vector[i];
  qsort(component_sq, vector_size, sizeof(double), SortAscending);

  /* Divide component squares by the largest one to avoid overflow. */

  max_component_sq = component_sq[vector_size-1];
  if (max_component_sq <= 0.0) /* All components are null. */
    return 0.0;
  vector_size--;
  for (i = 0; i < vector_size; i++)
    component_sq[i] /= max_component_sq;

  /* Sum component squares in ascending order, to avoid loss of significance. */

  norm = 0.0;
  for (i = 0; i < vector_size; i++)
    norm += component_sq[i];
  norm += 1.0;

  /* Renormalise. */

  norm = sqrt(max_component_sq * norm);
  free(component_sq);
  return norm;
}

/** NormSquared.

    - Computes the square of the norm of a vector. **/

double NormSquared(double *vector, unsigned vector_size)
{
  unsigned i;
  double *component_sq, max_component_sq, norm;

  /* Generate an auxiliary vector of component squares, in ascending order. */

  component_sq = malloc(vector_size * sizeof(double));
  for (i = 0; i < vector_size; i++)
    component_sq[i] = vector[i] * vector[i];
  qsort(component_sq, vector_size, sizeof(double), SortAscending);

  /* Divide component squares by the largest one to avoid overflow. */

  max_component_sq = component_sq[vector_size-1];
  if (max_component_sq <= 0.0) /* All components are null. */
    return 0.0;
  vector_size--;
  for (i = 0; i < vector_size; i++)
    component_sq[i] /= max_component_sq;

  /* Sum component squares in ascending order, to avoid loss of significance. */

  norm = 0.0;
  for (i = 0; i < vector_size; i++)
    norm += component_sq[i];
  norm += 1.0;

  /* Renormalise. */

  norm *= max_component_sq;
  free(component_sq);
  return norm;
}

/** AngleCosine.

    - Computes the cosine of the angle between two 3D vectors. **/

double AngleCosine(double *vector_1, double *vector_2)
{
  double norm_1, norm_2, cosine;

  norm_1 = Norm(vector_1, 3);
  norm_2 = Norm(vector_2, 3);
  if (norm_1 <= 0.0 || norm_2 <= 0.0) /* At least one vector is null. */
    return 0.0;
  cosine = ScalarProduct(vector_1, vector_2, 3) / (norm_1 * norm_2);
  if (cosine > 1.0) /* Correct eventual round-off errors. */
    cosine = 1.0;
  else if (cosine < -1.0)
    cosine = -1.0;
  return cosine;
}

/** SphericalToCartesian.

    - Converts a 3D vector from spherical (radius, polar angle, azimuth) to Cartesian form.
    - Initial and final vectors may be the same. **/

void SphericalToCartesian(double *vec_in, double *vec_out)
{
  double sin_polar, cos_polar, radius, azimuth;

  radius = vec_in[MATH_RADIUS];
  sin_polar = sin(vec_in[MATH_POLAR]);
  cos_polar = cos(vec_in[MATH_POLAR]);
  azimuth = vec_in[MATH_AZIMUTH];
  vec_out[MATH_X] = radius * sin_polar * cos(azimuth);
  vec_out[MATH_Y] = radius * sin_polar * sin(azimuth);
  vec_out[MATH_Z] = radius * cos_polar;
}

/** PseudoSphericalToCartesian.

    - Converts a 3D vector from pseudo spherical (radius, polar angle cosine, azimuth) to Cartesian form.
    - Initial and final vectors may be the same. **/

void PseudoSphericalToCartesian(double *vec_in, double *vec_out)
{
  double cos_polar, sin_polar, radius, azimuth;

  radius = vec_in[MATH_RADIUS];
  azimuth = vec_in[MATH_AZIMUTH];
  cos_polar = vec_in[MATH_MU];
  sin_polar = 1.0 - cos_polar * cos_polar;
  if (sin_polar <= 0.0) /* Correct eventual round-off errors. */
    sin_polar = 0.0;
  else
    sin_polar = sqrt(sin_polar);
  vec_out[MATH_X] = radius * sin_polar * cos(azimuth);
  vec_out[MATH_Y] = radius * sin_polar * sin(azimuth);
  vec_out[MATH_Z] = radius * cos_polar;
}

/** CartesianToSpherical.

    - Converts a 3D vector from Cartesian to spherical (radius, polar angle, azimuth) form.
    - Initial and final vectors may be the same. **/

void CartesianToSpherical(double *vec_in, double *vec_out)
{
  double radius, azimuth, polar;

  radius = Norm(vec_in, 3);
  if (radius > 0.0)
    polar = acos(vec_in[MATH_Z] / radius);
  else
    polar = 0.0;
  azimuth = atan2(vec_in[MATH_Y], vec_in[MATH_X]);
  vec_out[MATH_RADIUS] = radius;
  vec_out[MATH_POLAR] = polar;
  vec_out[MATH_AZIMUTH] = azimuth;
}

/** CartesianToPseudoSpherical.

    - Converts a 3D vector from Cartesian to pseudo spherical (radius, polar angle cosine, azimuth) form.
    - Initial and final vectors may be the same. **/

void CartesianToPseudoSpherical(double *vec_in, double *vec_out)
{
  double radius, cos_polar, azimuth;

  radius = Norm(vec_in, 3);
  if (radius > 0.0)
    cos_polar = vec_in[MATH_Z] / radius;
  else
    cos_polar = 1.0;
  azimuth = atan2(vec_in[MATH_Y], vec_in[MATH_X]);
  vec_out[MATH_RADIUS] = radius;
  vec_out[MATH_MU] = cos_polar;
  vec_out[MATH_AZIMUTH] = azimuth;
}

/** Rotate.

    - Rotates a vector.
    - Vector is rotated counterclockwise (or axes clockwise) as seen from the tip of the rotation axis specified by argument 'axis'.
    - Initial and final vectors may be the same. **/

void Rotate(double *vec_in, double *vec_out, unsigned axis, double angle)
{
  unsigned i, j;
  double cos_angle, sin_angle, x, y;

  cos_angle = cos(angle);
  sin_angle = sin(angle);
  axis %= 3;
  vec_out[axis] = vec_in[axis];
  i = (axis + 1) % 3;
  j = (axis + 2) % 3;
  x = vec_in[i];
  y = vec_in[j];
  vec_out[i] = x * cos_angle - y * sin_angle;
  vec_out[j] = x * sin_angle + y * cos_angle;
}

/** GetRandomSeed.

    - Provides a random seed by reading garbage from /dev/urandom. **/

unsigned GetRandomSeed()
{
  FILE *random_file;
  unsigned seed;
  int error;

  seed = 0;
  random_file = fopen("/dev/urandom", "rb");
  if (!random_file)
    return 0;
  do
  {
    error = fread(&seed, 1, sizeof(unsigned int), random_file);
    if (error != sizeof(unsigned int))
      return 0;
  }
  while (!seed);
  fclose(random_file);
  return seed;
}

/** InitialiseRandom.

    - Initialises the random number generator. **/

int InitialiseRandom(const gsl_rng_type *random_generator)
{
  RandomSeed = GetRandomSeed();
  if (!RandomSeed)
    return EXIT_FAILURE;
  RandomNumberGenerator = gsl_rng_alloc(random_generator);
  if (!RandomNumberGenerator)
    return EXIT_FAILURE;
  gsl_rng_set(RandomNumberGenerator, RandomSeed);
  return EXIT_SUCCESS;
}

/** FinaliseRandom.

    - Finalises the random number generator. **/

int FinaliseRandom()
{
  /* Release random number generator memory. */

  if (RandomNumberGenerator)
  {
    gsl_rng_free(RandomNumberGenerator);
    return EXIT_SUCCESS;
  }
  return EXIT_FAILURE;
}

/** MetropolisHastings.

    - Constructs a Markov chain using Metropolis-Hastings sampling from a given PDF ('sampling_pdf').
    - 'states' contains the actual Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles. The initial state must be stored in
      the first element of 'states'. 'states' must be allocated by the calling function.
    - 'proposals' is the chain of proposals derived from each state (same length and element size as 'states'). It must be allocated by the calling function.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sampling_pdf' must point to a function computing the PDF sampled by the Markov chain. Its only argument is a state of the chain; any other parameter must be
      passed through global variables.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'transition' must point to a function computing the proposed final state of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the
      final and initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'proposals_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the proposals is desired, or set to NULL otherwise.
    - 'states_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the states is desired, or set to NULL otherwise.
    - If the initial state set in 'states' has a null value of the sampling PDF, the first transition probability is set to unity. Once a state with a non-null value
      of the sampling PDF is reached, the sampling PDF values do not vanish over the rest of the Markov chain by construction.
    - The order of the state and proposal chains is the following: any element of 'proposals' is sampled from the corresponding element of 'states', whereas any
      element of 'states' (except the first one) is the previous element of either 'states' or 'proposals'.
    - If 'save_states' is set to null, only the final state and proposal are given. Otherwise, the full chains are saved.
    - If 'log_flag' is non null, 'sampling_pdf' is assumed to give the natural logarithm of the PDF.
    - Returns the acceptance ratio of the chain. **/

double MetropolisHastings(double *states, double *proposals, double *proposals_pdf, double *states_pdf, unsigned state_size, unsigned chain_size, unsigned save_states,
                          unsigned log_flag, double *lower_bound, double *upper_bound, double *step,
			  void (*transition)(double *, double *, double *, double *, double *, unsigned),
                          double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned), double (*sampling_pdf)(double *))
{
  unsigned i, accepted_transitions, state_size_bytes;
  double direct_transition_pdf, inverse_transition_pdf, pdf_ratio, state_pdf, proposal_pdf, *proposal, *state, *old_state, acceptance_ratio;

  /* Set initial stuff. */

  state_size_bytes = state_size * sizeof(double);
  proposal = proposals;
  state = states;
  state_pdf = sampling_pdf(state);
  if (states_pdf)
    states_pdf[0] = state_pdf;

  /* Construct the Markov chain. */

  accepted_transitions = 0;
  chain_size--;
  for (i = 0; i < chain_size; i++)
  {
    /* Set new proposal. */

    transition(proposal, state, lower_bound, upper_bound, step, state_size);
    proposal_pdf = sampling_pdf(proposal);
    if (proposals_pdf && save_states)
      proposals_pdf[i] = proposal_pdf;

    /* Compute transition probabilities. */

    direct_transition_pdf = transition_pdf(proposal, state, lower_bound, upper_bound, step, state_size);
    inverse_transition_pdf = transition_pdf(state, proposal, lower_bound, upper_bound, step, state_size);
    if (!log_flag && state_pdf <= 0.0)
      pdf_ratio = 1.1;
    else if (direct_transition_pdf <= 0.0)
      pdf_ratio = -0.1;
    else if (log_flag)
      pdf_ratio = exp(proposal_pdf - state_pdf) * inverse_transition_pdf / direct_transition_pdf;
    else
      pdf_ratio = proposal_pdf * inverse_transition_pdf / (state_pdf * direct_transition_pdf);

    /* Accept of reject transition. */

    if (save_states)
    {
      old_state = state;
      state += state_size;
    }
    if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
    {
      memcpy(state, proposal, state_size_bytes);
      state_pdf = proposal_pdf;
      accepted_transitions++;
    }
    else if (save_states)
      memcpy(state, old_state, state_size_bytes);
    if (save_states)
    {
      proposal += state_size;
      if (states_pdf)
        states_pdf[i+1] = state_pdf;
    }
  }

  /* Compute final stuff. */

  transition(proposal, state, lower_bound, upper_bound, step, state_size);
  acceptance_ratio = accepted_transitions / (double) chain_size;
  if (proposals_pdf && save_states)
    proposals_pdf[chain_size] = sampling_pdf(proposal);
  return acceptance_ratio;
}

/** ParallelTempering.

    - Constructs a Markov chain using Metropolis-Hastings sampling from a given PDF ('sampling_pdf'), with parallel tempering.
    - 'states' contains the cold Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles. The initial state must be stored in
      the first element of 'states'. 'states' must be allocated by the calling function.
    - 'proposals' is the chain of proposals derived from each state (same length and element size as 'states'). It must be allocated by the calling function.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sampling_pdf' must point to a function computing the PDF sampled by the cold Markov chain. Its only argument is a state of the chain; any other parameter must
      be passed through global variables.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'transition' must point to a function computing the proposed final state of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the
      final and initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'proposals_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the proposals is desired, or set to NULL otherwise.
    - 'states_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the states is desired, or set to NULL otherwise.
    - If the initial state set in 'states' has a null value of the sampling PDF, the first transition probability is set to unity. Once a state with a non-null value
      of the sampling PDF is reached, the sampling PDF values do not vanish over the rest of the Markov chain by construction.
    - The order of the state and proposal chains is the following: any element of 'proposals' is sampled from the corresponding element of 'states', whereas any
      element of 'states' (except the first one) is the previous element of either 'states' or 'proposals'.
    - If 'save_states' is set to null, only the final state and proposal are given. Otherwise, the full chains are saved.
    - If 'log_flag' is non null, 'sampling_pdf' is assumed to give the natural logarithm of the PDF.
    - The number of hot chains in the parallel-tempering scheme is 'hot_chains_count', which have tempering factors given by 'tempering_factors'.
    - 'exchange_probability' gives the probability that two chains are interchanged in a given step.
    - Returns the acceptance ratio of the chain. **/

double ParallelTempering(double *states, double *proposals, double *proposals_pdf, double *states_pdf, unsigned state_size, unsigned chain_size, unsigned save_states,
                         unsigned log_flag, double *lower_bound, double *upper_bound, double *step,
                         void (*transition)(double *, double *, double *, double *, double *, unsigned),
                         double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned), double (*sampling_pdf)(double *),
                         unsigned hot_chains_count, double *tempering_factors, double exchange_probability)
{
  unsigned i, j, accepted_transitions, state_size_bytes;
  double direct_transition_pdf, inverse_transition_pdf, pdf_ratio, state_pdf, proposal_pdf, *proposal, *state, *old_state, acceptance_ratio, x, *aux_state;
  double *cool_state, hot_proposals_pdf, factor, *hot_proposals, *hot_states, *hot_states_pdf, *cool_state_pdf;

  /* Set initial stuff. */

  state_size_bytes = state_size * sizeof(double);
  proposal = proposals;
  state = states;
  state_pdf = sampling_pdf(state);
  if (states_pdf)
    states_pdf[0] = state_pdf;
  hot_proposals = malloc(hot_chains_count * state_size_bytes);
  hot_states = malloc(hot_chains_count * state_size_bytes);
  aux_state = malloc(state_size_bytes);
  for (i = 0; i < hot_chains_count; i++)
    memcpy(hot_states + i * state_size, state, state_size_bytes); /* Copy initial state of cold chain to all hot chains. */
  hot_states_pdf = malloc(hot_chains_count * sizeof(double));
  for (i = 0; i < hot_chains_count; i++)
  {
    if (log_flag)
      hot_states_pdf[i] = state_pdf * tempering_factors[i];
    else
      hot_states_pdf[i] = pow(state_pdf, tempering_factors[i]);
  }

  /* Construct the Markov chains. */

  accepted_transitions = 0;
  chain_size--;
  for (i = 0; i < chain_size; i++)
  {
    /* Set new proposal for cold chain. */

    transition(proposal, state, lower_bound, upper_bound, step, state_size);
    proposal_pdf = sampling_pdf(proposal);
    if (proposals_pdf && save_states)
      proposals_pdf[i] = proposal_pdf;

    /* Compute transition probabilities for cold chain. */

    direct_transition_pdf = transition_pdf(proposal, state, lower_bound, upper_bound, step, state_size);
    inverse_transition_pdf = transition_pdf(state, proposal, lower_bound, upper_bound, step, state_size);
    if (!log_flag && state_pdf <= 0.0)
      pdf_ratio = 1.1;
    else if (direct_transition_pdf <= 0.0)
      pdf_ratio = -0.1;
    else if (log_flag)
      pdf_ratio = exp(proposal_pdf - state_pdf) * inverse_transition_pdf / direct_transition_pdf;
    else
      pdf_ratio = proposal_pdf * inverse_transition_pdf / (state_pdf * direct_transition_pdf);

    /* Accept of reject transition for cold chain. */

    if (save_states)
    {
      old_state = state;
      state += state_size;
    }
    if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
    {
      memcpy(state, proposal, state_size_bytes);
      state_pdf = proposal_pdf;
      accepted_transitions++;
    }
    else if (save_states)
      memcpy(state, old_state, state_size_bytes);
    if (save_states)
    {
      proposal += state_size;
      if (states_pdf)
        states_pdf[i+1] = state_pdf;
    }

    /* Hot chains. */

    for (j = 0; j < hot_chains_count; j++)
    {
      /* Set new proposal. */

      transition(hot_proposals + j * state_size, hot_states + j * state_size, lower_bound, upper_bound, step, state_size);
      hot_proposals_pdf = sampling_pdf(hot_proposals + j * state_size);
      if (log_flag)
        hot_proposals_pdf *= tempering_factors[j];
      else
        hot_proposals_pdf = pow(hot_proposals_pdf, tempering_factors[j]);

      /* Compute transition probabilities. */

      direct_transition_pdf = transition_pdf(hot_proposals + j * state_size, hot_states + j * state_size, lower_bound, upper_bound, step, state_size);
      inverse_transition_pdf = transition_pdf(hot_states + j * state_size, hot_proposals + j * state_size, lower_bound, upper_bound, step, state_size);
      if (!log_flag && hot_states_pdf[j] <= 0.0)
        pdf_ratio = 1.1;
      else if (direct_transition_pdf <= 0.0)
        pdf_ratio = -0.1;
      else if (log_flag)
        pdf_ratio = exp(hot_proposals_pdf - hot_states_pdf[j]) * inverse_transition_pdf / direct_transition_pdf;
      else
        pdf_ratio = hot_proposals_pdf * inverse_transition_pdf / (hot_states_pdf[j] * direct_transition_pdf);

      /* Accept of reject transition. */

      if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
      {
        memcpy(hot_states + j * state_size, hot_proposals + j * state_size, state_size_bytes);
        hot_states_pdf[j] = hot_proposals_pdf;
      }
    }

    /* Exchange chain states. */

    if (gsl_rng_uniform(RandomNumberGenerator) < exchange_probability)
    {
      j = gsl_rng_uniform_int(RandomNumberGenerator, hot_chains_count);
      factor = tempering_factors[j];
      if (j)
      {
        factor /= tempering_factors[j-1];
        cool_state_pdf = hot_states_pdf + j - 1;
        cool_state = hot_states + (j - 1) * state_size;
      }
      else
      {
        cool_state_pdf = &state_pdf;
        cool_state = state;
      }
      if (log_flag)
        pdf_ratio = exp(*cool_state_pdf * (factor - 1.0) + hot_states_pdf[j] * (1.0 / factor - 1.0));
      else
        pdf_ratio = pow(*cool_state_pdf, factor - 1.0) * pow(hot_states_pdf[j], 1.0 / factor - 1.0);
      if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
      {
        if (log_flag)
        {
          x = hot_states_pdf[j];
          hot_states_pdf[j] = *cool_state_pdf * factor;
          *cool_state_pdf = x / factor;
        }
        else
        {
          x = hot_states_pdf[j];
          hot_states_pdf[j] = pow(*cool_state_pdf, factor);
          *cool_state_pdf = pow(x, 1.0 / factor);
        }
        memcpy(aux_state, hot_states + j * state_size, state_size_bytes);
        memcpy(hot_states + j * state_size, cool_state, state_size_bytes);
        memcpy(cool_state, aux_state, state_size_bytes);
      }
    }
  }
  transition(proposal, state, lower_bound, upper_bound, step, state_size);
  acceptance_ratio = accepted_transitions / (double) chain_size;
  if (proposals_pdf && save_states)
    proposals_pdf[chain_size] = sampling_pdf(proposal);
  free(hot_proposals);
  free(hot_states);
  free(hot_states_pdf);
  free(aux_state);
  return acceptance_ratio;
}

/** TanWeights.

    - Computes the weights proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735) to use Metropolis-Hastings sampling for the evaluation of integrals via the Monte
      Carlo method.
    - 'states' contains the actual Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles.
    - 'proposals' contains the chain of proposals derived from each state (same length and element size as 'states').
    - 'proposals_pdf' is an array of doubles of the same length of the Markov chain, containing the sampling PDF values for the proposals.
    - If the present chain is an extension of an old one, 'old_chain_size' is the size of the latter.
    - 'weights' is an array of 'chain_size' elements for storing the resulting weights. It must be allocated by the calling function. Elements of 'weights' between
      'old_chain_size' and 'chain_size' are erased before computing.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sort_component' indicates the component of the state used for sorting them, to reduce the computational cost if the PDF is bound.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables. **/

void TanWeights(double *states, double *proposals, double *proposals_pdf, double *weights, unsigned state_size, unsigned chain_size, unsigned old_chain_size,
                double *lower_bound, double *upper_bound, double *step, double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned),
                unsigned sort_component)
{
  unsigned i, j, k, m, n;
  double *proposal, **pointers, pdf;

  if (sort_component >= state_size)
    SortComponent = 0;
  else
    SortComponent = sort_component;
  memset(weights + old_chain_size, 0, (chain_size - old_chain_size) * sizeof(double));
  pointers = malloc(chain_size * sizeof(double *));
  for (i = 0; i < chain_size; i++)
    pointers[i] = states + i * state_size;
  qsort(pointers, chain_size, sizeof(double *), SortIndexesAscending);
  for (i = 0; i < chain_size; i++)
  {
    k = pointers[i] - states;
    m = k / state_size;
    proposal = proposals + k;
    for (j = i; j < chain_size; j++)
    {
      n = (pointers[j] - states) / state_size;
      if (m < old_chain_size && n < old_chain_size)
        continue; /* Avoid re-computation of weights already computed. */
      pdf = transition_pdf(proposal, pointers[j], lower_bound, upper_bound, step, state_size);
      if (pdf <= 0.0)
        break;
      weights[m] += pdf;
    }
    for (j = 1; j <= i; j++)
    {
      n = (pointers[i-j] - states) / state_size;
      if (m < old_chain_size && n < old_chain_size)
        continue;
      pdf = transition_pdf(proposal, pointers[i-j], lower_bound, upper_bound, step, state_size);
      if (pdf <= 0.0)
        break;
      weights[m] += pdf;
    }
    if (weights[m] > 0.0)
      weights[m] = proposals_pdf[m] / weights[m];
  }
  free(pointers);
}

/** TanNormalisation.

    - Computes the PDF normalisation proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735), together with an estimate of its undertainty ('error').
    - 'weights' is the array of weights (of size 'element_count') computed by 'TanWeights'. **/

double TanNormalisation(double *weights, unsigned element_count, double *error)
{
  unsigned i;
  double norm, **pointers, x;

  /* Perform weighted sum. */

  pointers = malloc(element_count * sizeof(double *));
  for (i = 0; i < element_count; i++)
    pointers[i] = weights + i;
  qsort(pointers, element_count, sizeof(double *), SortIndexesAscending);
  norm = 0.0;
  *error = 0.0;
  for (i = 0; i < element_count; i++)
  {
    x = pointers[i][0];
    norm += x;
    *error += x * x;
  }
  *error = sqrt(*error - norm * norm / element_count);
  free(pointers);
  return norm;
}

/** TanMean.

    - Computes the mean of a function as proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735), together with an estimate of its undertainty ('error').
    - 'weights' is the array of weights (of size 'element_count') computed by 'TanWeights'.
    - 'norm' is the PDF normalisation computed by 'TanNormalisation'.
    - 'function' is the array of function values (of size 'element_count') at the proposal states. **/

double TanMean(double *weights, double *function, double norm, unsigned element_count, double *error)
{
  unsigned i;
  double mean, x;

  /* Perform weighted sum. */

  mean = 0.0;
  *error = 0.0;
  for (i = 0; i < element_count; i++)
    mean += weights[i] * function[i];
  mean /= norm;
  for (i = 0; i < element_count; i++)
  {
    x = (function[i] - mean) * weights[i];
    *error += x * x;
  }
  *error = sqrt(*error) / norm;
  return mean;
}

/** UniformTransition.

    - Samples an n-dimensional transition in a Markov chain, from a uniform PDF.
    - 'state' contains the present state of the Markov chain, comprising 'state_size' doubles.
    - 'proposal' will store the resulting proposed state (same size as 'state').
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step. **/

void UniformTransition(double *proposal, double *state, double *lower_bound, double *upper_bound, double *step, unsigned state_size)
{
  unsigned i;
  double min, max;

  for (i = 0; i < state_size; i++)
  {
    if (step[i] <= 0.0)
      proposal[i] = state[i];
    else
    {
      min = state[i] - 0.5 * step[i];
      max = min + step[i];
      if (min < lower_bound[i])
        min = lower_bound[i];
      if (max > upper_bound[i])
        max = upper_bound[i];
      proposal[i] = min + (max - min) * gsl_rng_uniform(RandomNumberGenerator);
    }
  }
}

/** UniformTransitionPDF.

    - Computes the n-dimensional PDF value of a Markov chain transition, using a uniform PDF.
    - 'state' contains the state of the Markov chain from which the transition occurs, comprising 'state_size' doubles.
    - 'proposal' contains the proposed state of the chain after the transition (same size as 'state').
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step. **/

double UniformTransitionPDF(double *proposal, double *state, double *lower_bound, double *upper_bound, double *step, unsigned state_size)
{
  unsigned i;
  double min, max, density;

  density = 1.0;
  for (i = 0; i < state_size; i++)
  {
    if (step[i] <= 0.0)
      continue;
    min = state[i] - 0.5 * step[i];
    max = min + step[i];
    if (min < lower_bound[i])
      min = lower_bound[i];
    if (max > upper_bound[i])
      max = upper_bound[i];
    if (proposal[i] < min || proposal[i] > max)
      return 0.0;
    density /= max - min;
  }
  return density;
}

/** PowerLawDeviate.

    - Generates a power-law deviate with an index 'index' in any closed interval ['min', 'max']. **/

double PowerLawDeviate(double min, double max, double index)
{
  double deviate, std_deviate;

  std_deviate = gsl_rng_uniform(RandomNumberGenerator);
  if (index == -1.0)
    deviate = min * exp(std_deviate * log(max / min));
  else if (index == 0.0)
    deviate = min + (max - min) * std_deviate;
  else
  {
    index += 1.0;
    min = pow(min, index);
    max = pow(max, index);
    deviate = pow(min + (max - min) * std_deviate, 1.0 / index);
  }
  return deviate;
}

/** IsotropicDirection.

    - Generates a random direction isotropically distributed within a cone of a given semi-aperture 'semi_aperture'.
    - The output direction has unit norm, by construction.
    - Relies on the fact that, for an isotropic distribution of directions in three dimensions, the projection onto any axis is distributed uniformly in [-1,1]. **/

void IsotropicDirection(double semi_aperture, double *direction)
{
  double cos_polar, sin_polar, azimuth, cos_polar_max;

  /* Sample polar angle cosine with uniform PDF in [cos_polar_max, 1]. */

  cos_polar_max = cos(semi_aperture);
  cos_polar = cos_polar_max + gsl_rng_uniform(RandomNumberGenerator) * (1.0 - cos_polar_max);
  sin_polar = 1.0 - cos_polar * cos_polar;
  if (sin_polar > 0.0) /* Correct eventual round-off errors. */
    sin_polar = sqrt(sin_polar);
  else
    sin_polar = 0.0;

  /* Sample azimuth with uniform PDF in [0, 2 pi). */

  azimuth = MATH_2PI * gsl_rng_uniform(RandomNumberGenerator);

  /* Compute direction. */

  direction[MATH_X] = sin_polar * cos(azimuth);
  direction[MATH_Y] = sin_polar * sin(azimuth);
  direction[MATH_Z] = cos_polar;
}

/** StandardGaussianDeviates.

    - Generates a couple of standard gaussian deviates.
    - Deviates are stored in 'deviates', which must be allocated by the calling program. **/

void StandardGaussianDeviates(double *deviates)
{
  double x, y, r;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r);
  deviates[0] = x * r;
  deviates[1] = y * r;
}

/** GaussianDeviates.

    - Generates a couple of gaussian deviates with mean 'mean' and standard deviation 'std_dev'.
    - Deviates are stored in 'deviates', which must be allocated by the calling program. **/

void GaussianDeviates(double *deviates, double mean, double std_dev)
{
  double x, y, r;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r) * std_dev;
  deviates[0] = x * r + mean;
  deviates[1] = y * r + mean;
}

/** ComplexGaussianDeviates.

    - Generates a gaussian deviate with mean 'mean' and standard deviation 'std_dev' in the complex plane. **/

complex ComplexGaussianDeviates(complex mean, complex std_dev)
{
  double x, y, r;
  complex deviate;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r);
  deviate = mean + r * (x * creal(std_dev) + I * y * cimag(std_dev));
  return deviate;
}

#endif

/*** End of 'mathematics.h'. ***/