#include <petsc.h>
#include <petscmath.h>
#include "compressibleFlow.h"
#include "mesh.h"
#include "petscdmplex.h"
#include "petscts.h"

//MMS from Verification of a Compressible CFD Code using the Method of Manufactured Solutions, Christopher J. Roy,† Thomas M. Smith,‡ and Curtis C. Ober§

// Define
#define Pi PETSC_PI
#define Sin PetscSinReal
#define Cos PetscCosReal
#define Power PetscPowReal

typedef struct {
    PetscReal phiO;
    PetscReal phiX;
    PetscReal phiY;
    PetscReal phiZ;
    PetscReal aPhiX;
    PetscReal aPhiY;
    PetscReal aPhiZ;
} PhiConstants;

typedef struct {
    PetscInt dim;
    PhiConstants rho;
    PhiConstants u;
    PhiConstants v;
    PhiConstants w;
    PhiConstants p;
    PetscReal L;
    PetscReal gamma;
    PetscReal R;
    PetscReal mu;
} Constants;

typedef struct {
    Constants constants;
    FlowData flowData;
} ProblemSetup;

static PetscErrorCode EulerExact(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscFunctionBeginUser;

    Constants *constants = (Constants *)ctx;
    PetscReal L = constants->L;
    PetscReal gamma = constants->gamma;

    PetscReal rhoO = constants->rho.phiO;
    PetscReal rhoX = constants->rho.phiX;
    PetscReal rhoY = constants->rho.phiY;
    PetscReal rhoZ = constants->rho.phiZ;
    PetscReal aRhoX = constants->rho.aPhiX;
    PetscReal aRhoY = constants->rho.aPhiY;
    PetscReal aRhoZ = constants->rho.aPhiZ;

    PetscReal uO = constants->u.phiO;
    PetscReal uX = constants->u.phiX;
    PetscReal uY = constants->u.phiY;
    PetscReal uZ = constants->u.phiZ;
    PetscReal aUX = constants->u.aPhiX;
    PetscReal aUY = constants->u.aPhiY;
    PetscReal aUZ = constants->u.aPhiZ;

    PetscReal vO = constants->v.phiO;
    PetscReal vX = constants->v.phiX;
    PetscReal vY = constants->v.phiY;
    PetscReal vZ = constants->v.phiZ;
    PetscReal aVX = constants->v.aPhiX;
    PetscReal aVY = constants->v.aPhiY;
    PetscReal aVZ = constants->v.aPhiZ;

    PetscReal wO = constants->w.phiO;
    PetscReal wX = constants->w.phiX;
    PetscReal wY = constants->w.phiY;
    PetscReal wZ = constants->w.phiZ;
    PetscReal aWX = constants->w.aPhiX;
    PetscReal aWY = constants->w.aPhiY;
    PetscReal aWZ = constants->w.aPhiZ;

    PetscReal pO = constants->p.phiO;
    PetscReal pX = constants->p.phiX;
    PetscReal pY = constants->p.phiY;
    PetscReal pZ = constants->p.phiZ;
    PetscReal aPX = constants->p.aPhiX;
    PetscReal aPY = constants->p.aPhiY;
    PetscReal aPZ = constants->p.aPhiZ;

    PetscReal x = xyz[0];
    PetscReal y = xyz[1];
    PetscReal z = dim > 2? xyz[2] : 0.0;

    u[RHO] = rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L);
    u[RHOE] = (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.);

    u[RHOU + 0] = (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
           (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L));
    u[RHOU + 1] = (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
           (vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L));

    if(dim > 2){
        u[RHOU + 2] = (wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*
               (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L));
    }

    PetscFunctionReturn(0);
}

static PetscErrorCode EulerExactTimeDerivative(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscFunctionBeginUser;
    u[RHO] = 0.0;
    u[RHOE] = 0.0;
    u[RHOU + 0] = 0.0;
    u[RHOU + 1] = 0.0;
    if(dim > 2) {
        u[RHOU + 2] = 0.0;
    }

    PetscFunctionReturn(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);

    ierr = VecViewFromOptions(u, NULL, "-sol_view");CHKERRQ(ierr);
    ierr = PetscPrintf(PetscObjectComm((PetscObject)dm), "TS at %f\n", time);CHKERRQ(ierr);

    // Compute the error
    void            *exactCtxs[1];
    PetscErrorCode (*exactFuncs[1])(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx);
    PetscDS          ds;
    ierr = DMGetDS(dm, &ds);CHKERRQ(ierr);

    // Get the exact solution
    ierr = PetscDSGetExactSolution(ds, 0, &exactFuncs[0], &exactCtxs[0]);CHKERRQ(ierr);

    // Create an vector to hold the exact solution
    Vec exactVec;
    ierr = VecDuplicate(u, &exactVec);CHKERRQ(ierr);
    ierr = DMProjectFunction(dm,time,exactFuncs,exactCtxs,INSERT_ALL_VALUES,exactVec);CHKERRQ(ierr);

    // For each component, compute the l2 norms
    ierr = VecAXPY(exactVec, -1.0, u);CHKERRQ(ierr);

    PetscReal ferrors[4];
    ierr = VecSetBlockSize(exactVec, 4);CHKERRQ(ierr);

    // Compute the L2 Difference
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Timestep: %04d time = %-8.4g \t\n", (int) step, (double) time);CHKERRQ(ierr);
    ierr =  VecStrideNormAll(exactVec, NORM_2,ferrors);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_2 Error: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double) ferrors[0], (double) ferrors[1], (double) ferrors[2], (double) ferrors[3]);CHKERRQ(ierr);
    ierr =  VecStrideNormAll(exactVec, NORM_INFINITY,ferrors);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_Inf Error: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double) ferrors[0], (double) ferrors[1], (double) ferrors[2], (double) ferrors[3]);CHKERRQ(ierr);

    ierr = VecDestroy(&exactVec);CHKERRQ(ierr);

    PetscFunctionReturn(0);
}

static PetscErrorCode SourceRho(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscFunctionBeginUser;

    Constants *constants = (Constants *)ctx;
    PetscReal L = constants->L;
    PetscReal gamma = constants->gamma;

    PetscReal rhoO = constants->rho.phiO;
    PetscReal rhoX = constants->rho.phiX;
    PetscReal rhoY = constants->rho.phiY;
    PetscReal rhoZ = constants->rho.phiZ;
    PetscReal aRhoX = constants->rho.aPhiX;
    PetscReal aRhoY = constants->rho.aPhiY;
    PetscReal aRhoZ = constants->rho.aPhiZ;

    PetscReal uO = constants->u.phiO;
    PetscReal uX = constants->u.phiX;
    PetscReal uY = constants->u.phiY;
    PetscReal uZ = constants->u.phiZ;
    PetscReal aUX = constants->u.aPhiX;
    PetscReal aUY = constants->u.aPhiY;
    PetscReal aUZ = constants->u.aPhiZ;

    PetscReal vO = constants->v.phiO;
    PetscReal vX = constants->v.phiX;
    PetscReal vY = constants->v.phiY;
    PetscReal vZ = constants->v.phiZ;
    PetscReal aVX = constants->v.aPhiX;
    PetscReal aVY = constants->v.aPhiY;
    PetscReal aVZ = constants->v.aPhiZ;

    PetscReal wO = constants->w.phiO;
    PetscReal wX = constants->w.phiX;
    PetscReal wY = constants->w.phiY;
    PetscReal wZ = constants->w.phiZ;
    PetscReal aWX = constants->w.aPhiX;
    PetscReal aWY = constants->w.aPhiY;
    PetscReal aWZ = constants->w.aPhiZ;

    PetscReal pO = constants->p.phiO;
    PetscReal pX = constants->p.phiX;
    PetscReal pY = constants->p.phiY;
    PetscReal pZ = constants->p.phiZ;
    PetscReal aPX = constants->p.aPhiX;
    PetscReal aPY = constants->p.aPhiY;
    PetscReal aPZ = constants->p.aPhiZ;

    PetscReal x = xyz[0];
    PetscReal y = xyz[1];
    PetscReal z = dim > 2? xyz[2] : 0.0;

    u[0] = (aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) +
                                               uX*Sin((aUX*Pi*x)/L)))/L + (aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*
                                                                           (wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L)))/L +
           (aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                         rhoZ*Sin((aRhoZ*Pi*z)/L)))/L + (aVY*Pi*vY*Cos((aVY*Pi*y)/L)*
                                                                         (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L)))/L -
           (aRhoY*Pi*rhoY*Sin((aRhoY*Pi*y)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) +
                                               vZ*Sin((aVZ*Pi*z)/L)))/L - (aWZ*Pi*wZ*
                                                                           (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                           Sin((aWZ*Pi*z)/L))/L;
    PetscFunctionReturn(0);
}

static PetscErrorCode SourceRhoU(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscFunctionBeginUser;

    Constants *constants = (Constants *)ctx;
    PetscReal L = constants->L;
    PetscReal gamma = constants->gamma;

    PetscReal rhoO = constants->rho.phiO;
    PetscReal rhoX = constants->rho.phiX;
    PetscReal rhoY = constants->rho.phiY;
    PetscReal rhoZ = constants->rho.phiZ;
    PetscReal aRhoX = constants->rho.aPhiX;
    PetscReal aRhoY = constants->rho.aPhiY;
    PetscReal aRhoZ = constants->rho.aPhiZ;

    PetscReal uO = constants->u.phiO;
    PetscReal uX = constants->u.phiX;
    PetscReal uY = constants->u.phiY;
    PetscReal uZ = constants->u.phiZ;
    PetscReal aUX = constants->u.aPhiX;
    PetscReal aUY = constants->u.aPhiY;
    PetscReal aUZ = constants->u.aPhiZ;

    PetscReal vO = constants->v.phiO;
    PetscReal vX = constants->v.phiX;
    PetscReal vY = constants->v.phiY;
    PetscReal vZ = constants->v.phiZ;
    PetscReal aVX = constants->v.aPhiX;
    PetscReal aVY = constants->v.aPhiY;
    PetscReal aVZ = constants->v.aPhiZ;

    PetscReal wO = constants->w.phiO;
    PetscReal wX = constants->w.phiX;
    PetscReal wY = constants->w.phiY;
    PetscReal wZ = constants->w.phiZ;
    PetscReal aWX = constants->w.aPhiX;
    PetscReal aWY = constants->w.aPhiY;
    PetscReal aWZ = constants->w.aPhiZ;

    PetscReal pO = constants->p.phiO;
    PetscReal pX = constants->p.phiX;
    PetscReal pY = constants->p.phiY;
    PetscReal pZ = constants->p.phiZ;
    PetscReal aPX = constants->p.aPhiX;
    PetscReal aPY = constants->p.aPhiY;
    PetscReal aPZ = constants->p.aPhiZ;

    PetscReal x = xyz[0];
    PetscReal y = xyz[1];
    PetscReal z = dim > 2? xyz[2] : 0.0;

    u[0] = -((aPX*Pi*pX*Sin((aPX*Pi*x)/L))/L) + (aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*
                                                 Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2))/L +
           (aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) +
                                               uX*Sin((aUX*Pi*x)/L))*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L)))/
           L + (2*aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) +
                                               uX*Sin((aUX*Pi*x)/L))*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                                                      rhoZ*Sin((aRhoZ*Pi*z)/L)))/L + (aVY*Pi*vY*Cos((aVY*Pi*y)/L)*
                                                                                (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L)))/L -
           (aUZ*Pi*uZ*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*
            (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
            Sin((aUZ*Pi*z)/L))/L - (aRhoY*Pi*rhoY*
                                    (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*Sin((aRhoY*Pi*y)/L)*
                                    (vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L -
           (aUY*Pi*uY*Sin((aUY*Pi*y)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                         rhoZ*Sin((aRhoZ*Pi*z)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) +
                                                                    vZ*Sin((aVZ*Pi*z)/L)))/L - (aWZ*Pi*wZ*
                                                                                (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                Sin((aWZ*Pi*z)/L))/L;

    u[1] = (aPY*Pi*pY*Cos((aPY*Pi*y)/L))/L - (aVX*Pi*vX*
                                              (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*Sin((aVX*Pi*x)/L)*
                                              (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L)))/L +
           (aVZ*Pi*vZ*Cos((aVZ*Pi*z)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) +
                                         wY*Sin((aWY*Pi*y)/L))*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                                                rhoZ*Sin((aRhoZ*Pi*z)/L)))/L + (aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*
                                                                                (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
                                                                                (vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L +
           (aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) +
                                               wY*Sin((aWY*Pi*y)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/
           L + (aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                             rhoZ*Sin((aRhoZ*Pi*z)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) +
                                                                        vZ*Sin((aVZ*Pi*z)/L)))/L + (2*aVY*Pi*vY*Cos((aVY*Pi*y)/L)*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                (vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L -
           (aRhoY*Pi*rhoY*Sin((aRhoY*Pi*y)/L)*Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) +
                                                    vZ*Sin((aVZ*Pi*z)/L),2))/L - (aWZ*Pi*wZ*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                (vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L))*Sin((aWZ*Pi*z)/L))/L;

    if(dim >2){
        u[2] = (aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) +
                                                   uX*Sin((aUX*Pi*x)/L))*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L)))/
               L + (aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*
                    Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2))/L -
               (aPZ*Pi*pZ*Sin((aPZ*Pi*z)/L))/L + (aWX*Pi*wX*Cos((aWX*Pi*x)/L)*
                                                  (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
                                                  (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L)))/L +
               (aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) +
                                             wY*Sin((aWY*Pi*y)/L))*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                                                    rhoZ*Sin((aRhoZ*Pi*z)/L)))/L + (aVY*Pi*vY*Cos((aVY*Pi*y)/L)*
                                                                                (wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L)))/L -
               (aRhoY*Pi*rhoY*Sin((aRhoY*Pi*y)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) +
                                                   wY*Sin((aWY*Pi*y)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/
               L + (aWY*Pi*wY*Cos((aWY*Pi*y)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) +
                                                 rhoZ*Sin((aRhoZ*Pi*z)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) +
                                                                            vZ*Sin((aVZ*Pi*z)/L)))/L - (2*aWZ*Pi*wZ*
                                                                                (wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*
                                                                                (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                Sin((aWZ*Pi*z)/L))/L;
    }
    PetscFunctionReturn(0);
}

static PetscErrorCode SourceRhoE(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscFunctionBeginUser;

    Constants *constants = (Constants *)ctx;
    PetscReal L = constants->L;
    PetscReal gamma = constants->gamma;

    PetscReal rhoO = constants->rho.phiO;
    PetscReal rhoX = constants->rho.phiX;
    PetscReal rhoY = constants->rho.phiY;
    PetscReal rhoZ = constants->rho.phiZ;
    PetscReal aRhoX = constants->rho.aPhiX;
    PetscReal aRhoY = constants->rho.aPhiY;
    PetscReal aRhoZ = constants->rho.aPhiZ;

    PetscReal uO = constants->u.phiO;
    PetscReal uX = constants->u.phiX;
    PetscReal uY = constants->u.phiY;
    PetscReal uZ = constants->u.phiZ;
    PetscReal aUX = constants->u.aPhiX;
    PetscReal aUY = constants->u.aPhiY;
    PetscReal aUZ = constants->u.aPhiZ;

    PetscReal vO = constants->v.phiO;
    PetscReal vX = constants->v.phiX;
    PetscReal vY = constants->v.phiY;
    PetscReal vZ = constants->v.phiZ;
    PetscReal aVX = constants->v.aPhiX;
    PetscReal aVY = constants->v.aPhiY;
    PetscReal aVZ = constants->v.aPhiZ;

    PetscReal wO = constants->w.phiO;
    PetscReal wX = constants->w.phiX;
    PetscReal wY = constants->w.phiY;
    PetscReal wZ = constants->w.phiZ;
    PetscReal aWX = constants->w.aPhiX;
    PetscReal aWY = constants->w.aPhiY;
    PetscReal aWZ = constants->w.aPhiZ;

    PetscReal pO = constants->p.phiO;
    PetscReal pX = constants->p.phiX;
    PetscReal pY = constants->p.phiY;
    PetscReal pZ = constants->p.phiZ;
    PetscReal aPX = constants->p.aPhiX;
    PetscReal aPY = constants->p.aPhiY;
    PetscReal aPZ = constants->p.aPhiZ;

    PetscReal x = xyz[0];
    PetscReal y = xyz[1];
    PetscReal z = dim > 2? xyz[2] : 0.0;

    u[0] = -((aPX*Pi*pX*Sin((aPX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L)))/L) + (aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L)))/L +
           (aVY*Pi*vY*Cos((aVY*Pi*y)/L)*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L)))/L - (aPZ*Pi*pZ*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*Sin((aPZ*Pi*z)/L))/L +
           (aPY*Pi*pY*Cos((aPY*Pi*y)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L + (rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L))*
                                                                                ((aRhoY*Pi*rhoY*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))*Sin((aRhoY*Pi*y)/L))/((-1. + gamma)*L*Power(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L),2)) +
                                                                                (aPY*Pi*pY*Cos((aPY*Pi*y)/L))/((-1. + gamma)*L*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                ((-2*aUY*Pi*uY*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*Sin((aUY*Pi*y)/L))/L + (2*aWY*Pi*wY*Cos((aWY*Pi*y)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L)))/L +
                                                                                (2*aVY*Pi*vY*Cos((aVY*Pi*y)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L)/2.) + (uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                (-((aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L)))/((-1. + gamma)*L*Power(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L),2))) -
                                                                                (aPX*Pi*pX*Sin((aPX*Pi*x)/L))/((-1. + gamma)*L*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                ((2*aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L)))/L + (2*aWX*Pi*wX*Cos((aWX*Pi*x)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L)))/L -
                                                                                (2*aVX*Pi*vX*Sin((aVX*Pi*x)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L)/2.) + (aRhoX*Pi*rhoX*Cos((aRhoX*Pi*x)/L)*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*
                                                                                ((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.))/L +
           (aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.))/L +
           (aUX*Pi*uX*Cos((aUX*Pi*x)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.))/L +
           (aVY*Pi*vY*Cos((aVY*Pi*y)/L)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.))/L -
           (aRhoY*Pi*rhoY*Sin((aRhoY*Pi*y)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L))*((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.))/L -
           (aWZ*Pi*wZ*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))*Sin((aWZ*Pi*z)/L))/L - (aWZ*Pi*wZ*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
                                                                                ((pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L))/((-1. + gamma)*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
                                                                                (Power(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L),2) + Power(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L),2) + Power(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L),2))/2.)*Sin((aWZ*Pi*z)/L))/L +
           (wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))*
           (-((aRhoZ*Pi*rhoZ*Cos((aRhoZ*Pi*z)/L)*(pO + pX*Cos((aPX*Pi*x)/L) + pZ*Cos((aPZ*Pi*z)/L) + pY*Sin((aPY*Pi*y)/L)))/((-1. + gamma)*L*Power(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L),2))) -
            (aPZ*Pi*pZ*Sin((aPZ*Pi*z)/L))/((-1. + gamma)*L*(rhoO + rhoY*Cos((aRhoY*Pi*y)/L) + rhoX*Sin((aRhoX*Pi*x)/L) + rhoZ*Sin((aRhoZ*Pi*z)/L))) +
            ((-2*aUZ*Pi*uZ*(uO + uY*Cos((aUY*Pi*y)/L) + uZ*Cos((aUZ*Pi*z)/L) + uX*Sin((aUX*Pi*x)/L))*Sin((aUZ*Pi*z)/L))/L + (2*aVZ*Pi*vZ*Cos((aVZ*Pi*z)/L)*(vO + vX*Cos((aVX*Pi*x)/L) + vY*Sin((aVY*Pi*y)/L) + vZ*Sin((aVZ*Pi*z)/L)))/L -
             (2*aWZ*Pi*wZ*(wO + wZ*Cos((aWZ*Pi*z)/L) + wX*Sin((aWX*Pi*x)/L) + wY*Sin((aWY*Pi*y)/L))*Sin((aWZ*Pi*z)/L))/L)/2.);
    PetscFunctionReturn(0);
}

static PetscErrorCode PhysicsBoundary_Euler(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx) {
    PetscFunctionBeginUser;
    Constants *constants = (Constants *)ctx;

    // Offset the calc assuming the cells are square
    PetscReal x[3];

    for(PetscInt i =0; i < constants->dim; i++){
        x[i] = c[i] + n[i]*0.5;
    }

    EulerExact(constants->dim, time, x, 0, a_xG, ctx);
    PetscFunctionReturn(0);
}

static PetscErrorCode ComputeRHSWithSourceTerms(DM dm, PetscReal time, Vec locXVec, Vec globFVec, void *ctx){
    PetscFunctionBeginUser;
    PetscErrorCode ierr;
    ProblemSetup *setup = (ProblemSetup *)ctx;

    // Call the flux calculation
    ierr = DMPlexTSComputeRHSFunctionFVM(dm, time, locXVec, globFVec, setup->flowData);CHKERRQ(ierr);

    // Convert the dm to a plex
    DM plex;
    DMConvert(dm, DMPLEX, &plex);

    // Extract the cell geometry, and the dm that holds the information
    Vec cellgeom;
    DM dmCell;
    const PetscScalar *cgeom;
    ierr = DMPlexGetGeometryFVM(plex, NULL, &cellgeom, NULL);CHKERRQ(ierr);
    ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
    ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);

    // Get the cell start and end for the fv cells
    PetscInt cStart, cEnd;
    ierr = DMPlexGetSimplexOrBoxCells(dmCell, 0, &cStart, &cEnd);CHKERRQ(ierr);

    // create a local f vector
    Vec locFVec;
    PetscScalar  *locFArray;
    ierr = DMGetLocalVector(dm, &locFVec);CHKERRQ(ierr);
    ierr = VecZeroEntries(locFVec);CHKERRQ(ierr);
    ierr = VecGetArray(locFVec, &locFArray);CHKERRQ(ierr);

    // get the current values
    const PetscScalar      *locXArray;
    ierr = VecGetArrayRead(locXVec, &locXArray);CHKERRQ(ierr);

    PetscInt rank;
    MPI_Comm_rank(PETSC_COMM_WORLD, &rank);

    // March over each cell volume
    for (PetscInt c = cStart; c < cEnd; ++c) {
        PetscFVCellGeom       *cg;
        const PetscReal           *xc;
        PetscReal           *fc;

        ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);CHKERRQ(ierr);
        ierr = DMPlexPointLocalFieldRead(plex, c, 0, locXArray, &xc);CHKERRQ(ierr);
        ierr = DMPlexPointGlobalFieldRef(plex, c, 0, locFArray, &fc);CHKERRQ(ierr);

        if(fc) {  // must be real cell and not ghost
            SourceRho(setup->constants.dim, time, cg->centroid, 0, fc + RHO, &setup->constants);
            SourceRhoE(setup->constants.dim, time, cg->centroid, 0, fc + RHOE, &setup->constants);
            SourceRhoU(setup->constants.dim, time, cg->centroid, 0, fc + RHOU, &setup->constants);
        }
    }

    // restore the cell geometry
    ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(locXVec, &locXArray);CHKERRQ(ierr);
    ierr = VecRestoreArray(locFVec, &locFArray);CHKERRQ(ierr);

    ierr = DMLocalToGlobalBegin(dm, locFVec, ADD_VALUES, globFVec);CHKERRQ(ierr);
    ierr = DMLocalToGlobalEnd(dm, locFVec, ADD_VALUES, globFVec);CHKERRQ(ierr);
    ierr = DMRestoreLocalVector(dm, &locFVec);CHKERRQ(ierr);

//    {// check rhs,
//        // temp read current residual
//        const PetscScalar *currentFArray;
//        ierr = VecGetArrayRead(globFVec, &currentFArray);CHKERRQ(ierr);
//        ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
//
//        // March over each cell volume
//        for (PetscInt c = cStart; c < cEnd; ++c) {
//            PetscFVCellGeom       *cg;
//            const PetscReal           *fcCurrent;
//
//            ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);CHKERRQ(ierr);
//            ierr = DMPlexPointGlobalFieldRead(plex, c, 0, currentFArray, &fcCurrent);CHKERRQ(ierr);
//
//            if(fcCurrent) {  // must be real cell and not ghost
//                if(PetscAbsReal(cg->centroid[0] - .5 ) < 1E-8 && PetscAbsReal(cg->centroid[1] - .5 )  < 1E-8){
//                    printf("Residual(%f, %f): %f %f %f %f\n", cg->centroid[0], cg->centroid[1], fcCurrent[0], fcCurrent[1], fcCurrent[2], fcCurrent[3]);
//                }
//            }
//        }
//
//        // temp return current residual
//        ierr = VecRestoreArrayRead(globFVec, &currentFArray);CHKERRQ(ierr);
//        ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
//    }

    PetscFunctionReturn(0);
}

PetscErrorCode ComputeRHS(TS ts, DM dm, PetscReal t, Vec u, PetscInt blockSize, PetscReal residualNorm2[], PetscReal residualNormInf[], PetscReal start[], PetscReal end[])
{
    MPI_Comm       comm;
    Vec            r;
    PetscErrorCode ierr;

    PetscFunctionBeginUser;
    Vec sol;
    ierr = VecDuplicate(u, &sol);CHKERRQ(ierr);
    ierr = VecCopy(u, sol);CHKERRQ(ierr);

    ierr = PetscObjectGetComm((PetscObject) ts, &comm);CHKERRQ(ierr);
    ierr = DMComputeExactSolution(dm, t, sol, NULL);CHKERRQ(ierr);
    ierr = VecDuplicate(u, &r);CHKERRQ(ierr);
    ierr = TSComputeRHSFunction(ts, t, sol, r);CHKERRQ(ierr);

    // zero out the norms
    for(PetscInt b =0; b < blockSize; b++){
        residualNorm2[b] = 0.0;
        residualNormInf[b] = 0.0;
    }

    {//March over each cell
        // Extract the cell geometry, and the dm that holds the information
        Vec cellgeom;
        DM dmCell;
        PetscInt dim;
        const PetscScalar *cgeom;
        ierr = DMPlexGetGeometryFVM(dm, NULL, &cellgeom, NULL);CHKERRQ(ierr);
        ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
        ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);

        // Get the cell start and end for the fv cells
        PetscInt cStart, cEnd;
        ierr = DMPlexGetSimplexOrBoxCells(dmCell, 0, &cStart, &cEnd);CHKERRQ(ierr);

        // temp read current residual
        const PetscScalar *currentRHS;
        ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
        ierr = VecGetArrayRead(r, &currentRHS);CHKERRQ(ierr);

        // Count up the cells
        PetscInt count = 0;

        // March over each cell volume
        for (PetscInt c = cStart; c < cEnd; ++c) {
            PetscFVCellGeom       *cg;
            const PetscReal           *rhsCurrent;

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

            PetscBool countCell = PETSC_TRUE;
            for(PetscInt d =0; d < dim; d++){
                countCell = cg->centroid[d] < start[d] || cg->centroid[d] > end[d] ? PETSC_FALSE : countCell;
            }

            if(rhsCurrent && countCell ) {  // must be real cell and not ghost
                for(PetscInt b =0; b < blockSize; b++){
                    residualNorm2[b] += PetscSqr(rhsCurrent[b]);
                    residualNormInf[b] = PetscMax(residualNormInf[b], PetscAbs(rhsCurrent[b]));
                }
                count++;
            }
        }

        // normalize the norm2
        for(PetscInt b =0; b < blockSize; b++){
            residualNorm2[b] = PetscSqrtReal(residualNorm2[b]/count);
        }

        // temp return current residual
        ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
        ierr = VecRestoreArrayRead(r, &currentRHS);CHKERRQ(ierr);
    }

    ierr = VecDestroy(&sol);CHKERRQ(ierr);
    ierr = VecDestroy(&r);CHKERRQ(ierr);
    PetscFunctionReturn(0);
}

int main(int argc, char **argv)
{

    // Setup the problem
    Constants constants;

    // sub sonic
    constants.dim = 2;
    constants.L = 1.0;
    constants.gamma = 1.4;
    constants.R = 287.0;
    constants.mu = 10;

    constants.rho.phiO = 1.0;
    constants.rho.phiX = 0.15;
    constants.rho.phiY = -0.1;
    constants.rho.phiZ = 0.0;
    constants.rho.aPhiX = 1.0;
    constants.rho.aPhiY = 0.5;
    constants.rho.aPhiZ = 0.0;

    constants.u.phiO = 70;
    constants.u.phiX = 5;
    constants.u.phiY = -7;
    constants.u.phiZ = 0.0;
    constants.u.aPhiX = 1.5;
    constants.u.aPhiY = 0.6;
    constants.u.aPhiZ = 0.0;

    constants.v.phiO = 90;
    constants.v.phiX = -15;
    constants.v.phiY = -8.5;
    constants.v.phiZ = 0.0;
    constants.v.aPhiX = 0.5;
    constants.v.aPhiY = 2.0/3.0;
    constants.v.aPhiZ = 0.0;

    constants.w.phiO = 0.0;
    constants.w.phiX = 0.0;
    constants.w.phiY = 0.0;
    constants.w.phiZ = 0.0;
    constants.w.aPhiX = 0.0;
    constants.w.aPhiY = 0.0;
    constants.w.aPhiZ = 0.0;

    constants.p.phiO = 1E5;
    constants.p.phiX = 0.2E5;
    constants.p.phiY = 0.5E5;
    constants.p.phiZ = 0.0;
    constants.p.aPhiX = 2.0;
    constants.p.aPhiY = 1.0;
    constants.p.aPhiZ = 0.0;

    PetscErrorCode ierr;
    // create the mesh
    // setup the ts
    DM dm;                 /* problem definition */
    TS ts;                 /* timestepper */

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

    // Create a ts
    ierr = TSCreate(PETSC_COMM_WORLD, &ts);CHKERRQ(ierr);
    ierr = TSSetProblemType(ts, TS_NONLINEAR);CHKERRQ(ierr);
    ierr = TSSetType(ts, TSEULER);CHKERRQ(ierr);
    ierr = TSSetExactFinalTime(ts, TS_EXACTFINALTIME_MATCHSTEP);CHKERRQ(ierr);

    // Create a mesh
    // hard code the problem setup
    PetscReal start[] = {0.0, 0.0};
    PetscReal end[] = {constants.L, constants.L};
    PetscInt nx[] = {4, 4};
    DMBoundaryType bcType[] = {DM_BOUNDARY_NONE, DM_BOUNDARY_NONE};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, constants.dim, PETSC_FALSE, nx, start, end, bcType, PETSC_TRUE, &dm);CHKERRQ(ierr);

    // Setup the flow data
    FlowData flowData;     /* store some of the flow data*/
    ierr = FlowCreate(&flowData);CHKERRQ(ierr);

    // Combine the flow data
    ProblemSetup problemSetup;
    problemSetup.flowData = flowData;
    problemSetup.constants = constants;

    //Setup
    CompressibleFlow_SetupDiscretization(flowData, &dm);

    // Add in the flow parameters
    PetscScalar params[TOTAL_COMPRESSIBLE_FLOW_PARAMETERS];
    params[CFL] = 0.5;
    params[GAMMA] = constants.gamma;

    // set up the finite volume fluxes
    CompressibleFlow_StartProblemSetup(flowData, TOTAL_COMPRESSIBLE_FLOW_PARAMETERS, params);
    DMView(flowData->dm, PETSC_VIEWER_STDERR_SELF);
    // Add in any boundary conditions
    PetscDS prob;
    ierr = DMGetDS(flowData->dm, &prob);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    const PetscInt idsLeft[]= {1, 2, 3, 4};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall left", "Face Sets", 0, 0, NULL, (void (*)(void))PhysicsBoundary_Euler, NULL, 4, idsLeft, &constants);CHKERRQ(ierr);

    // Complete the problem setup
    ierr = CompressibleFlow_CompleteProblemSetup(flowData, ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Override the flow calc for now
    ierr = DMTSSetRHSFunctionLocal(flowData->dm, ComputeRHSWithSourceTerms, &problemSetup);CHKERRQ(ierr);

    // Name the flow field
    ierr = PetscObjectSetName(((PetscObject)flowData->flowField), "Numerical Solution");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Setup the TS
    ierr = TSSetFromOptions(ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSMonitorSet(ts, MonitorError, &constants, NULL);CHKERRQ(ierr);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetMaxTime(ts, 0.01);CHKERRQ(ierr);

    // set the initial conditions
    PetscErrorCode     (*func[2]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {EulerExact};
    void* ctxs[1] ={&constants};
    ierr    = DMProjectFunction(flowData->dm,0.0,func,ctxs,INSERT_ALL_VALUES,flowData->flowField);CHKERRQ(ierr);

    // for the mms, add the exact solution
    ierr = PetscDSSetExactSolution(prob, 0, EulerExact, &constants);CHKERRQ(ierr);
    ierr = PetscDSSetExactSolutionTimeDerivative(prob, 0, EulerExactTimeDerivative, &constants);CHKERRQ(ierr);

    // Output the mesh
    ierr = DMViewFromOptions(dm, NULL, "-dm_view");CHKERRQ(ierr);

    TSSetMaxSteps(ts, 1);
    ierr = TSSolve(ts,flowData->flowField);CHKERRQ(ierr);

    // Check the current residual
    PetscReal l2Residual[4];
    PetscReal infResidual[4];

    // Only take the residual over the central 1/3
    PetscReal resStart[2] = {1.0/3.0, 1.0/3.0};
    PetscReal resEnd[2] = {2.0/3.0, 2.0/3.0};

    ierr = ComputeRHS(ts, flowData->dm, 0.0, flowData->flowField, 4, l2Residual, infResidual, resStart, resEnd);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_2 Residual: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double) l2Residual[0], (double) l2Residual[1], (double) l2Residual[2], (double) l2Residual[3]);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_Inf Residual: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double) infResidual[0], (double) infResidual[1], (double) infResidual[2], (double) infResidual[3]);CHKERRQ(ierr);

    PetscReal time = 0.0;

//    {
//        Vec sol;
//        VecDuplicate(flowData->flowField, &sol);
//        VecCopy(flowData->flowField, sol);
//
//        SNES snes;
//        ierr = TSGetSNES(ts, &snes);CHKERRQ(ierr);
//        ierr = DMSNESCheckDiscretization(snes, flowData->dm, time, sol, -1.0, NULL);CHKERRQ(ierr);
//
//        Vec u_t;
//        ierr = DMGetGlobalVector(flowData->dm, &u_t);CHKERRQ(ierr);
//        ierr = DMTSCheckResidual(ts, flowData->dm, time, sol, u_t, -1.0, NULL);CHKERRQ(ierr);
//        ierr = DMRestoreGlobalVector(flowData->dm, &u_t);CHKERRQ(ierr);
//
//        VecDestroy(&sol);
//    }

//    {
//        Vec sol;
//        VecDuplicate(flowData->flowField, &sol);
//        VecCopy(flowData->flowField, sol);
//
//        SNES snes;
//        ierr = TSGetSNES(ts, &snes);CHKERRQ(ierr);
//        ierr = DMSNESCheckDiscretization(snes, flowData->dm, time, sol, -1.0, NULL);CHKERRQ(ierr);
//
//        Vec u_t;
//        ierr = DMGetGlobalVector(flowData->dm, &u_t);CHKERRQ(ierr);
//        ierr = DMTSCheckResidual(ts, flowData->dm, time, sol, u_t, -1.0, NULL);CHKERRQ(ierr);
//        ierr = DMRestoreGlobalVector(flowData->dm, &u_t);CHKERRQ(ierr);
//
//        VecDestroy(&sol);
//    }

    return PetscFinalize();

}


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_roots.h>

#include "ccl.h"

/* --------- ROUTINE: h_over_h0 ---------
INPUT: scale factor, cosmology
TASK: Compute E(a)=H(a)/H0
*/
static double h_over_h0(double a, ccl_cosmology * cosmo, int *status)
{
  // Check if massive neutrinos are present - if not, we don't need to
  // compute their contribution
  double Om_mass_nu;
  if ((cosmo->params.N_nu_mass)>1e-12) {
    Om_mass_nu = ccl_Omeganuh2(
      a, cosmo->params.N_nu_mass, cosmo->params.m_nu, cosmo->params.T_CMB,
      status) / (cosmo->params.h) / (cosmo->params.h);
  }
  else {
    Om_mass_nu = 0;
  }

  /* Calculate h^2 using the formula (eqn 2 in the CCL paper):
    E(a)^2 = Omega_m a^-3 +
             Omega_l a^(-3*(1+w0+wa)) exp(3*wa*(a-1)) +
             Omega_k a^-2 +
             (Omega_g + Omega_nu_rel) a^-4 +
             Om_mass_nu
  */
  return sqrt(
    (cosmo->params.Omega_c + cosmo->params.Omega_b +
     cosmo->params.Omega_l *
       pow(a,-3*(cosmo->params.w0+cosmo->params.wa)) *
       exp(3*cosmo->params.wa*(a-1)) +
     cosmo->params.Omega_k * a +
     (cosmo->params.Omega_g + cosmo->params.Omega_nu_rel) / a +
     Om_mass_nu * a*a*a) / (a*a*a));
}

/* --------- ROUTINE: ccl_omega_x ---------
INPUT: cosmology object, scale factor, species label
TASK: Compute the density relative to critical, Omega(a) for a given species.
Possible values for "label":
ccl_species_crit_label <- critical (physical)
ccl_species_m_label <- matter
ccl_species_l_label <- DE
ccl_species_g_label <- radiation
ccl_species_k_label <- curvature
ccl_species_ur_label <- massless neutrinos
ccl_species_nu_label <- massive neutrinos
*/
double ccl_omega_x(ccl_cosmology * cosmo, double a, ccl_species_x_label label, int *status)
{
  // If massive neutrinos are present, compute the phase-space integral and
  // get OmegaNuh2. If not, set OmegaNuh2 to zero.
  double OmNuh2;
  if ((cosmo->params.N_nu_mass) > 0.0001) {
    // Call the massive neutrino density function just once at this redshift.
    OmNuh2 = ccl_Omeganuh2(a, cosmo->params.N_nu_mass, cosmo->params.m_nu,
                           cosmo->params.T_CMB, status);
  }
  else {
    OmNuh2 = 0.;
  }

  double hnorm = h_over_h0(a, cosmo, status);

  switch(label) {
    case ccl_species_crit_label :
      return 1.;
    case ccl_species_m_label :
      return (cosmo->params.Omega_c + cosmo->params.Omega_b) / (a*a*a) / hnorm / hnorm +
          OmNuh2 / (cosmo->params.h) / (cosmo->params.h) / hnorm / hnorm;
    case ccl_species_l_label :
      return
        cosmo->params.Omega_l *
        pow(a,-3 * (1 + cosmo->params.w0 + cosmo->params.wa)) *
        exp(3 * cosmo->params.wa * (a-1)) / hnorm / hnorm;
    case ccl_species_g_label :
      return cosmo->params.Omega_g / (a*a*a*a) / hnorm / hnorm;
    case ccl_species_k_label :
      return cosmo->params.Omega_k / (a*a) / hnorm / hnorm;
    case ccl_species_ur_label :
      return cosmo->params.Omega_nu_rel / (a*a*a*a) / hnorm / hnorm;
    case ccl_species_nu_label :
      return OmNuh2 / (cosmo->params.h) / (cosmo->params.h) / hnorm / hnorm;
    default:
      *status = CCL_ERROR_PARAMETERS;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_omega_x(): Species %d not supported\n", label);
      return NAN;
  }
}

/* --------- ROUTINE: ccl_rho_x ---------
INPUT: cosmology object, scale factor, species label
TASK: Compute rho_x(a), with x defined by species label.
Possible values for "label":
ccl_species_crit_label <- critical (physical)
ccl_species_m_label <- matter (physical)
ccl_species_l_label <- DE (physical)
ccl_species_g_label <- radiation (physical)
ccl_species_k_label <- curvature (physical)
ccl_species_ur_label <- massless neutrinos (physical)
ccl_species_nu_label <- massive neutrinos (physical)
*/
double ccl_rho_x(ccl_cosmology * cosmo, double a, ccl_species_x_label label, int is_comoving, int *status)
{
  double comfac;
  if (is_comoving) {
     comfac = a*a*a;
  } else {
      comfac = 1.0;
  }
  double hnorm = h_over_h0(a, cosmo, status);
  double rhocrit =
    ccl_constants.RHO_CRITICAL *
    (cosmo->params.h) *
    (cosmo->params.h) * hnorm * hnorm * comfac;

  return rhocrit * ccl_omega_x(cosmo, a, label, status);
}

/* --------- ROUTINE: ccl_mu_MG ---------
INPUT: cosmology object, scale factor
TASK: Compute mu(a) where mu is one of the the parameterizating functions
of modifications to GR in the quasistatic approximation.
*/

double ccl_mu_MG(ccl_cosmology * cosmo, double a, int *status)
{
    // This function can be extended to include other
    // z-dependences for mu in the future.
    return cosmo->params.mu_0 * ccl_omega_x(cosmo, a, ccl_species_l_label, status) / cosmo->params.Omega_l;
}

/* --------- ROUTINE: ccl_Sig_MG ---------
INPUT: cosmology object, scale factor
TASK: Compute Sigma(a) where Sigma is one of the the parameterizating functions
of modifications to GR in the quasistatic approximation.
*/

double ccl_Sig_MG(ccl_cosmology * cosmo, double a, int *status)
{
    // This function can be extended to include other
    // z-dependences for Sigma in the future.
    return cosmo->params.sigma_0 * ccl_omega_x(cosmo, a, ccl_species_l_label, status) / cosmo->params.Omega_l;
}

// Structure to hold parameters of chi_integrand
typedef struct {
  ccl_cosmology *cosmo;
  int * status;
} chipar;

/* --------- ROUTINE: chi_integrand ---------
INPUT: scale factor
TASK: compute the integrand of the comoving distance
*/
static double chi_integrand(double a, void * params_void)
{
  ccl_cosmology * cosmo = ((chipar *)params_void)->cosmo;
  int *status = ((chipar *)params_void)->status;

  return ccl_constants.CLIGHT_HMPC/(a*a*h_over_h0(a, cosmo, status));
}

/* --------- ROUTINE: growth_ode_system ---------
INPUT: scale factor
TASK: Define the ODE system to be solved in order to compute the growth (of the density)
*/
static int growth_ode_system(double a,const double y[],double dydt[],void *params)
{
  int status = 0;
  ccl_cosmology * cosmo = params;

  double hnorm=h_over_h0(a,cosmo, &status);
  double om=ccl_omega_x(cosmo, a, ccl_species_m_label, &status);

  dydt[1]=1.5*hnorm*a*om*y[0];
  dydt[0]=y[1]/(a*a*a*hnorm);

  return status;
}

/* --------- ROUTINE: growth_ode_system_muSig ---------
INPUT: scale factor
TASK: Define the ODE system to be solved in order to compute the growth (of the density)
* in the case in which we use the mu / Sigma quasistatic parameterisation of modified gravity
*/
static int growth_ode_system_muSig(double a,const double y[],double dydt[],void *params)
{
  int status = 0;
  ccl_cosmology * cosmo = params;

  double hnorm=h_over_h0(a,cosmo, &status);
  double om=ccl_omega_x(cosmo, a, ccl_species_m_label, &status);

  double mu = ccl_mu_MG(cosmo, a, &status);
  dydt[1]=1.5*hnorm*a*om*y[0]*(1. + mu);

  dydt[0]=y[1]/(a*a*a*hnorm);

  return status;
}

/* --------- ROUTINE: df_integrand ---------
INPUT: scale factor, spline object
TASK: Compute integrand from modified growth function
*/
static double df_integrand(double a,void * spline_void)
{
  if(a<=0)
    return 0;
  else {
    gsl_spline *df_a_spline=(gsl_spline *)spline_void;

    return gsl_spline_eval(df_a_spline,a,NULL)/a;
  }
}

/* --------- ROUTINE: growth_factor_and_growth_rate ---------
INPUT: scale factor, cosmology
TASK: compute the growth (D(z)) and the growth rate, logarithmic derivative (f?)
*/

static int growth_factor_and_growth_rate(double a, double *gf, double *fg, ccl_cosmology *cosmo, int *stat)
{
  if(a < cosmo->gsl_params.EPS_SCALEFAC_GROWTH) {
    *gf = a;
    *fg = 1;
    return 0;
  }
  else {
    int gslstatus;
    double y[2];
    double ainit = cosmo->gsl_params.EPS_SCALEFAC_GROWTH;

    // if mu0 == 0, call normal growth_ode_system, otherwise call growth_ode_system_muSig
    if (cosmo->params.mu_0 > 1e-12 || cosmo->params.mu_0 < -1e-12) {
      gsl_odeiv2_system sys = {growth_ode_system_muSig, NULL, 2, cosmo};

      gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(
        &sys, gsl_odeiv2_step_rkck,
        0.1*cosmo->gsl_params.EPS_SCALEFAC_GROWTH, 0, cosmo->gsl_params.ODE_GROWTH_EPSREL);

      if (d == NULL) {
        return CCL_ERROR_MEMORY;
      }

      y[0] = cosmo->gsl_params.EPS_SCALEFAC_GROWTH;
      y[1] = (
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH *
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH *
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH *
        h_over_h0(cosmo->gsl_params.EPS_SCALEFAC_GROWTH, cosmo, stat));

      gslstatus = gsl_odeiv2_driver_apply(d, &ainit, a, y);
      gsl_odeiv2_driver_free(d);

      if(gslstatus != GSL_SUCCESS) {
        ccl_raise_gsl_warning(gslstatus, "ccl_background.c: growth_factor_and_growth_rate():");
        return 0;
      }

      *gf = y[0];
      *fg = y[1]/(a*a*h_over_h0(a, cosmo, stat)*y[0]);
      return 0;
    }
    else {
      gsl_odeiv2_system sys = {growth_ode_system, NULL, 2, cosmo};

      gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(
        &sys, gsl_odeiv2_step_rkck,
        0.1*cosmo->gsl_params.EPS_SCALEFAC_GROWTH, 0, cosmo->gsl_params.ODE_GROWTH_EPSREL);

      if (d == NULL) {
        return CCL_ERROR_MEMORY;
      }

      y[0] = cosmo->gsl_params.EPS_SCALEFAC_GROWTH;
      y[1] = (
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH *
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH *
        cosmo->gsl_params.EPS_SCALEFAC_GROWTH*
        h_over_h0(cosmo->gsl_params.EPS_SCALEFAC_GROWTH, cosmo, stat));

      gslstatus = gsl_odeiv2_driver_apply(d, &ainit, a, y);
      gsl_odeiv2_driver_free(d);

      if(gslstatus != GSL_SUCCESS) {
        ccl_raise_gsl_warning(gslstatus, "ccl_background.c: growth_factor_and_growth_rate():");
        return 0;
      }

      *gf = y[0];
      *fg = y[1]/(a*a*h_over_h0(a, cosmo, stat)*y[0]);

      return 0;
    }
  }
}

/* --------- ROUTINE: compute_chi ---------
INPUT: scale factor, cosmology
OUTPUT: chi -> radial comoving distance
TASK: compute radial comoving distance at a
*/
void compute_chi(double a, ccl_cosmology *cosmo, double * chi, int * stat)
{
  int gslstatus;
  double result;
  chipar p;

  p.cosmo=cosmo;
  p.status=stat;

  gsl_integration_cquad_workspace * workspace = NULL;

  gsl_function F;
  F.function = &chi_integrand;
  F.params = &p;

  workspace = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);

  if (workspace == NULL) {
      *stat = CCL_ERROR_MEMORY;
  } else {
    //TODO: CQUAD is great, but slower than other methods. This could be sped up if it becomes an issue.
    gslstatus=gsl_integration_cquad(
      &F, a, 1.0, 0.0, cosmo->gsl_params.INTEGRATION_DISTANCE_EPSREL, workspace, &result, NULL, NULL);
    *chi=result/cosmo->params.h;

    if (gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_background.c: compute_chi():");
      *stat = CCL_ERROR_COMPUTECHI;
    }
  }

  gsl_integration_cquad_workspace_free(workspace);
}

//Root finding for a(chi)
typedef struct {
  double chi;
  ccl_cosmology *cosmo;
  int * status;
} Fpar;

static double fzero(double a,void *params)
{
  double chi,chia,a_use=a;

  chi=((Fpar *)params)->chi;
  compute_chi(a_use,((Fpar *)params)->cosmo,&chia, ((Fpar *)params)->status);

  return chi-chia;
}

static double dfzero(double a,void *params)
{
  ccl_cosmology *cosmo=((Fpar *)params)->cosmo;
  int *stat = ((Fpar *)params)->status;

  chipar p;
  p.cosmo=cosmo;
  p.status=stat;

  return chi_integrand(a,&p)/cosmo->params.h;
}

static void fdfzero(double a,void *params,double *f,double *df)
{
  *f=fzero(a,params);
  *df=dfzero(a,params);
}

/* --------- ROUTINE: a_of_chi ---------
INPUT: comoving distance chi, cosmology, stat, a_old, gsl_root_fdfsolver
OUTPUT: scale factor
TASK: compute the scale factor that corresponds to a given comoving distance chi
Note: This routine uses a root solver to find an a such that compute_chi(a) = chi.
The root solver uses the derivative of compute_chi (which is chi_integrand) and
the value itself.
*/
static void a_of_chi(double chi, ccl_cosmology *cosmo, int* stat, double *a_old, gsl_root_fdfsolver *s)
{
  if(chi==0) {
    *a_old=1;
  }
  else {
    Fpar p;
    gsl_function_fdf FDF;
    double a_previous,a_current=*a_old;

    p.cosmo=cosmo;
    p.chi=chi;
    p.status=stat;
    FDF.f=&fzero;
    FDF.df=&dfzero;
    FDF.fdf=&fdfzero;
    FDF.params=&p;
    gsl_root_fdfsolver_set(s,&FDF,a_current);

    int iter=0, gslstatus;
    do {
      iter++;
      gslstatus=gsl_root_fdfsolver_iterate(s);
      if(gslstatus!=GSL_SUCCESS) ccl_raise_gsl_warning(gslstatus, "ccl_background.c: a_of_chi():");
      a_previous=a_current;
      a_current=gsl_root_fdfsolver_root(s);
      gslstatus=gsl_root_test_delta(a_current, a_previous, 0, cosmo->gsl_params.ROOT_EPSREL);
    } while(gslstatus==GSL_CONTINUE && iter <= cosmo->gsl_params.ROOT_N_ITERATION);

    *a_old=a_current;

    // Allows us to pass a status to h_over_h0 for the neutrino integral calculation.
    if(gslstatus==GSL_SUCCESS) {
      *stat = *(p.status);
    }
    else {
      ccl_raise_gsl_warning(gslstatus, "ccl_background.c: a_of_chi():");
      *stat = CCL_ERROR_COMPUTECHI;
    }
  }
}

/* ----- ROUTINE: ccl_cosmology_compute_distances ------
INPUT: cosmology
TASK: if not already there, make a table of comoving distances and of E(a)
*/

void ccl_cosmology_compute_distances(ccl_cosmology * cosmo, int *status)
{
  //Do nothing if everything is computed already
  if(cosmo->computed_distances)
    return;

  // Create logarithmically and then linearly-spaced values of the scale factor
  int na = cosmo->spline_params.A_SPLINE_NA+cosmo->spline_params.A_SPLINE_NLOG-1;
  double * a = ccl_linlog_spacing(
    cosmo->spline_params.A_SPLINE_MINLOG, cosmo->spline_params.A_SPLINE_MIN,
    cosmo->spline_params.A_SPLINE_MAX, cosmo->spline_params.A_SPLINE_NLOG,
    cosmo->spline_params.A_SPLINE_NA);
  // Allocate arrays for all three of E(a), chi(a), and a(chi)
  double *E_a = malloc(sizeof(double)*na);
  double *chi_a = malloc(sizeof(double)*na);
  // Allocate E(a) and chi(a) splines
  gsl_spline * E = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  gsl_spline * chi = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  // a(chi) spline allocated below

  //Check for too little memory
  if (a == NULL || E_a == NULL || chi_a == NULL || E == NULL || chi == NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(
      cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): ran out of memory\n");
  }

  //Check for messed up scale factor conditions
  if (!*status){
    if ((fabs(a[0]-cosmo->spline_params.A_SPLINE_MINLOG)>1e-5) ||
        (fabs(a[na-1]-cosmo->spline_params.A_SPLINE_MAX)>1e-5) ||
        (a[na-1]>1.0)) {
      *status = CCL_ERROR_LINSPACE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_cosmology_compute_distances(): Error "
        "creating first logarithmic and then linear spacing in a\n");
    }
  }

  // Fill in E(a) - note, this step cannot change the status variable
  if (!*status)
    for (int i=0; i<na; i++)
      E_a[i] = h_over_h0(a[i], cosmo, status);

  // Create a E(a) spline
  if (!*status){
    if (gsl_spline_init(E, a, E_a, na)){
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): Error creating  E(a) spline\n");
    }
  }

  // Compute chi(a)
  if (!*status){
    for (int i=0; i<na; i++)
      compute_chi(a[i], cosmo, &chi_a[i], status);
    if (*status){
      *status = CCL_ERROR_INTEG;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): chi(a) integration error \n");
    }
  }

  // Initialize chi(a) spline
  if (!*status){
    if (gsl_spline_init(chi, a, chi_a, na)){//in Mpc
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): Error creating  chi(a) spline\n");
    }
  }

  if (*status){ //If there was an error, free the GSL splines and return
    gsl_spline_free(E); // Note: you are allowed to call gsl_free() on NULL
    gsl_spline_free(chi);
    E = NULL;
    chi = NULL;
  }

  // Set up the boundaries for the a(chi) spline
  double dchi, chi0, chif, a0, af;
  if(!*status){
    dchi=5.;
    chi0=chi_a[na-1];
    chif=chi_a[0];
    a0=a[na-1];
    af=a[0];
  }

  //TODO: The interval in chi (5. Mpc) should be made a macro
  free(a); //Free these, in preparation for making a(chi) splines
  free(E_a);
  free(chi_a);
  a = NULL;
  E_a = NULL;
  chi_a = NULL;
  //Note: you are allowed to call free() on NULL

  ////////////////////////////////////////////////////////////////////////////////////////////////////
  //Below here na (length of some arrays) changes, so this function has to be split at this point.
  ////////////////////////////////////////////////////////////////////////////////////////////////////

  na = (int)((chif-chi0)/dchi);
  dchi  = (chif-chi0)/na; // <=5, since na is an integer
  //Allocate new arrays for a and chi(a)
  chi_a = ccl_linear_spacing(chi0, chif, na);
  a     = malloc(sizeof(double)*na);
  //Allocate space for GSL root finders
  const gsl_root_fdfsolver_type *T=gsl_root_fdfsolver_newton;
  gsl_root_fdfsolver *s = NULL;
  gsl_spline *achi;

  s = gsl_root_fdfsolver_alloc(T);
  achi = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);

  //Check for too little memory
  if (!*status){
    if (a == NULL || chi_a == NULL || s == NULL || achi == NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): ran out of memory\n");
    }
    else if (fabs(chi_a[0]-chi0) > 1e-5 || fabs(chi_a[na-1]-chif) > 1e-5) { //Check for messed up chi conditions
      *status = CCL_ERROR_LINSPACE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): Error creating linear spacing in chi\n");
    }
  }

  // Calculate a(chi)
  if (!*status){
    a[0]=a0; a[na-1]=af;
    for(int i=1;i<na-1;i++) {
      // we are using the previous value as a guess here to help the root finder
      // as long as we use small steps in a this should be fine
      a_of_chi(chi_a[i],cosmo, status, &a0, s);
      a[i]=a0;
    }
    if(*status) {
      *status = CCL_ERROR_ROOT;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): a(chi) root-finding error \n");
    }
  }

  // Initialize the a(chi) spline
  if (!*status){
    if(gsl_spline_init(achi, chi_a, a, na)){
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_distances(): Error creating  a(chi) spline\n");
    }
  }

  free(a);
  free(chi_a); //Note: you are allowed to call free() on NULL
  gsl_root_fdfsolver_free(s);
  if (*status){//If there was an error, free the GSL splines and return
    gsl_spline_free(E); //Note: you are allowed to call gsl_free() on NULL
    gsl_spline_free(chi);
    gsl_spline_free(achi);
  }

  if (*status == 0) {
    //If there were no errors, attach the splines to the cosmo struct and end the function.
    cosmo->data.E             = E;
    cosmo->data.chi           = chi;
    cosmo->data.achi          = achi;
    cosmo->computed_distances = true;
  }
}

/* ----- ROUTINE: ccl_cosmology_distances_from_input ------
INPUT: cosmology, scale factor array, comoving distance chi computed at the scale factor array values
TASK: if not already there, make a table of comoving distances from an input array
*/
void ccl_cosmology_distances_from_input(ccl_cosmology * cosmo, int na, double a[], double chi_a[], double E_a[],
   int *status)
{
  double *chi_a_reversed = NULL;
  double *a_reversed = NULL;

  //Do nothing if everything is computed already
  if(cosmo->computed_distances)
    return;

  // Allocate E(a), chi(a) and a(chi) splines
  gsl_spline * E = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  gsl_spline * chi = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  gsl_spline * achi = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);

  //Check for too little memory
  if (a == NULL || E_a == NULL || chi_a == NULL || E == NULL || chi == NULL || achi == NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(
      cosmo, "ccl_background.c: ccl_cosmology_distances_from_input(): ran out of memory\n");
  }

  // Update the minimum scale factor value used by the user, which is necessary because it is used in ccl_tracers.c
  cosmo->spline_params.A_SPLINE_MIN = a[0];

  // Initialize a E(a) spline
  if (!*status){
    if (gsl_spline_init(E, a, E_a, na)){
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_distances_from_input(): Error creating  E(a) spline\n");
    }
  }

  // Initialize chi(a) spline
  if (!*status){
    if (gsl_spline_init(chi, a, chi_a, na)){//in Mpc
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_distances_from_input(): Error creating  chi(a) spline\n");
    }
  }

  // Reverse the order of chi(a) so that we can initialize the a(chi) spline, which needs monotonically decreasing x-array.
  if (!*status) {
    chi_a_reversed = malloc(na*sizeof(double));
    a_reversed = malloc(na*sizeof(double));
    if (chi_a_reversed == NULL || a_reversed == NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_distances_from_input(): ran out of memory\n");
    }
    else {
      for (int i=0; i<na; i++) {
        chi_a_reversed[i] = chi_a[na-1-i];
        a_reversed[i] = a[na-1-i];
      }
    }
  }

  // Initialize a(chi) spline
  if (!*status){
    if (gsl_spline_init(achi, chi_a_reversed, a_reversed, na)){
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_distances_from_input(): Error creating  chi(a) spline\n");
    }
  }
  free(chi_a_reversed);
  free(a_reversed);

  if (*status){ //If there was an error, free the GSL splines and return
    gsl_spline_free(E); // Note: you are allowed to call gsl_free() on NULL
    gsl_spline_free(chi);
    gsl_spline_free(achi);
    E = NULL;
    chi = NULL;
    achi = NULL;
  }

  if (*status == 0) {
    //If there were no errors, attach the splines to the cosmo struct and end the function.
    cosmo->data.E             = E;
    cosmo->data.chi           = chi;
    cosmo->data.achi          = achi;
    cosmo->computed_distances = true;
  }

  return;
}

/* ----- ROUTINE: ccl_cosmology_growth_from_input ------
INPUT: cosmology, scale factor array, growth array, growth rate array
TASK: if not already there, create growth splines with the input arrays and store them.
*/
void ccl_cosmology_growth_from_input(ccl_cosmology* cosmo, int na, double a[],
                                     double growth_arr[], double fgrowth_arr[], int* status)
{
  int chistatus;
  if (cosmo->computed_growth)
    return;

  double *growth_normed = NULL;
  gsl_spline * growth = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  gsl_spline * fgrowth = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
  //The last element corresponds to a=1 (which is checked for in python).
  double growth0 = growth_arr[na-1];

  if (growth == NULL || fgrowth == NULL) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_growth_from_input(): ran out of memory\n");
  }

  // Need to normalize the growth factor
  if (*status == 0) {
    growth_normed = malloc(na*sizeof(double));
    if (growth_normed == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
           cosmo, "ccl_background.c: ccl_cosmology_growth_from_input(): ran out of memory\n");
    }
  }

  if (*status == 0) {
    for(int ia=0; ia<na; ia++)
      growth_normed[ia] = growth_arr[ia]/growth0;

    chistatus = gsl_spline_init(growth, a, growth_normed, na);
    if (chistatus) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_growth_from_input(): Error creating D(a) spline\n");
    }
  }

  if (*status == 0) {
    chistatus = gsl_spline_init(fgrowth, a, fgrowth_arr, na);
    if (chistatus) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_growth_from_input(): Error creating f(a) spline\n");
    }
  }

  if (*status){ //If there was an error, free the GSL splines and return
    gsl_spline_free(growth);
    gsl_spline_free(fgrowth);
    growth = NULL;
    fgrowth = NULL;
  }

  if (*status == 0) {
    // assign all the splines we've just made to the structure.
    cosmo->data.growth = growth;
    cosmo->data.fgrowth = fgrowth;
    cosmo->data.growth0 = growth0;
    cosmo->computed_growth = true;
  }

  free(growth_normed);
}

/* ----- ROUTINE: ccl_cosmology_compute_growth ------
INPUT: cosmology
TASK: if not already there, make a table of growth function and growth rate
      normalize growth to input parameter growth0
*/
void ccl_cosmology_compute_growth(ccl_cosmology* cosmo, int* status)
{
  if (cosmo->computed_growth)
    return;

  // Create logarithmically and then linearly-spaced values of the scale factor
  int chistatus = 0, na = cosmo->spline_params.A_SPLINE_NA+cosmo->spline_params.A_SPLINE_NLOG-1;
  double *a = NULL;
  gsl_integration_cquad_workspace * workspace = NULL;
  gsl_function F;
  gsl_spline *df_a_spline = NULL;
  double *df_arr = NULL;
  gsl_spline *df_z_spline = NULL;
  int status_mg = 0, gslstatus;
  double growth0, fgrowth0;
  double *y = NULL;
  double *y2 = NULL;
  double df, integ;

  if (*status == 0) {
    a = ccl_linlog_spacing(
      cosmo->spline_params.A_SPLINE_MINLOG, cosmo->spline_params.A_SPLINE_MIN,
      cosmo->spline_params.A_SPLINE_MAX, cosmo->spline_params.A_SPLINE_NLOG,
      cosmo->spline_params.A_SPLINE_NA);

    if (a == NULL ||
        (fabs(a[0]-cosmo->spline_params.A_SPLINE_MINLOG) > 1e-5) ||
        (fabs(a[na-1]-cosmo->spline_params.A_SPLINE_MAX) > 1e-5) ||
        (a[na-1] > 1.0)) {
      *status = CCL_ERROR_LINSPACE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_cosmology_compute_growth(): "
        "Error creating logarithmically and then linear spacing in a\n");
    }
  }

  if(cosmo->params.has_mgrowth) {
    if (*status == 0) {
      df_arr = malloc(na*sizeof(double));

      if(df_arr == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
      }
    }

    // Generate spline for Delta f(z) that we will then interpolate into an array of a
    if (*status == 0) {
      df_z_spline = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, cosmo->params.nz_mgrowth);
      if (df_z_spline == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
      }
    }

    if (*status == 0) {
      chistatus = gsl_spline_init(df_z_spline,cosmo->params.z_mgrowth,
                                  cosmo->params.df_mgrowth,
                                  cosmo->params.nz_mgrowth);

      if(chistatus) {
        *status = CCL_ERROR_SPLINE;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): Error creating Delta f(z) spline\n");
      }
    }

    if (*status == 0) {
      for (int i=0; i<na; i++) {
        if(a[i]>0) {
          double z=1./a[i]-1.;

          if(z<=cosmo->params.z_mgrowth[0])
            df_arr[i]=cosmo->params.df_mgrowth[0];
          else if(z>cosmo->params.z_mgrowth[cosmo->params.nz_mgrowth-1])
            df_arr[i]=cosmo->params.df_mgrowth[cosmo->params.nz_mgrowth-1];
          else
              chistatus |= gsl_spline_eval_e(df_z_spline,z,NULL,&df_arr[i]);
        } else {
            df_arr[i]=0;
        }
      }
      if(chistatus) {
        *status = CCL_ERROR_SPLINE;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): Error evaluating Delta f(z) spline\n");
      }
    }

    // Generate Delta(f) spline
    if (*status == 0) {
      df_a_spline = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE,na);

      if (df_a_spline == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
      }
    }

    if (*status == 0) {
      chistatus = gsl_spline_init(df_a_spline, a, df_arr, na);

      if (chistatus) {
        *status = CCL_ERROR_SPLINE;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): Error creating Delta f(a) spline\n");
      }
    }

    if (*status == 0) {
      workspace = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
      F.function = &df_integrand;
      F.params = df_a_spline;
    }
  }

  // allocate space for y, which will be all three
  // of E(a), chi(a), D(a) and f(a) in turn.
  if (*status == 0) {
    y = malloc(sizeof(double)*na);
    if (y == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
    }
  }

  if (*status == 0) {
    y2 = malloc(sizeof(double)*na);
    if (y2 == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
    }
  }

  if (*status == 0) {
    // Get the growth factor and growth rate at z=0
    chistatus |= growth_factor_and_growth_rate(1., &growth0, &fgrowth0, cosmo, status);

    // Get the growth factor and growth rate at other redshifts
    for(int i=0; i<na; i++) {
      chistatus |= growth_factor_and_growth_rate(a[i], &(y[i]), &(y2[i]), cosmo, status);

      if(cosmo->params.has_mgrowth) {
        if(a[i]>0) {
          // Add modification to f
          gslstatus = gsl_spline_eval_e(df_a_spline, a[i], NULL, &df);
          if (gslstatus != GSL_SUCCESS) {
            ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_cosmology_compute_growth():");
            status_mg |= gslstatus;
          }
          y2[i] += df;

          // Multiply D by exp(-int(df))
          gslstatus = gsl_integration_cquad(
            &F, a[i], 1.0, 0.0, cosmo->gsl_params.INTEGRATION_DISTANCE_EPSREL,
            workspace, &integ, NULL, NULL);
          if (gslstatus != GSL_SUCCESS) {
            ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_cosmology_compute_growth():");
            status_mg |= gslstatus;
          }
          y[i] *= exp(-integ);
        }
      }
      // Normalizing to the growth factor to the growth today
      y[i] /= growth0;
    }

    if (chistatus || status_mg || *status) {
      if (chistatus) {
        *status = CCL_ERROR_INTEG;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): integral for linear growth factor didn't converge\n");
      }
      if(status_mg) {
        *status = CCL_ERROR_INTEG;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): integral for MG growth factor didn't converge\n");
      }
    }
  }

  gsl_spline *growth = NULL;
  gsl_spline *fgrowth = NULL;

  if (*status == 0) {
    growth = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);
    fgrowth = gsl_spline_alloc(cosmo->spline_params.A_SPLINE_TYPE, na);

    if (growth == NULL || fgrowth == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): ran out of memory\n");
    }
  }

  if (*status == 0) {
    chistatus = gsl_spline_init(growth, a, y, na);

    if (chistatus) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): Error creating D(a) spline\n");
    }
  }

  if (*status == 0) {
    chistatus = gsl_spline_init(fgrowth, a, y2, na);
    if (chistatus) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_cosmology_compute_growth(): Error creating f(a) spline\n");
    }
  }

  if (*status == 0) {
    // assign all the splines we've just made to the structure.
    cosmo->data.growth = growth;
    cosmo->data.fgrowth = fgrowth;
    cosmo->data.growth0 = growth0;
    cosmo->computed_growth = true;
  }
  else {
    gsl_spline_free(growth);
    gsl_spline_free(fgrowth);
  }

  free(a);
  free(y);
  free(y2);
  free(df_arr);
  gsl_spline_free(df_z_spline);
  gsl_spline_free(df_a_spline);
  gsl_integration_cquad_workspace_free(workspace);
}

//Expansion rate normalized to 1 today

double ccl_h_over_h0(ccl_cosmology * cosmo, double a, int* status)
{

  if(!cosmo->computed_distances) {
    *status = CCL_ERROR_DISTANCES_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_background.c: ccl_h_over_h0(): distance splines have not been precomputed!");
    return NAN;
  }

  double h_over_h0;
  int gslstatus = gsl_spline_eval_e(cosmo->data.E, a, NULL, &h_over_h0);
  if(gslstatus != GSL_SUCCESS) {
    ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_h_over_h0():");
    *status = gslstatus;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: ccl_h_over_h0(): Scale factor outside interpolation range.\n");
    return NAN;
  }

  return h_over_h0;
}

void ccl_h_over_h0s(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_h_over_h0(cosmo, a[i], &_status);
    *status |= _status;
  }
}

// Distance-like function examples, all in Mpc
double ccl_comoving_radial_distance(ccl_cosmology * cosmo, double a, int * status)
{
  if((a > (1.0 - 1.e-8)) && (a<=1.0)) {
    return 0.;
  }
  else if(a>1.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: scale factor cannot be larger than 1.\n");
    return NAN;
  }
  else {
    if(!cosmo->computed_distances) {
      *status = CCL_ERROR_DISTANCES_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_comoving_radial_distance(): distance splines have not been precomputed!");
      return NAN;
    }

    double crd;
    int gslstatus = gsl_spline_eval_e(cosmo->data.chi, a, NULL, &crd);
    if(gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_comoving_radial_distance():");
      *status = gslstatus;
      ccl_cosmology_set_status_message(
        cosmo, "ccl_background.c: ccl_comoving_radial_distance(): Scale factor outside interpolation range.\n");
      return NAN;
    }
    return crd;
  }
}

void ccl_comoving_radial_distances(ccl_cosmology * cosmo, int na, double a[], double output[], int* status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_comoving_radial_distance(cosmo, a[i], &_status);
    *status |= _status;
  }
}

double ccl_sinn(ccl_cosmology *cosmo, double chi, int *status)
{
  //////
  //         { sin(x)  , if k==1
  // sinn(x)={  x      , if k==0
  //         { sinh(x) , if k==-1
  switch(cosmo->params.k_sign) {
  case -1:
    return sinh(cosmo->params.sqrtk * chi) / cosmo->params.sqrtk;
  case 1:
    return sin(cosmo->params.sqrtk*chi) / cosmo->params.sqrtk;
  case 0:
    return chi;
  default:
    *status = CCL_ERROR_PARAMETERS;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: ccl_sinn: ill-defined cosmo->params.k_sign = %d",
                                     cosmo->params.k_sign);
    return NAN;
  }
}

double ccl_comoving_angular_distance(ccl_cosmology * cosmo, double a, int* status)
{
  if((a > (1.0 - 1.e-8)) && (a<=1.0)) {
    return 0.;
  }
  else if(a>1.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: scale factor cannot be larger than 1.\n");
    return NAN;
  }
  else {
    if (!cosmo->computed_distances) {
      *status = CCL_ERROR_DISTANCES_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_comoving_angular_distance(): distance splines have not been precomputed!");
      return NAN;
    }

    double chi;
    int gslstatus = gsl_spline_eval_e(cosmo->data.chi, a, NULL, &chi);
    if(gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_comoving_angular_distance():");
      *status |= gslstatus;
      ccl_cosmology_set_status_message(cosmo, "ccl_background.c: ccl_comoving_angular_distance(): Scale factor outside interpolation range.\n");
      return NAN;
    }
    return ccl_sinn(cosmo,chi,status);
  }
}

void ccl_comoving_angular_distances(ccl_cosmology * cosmo, int na, double a[],
                                    double output[], int* status)
{
  int _status;

  for (int i=0; i < na; i++) {
    _status = 0;
    output[i] = ccl_comoving_angular_distance(cosmo, a[i], &_status);
    *status |= _status;
  }
}

double ccl_angular_diameter_distance(ccl_cosmology * cosmo, double a1, double a2, int* status)
{
  if(a1>1. || a2>1. || a1<a2) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo,"ccl_background.c: ccl_angular_diameter_distance(): error on input scale factor.");
    return NAN;
  } else {
      if (!cosmo->computed_distances) {
          *status = CCL_ERROR_DISTANCES_INIT;
          ccl_cosmology_set_status_message(cosmo,"ccl_background.c: ccl_angular_diameter_distance(): distance splines have not been precomputed!");
          return NAN;
      }
      double chi1,chi2;
      int gslstatus = gsl_spline_eval_e(cosmo->data.chi, a1, NULL, &chi1);
      if(gslstatus != GSL_SUCCESS) {
          ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_angular_diameter_distance():");
          *status |= gslstatus;
          ccl_cosmology_set_status_message(cosmo,"ccl_background.c: ccl_angular_diameter_distance(): Scale factor outside interpolation range.\n");
          return NAN;
      }
      gslstatus = gsl_spline_eval_e(cosmo->data.chi, a2, NULL, &chi2);
      if(gslstatus != GSL_SUCCESS) {
          ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_angular_diameter_distance():");
          *status |= gslstatus;
          ccl_cosmology_set_status_message(cosmo,"ccl_background.c: ccl_angular_diameter_distance(): Scale factor outside interpolation range.\n");
          return NAN;
      }
      return a2*ccl_sinn(cosmo,chi2-chi1,status);
  }
}

void ccl_angular_diameter_distances(ccl_cosmology * cosmo, int na, double a1[], double a2[],
                                    double output[], int* status)
{
  int _status;

  for (int i=0; i < na; i++) {
    _status = 0;
    output[i] = ccl_angular_diameter_distance(cosmo, a1[i], a2[i], &_status);
    *status |= _status;
  }
}

double ccl_luminosity_distance(ccl_cosmology * cosmo, double a, int* status)
{
  return ccl_comoving_angular_distance(cosmo, a, status) / a;
}

void ccl_luminosity_distances(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_luminosity_distance(cosmo, a[i], &_status);
    *status |= _status;
  }
}

double ccl_distance_modulus(ccl_cosmology * cosmo, double a, int* status)
{
  if((a > (1.0 - 1.e-8)) && (a<=1.0)) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: distance_modulus undefined for a=1.\n");
    return NAN;
  } else if(a>1.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: scale factor cannot be larger than 1.\n");
    return NAN;
  } else {
    if (!cosmo->computed_distances) {
      *status = CCL_ERROR_DISTANCES_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_distance_modulus(): distance splines have not been precomputed!");
      return NAN;
    }
    /* distance modulus = 5 * log10(d) - 5
       Since d in CCL is in Mpc, you get
         5*log10(10^6) - 5 = 30 - 5 = 25
      for the constant.
    */
    return 5 * log10(ccl_luminosity_distance(cosmo, a, status)) + 25;
  }
}

void ccl_distance_moduli(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_distance_modulus(cosmo, a[i], &_status);
    *status |= _status;
  }
}

//Scale factor for a given distance
double ccl_scale_factor_of_chi(ccl_cosmology * cosmo, double chi, int * status)
{
  if((chi < 1.e-8) && (chi>=0.)) {
    return 1.;
  }
  else if(chi<0.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: distance cannot be smaller than 0.\n");
    return NAN;
  }
  else {
    if (!cosmo->computed_distances) {
      *status = CCL_ERROR_DISTANCES_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_scale_factor_of_chi(): distance splines have not been precomputed!");
      return NAN;
    }
    double a;
    int gslstatus = gsl_spline_eval_e(cosmo->data.achi, chi, NULL, &a);
    if(gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_scale_factor_of_chi():");
      *status |= gslstatus;
    }
    return a;
  }
}

void ccl_scale_factor_of_chis(ccl_cosmology * cosmo, int nchi, double chi[], double output[], int * status)
{
  int _status;

  for (int i=0; i<nchi; i++) {
    _status = 0;
    output[i] = ccl_scale_factor_of_chi(cosmo, chi[i], &_status);
    *status |= _status;
  }
}

double ccl_growth_factor(ccl_cosmology * cosmo, double a, int * status)
{
  if(a==1.){
    return 1.;
  }
  else if(a>1.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(
      cosmo, "ccl_background.c: scale factor cannot be larger than 1.");
    return NAN;
  }
  else {
    if (!cosmo->computed_growth){
      *status = CCL_ERROR_GROWTH_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_growth_factor(): growth factor splines have not been precomputed!");
      return NAN;
    }
    if (*status != CCL_ERROR_NOT_IMPLEMENTED) {
      double D;
      int gslstatus = gsl_spline_eval_e(cosmo->data.growth, a, NULL, &D);
      if(gslstatus != GSL_SUCCESS) {
        ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_growth_factor():");
        *status |= gslstatus;
        ccl_cosmology_set_status_message(
          cosmo, "ccl_background.c: ccl_growth_factor(): Scale factor outside interpolation range.");
        return NAN;
      }
      return D;
    }
    else {
      return NAN;
    }
  }
}

void ccl_growth_factors(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_growth_factor(cosmo, a[i], &_status);
    *status |= _status;
  }
}

double ccl_growth_factor_unnorm(ccl_cosmology * cosmo, double a, int * status)
{

  if (!cosmo->computed_growth) {
    *status = CCL_ERROR_GROWTH_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_background.c: ccl_growth_factor_unnorm(): growth factor splines have not been precomputed!");
  }

  if (*status != CCL_ERROR_NOT_IMPLEMENTED) {
      return cosmo->data.growth0 * ccl_growth_factor(cosmo, a, status);
  } else {
    return NAN;
  }
}

void ccl_growth_factors_unnorm(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_growth_factor_unnorm(cosmo, a[i], &_status);
    *status |= _status;
  }
}

double ccl_growth_rate(ccl_cosmology * cosmo, double a, int * status)
{
  if(a>1.) {
    *status = CCL_ERROR_COMPUTECHI;
    ccl_cosmology_set_status_message(cosmo, "ccl_background.c: scale factor cannot be larger than 1.\n");
    return NAN;
  } else {
    if (!cosmo->computed_growth) {
      *status = CCL_ERROR_GROWTH_INIT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_background.c: ccl_growth_rate(): growth rate splines have not been precomputed!");
    }
    if(*status != CCL_ERROR_NOT_IMPLEMENTED) {
      double g;
      int gslstatus = gsl_spline_eval_e(cosmo->data.fgrowth, a, NULL ,&g);
      if(gslstatus != GSL_SUCCESS) {
        ccl_raise_gsl_warning(gslstatus, "ccl_background.c: ccl_growth_rate():");
        *status |= gslstatus;
        ccl_cosmology_set_status_message(cosmo, "ccl_background.c: ccl_growth_rate(): Scale factor outside interpolation range.\n");
        return NAN;
      }
      return g;
    } else {
        return NAN;
    }
  }
}

void ccl_growth_rates(ccl_cosmology * cosmo, int na, double a[], double output[], int * status)
{
  int _status;

  for (int i=0; i<na; i++) {
    _status = 0;
    output[i] = ccl_growth_rate(cosmo, a[i], &_status);
    *status |= _status;
  }
}


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The F distribution has the form

   p(x) dx = (nu1^(nu1/2) nu2^(nu2/2) Gamma((nu1 + nu2)/2) /
   Gamma(nu1/2) Gamma(nu2/2)) *
   x^(nu1/2 - 1) (nu2 + nu1 * x)^(-nu1/2 -nu2/2) dx

   The method used here is the one described in Knuth */

double
gsl_ran_fdist (const gsl_rng * r, const double nu1, const double nu2)
{

  double Y1 =  gsl_ran_gamma (r, nu1 / 2, 2.0);
  double Y2 =  gsl_ran_gamma (r, nu2 / 2, 2.0);

  double f = (Y1 * nu2) / (Y2 * nu1);

  return f;
}

double
gsl_ran_fdist_pdf (const double x, const double nu1, const double nu2)
{
  if (x < 0)
    {
      return 0 ;
    }
  else
    {
      double p;
      double lglg = (nu1 / 2) * log (nu1) + (nu2 / 2) * log (nu2) ;

      double lg12 = gsl_sf_lngamma ((nu1 + nu2) / 2);
      double lg1 = gsl_sf_lngamma (nu1 / 2);
      double lg2 = gsl_sf_lngamma (nu2 / 2);

      p = exp (lglg + lg12 - lg1 - lg2)
	* pow (x, nu1 / 2 - 1) * pow (nu2 + nu1 * x, -nu1 / 2 - nu2 / 2);

      return p;
    }
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gbpLib.h>
#include <gbpInterpolate.h>

double interpolate_integral(interp_info *interp, double x_lo, double x_hi) {
    double x_lo_interp;
    double x_hi_interp;
    double x_lo_tmp;
    double x_hi_tmp;
    double x_min;
    double x_max;
    double y_x_min;
    double y_x_max;
    double r_val;
    double alpha;
    int    flag_error = GBP_FALSE;

    x_min   = ((interp_info *)interp)->x[0];
    x_max   = ((interp_info *)interp)->x[((interp_info *)interp)->n - 1];
    y_x_min = ((interp_info *)interp)->y[0];
    y_x_max = ((interp_info *)interp)->y[((interp_info *)interp)->n - 1];

    r_val       = 0.;
    x_lo_interp = x_lo;
    x_hi_interp = x_hi;

    if(x_hi > x_lo) {
        // Extrapolate integral to low-x assuming dI prop. to x^alpha
        if(x_lo < x_min) {
            x_lo_tmp = x_lo;
            x_hi_tmp = GBP_MIN(x_hi, x_min);
            alpha    = interpolate_derivative(interp, ((interp_info *)interp)->x[0]);
            if(alpha < -1. && x_lo_tmp * x_hi_tmp == 0.) {
                fprintf(stderr, "ERROR: integration extrapolation to low-x is divergent! (alpha=%lf)\n", alpha);
                flag_error = GBP_TRUE;
            } else
                r_val += (y_x_min / (alpha + 1.)) * (pow(x_hi_tmp / x_min, alpha + 1.) - pow(x_lo_tmp / x_min, alpha + 1.));
            x_lo_interp = x_min;
        }

        // Extrapolate integral to high-x assuming dI prop. to x^alpha
        if(x_hi > x_max) {
            x_lo_tmp = GBP_MAX(x_max, x_lo);
            x_hi_tmp = x_hi;
            alpha    = interpolate_derivative(interp, ((interp_info *)interp)->x[((interp_info *)interp)->n - 1]);
            if(alpha < 0. && x_lo_tmp * x_hi_tmp == 0.) {
                fprintf(stderr, "ERROR: integration extrapolation to high-x is divergent! (alpha=%lf)\n", alpha);
                flag_error = GBP_TRUE;
            } else
                r_val += (y_x_min / (alpha + 1.)) * (pow(x_hi_tmp / x_min, alpha + 1.) - pow(x_lo_tmp / x_min, alpha + 1.));
            x_hi_interp = x_max;
        }

        r_val += gsl_interp_eval_integ(((interp_info *)interp)->interp,
                                       ((interp_info *)interp)->x,
                                       ((interp_info *)interp)->y,
                                       x_lo_interp,
                                       x_hi_interp,
                                       ((interp_info *)interp)->accel);
    }
    return (r_val);
}


/* randist/gauss.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2006 James Theiler, Brian Gough
 * Copyright (C) 2006 Charles Karney
 *
 * 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.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Of the two methods provided below, I think the Polar method is more
 * efficient, but only when you are actually producing two random
 * deviates.  We don't produce two, because then we'd have to save one
 * in a static variable for the next call, and that would screws up
 * re-entrant or threaded code, so we only produce one.  This makes
 * the Ratio method suddenly more appealing.
 *
 * [Added by Charles Karney] We use Leva's implementation of the Ratio
 * method which avoids calling log() nearly all the time and makes the
 * Ratio method faster than the Polar method (when it produces just one
 * result per call).  Timing per call (gcc -O2 on 866MHz Pentium,
 * average over 10^8 calls)
 *
 *   Polar method: 660 ns
 *   Ratio method: 368 ns
 *
 */

/* Polar (Box-Mueller) method; See Knuth v2, 3rd ed, p122 */

double
gsl_ran_gaussian (const gsl_rng * r, const double sigma)
{
  double x, y, r2;

  do
    {
      /* choose x,y in uniform square (-1,-1) to (+1,+1) */
      x = -1 + 2 * gsl_rng_uniform_pos (r);
      y = -1 + 2 * gsl_rng_uniform_pos (r);

      /* see if it is in the unit circle */
      r2 = x * x + y * y;
    }
  while (r2 > 1.0 || r2 == 0);

  /* Box-Muller transform */
  return sigma * y * sqrt (-2.0 * log (r2) / r2);
}

/* Ratio method (Kinderman-Monahan); see Knuth v2, 3rd ed, p130.
 * K+M, ACM Trans Math Software 3 (1977) 257-260.
 *
 * [Added by Charles Karney] This is an implementation of Leva's
 * modifications to the original K+M method; see:
 * J. L. Leva, ACM Trans Math Software 18 (1992) 449-453 and 454-455. */

double
gsl_ran_gaussian_ratio_method (const gsl_rng * r, const double sigma)
{
  double u, v, x, y, Q;
  const double s = 0.449871;    /* Constants from Leva */
  const double t = -0.386595;
  const double a = 0.19600;
  const double b = 0.25472;
  const double r1 = 0.27597;
  const double r2 = 0.27846;

  do                            /* This loop is executed 1.369 times on average  */
    {
      /* Generate a point P = (u, v) uniform in a rectangle enclosing
         the K+M region v^2 <= - 4 u^2 log(u). */

      /* u in (0, 1] to avoid singularity at u = 0 */
      u = 1 - gsl_rng_uniform (r);

      /* v is in the asymmetric interval [-0.5, 0.5).  However v = -0.5
         is rejected in the last part of the while clause.  The
         resulting normal deviate is strictly symmetric about 0
         (provided that v is symmetric once v = -0.5 is excluded). */
      v = gsl_rng_uniform (r) - 0.5;

      /* Constant 1.7156 > sqrt(8/e) (for accuracy); but not by too
         much (for efficiency). */
      v *= 1.7156;

      /* Compute Leva's quadratic form Q */
      x = u - s;
      y = fabs (v) - t;
      Q = x * x + y * (a * y - b * x);

      /* Accept P if Q < r1 (Leva) */
      /* Reject P if Q > r2 (Leva) */
      /* Accept if v^2 <= -4 u^2 log(u) (K+M) */
      /* This final test is executed 0.012 times on average. */
    }
  while (Q >= r1 && (Q > r2 || v * v > -4 * u * u * log (u)));

  return sigma * (v / u);       /* Return slope */
}

double
gsl_ran_gaussian_pdf (const double x, const double sigma)
{
  double u = x / fabs (sigma);
  double p = (1 / (sqrt (2 * M_PI) * fabs (sigma))) * exp (-u * u / 2);
  return p;
}

double
gsl_ran_ugaussian (const gsl_rng * r)
{
  return gsl_ran_gaussian (r, 1.0);
}

double
gsl_ran_ugaussian_ratio_method (const gsl_rng * r)
{
  return gsl_ran_gaussian_ratio_method (r, 1.0);
}

double
gsl_ran_ugaussian_pdf (const double x)
{
  return gsl_ran_gaussian_pdf (x, 1.0);
}


//This is a simple example meant to test your capabilities with basic library linking functionality. All you have to do is return the output to modelica.

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

double sf_bessel(double x)
{
	double y = gsl_sf_bessel_J0 (x);
	return y;
}

//The main function has been provided solely for the sake of testing the function. It will be removed when the problem set is formally passed on.
/*
int main(void)
{
	double x = 5;
	double a = sf_bessel(x);

	printf("a = %f \n",a);
	return 0;
}
*/


#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <float.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <cblas.h>

/* Your function must have the following signature: */

void sgemm( int m, int n, float *A, float *C );

/* The benchmarking program */

int main( int argc, char **argv )
{
	srand(time(NULL));

	if(argc != 3)
	{
		printf("Incorrect number of arguments!\n");
		printf("Usage: ./bench-test M N\n");
		exit(0);
	}

	int m  = atoi(argv[1]);
	int n  = atoi(argv[2]);

	// Change these values to modify range checking
	if(m < 1000 || m > 10000 || n < 32 || n > 10000)
	{
		printf("Dimensions provided are out of the range of matrices we will be testing!  Change tester.c to modify range checking.\n");
		exit(0);
	}

	/* Allocate and fill 2 random matrices A, C */
	float *A = (float*) malloc( m * n * sizeof(float) );
	float *C = (float*) malloc( m * m * sizeof(float) );

	for( int i = 0; i < m*n; i++ ) A[i] = 2 * drand48() - 1;
	for( int i = 0; i < m*m; i++ ) C[i] = 2 * drand48() - 1;

	/* measure Gflop/s rate; time a sufficiently long sequence of calls to eliminate noise */
	double Gflop_s, seconds = -1.0;
	for( int n_iterations = 1; seconds < 0.1; n_iterations *= 2 )
	{
		/* warm-up */
		sgemm( m, n, A, C );

		/* measure time */
		struct timeval start, end;
		gettimeofday( &start, NULL );
		for( int i = 0; i < n_iterations; i++ )
			sgemm( m,n, A, C );
		gettimeofday( &end, NULL );
		seconds = (end.tv_sec - start.tv_sec) + 1.0e-6 * (end.tv_usec - start.tv_usec);

		/* compute Gflop/s rate */
		Gflop_s = 2e-9 * n_iterations * m * m * n / seconds;
	}

	printf( "%d by %d matrix \t %g Gflop/s\n", m, n, Gflop_s );

	/* Ensure that error does not exceed the theoretical error bound */

	/* Set initial C to 0 and do matrix multiply of A*B */
	memset( C, 0, sizeof( float ) * m * m );
	sgemm( m,n, A, C );

	/* Subtract A*B from C using standard sgemm (note that this should be 0 to within machine roundoff) */
	cblas_sgemm( CblasColMajor,CblasNoTrans,CblasTrans, m,m,n, -1, A,m, A,m, 1, C,m );

	/* Subtract the maximum allowed roundoff from each element of C */
	for( int i = 0; i < m*n; i++ ) A[i] = fabs( A[i] );
	for( int i = 0; i < m*m; i++ ) C[i] = fabs( C[i] );
	cblas_sgemm( CblasColMajor,CblasNoTrans,CblasTrans, m,m,n, -3.0*FLT_EPSILON*n, A,m, A,m, 1, C,m );

	/* After this test if any element in C is still positive something went wrong in square_sgemm */
	for( int i = 0; i < m * m; i++ )
	  	if( C[i] > 0 ) {
			printf( "FAILURE: error in matrix multiply exceeds an acceptable margin\n" );
		return -1;
	}

	/* release memory */
	free( C );
	free( A );
	return 0;
}