#include <petsc.h>
#include "poissonfunctions.h"

PetscErrorCode Poisson1DFunctionLocal(DMDALocalInfo *info, double *au,
                                   double *aF, PoissonCtx *user) {
    PetscErrorCode ierr;
    int          i;
    double       xmax[1], xmin[1], h, x, ue, uw;
    ierr = DMDAGetBoundingBox(info->da,xmin,xmax); CHKERRQ(ierr);
    h = (xmax[0] - xmin[0]) / (info->mx - 1);
    for (i = info->xs; i < info->xs + info->xm; i++) {
        x = xmin[0] + i * h;
        if (i==0 || i==info->mx-1) {
            aF[i] = au[i] - user->g_bdry(x,0.0,0.0,user);
            aF[i] *= user->cx * (2.0 / h);
        } else {
            ue = (i+1 == info->mx-1) ? user->g_bdry(x+h,0.0,0.0,user)
                                     : au[i+1];
            uw = (i-1 == 0)          ? user->g_bdry(x-h,0.0,0.0,user)
                                     : au[i-1];
            aF[i] = user->cx * (2.0 * au[i] - uw - ue) / h
                    - h * user->f_rhs(x,0.0,0.0,user);
        }
    }
    ierr = PetscLogFlops(9.0*info->xm);CHKERRQ(ierr);
    return 0;
}

//STARTFORM2DFUNCTION
PetscErrorCode Poisson2DFunctionLocal(DMDALocalInfo *info, double **au,
                                   double **aF, PoissonCtx *user) {
    PetscErrorCode ierr;
    int     i, j;
    double  xymin[2], xymax[2], hx, hy, darea, scx, scy, scdiag, x, y,
            ue, uw, un, us;
    ierr = DMDAGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    darea = hx * hy;
    scx = user->cx * hy / hx;
    scy = user->cy * hx / hy;
    scdiag = 2.0 * (scx + scy);    // diagonal scaling
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = xymin[1] + j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = xymin[0] + i * hx;
            if (i==0 || i==info->mx-1 || j==0 || j==info->my-1) {
                aF[j][i] = au[j][i] - user->g_bdry(x,y,0.0,user);
                aF[j][i] *= scdiag;
            } else {
                ue = (i+1 == info->mx-1) ? user->g_bdry(x+hx,y,0.0,user)
                                         : au[j][i+1];
                uw = (i-1 == 0)          ? user->g_bdry(x-hx,y,0.0,user)
                                         : au[j][i-1];
                un = (j+1 == info->my-1) ? user->g_bdry(x,y+hy,0.0,user)
                                         : au[j+1][i];
                us = (j-1 == 0)          ? user->g_bdry(x,y-hy,0.0,user)
                                         : au[j-1][i];
                aF[j][i] = scdiag * au[j][i]
                           - scx * (uw + ue) - scy * (us + un)
                           - darea * user->f_rhs(x,y,0.0,user);
            }
        }
    }
    ierr = PetscLogFlops(11.0*info->xm*info->ym);CHKERRQ(ierr);
    return 0;
}
//ENDFORM2DFUNCTION

PetscErrorCode Poisson3DFunctionLocal(DMDALocalInfo *info, double ***au,
                                   double ***aF, PoissonCtx *user) {
    PetscErrorCode ierr;
    int    i, j, k;
    double xyzmin[3], xyzmax[3], hx, hy, hz, dvol, scx, scy, scz, scdiag,
           x, y, z, ue, uw, un, us, uu, ud;
    ierr = DMDAGetBoundingBox(info->da,xyzmin,xyzmax); CHKERRQ(ierr);
    hx = (xyzmax[0] - xyzmin[0]) / (info->mx - 1);
    hy = (xyzmax[1] - xyzmin[1]) / (info->my - 1);
    hz = (xyzmax[2] - xyzmin[2]) / (info->mz - 1);
    dvol = hx * hy * hz;
    scx = user->cx * dvol / (hx*hx);
    scy = user->cy * dvol / (hy*hy);
    scz = user->cz * dvol / (hz*hz);
    scdiag = 2.0 * (scx + scy + scz);
    for (k = info->zs; k < info->zs + info->zm; k++) {
        z = xyzmin[2] + k * hz;
        for (j = info->ys; j < info->ys + info->ym; j++) {
            y = xyzmin[1] + j * hy;
            for (i = info->xs; i < info->xs + info->xm; i++) {
                x = xyzmin[0] + i * hx;
                if (   i==0 || i==info->mx-1
                    || j==0 || j==info->my-1
                    || k==0 || k==info->mz-1) {
                    aF[k][j][i] = au[k][j][i] - user->g_bdry(x,y,z,user);
                    aF[k][j][i] *= scdiag;
                } else {
                    ue = (i+1 == info->mx-1) ? user->g_bdry(x+hx,y,z,user)
                                             : au[k][j][i+1];
                    uw = (i-1 == 0)          ? user->g_bdry(x-hx,y,z,user)
                                             : au[k][j][i-1];
                    un = (j+1 == info->my-1) ? user->g_bdry(x,y+hy,z,user)
                                             : au[k][j+1][i];
                    us = (j-1 == 0)          ? user->g_bdry(x,y-hy,z,user)
                                             : au[k][j-1][i];
                    uu = (k+1 == info->mz-1) ? user->g_bdry(x,y,z+hz,user)
                                             : au[k+1][j][i];
                    ud = (k-1 == 0)          ? user->g_bdry(x,y,z-hz,user)
                                             : au[k-1][j][i];
                    aF[k][j][i] = scdiag * au[k][j][i]
                        - scx * (uw + ue) - scy * (us + un) - scz * (uu + ud)
                        - dvol * user->f_rhs(x,y,z,user);
                }
            }
        }
    }
    ierr = PetscLogFlops(14.0*info->xm*info->ym*info->zm);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode Poisson1DJacobianLocal(DMDALocalInfo *info, PetscScalar *au,
                                   Mat J, Mat Jpre, PoissonCtx *user) {
    PetscErrorCode  ierr;
    int          i,ncols;
    double       xmin[1], xmax[1], h, v[3];
    MatStencil   col[3],row;

    ierr = DMDAGetBoundingBox(info->da,xmin,xmax); CHKERRQ(ierr);
    h = (xmax[0] - xmin[0]) / (info->mx - 1);
    for (i = info->xs; i < info->xs+info->xm; i++) {
        row.i = i;
        col[0].i = i;
        ncols = 1;
        if (i==0 || i==info->mx-1) {
            v[0] = user->cx * 2.0 / h;
        } else {
            v[0] = user->cx * 2.0 / h;
            if (i-1 > 0) {
                col[ncols].i = i-1;  v[ncols++] = - user->cx / h;
            }
            if (i+1 < info->mx-1) {
                col[ncols].i = i+1;  v[ncols++] = - user->cx / h;
            }
        }
        ierr = MatSetValuesStencil(Jpre,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
    }

    ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}

PetscErrorCode Poisson2DJacobianLocal(DMDALocalInfo *info, PetscScalar **au,
                                   Mat J, Mat Jpre, PoissonCtx *user) {
    PetscErrorCode  ierr;
    double       xymin[2], xymax[2], hx, hy, scx, scy, scdiag, v[5];
    int          i,j,ncols;
    MatStencil   col[5],row;

    ierr = DMDAGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    scx = user->cx * hy / hx;
    scy = user->cy * hx / hy;
    scdiag = 2.0 * (scx + scy);
    for (j = info->ys; j < info->ys+info->ym; j++) {
        row.j = j;
        col[0].j = j;
        for (i = info->xs; i < info->xs+info->xm; i++) {
            row.i = i;
            col[0].i = i;
            ncols = 1;
            v[0] = scdiag;
            if (i>0 && i<info->mx-1 && j>0 && j<info->my-1) {
                if (i-1 > 0) {
                    col[ncols].j = j;    col[ncols].i = i-1;  v[ncols++] = - scx;  }
                if (i+1 < info->mx-1) {
                    col[ncols].j = j;    col[ncols].i = i+1;  v[ncols++] = - scx;  }
                if (j-1 > 0) {
                    col[ncols].j = j-1;  col[ncols].i = i;    v[ncols++] = - scy;  }
                if (j+1 < info->my-1) {
                    col[ncols].j = j+1;  col[ncols].i = i;    v[ncols++] = - scy;  }
            }
            ierr = MatSetValuesStencil(Jpre,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }

    ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}

PetscErrorCode Poisson3DJacobianLocal(DMDALocalInfo *info, PetscScalar ***au,
                                   Mat J, Mat Jpre, PoissonCtx *user) {
    PetscErrorCode  ierr;
    double       xyzmin[3], xyzmax[3], hx, hy, hz, dvol, scx, scy, scz, scdiag, v[7];
    int          i,j,k,ncols;
    MatStencil   col[7],row;

    ierr = DMDAGetBoundingBox(info->da,xyzmin,xyzmax); CHKERRQ(ierr);
    hx = (xyzmax[0] - xyzmin[0]) / (info->mx - 1);
    hy = (xyzmax[1] - xyzmin[1]) / (info->my - 1);
    hz = (xyzmax[2] - xyzmin[2]) / (info->mz - 1);
    dvol = hx * hy * hz;
    scx = user->cx * dvol / (hx*hx);
    scy = user->cy * dvol / (hy*hy);
    scz = user->cz * dvol / (hz*hz);
    scdiag = 2.0 * (scx + scy + scz);
    for (k = info->zs; k < info->zs+info->zm; k++) {
        row.k = k;
        col[0].k = k;
        for (j = info->ys; j < info->ys+info->ym; j++) {
            row.j = j;
            col[0].j = j;
            for (i = info->xs; i < info->xs+info->xm; i++) {
                row.i = i;
                col[0].i = i;
                ncols = 1;
                v[0] = scdiag;
                if (i>0 && i<info->mx-1 && j>0 && j<info->my-1 && k>0 && k<info->mz-1) {
                    if (i-1 > 0) {
                        col[ncols].k = k;    col[ncols].j = j;    col[ncols].i = i-1;
                        v[ncols++] = - scx;
                    }
                    if (i+1 < info->mx-1) {
                        col[ncols].k = k;    col[ncols].j = j;    col[ncols].i = i+1;
                        v[ncols++] = - scx;
                    }
                    if (j-1 > 0) {
                        col[ncols].k = k;    col[ncols].j = j-1;  col[ncols].i = i;
                        v[ncols++] = - scy;
                    }
                    if (j+1 < info->my-1) {
                        col[ncols].k = k;    col[ncols].j = j+1;  col[ncols].i = i;
                        v[ncols++] = - scy;
                    }
                    if (k-1 > 0) {
                        col[ncols].k = k-1;  col[ncols].j = j;    col[ncols].i = i;
                        v[ncols++] = - scz;
                    }
                    if (k+1 < info->mz-1) {
                        col[ncols].k = k+1;  col[ncols].j = j;    col[ncols].i = i;
                        v[ncols++] = - scz;
                    }
                }
                ierr = MatSetValuesStencil(Jpre,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
            }
        }
    }
    ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}

PetscErrorCode InitialState(DM da, InitialType it, PetscBool gbdry,
                            Vec u, PoissonCtx *user) {
    PetscErrorCode ierr;
    DMDALocalInfo  info;
    PetscRandom    rctx;
    switch (it) {
        case ZEROS:
            ierr = VecSet(u,0.0); CHKERRQ(ierr);
            break;
        case RANDOM:
            ierr = PetscRandomCreate(PETSC_COMM_WORLD,&rctx); CHKERRQ(ierr);
            ierr = VecSetRandom(u,rctx); CHKERRQ(ierr);
            ierr = PetscRandomDestroy(&rctx); CHKERRQ(ierr);
            break;
        default:
            SETERRQ(PETSC_COMM_WORLD,4,"invalid InitialType ... how did I get here?\n");
    }
    if (!gbdry) {
        return 0;
    }
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    switch (info.dim) {
        case 1:
        {
            int    i;
            double xmax[1], xmin[1], h, x, *au;
            ierr = DMDAVecGetArray(da, u, &au); CHKERRQ(ierr);
            ierr = DMDAGetBoundingBox(da,xmin,xmax); CHKERRQ(ierr);
            h = (xmax[0] - xmin[0]) / (info.mx - 1);
            for (i = info.xs; i < info.xs + info.xm; i++) {
                if (i==0 || i==info.mx-1) {
                    x = xmin[0] + i * h;
                    au[i] = user->g_bdry(x,0.0,0.0,user);
                }
            }
            ierr = DMDAVecRestoreArray(da, u, &au); CHKERRQ(ierr);
            break;
        }
        case 2:
        {
            int     i, j;
            double  xymin[2], xymax[2], hx, hy, x, y, **au;
            ierr = DMDAVecGetArray(da, u, &au); CHKERRQ(ierr);
            ierr = DMDAGetBoundingBox(da,xymin,xymax); CHKERRQ(ierr);
            hx = (xymax[0] - xymin[0]) / (info.mx - 1);
            hy = (xymax[1] - xymin[1]) / (info.my - 1);
            for (j = info.ys; j < info.ys + info.ym; j++) {
                y = xymin[1] + j * hy;
                for (i = info.xs; i < info.xs + info.xm; i++) {
                    if (i==0 || i==info.mx-1 || j==0 || j==info.my-1) {
                        x = xymin[0] + i * hx;
                        au[j][i] = user->g_bdry(x,y,0.0,user);
                    }
                }
            }
            ierr = DMDAVecRestoreArray(da, u, &au); CHKERRQ(ierr);
            break;
        }
        case 3:
        {
            int     i, j, k;
            double  xyzmin[3], xyzmax[3], hx, hy, hz, x, y, z, ***au;
            ierr = DMDAVecGetArray(da, u, &au); CHKERRQ(ierr);
            ierr = DMDAGetBoundingBox(da,xyzmin,xyzmax); CHKERRQ(ierr);
            hx = (xyzmax[0] - xyzmin[0]) / (info.mx - 1);
            hy = (xyzmax[1] - xyzmin[1]) / (info.my - 1);
            hz = (xyzmax[2] - xyzmin[2]) / (info.mz - 1);
            for (k = info.zs; k < info.zs+info.zm; k++) {
                z = xyzmin[2] + k * hz;
                for (j = info.ys; j < info.ys + info.ym; j++) {
                    y = xyzmin[1] + j * hy;
                    for (i = info.xs; i < info.xs + info.xm; i++) {
                        if (i==0 || i==info.mx-1 || j==0 || j==info.my-1
                                 || k==0 || k==info.mz-1) {
                            x = xyzmin[0] + i * hx;
                            au[k][j][i] = user->g_bdry(x,y,z,user);
                        }
                    }
                }
            }
            ierr = DMDAVecRestoreArray(da, u, &au); CHKERRQ(ierr);
            break;
        }
        default:
            SETERRQ(PETSC_COMM_WORLD,5,"invalid dim from DMDALocalInfo\n");
    }
    return 0;
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>

#ifndef RANFUN_H
#define RANFUN_H

#define min(x,y) ((x) < (y) ? (x) : (y))
#define max(x,y) ((x) > (y) ? (x) : (y))

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

//interface for gsl_compare
// x==y, 0, x<y, -1, x>y, 1.
int fcmp (double x, double y, double epsilon = 0.00000000001);

/*Interface with gsl*/

/*Available random number generators from gsl
 gsl_rng_taus

       see http://sourceware.cygnus.com/gsl/ref/gsl-ref_5.html*/

#define GENERATOR gsl_rng_taus

/*to use inline functions in gsl (in c++)*/
/*define  DHAVE_INLINE at compile time*/
#ifdef DHAVE_INLINE
#define  INLINE inline
#endif

#ifndef  DHAVE_INLINE
#define  INLINE /*nothing*/
#endif

void Seed(unsigned long int s);

unsigned long int GetSeed();

double Un01();  /*Un01() */

double Unab(double a, double b);  /*U(a,b]*/

unsigned int Un0n(int n); /*Uniform integer in { 0, 1, ..., n-1 }*/

int Sign(); /*random -1 or 1*/

double NorSim(double m, double sigma); /*Normal*/

double ExpSim(double mu); /*p(x) dx = {1 \over \mu} \exp(-x/\mu) dx*/

double GammaSim(double a, double b); /*Gamma: p(x) dx = K x^{a-1} e^{-x/b} dx*/
double GammaDens(double x, double a, double b); /*density for beta, as above */

double BetaSim(double a, double b); /*Beta: p(x) dx = K x^{a-1} (1-x)^{b-1} dx */
double BetaDens(double x, double a, double b); /*density for beta, as above */

unsigned int BiSim( double p, int n); /*Binomial: p^k (1-p)^{n-k} */

/*Log of Special function Gamma*/

double LogGamma(double x);

#endif


// lasp_math_raw.h
//
// Author: J.A. de Jong - ASCEE
//
// Description: Raw math routines working on raw arrays of floats and
// complex numbers.
//////////////////////////////////////////////////////////////////////
#pragma once
#ifndef LASP_MATH_RAW_H
#define LASP_MATH_RAW_H
#include "lasp_assert.h"
#include "lasp_tracer.h"
#include "lasp_types.h"
#include <float.h>
#include <math.h>

#if LASP_USE_BLAS == 1
#include <cblas.h>
#elif LASP_USE_BLAS == 0
#else
#error "LASP_USE_BLAS should be set to either 0 or 1"
#endif

#ifdef LASP_DOUBLE_PRECISION
#define c_real creal
#define c_imag cimag
#define d_abs fabs
#define c_abs cabs
#define c_conj conj
#define d_atan2 atan2
#define d_acos acos
#define d_sqrt sqrt
#define c_exp cexp
#define d_sin sin
#define d_cos cos
#define d_pow pow
#define d_log10 log10
#define d_ln log
#define d_epsilon (DBL_EPSILON)

#else  // LASP_DOUBLE_PRECISION not defined
#define c_conj conjf
#define c_real crealf
#define c_imag cimagf
#define d_abs fabsf
#define c_abs cabsf
#define d_atan2 atan2f
#define d_acos acosf
#define d_sqrt sqrtf
#define c_exp cexpf
#define d_sin sinf
#define d_cos cosf
#define d_pow powf
#define d_log10 log10f
#define d_ln logf
#define d_epsilon (FLT_EPSILON)
#endif // LASP_DOUBLE_PRECISION

/// Positive infinite
#define d_inf (INFINITY)

#ifdef M_PI
static const d number_pi = M_PI;
#else
static const d number_pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
#endif

/**
 * Set all elements in an array equal to val
 *
 * @param to
 * @param val
 * @param size
 */
static inline void d_set(d to[],d val,us size) {
    for(us i=0;i<size;i++) {
        to[i]=val;
    }
}
/**
 * Set all elements in an array equal to val
 *
 * @param to
 * @param val
 * @param size
 */
static inline void c_set(c to[],c val,us size) {
    for(us i=0;i<size;i++) {
        to[i]=val;
    }
}

/**
 * Return the maximum of two doubles
 *
 * @param a value 1
 * @param b value 2
 *
 * @returns the maximum of value 1 and 2
 */
static inline d d_max(const d a,const d b) {
    return a>b?a:b;
}

/**
 * Return the dot product of two arrays, one of them complex-valued,
 * the other real-valued
 *
 * @param a the complex-valued array
 * @param b the real-valued array
 * @param size the size of the arrays. *Should be equal-sized!*
 *
 * @return the dot product
 */
static inline c cd_dot(const c a[],const d b[],us size){
    c result = 0;
    us i;
    for(i=0;i<size;i++){
        result+=a[i]*b[i];
    }
    return result;
}
/**
 * Return the dot product of two complex-valued arrays. Wraps BLAS
 * when LASP_USE_BLAS == 1.
 *
 * @param a complex-valued array
 * @param b complex-valued array
 * @param size the size of the arrays. *Should be equal-sized!*
 *
 * @return the dot product
 */
static inline c cc_dot(const c a[],const c b[],us size){
    #if LASP_USE_BLAS == 1
    WARN("CBlas zdotu not yet tested");
    #if LASP_DOUBLE_PRECISION
    // assert(0);
    return cblas_zdotu(size,(d*) a,1,(d*) b,1);
    #else
    return cblas_cdotu(size,(d*) a,1,(d*) b,1);
    #endif
    #else
    c result = 0;
    us i;
    for(i=0;i<size;i++){
        result+=a[i]*b[i];
    }
    return result;
    #endif
}

/**
 * Compute the dot product of two real arrays.
 *
 * @param a First array.
 * @param b Second array.
 * @param size Size of the arrays.
 * @return The result.
 */
static inline d d_dot(const d a[],const d b[],const us size){
    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    return cblas_ddot(size,a,1,b,1);
    #else  // Single precision function
    return cblas_sdot(size,a,1,b,1);
    #endif
    #else  // No BLAS, do it manually
    d result = 0;
    us i;
    for(i=0;i<size;i++){
        result+=a[i]*b[i];
    }
    return result;
    #endif
}

/**
 * Copy array of floats.
 *
 * @param to : Array to write to
 * @param from : Array to read from
 * @param size : Size of arrays
 * @param to_inc : Mostly equal to 1, the stride of the array to copy to
 * @param from_inc : Mostly equal to 1, the stride of the array to copy from
 */
static inline void d_copy(d to[],
                          const d from[],
                          const us size,
                          const us to_inc,
                          const us from_inc){
    #if LASP_USE_BLAS == 1
    cblas_dcopy(size,from,from_inc,to,to_inc);
    #else
    us i;
    for(i=0;i<size;i++)
        to[i*to_inc] = from[i*from_inc];
    #endif
}

/**
 * Copy array of floats to array of complex floats. Imaginary part set
 * to zero.
 *
 * @param to : Array to write to
 * @param from : Array to read from
 * @param size : Size of arrays
 */
static inline void cd_copy(c to[],const d from[],const us size) {
    us i;
    for(i=0;i<size;i++) {
        to[i] = (c) (from[i]);
        dbgassert(cimag(to[i]) == 0,"Imaginary part not equal to zero");
    }
}

/**
 * Copy float vector to complex vector. Imaginary part set
 * to zero.
 *
 * @param to : Vector to write to
 * @param from : Vector to read from
 */
static inline void c_copy(c to[],const c from[],const us size){

    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    cblas_zcopy(size,(d*) from,1,(d*) to,1);
    #else
    cblas_ccopy(size,(d*) from,1,(d*) to,1);
    #endif
    #else
    us i;
    for(i=0;i<size;i++)
        to[i] = from[i];
    #endif
}
/**
 * Multiply y with fac, and add result to x
 *
 * @param x[in,out] Array to add to
 * @param y[in] Array to add to x
 * @param[in] fac Factor with which to multiply y
 * @param[in] size Size of the arrays
 */
static inline void d_add_to(d x[],const d y[],
                            const d fac,const us size){
    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    cblas_daxpy(size,fac,y,1,x,1);
    #else
    cblas_saxpy(size,fac,y,1,x,1);
    #endif
    #else
    us i;
    for(i=0;i<size;i++)
        x[i]+=fac*y[i];
    #endif
}
/**
 * x = x + fac*y
 *
 * @param[in,out] x Array to add to
 * @param[in] y Array to add to x
 * @param[in] fac Factor with which to multiply y
 * @param[in] size Size of the arrays
 */
static inline void c_add_to(c x[],const c y[],
                            const c fac,const us size){
    fsTRACE(15);
    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    cblas_zaxpy(size,(d*) &fac,(d*) y,1,(d*) x,1);
    #else
    cblas_caxpy(size,(d*) &fac,(d*) y,1,(d*) x,1);
    #endif
    #else
    us i;
    for(i=0;i<size;i++)
        x[i]+=fac*y[i];
    #endif
    feTRACE(15);
}

/**
 * Scale an array of doubles
 *
 * @param a array
 * @param scale_fac scale factor
 * @param size size of the array
 */
static inline void d_scale(d a[],const d scale_fac,us size){
    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    cblas_dscal(size,scale_fac,a,1);
    #else
    cblas_sscal(size,scale_fac,a,1);
    #endif
    #else
    us i;
    for(i=0;i<size;i++)
        a[i]*=scale_fac;
    #endif
}

/**
 * Scale an array of complex floats
 *
 * @param a array
 * @param scale_fac scale factor
 * @param size size of the array
 */
static inline void c_scale(c a[],const c scale_fac,us size){
    #if LASP_USE_BLAS == 1
    // Complex argument should be given in as array of two double
    // values. The first the real part, the second the imaginary
    // part. Fortunately the (c) type stores the two values in this
    // order. To be portable and absolutely sure anything goes well,
    // we convert it explicitly here.

    #if LASP_DOUBLE_PRECISION
    cblas_zscal(size,(d*) &scale_fac,(d*) a,1);
    #else
    cblas_cscal(size,(d*) &scale_fac,(d*) a,1);
    #endif
    #else
    us i;
    for(i=0;i<size;i++)
        a[i]*=scale_fac;
    #endif
}

/**
 * Compute the maximum value of an array
 *
 * @param a array
 * @param size size of the array
 * @return maximum
 */
static inline d darray_max(const d a[],us size){
    us i;
    d max = a[0];
    for(i=1;i<size;i++){
        if(a[i] > max) max=a[i];
    }
    return max;
}
/**
 * Compute the minimum of an array
 *
 * @param a array
 * @param size size of the array
 * @return minimum
 */
static inline d d_min(const d a[],us size){
    us i;
    d min = a[0];
    for(i=1;i<size;i++){
        if(a[i] > min) min=a[i];
    }
    return min;
}

/**
 * Compute the \f$ L_2 \f$ norm of an array of doubles
 *
 * @param a Array
 * @param size Size of array
 */
static inline d d_norm(const d a[],us size){
    #if LASP_USE_BLAS == 1
    return cblas_dnrm2(size,a,1);
    #else
    d norm = 0;
    us i;
    for(i=0;i<size;i++){
        norm+=a[i]*a[i];
    }
    norm = d_sqrt(norm);
    return norm;
    #endif

}

/**
 * Compute the \f$ L_2 \f$ norm of an array of complex floats
 *
 * @param a Array
 * @param size Size of array
 */
static inline d c_norm(const c a[],us size){
    #if LASP_USE_BLAS == 1
    return cblas_dznrm2(size,(d*) a,1);
    #else
    d norm = 0;
    us i;
    for(i=0;i<size;i++){
        d absa = c_abs(a[i]);
        norm+=absa*absa;
    }
    norm = d_sqrt(norm);
    return norm;
    #endif

}

/**
 * Computes the element-wise vector product, or Hadamard product of
 * arr1 and arr2
 *
 * @param res Where the result will be stored
 * @param arr1 Array 1
 * @param vec2 Array 2
 * @param size: Size of the arrays
 */
void d_elem_prod_d(d res[],
                   const d arr1[],
                   const d arr2[],
                   const us size);

/**
 * Computes the element-wise vector product, or Hadamard product of
 * arr1 and arr2 for complex floats
 *
 * @param res Where the result will be stored
 * @param arr1 Array 1
 * @param vec2 Array 2
 * @param size: Size of the arrays
 */
void c_hadamard(c res[],
                const c arr1[],
                const c arr2[],
                const us size);

/**
 * Compute the complex conjugate of a complex vector and store the
 * result.
 *
 * @param res Result vector
 * @param in Input vector
 * @param size Size of the vector
 */
static inline void carray_conj(c res[],const c in[],const us size) {
    // First set the result vector to zero
    fsTRACE(15);
    c_set(res,0,size);
    #if LASP_USE_BLAS == 1
    #if LASP_DOUBLE_PRECISION
    // Cast as a float, scale all odd elements with minus one to find
    // the complex conjugate.
    cblas_daxpy(size ,1.0,(d*) in,2,(d*) res,2);
    cblas_daxpy(size,-1.0,&((d*) in)[1],2,&((d*) res)[1],2);
    #else
    cblas_faxpy(size ,1,(d*) in,2,(d*) res,2);
    cblas_faxpy(size,-1,&((d*) in)[1],2,&((d*) res)[1],2);
    #endif  // LASP_DOUBLE_PRECISION
    #else
    for(us i=0;i<size;i++) {
        res[i] = c_conj(in[i]);
    }
    #endif  // LASP_USE_BLAS
    feTRACE(15);
}
/**
 * In place complex conjugation
 *
 * @param res Result vector
 * @param size Size of the vector
 */
static inline void c_conj_inplace(c res[],us size) {
    #if LASP_USE_BLAS
    #if LASP_DOUBLE_PRECISION
    // Cast as a float, scale all odd elements with minus one to find
    // the complex conjugate.
    cblas_dscal(size,-1,&((d*) res)[1],2);
    #else
    cblas_sscal(size,-1,&((d*) res)[1],2);
    #endif  // LASP_DOUBLE_PRECISION
    #else
    for(us i=0;i<size;i++) {
        res[i] = c_conj(res[i]);
    }
    #endif  // LASP_USE_BLAS
}

#endif // LASP_MATH_RAW_H
//////////////////////////////////////////////////////////////////////


#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include "scf.h"

/* Computing Phi_nl(r), which is the radial part of the basis functions
 *
 * The Gegenbauer polynomials for the host halo and subhalos can either be computed
 * using my own hard coded implementation (see gegenbauer.c) or GSL's implementation.
 *
 */

double int_power(float x, int n)
{
    return gsl_pow_int(x, n);
}

#ifdef USE_GSL_GEGENBAUER_HOST
float Phi_nl(double Cna, int l, float r)
{
    return -1.0f * int_power(r,l)/int_power(1.0f+r, 2.0*l+1) * (float)Cna * ROOT_FOUR_PI;
}

float dPhi_nl_dr(double Cna, float phinl, int n, int l, float r)
{
    int two_l = 2*l;
	float r1 = r+1.f;

	float val = phinl * ( l/r - (two_l+1.0f)/r1 );

	if(n>0)
	{
		val -= ROOT_FOUR_PI * int_power(r,l)*4.0f*(two_l+1.5f) * (float)Cna / int_power(r1,two_l+3);
	}

	return val;
}

#else
float Phi_nl(int n, int l, float r)
{
    float xi = (r-1.0f)/(r+1.0f);

    return -1.0f * int_power(r,l)/int_power(1.0f+r, 2.0*l+1) * Cna(n,2.0f*l+1.5f,xi) * ROOT_FOUR_PI;
}

float dPhi_nl_dr(float phinl, int n, int l, float r)
{
    int two_l = 2*l;
	float r1 = r+1.f;

	float val = phinl * ( l/r - (two_l+1.0f)/r1 );

	if(n>0)
	{
		float xi = (r-1.f)/r1;

		val -= ROOT_FOUR_PI * int_power(r,l)*4.0f*(two_l+1.5f) * Cna(n-1,two_l+2.5f,xi) / int_power(r1,two_l+3);
	}

	return val;
}
#endif

#ifdef USE_GSL_GEGENBAUER_SUBHALO
float Phi_n0(double Cna, float r)
{
    return -1.0f / (1.0f+r) * (float)Cna * ROOT_FOUR_PI;
}

float dPhi_n0_dr(double Cna, float phin0, int n, float r)
{
    float r1 = r+1.f;

    float val = phin0 * -1.f/r1;

    if(n>0)
    {
        val -= ROOT_FOUR_PI * 6.f/r1/r1/r1 * (float)Cna;
    }

    return val;
}

#else
float Phi_n0(int n, float r)
{
    float xi = (r-1.0f)/(r+1.0f);

    return -1.0f / (1.0f+r) * Cna(n,1.5f,xi) * ROOT_FOUR_PI;
}

float dPhi_n0_dr(float phin0, int n, float r)
{
    float r1 = r+1.f;

    float val = phin0 * -1.f/r1;

    if(n>0)
    {
		float xi = (r-1.f)/r1;
        val -= ROOT_FOUR_PI * 6.f/r1/r1/r1 * Cna(n-1,2.5f,xi);
    }

    return val;
}

#endif


static char help[] =
"Solves time-dependent heat equation in 2D using TS.  Option prefix -ht_.\n"
"Equation is  u_t = D_0 laplacian u + f.  Domain is (0,1) x (0,1).\n"
"Boundary conditions are non-homogeneous Neumann in x and periodic in y.\n"
"Energy is conserved (for these particular conditions/source) and an extra\n"
"'monitor' is demonstrated.  Discretization is by centered finite differences.\n"
"Converts the PDE into a system  X_t = G(t,X) (PETSc type 'nonlinear') by\n"
"method of lines.  Uses backward Euler time-stepping by default.\n";

#include <petsc.h>

typedef struct {
  double D0;    // conductivity
} HeatCtx;

static double f_source(double x, double y) {
    return 3.0 * exp(-25.0 * (x-0.6) * (x-0.6)) * sin(2.0*PETSC_PI*y);
}

static double gamma_neumann(double y) {
    return sin(6.0 * PETSC_PI * y);
}

extern PetscErrorCode Spacings(DMDALocalInfo*, double*, double*);
extern PetscErrorCode EnergyMonitor(TS, int, double, Vec, void*);
extern PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo*, double, double**,
                                           double**, HeatCtx*);
extern PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo*, double, double**,
                                           Mat, Mat, HeatCtx*);

int main(int argc,char **argv)
{
  PetscErrorCode ierr;
  HeatCtx        user;
  TS             ts;
  Vec            u;
  DM             da;
  DMDALocalInfo  info;
  double         t0, tf;
  PetscBool      monitorenergy = PETSC_FALSE;

  PetscInitialize(&argc,&argv,(char*)0,help);

  user.D0  = 1.0;
  ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "ht_", "options for heat", ""); CHKERRQ(ierr);
  ierr = PetscOptionsReal("-D0","constant thermal diffusivity",
           "heat.c",user.D0,&user.D0,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool("-monitor","also display total heat energy at each step",
           "heat.c",monitorenergy,&monitorenergy,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

//STARTDMDASETUP
  ierr = DMDACreate2d(PETSC_COMM_WORLD,
      DM_BOUNDARY_NONE, DM_BOUNDARY_PERIODIC, DMDA_STENCIL_STAR,
      5,4,PETSC_DECIDE,PETSC_DECIDE,  // default to hx=hx=0.25 grid
      1,1,                            // degrees of freedom, stencil width
      NULL,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMCreateGlobalVector(da,&u); CHKERRQ(ierr);
//ENDDMDASETUP

//STARTTSSETUP
  ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
  ierr = TSSetDM(ts,da); CHKERRQ(ierr);
  ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);
  ierr = DMDATSSetRHSFunctionLocal(da,INSERT_VALUES,
           (DMDATSRHSFunctionLocal)FormRHSFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDATSSetRHSJacobianLocal(da,
           (DMDATSRHSJacobianLocal)FormRHSJacobianLocal,&user); CHKERRQ(ierr);
  if (monitorenergy) {
      ierr = TSMonitorSet(ts,EnergyMonitor,&user,NULL); CHKERRQ(ierr);
  }
  ierr = TSSetType(ts,TSBDF); CHKERRQ(ierr);
  ierr = TSSetTime(ts,0.0); CHKERRQ(ierr);
  ierr = TSSetMaxTime(ts,0.1); CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,0.001); CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
//ENDTSSETUP

  // report on set up
  ierr = TSGetTime(ts,&t0); CHKERRQ(ierr);
  ierr = TSGetMaxTime(ts,&tf); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
           "solving on %d x %d grid for t0=%g to tf=%g ...\n",
           info.mx,info.my,t0,tf); CHKERRQ(ierr);

  // solve
  ierr = VecSet(u,0.0); CHKERRQ(ierr);   // initial condition
  ierr = TSSolve(ts,u); CHKERRQ(ierr);

  VecDestroy(&u);  TSDestroy(&ts);  DMDestroy(&da);
  return PetscFinalize();
}

PetscErrorCode Spacings(DMDALocalInfo *info, double *hx, double *hy) {
    if (hx)  *hx = 1.0 / (double)(info->mx-1);
    if (hy)  *hy = 1.0 / (double)(info->my);   // periodic direction
    return 0;
}

//STARTMONITOR
PetscErrorCode EnergyMonitor(TS ts, int step, double time, Vec u,
                             void *ctx) {
    PetscErrorCode ierr;
    HeatCtx        *user = (HeatCtx*)ctx;
    double         lenergy = 0.0, energy, dt, hx, hy, **au;
    int            i,j;
    MPI_Comm       com;
    DM             da;
    DMDALocalInfo  info;

    ierr = TSGetDM(ts,&da); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    ierr = DMDAVecGetArrayRead(da,u,&au); CHKERRQ(ierr);
    for (j = info.ys; j < info.ys + info.ym; j++) {
        for (i = info.xs; i < info.xs + info.xm; i++) {
            if ((i == 0) || (i == info.mx-1))
                lenergy += 0.5 * au[j][i];
            else
                lenergy += au[j][i];
        }
    }
    ierr = DMDAVecRestoreArrayRead(da,u,&au); CHKERRQ(ierr);
    ierr = Spacings(&info,&hx,&hy); CHKERRQ(ierr);
    lenergy *= hx * hy;
    ierr = PetscObjectGetComm((PetscObject)(da),&com); CHKERRQ(ierr);
    ierr = MPI_Allreduce(&lenergy,&energy,1,MPI_DOUBLE,MPI_SUM,com); CHKERRQ(ierr);
    ierr = TSGetTimeStep(ts,&dt); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,"  energy = %9.2e     nu = %8.4f\n",
                energy,user->D0*dt/(hx*hy)); CHKERRQ(ierr);
    return 0;
}
//ENDMONITOR

//STARTRHSFUNCTION
PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo *info,
                                    double t, double **au,
                                    double **aG, HeatCtx *user) {
  PetscErrorCode ierr;
  int      i, j, mx = info->mx;
  double   hx, hy, x, y, ul, ur, uxx, uyy;

  ierr = Spacings(info,&hx,&hy); CHKERRQ(ierr);
  for (j = info->ys; j < info->ys + info->ym; j++) {
      y = hy * j;
      for (i = info->xs; i < info->xs + info->xm; i++) {
          x = hx * i;
          // apply Neumann b.c.s
          ul = (i == 0) ? au[j][i+1] + 2.0 * hx * gamma_neumann(y)
                        : au[j][i-1];
          ur = (i == mx-1) ? au[j][i-1] : au[j][i+1];
          uxx = (ul - 2.0 * au[j][i]+ ur) / (hx*hx);
          // DMDA is periodic in y
          uyy = (au[j-1][i] - 2.0 * au[j][i]+ au[j+1][i]) / (hy*hy);
          aG[j][i] = user->D0 * (uxx + uyy) + f_source(x,y);
      }
  }
  return 0;
}
//ENDRHSFUNCTION

//STARTRHSJACOBIAN
PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo *info,
                                    double t, double **au,
                                    Mat J, Mat P, HeatCtx *user) {
    PetscErrorCode ierr;
    int            i, j, ncols;
    const double   D = user->D0;
    double         hx, hy, hx2, hy2, v[5];
    MatStencil     col[5],row;

    ierr = Spacings(info,&hx,&hy); CHKERRQ(ierr);
    hx2 = hx * hx;  hy2 = hy * hy;
    for (j = info->ys; j < info->ys+info->ym; j++) {
        row.j = j;  col[0].j = j;
        for (i = info->xs; i < info->xs+info->xm; i++) {
            // set up a standard 5-point stencil for the row
            row.i = i;
            col[0].i = i;
            v[0] = - 2.0 * D * (1.0 / hx2 + 1.0 / hy2);
            col[1].j = j-1;  col[1].i = i;    v[1] = D / hy2;
            col[2].j = j+1;  col[2].i = i;    v[2] = D / hy2;
            col[3].j = j;    col[3].i = i-1;  v[3] = D / hx2;
            col[4].j = j;    col[4].i = i+1;  v[4] = D / hx2;
            ncols = 5;
            // if at the boundary, edit the row back to 4 nonzeros
            if (i == 0) {
                ncols = 4;
                col[3].j = j;  col[3].i = i+1;  v[3] = 2.0 * D / hx2;
            } else if (i == info->mx-1) {
                ncols = 4;
                col[3].j = j;  col[3].i = i-1;  v[3] = 2.0 * D / hx2;
            }
            ierr = MatSetValuesStencil(P,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }

    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}
//ENDRHSJACOBIAN


#pragma once

#include <gsl/gsl>
#include <iostream>

#include "General.h"
#include "HyperionUtils/Concepts.h"
#include "Random.h"

namespace hyperion::math {
	using gsl::narrow_cast;
	using utils::concepts::SignedNumeric, utils::concepts::Integral,
		utils::concepts::SignedIntegral, utils::concepts::FloatingPoint;

	enum class Vec3Idx : size_t
	{
		X = 0ULL,
		Y,
		Z
	};

	template<SignedNumeric T = float>
	class Vec3 {
	  public:
		/// @brief Creates a default `Vec3`
		constexpr Vec3() noexcept = default;

		/// @brief Creates a new `Vec3` with the given x, y, and z components
		///
		/// @param x - The x component
		/// @param y - The y component
		/// @param z - The z component
		constexpr Vec3(T x, T y, T z) noexcept : elements{x, y, z} {
		}
		constexpr Vec3(const Vec3& vec) noexcept = default;
		constexpr Vec3(Vec3&& vec) noexcept = default;
		constexpr ~Vec3() noexcept = default;

		/// @brief Returns the x component
		///
		/// @return a const ref to the x component
		[[nodiscard]] inline constexpr auto x() const noexcept -> const T& {
			return elements[X];
		}

		/// @brief Returns the x component
		///
		/// @return a mutable (ie: non-const) ref to the x component
		[[nodiscard]] inline constexpr auto x() noexcept -> T& {
			return elements[X];
		}

		/// @brief Returns the y component
		///
		/// @return a const ref to the y component
		[[nodiscard]] inline constexpr auto y() const noexcept -> const T& {
			return elements[Y];
		}

		/// @brief Returns the y component
		///
		/// @return a mutable (ie: non-const) ref to the y component
		[[nodiscard]] inline constexpr auto y() noexcept -> T& {
			return elements[Y];
		}

		/// @brief Returns the z component
		///
		/// @return a const ref to the z component
		[[nodiscard]] inline constexpr auto z() const noexcept -> const T& {
			return elements[Z];
		}

		/// @brief Returns the z component
		///
		/// @return a mutable (ie: non-const) ref to the z component
		[[nodiscard]] inline constexpr auto z() noexcept -> T& {
			return elements[Z];
		}

		/// @brief Returns the magnitude (length) of the vector
		///
		/// @return The magnitude
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto magnitude() const noexcept -> TT {
			return General::sqrt(narrow_cast<TT>(magnitude_squared()));
		}

		/// @brief Returns the dot product of this and `vec`
		///
		/// @param vec - The vector to perform the dot product with
		///
		/// @return The dot product
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto dot_prod(const Vec3<TT>& vec) const noexcept -> TT {
			return narrow_cast<TT>(x()) * vec.x() + narrow_cast<TT>(y()) * vec.y()
				   + narrow_cast<TT>(z()) * vec.z();
		}

		/// @brief Returns the dot product of this and `vec`
		///
		/// @param vec - The vector to perform the dot product with
		///
		/// @return The dot product
		template<SignedIntegral TT = int>
		[[nodiscard]] inline constexpr auto dot_prod(const Vec3<TT>& vec) const noexcept -> T {
			return x() * narrow_cast<T>(vec.x()) + y() * narrow_cast<T>(vec.y())
				   + z() * narrow_cast<T>(vec.z());
		}

		/// @brief Performs the cross product
		///
		/// @param vec The vector to perform the cross product with
		///
		/// @return The cross product
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto
		cross_prod(const Vec3<TT>& vec) const noexcept -> Vec3<TT> {
			const auto _x = narrow_cast<TT>(y()) * vec.z() - narrow_cast<TT>(z()) * vec.y();
			const auto _y = narrow_cast<TT>(z()) * vec.x() - narrow_cast<TT>(x()) * vec.z();
			const auto _z = narrow_cast<TT>(x()) * vec.y() - narrow_cast<TT>(y()) * vec.x();
			return {_x, _y, _z};
		}

		/// @brief Performs the cross product
		///
		/// @param vec The vector to perform the cross product with
		///
		/// @return The cross product
		template<SignedIntegral TT = int>
		[[nodiscard]] inline constexpr auto cross_prod(const Vec3<TT>& vec) const noexcept -> Vec3 {
			const auto _x = y() * narrow_cast<T>(vec.z()) - z() * narrow_cast<T>(vec.y());
			const auto _y = z() * narrow_cast<T>(vec.x()) - x() * narrow_cast<T>(vec.z());
			const auto _z = x() * narrow_cast<T>(vec.y()) - y() * narrow_cast<T>(vec.x());
			return {_x, _y, _z};
		}

		/// @brief Returns **a** vector normal to this one
		/// @note this is not necessarily the **only** vector normal to this one
		///
		/// @return a vector normal to this
		[[nodiscard]] inline constexpr auto normal() const noexcept -> Vec3 {
			return cross_prod(
				Vec3<T>(narrow_cast<T>(1.0), narrow_cast<T>(0.0), narrow_cast<T>(0.0)));
		}

		/// @brief Returns this vector with normalized magnitude
		///
		/// @return this vector, normalized
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto normalized() const noexcept -> Vec3<TT> {
			return std::move(*this / magnitude<TT>());
		}

		template<SignedNumeric TT = float>
		[[nodiscard]] inline static constexpr auto random() noexcept -> Vec3<TT> {
			return {random_value<TT>(), random_value<TT>(), random_value<TT>()};
		}

		template<SignedNumeric TT = float>
		[[nodiscard]] inline static constexpr auto random(TT min, TT max) noexcept -> Vec3<TT> {
			return {random_value<TT>(min, max),
					random_value<TT>(min, max),
					random_value<TT>(min, max)};
		}

		template<FloatingPoint TT = float>
		[[nodiscard]] inline static constexpr auto random_in_unit_sphere() noexcept -> Vec3<TT> {
			do {
				auto val = random<TT>(narrow_cast<TT>(-1), narrow_cast<TT>(1));
				if(narrow_cast<TT>(val.magnitude_squared()) < narrow_cast<TT>(1)) {
					return val;
				}
			} while(true);
		}

		template<FloatingPoint TT = float>
		[[nodiscard]] inline static constexpr auto random_in_unit_disk() noexcept -> Vec3<TT> {
			do {
				auto val = random(narrow_cast<TT>(-1), narrow_cast<TT>(1));
				val.z() = narrow_cast<TT>(0);
				if(narrow_cast<TT>(val.magnitude_squared()) < narrow_cast<TT>(1)) {
					return val;
				}
			} while(true);
		}

		[[nodiscard]] inline constexpr auto is_approx_zero() noexcept -> bool {
			constexpr auto zero_tolerance = narrow_cast<T>(0.0001);
			return (General::abs(x()) < zero_tolerance) && (General::abs(y()) < zero_tolerance)
				   && (General::abs(z()) < zero_tolerance);
		}

		[[nodiscard]] inline constexpr auto
		reflected(const Vec3& surface_normal) const noexcept -> Vec3 {
			return std::move(
				*this - narrow_cast<T>(2) * (this->dot_prod(surface_normal) * surface_normal));
		}

		[[nodiscard]] inline constexpr auto
		refracted(const Vec3& surface_normal, T eta_external_over_eta_internal) noexcept -> Vec3 {
			const auto uv = *this;
			const auto cos_theta = General::min((-uv).dot_prod(surface_normal), narrow_cast<T>(1));

			auto out_perpendicular
				= eta_external_over_eta_internal * (uv + cos_theta * surface_normal);
			auto out_parallel = -General::sqrt(General::abs(
									narrow_cast<T>(1) - out_perpendicular.magnitude_squared()))
								* surface_normal;
			return out_perpendicular + out_parallel;
		}

		constexpr auto operator=(const Vec3& vec) noexcept -> Vec3& = default;
		constexpr auto operator=(Vec3&& vec) noexcept -> Vec3& = default;

		template<FloatingPoint TT = float>
		inline constexpr auto operator==(const Vec3<TT>& vec) const noexcept -> bool {
			const auto xEqual
				= General::abs<TT>(narrow_cast<TT>(x()) - vec.x()) < narrow_cast<TT>(0.01);
			const auto yEqual
				= General::abs<TT>(narrow_cast<TT>(y()) - vec.y()) < narrow_cast<TT>(0.01);
			const auto zEqual
				= General::abs<TT>(narrow_cast<TT>(z()) - vec.z()) < narrow_cast<TT>(0.01);
			return xEqual && yEqual && zEqual;
		}

		template<SignedIntegral TT = int>
		inline constexpr auto operator==(const Vec3<TT>& vec) const noexcept -> bool {
			if constexpr(FloatingPoint<T>) {
				const auto xEqual
					= General::abs<T>(x() - narrow_cast<T>(vec.x())) < narrow_cast<TT>(0.01);
				const auto yEqual
					= General::abs<T>(y() - narrow_cast<T>(vec.y())) < narrow_cast<TT>(0.01);
				const auto zEqual
					= General::abs<T>(z() - narrow_cast<T>(vec.z())) < narrow_cast<TT>(0.01);
				return xEqual && yEqual && zEqual;
			}
			else {
				return x() == narrow_cast<T>(vec.x()) && y() == narrow_cast<T>(vec.y())
					   && z() == narrow_cast<T>(vec.z());
			}
		}

		template<SignedNumeric TT = T>
		inline constexpr auto operator!=(const Vec3<TT>& vec) const noexcept -> bool {
			return !(*this == vec);
		}

		inline constexpr auto operator-() const noexcept -> Vec3 {
			return {-x(), -y(), -z()};
		}

		inline constexpr auto operator[](Vec3Idx i) const noexcept -> T {
			const auto index = static_cast<size_t>(i);
			return elements[index]; // NOLINT
		}
		inline constexpr auto operator[](Vec3Idx i) noexcept -> T& {
			const auto index = static_cast<size_t>(i);
			return elements[index]; // NOLINT
		}

		template<FloatingPoint TT = float>
		inline constexpr auto operator+(const Vec3<TT>& vec) const noexcept -> Vec3<TT> {
			return {narrow_cast<TT>(x()) + vec.x(),
					narrow_cast<TT>(y()) + vec.y(),
					narrow_cast<TT>(z()) + vec.z()};
		}

		template<SignedIntegral TT = int>
		inline constexpr auto operator+(const Vec3<TT>& vec) const noexcept -> Vec3 {
			return {x() + narrow_cast<T>(vec.x()),
					y() + narrow_cast<T>(vec.y()),
					z() + narrow_cast<T>(vec.z())};
		}

		template<SignedNumeric TT = T>
		inline constexpr auto operator+=(const Vec3<TT>& vec) noexcept -> Vec3 {
			x() += narrow_cast<T>(vec.x());
			y() += narrow_cast<T>(vec.y());
			z() += narrow_cast<T>(vec.z());
			return *this;
		}

		template<FloatingPoint TT = float>
		inline constexpr auto operator-(const Vec3<TT>& vec) const noexcept -> Vec3<TT> {
			return {narrow_cast<TT>(x()) - vec.x(),
					narrow_cast<TT>(y()) - vec.y(),
					narrow_cast<TT>(z()) - vec.z()};
		}

		template<SignedIntegral TT = int>
		inline constexpr auto operator-(const Vec3<TT>& vec) const noexcept -> Vec3 {
			return {x() - narrow_cast<T>(vec.x()),
					y() - narrow_cast<T>(vec.y()),
					z() - narrow_cast<T>(vec.z())};
		}

		template<SignedNumeric TT = T>
		inline constexpr auto operator-=(const Vec3<TT>& vec) noexcept -> Vec3& {
			x() -= narrow_cast<T>(vec.x());
			y() -= narrow_cast<T>(vec.y());
			z() -= narrow_cast<T>(vec.z());
			return *this;
		}

		inline constexpr auto operator*(FloatingPoint auto s) const noexcept -> Vec3<decltype(s)> {
			using TT = decltype(s);
			return {narrow_cast<TT>(x()) * s, narrow_cast<TT>(y()) * s, narrow_cast<TT>(z()) * s};
		}

		inline constexpr auto operator*(SignedIntegral auto s) const noexcept -> Vec3 {
			auto scalar = narrow_cast<T>(s);
			return {x() * scalar, y() * scalar, z() * scalar};
		}

		friend inline constexpr auto
		operator*(FloatingPoint auto lhs, const Vec3& rhs) noexcept -> Vec3<decltype(lhs)> {
			return rhs * lhs;
		}

		friend inline constexpr auto
		operator*(SignedIntegral auto lhs, const Vec3& rhs) noexcept -> Vec3 {
			return rhs * lhs;
		}

		inline constexpr auto operator*=(FloatingPoint auto s) noexcept -> Vec3& {
			using TT = decltype(s);
			x() = narrow_cast<T>(narrow_cast<TT>(x()) * s);
			y() = narrow_cast<T>(narrow_cast<TT>(y()) * s);
			z() = narrow_cast<T>(narrow_cast<TT>(z()) * s);
			return *this;
		}

		inline constexpr auto operator*=(SignedIntegral auto s) noexcept -> Vec3& {
			auto scalar = narrow_cast<T>(s);
			x() *= scalar;
			y() *= scalar;
			z() *= scalar;
			return *this;
		}

		inline constexpr auto operator/(FloatingPoint auto s) const noexcept -> Vec3<decltype(s)> {
			using TT = decltype(s);
			return {narrow_cast<TT>(x()) / s, narrow_cast<TT>(y()) / s, narrow_cast<TT>(z()) / s};
		}

		inline constexpr auto operator/(SignedIntegral auto s) const noexcept -> Vec3 {
			auto scalar = narrow_cast<T>(s);
			return {x() / scalar, y() / scalar, z() / scalar};
		}

		friend inline constexpr auto
		operator/(FloatingPoint auto lhs, const Vec3& rhs) noexcept -> Vec3<decltype(lhs)> {
			return rhs / lhs;
		}

		friend inline constexpr auto
		operator/(SignedIntegral auto lhs, const Vec3& rhs) noexcept -> Vec3 {
			return rhs / lhs;
		}

		inline constexpr auto operator/=(FloatingPoint auto s) noexcept -> Vec3& {
			using TT = decltype(s);
			x() = narrow_cast<T>(narrow_cast<TT>(x()) / s);
			y() = narrow_cast<T>(narrow_cast<TT>(y()) / s);
			z() = narrow_cast<T>(narrow_cast<TT>(z()) / s);
			return *this;
		}

		inline constexpr auto operator/=(SignedIntegral auto s) noexcept -> Vec3& {
			auto scalar = narrow_cast<T>(s);
			x() /= scalar;
			y() /= scalar;
			z() /= scalar;
			return *this;
		}

		friend inline constexpr auto
		operator<<(std::ostream& out, const Vec3& vec) noexcept -> std::ostream& {
			return out << vec.x() << ' ' << vec.y() << ' ' << vec.z();
		}

	  private:
		static constexpr size_t NUM_ELEMENTS = static_cast<size_t>(Vec3Idx::Z) + 1;
		T elements[NUM_ELEMENTS] // NOLINT
			= {narrow_cast<T>(0), narrow_cast<T>(0), narrow_cast<T>(0)};

		/// @brief Calculates the magnitude squared of this vector
		///
		/// @return The magnitude squared
		[[nodiscard]] inline constexpr auto magnitude_squared() const noexcept -> T {
			return x() * x() + y() * y() + z() * z();
		}

		/// Index for x component
		static constexpr size_t X = static_cast<size_t>(Vec3Idx::X);
		/// Index for y component
		static constexpr size_t Y = static_cast<size_t>(Vec3Idx::Y);
		/// Index for z component
		static constexpr size_t Z = static_cast<size_t>(Vec3Idx::Z);
	};

	// Deduction Guides

	template<FloatingPoint T>
	explicit Vec3(T, T, T) -> Vec3<T>;

	template<SignedIntegral T>
	explicit Vec3(T, T, T) -> Vec3<T>;

} // namespace hyperion::math


/*
 # Copyright (C) 2009-2013 Thierry Sousbie
 # University of Tokyo / CNRS, 2009
 #
 # This file is part of porject DisPerSE
 #
 #  Author          : Thierry Sousbie
 #  Contact         : tsousbie@gmail.com
 #
 #  Licenses        : This file is 'dual-licensed', you have to choose one
 #                    of the two licenses below to apply.
 #
 #                    CeCILL-C
 #                    The CeCILL-C license is close to the GNU LGPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #                or  CeCILL v2.0
 #                    The CeCILL license is compatible with the GNU GPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL_V2-en.html )
 #
 #  This software is governed either by the CeCILL or the CeCILL-C license
 #  under French law and abiding by the rules of distribution of free software.
 #  You can  use, modify and or redistribute the software under the terms of
 #  the CeCILL or CeCILL-C licenses as circulated by CEA, CNRS and INRIA
 #  at the following URL : "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL and CeCILL-C licenses and that you accept its terms.
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "simplex.h"

#ifdef HAVE_GSL

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

double SimplexVolumed(double **coord,int nvert, int ndims)
{
    int i,j,k;
    double det;
    int s;
    double m_data[(nvert-1)*(nvert-1)];
    double w_data[(nvert-1)*(nvert-1)];
/*
    double w_data[3*3] = {1.,0.,0.,
			  1.,1.,0.,
			  1.,1.,1.};*/
    //double wt_data[(nvert-1)*ndims];
    gsl_permutation * perm;

    gsl_matrix_view m = gsl_matrix_view_array (m_data, nvert-1, nvert-1);
    gsl_matrix_view w = gsl_matrix_view_array (w_data, nvert-1, ndims);
    //gsl_matrix_view wt = gsl_matrix_view_array (wt_data, nvert-1,ndims);

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(coord[1][i]-coord[0][i])*(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    perm = gsl_permutation_alloc (nvert-1);

    for (i=0,k=0;i<nvert-1;i++)
	for (j=0;j<nvert-1;j++,k++)
	    w_data[k]=coord[i+1][j]-coord[0][j];

    //gsl_matrix_transpose_memcpy (wt,w);

    gsl_blas_dgemm (CblasNoTrans, CblasTrans,
		    1.0, &w.matrix, &w.matrix,
		    0.0, &m.matrix);

    gsl_linalg_LU_decomp(&m.matrix,perm,&s);
    det=gsl_linalg_LU_det(&m.matrix,s);

    gsl_permutation_free(perm);

    for (i=2,j=1;i<=nvert-1;i++) j*=i;

    return sqrt(fabs(det))/j;
}

double SimplexVolumef(float **coord,int nvert, int ndims)
{
    int i,j,k;
    double det;
    int s;
    double m_data[(nvert-1)*(nvert-1)];
    double w_data[(nvert-1)*(nvert-1)];
/*
    double w_data[3*3] = {1.,0.,0.,
			  1.,1.,0.,
			  1.,1.,1.};*/
    //double wt_data[(nvert-1)*ndims];
    gsl_permutation * perm;

    gsl_matrix_view m = gsl_matrix_view_array (m_data, nvert-1, nvert-1);
    gsl_matrix_view w = gsl_matrix_view_array (w_data, nvert-1, ndims);
    //gsl_matrix_view wt = gsl_matrix_view_array (wt_data, nvert-1,ndims);

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(double)(coord[1][i]-coord[0][i])*(double)(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    perm = gsl_permutation_alloc (nvert-1);

    for (i=0,k=0;i<nvert-1;i++)
	for (j=0;j<nvert-1;j++,k++)
	    w_data[k]=coord[i+1][j]-coord[0][j];

    //gsl_matrix_transpose_memcpy (wt,w);

    gsl_blas_dgemm (CblasNoTrans, CblasTrans,
		    1.0, &w.matrix, &w.matrix,
		    0.0, &m.matrix);

    gsl_linalg_LU_decomp(&m.matrix,perm,&s);
    det=gsl_linalg_LU_det(&m.matrix,s);

    gsl_permutation_free(perm);

    for (i=2,j=1;i<=nvert-1;i++) j*=i;

    return sqrt(fabs(det))/j;
}

// center must be allocated to ndims vals at least.
// coord are the vertice coords
// center is the center of the resulting ndims-sphere
// function returns its radius squared
double SimplexSphere(double **coord, int ndims,double *center)
{
    double a_data[ndims*ndims];
    double b_data[ndims];

    gsl_matrix_view m=gsl_matrix_view_array (a_data, ndims, ndims);
    gsl_vector_view b=gsl_vector_view_array (b_data, ndims);
    gsl_vector_view x = gsl_vector_view_array (center,ndims);
    gsl_permutation * p = gsl_permutation_alloc (ndims);

    int s;
    int i,j,k;
    double d;

    for (i=0,k=0;i<ndims;i++)
    {
	b_data[i]=0;
	for (j=0;j<ndims;j++,k++)
	{
	    a_data[k]=((double)coord[0][j]-(double)coord[i+1][j]);
	    b_data[i]+=((double)coord[0][j]*(double)coord[0][j]-(double)coord[i+1][j]*(double)coord[i+1][j]);
	}
	b_data[i]*=0.5;
    }

    gsl_linalg_LU_decomp (&m.matrix, p, &s);
    gsl_linalg_LU_solve (&m.matrix, p, &b.vector, &x.vector);
    gsl_permutation_free (p);

    for (i=0,d=0;i<ndims;i++)
	d+=((double)center[i]-(double)coord[0][i])*((double)center[i]-(double)coord[0][i]);

    return d;
}

#else

double SimplexVolumef(float **coord,int nvert, int ndims)
{
    int i;
    double det;

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(double)(coord[1][i]-coord[0][i])*(double)(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    fprintf(stderr,"ERROR: Function 'SimplexVolume' needs GSL.\n");
    fprintf(stderr,"Please, recompile with GSL.\n");
    return -1;
}

double SimplexSphere(double **coords, int ndims,double *center)
{
    fprintf(stderr,"ERROR: Function 'SimplexSphere' needs GSL.\n");
    fprintf(stderr,"Please, recompile with GSL.\n");

    return -1;
}

#endif


/* siman/siman_tsp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Mark Galassi
 *
 * 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 <string.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_siman.h>
#include <gsl/gsl_ieee_utils.h>

/* set up parameters for this simulated annealing run */

#define N_TRIES 200		/* how many points do we try before stepping */
#define ITERS_FIXED_T 2000	/* how many iterations for each T? */
#define STEP_SIZE 1.0		/* max step size in random walk */
#define K 1.0			/* Boltzmann constant */
#define T_INITIAL 5000.0	/* initial temperature */
#define MU_T 1.002		/* damping factor for temperature */
#define T_MIN 5.0e-1

gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
			     K, T_INITIAL, MU_T, T_MIN};

struct s_tsp_city {
  const char * name;
  double lat, longitude;	/* coordinates */
};
typedef struct s_tsp_city Stsp_city;

void prepare_distance_matrix(void);
void exhaustive_search(void);
void print_distance_matrix(void);
double city_distance(Stsp_city c1, Stsp_city c2);
double Etsp(void *xp);
double Mtsp(void *xp, void *yp);
void Stsp(const gsl_rng * r, void *xp, double step_size);
void Ptsp(void *xp);

/* in this table, latitude and longitude are obtained from the US
   Census Bureau, at http://www.census.gov/cgi-bin/gazetteer */

Stsp_city cities[] = {{"Santa Fe",    35.68,   105.95},
		      {"Phoenix",     33.54,   112.07},
		      {"Albuquerque", 35.12,   106.62},
		      {"Clovis",      34.41,   103.20},
		      {"Durango",     37.29,   107.87},
		      {"Dallas",      32.79,    96.77},
		      {"Tesuque",     35.77,   105.92},
		      {"Grants",      35.15,   107.84},
		      {"Los Alamos",  35.89,   106.28},
		      {"Las Cruces",  32.34,   106.76},
		      {"Cortez",      37.35,   108.58},
		      {"Gallup",      35.52,   108.74}};

#define N_CITIES (sizeof(cities)/sizeof(Stsp_city))

double distance_matrix[N_CITIES][N_CITIES];

/* distance between two cities */
double city_distance(Stsp_city c1, Stsp_city c2)
{
  const double earth_radius = 6375.000;	/* 6000KM approximately */
  /* sin and cos of lat and long; must convert to radians */
  double sla1 = sin(c1.lat*M_PI/180), cla1 = cos(c1.lat*M_PI/180),
    slo1 = sin(c1.longitude*M_PI/180), clo1 = cos(c1.longitude*M_PI/180);
  double sla2 = sin(c2.lat*M_PI/180), cla2 = cos(c2.lat*M_PI/180),
    slo2 = sin(c2.longitude*M_PI/180), clo2 = cos(c2.longitude*M_PI/180);

  double x1 = cla1*clo1;
  double x2 = cla2*clo2;

  double y1 = cla1*slo1;
  double y2 = cla2*slo2;

  double z1 = sla1;
  double z2 = sla2;

  double dot_product = x1*x2 + y1*y2 + z1*z2;

  double angle = acos(dot_product);

  /* distance is the angle (in radians) times the earth radius */
  return angle*earth_radius;
}

/* energy for the travelling salesman problem */
double Etsp(void *xp)
{
  /* an array of N_CITIES integers describing the order */
  int *route = (int *) xp;
  double E = 0;
  unsigned int i;

  for (i = 0; i < N_CITIES; ++i) {
    /* use the distance_matrix to optimize this calculation; it had
       better be allocated!! */
    E += distance_matrix[route[i]][route[(i + 1) % N_CITIES]];
  }

  return E;
}

double Mtsp(void *xp, void *yp)
{
  int *route1 = (int *) xp, *route2 = (int *) yp;
  double distance = 0;
  unsigned int i;

  for (i = 0; i < N_CITIES; ++i) {
    distance += ((route1[i] == route2[i]) ? 0 : 1);
  }

  return distance;
}

/* take a step through the TSP space */
void Stsp(const gsl_rng * r, void *xp, double step_size)
{
  int x1, x2, dummy;
  int *route = (int *) xp;

  step_size = 0 ; /* prevent warnings about unused parameter */

  /* pick the two cities to swap in the matrix; we leave the first
     city fixed */
  x1 = (gsl_rng_get (r) % (N_CITIES-1)) + 1;
  do {
    x2 = (gsl_rng_get (r) % (N_CITIES-1)) + 1;
  } while (x2 == x1);

  dummy = route[x1];
  route[x1] = route[x2];
  route[x2] = dummy;
}

void Ptsp(void *xp)
{
  unsigned int i;
  int *route = (int *) xp;
  printf("  [");
  for (i = 0; i < N_CITIES; ++i) {
    printf(" %d ", route[i]);
  }
  printf("]  ");
}

int main(void)
{
  int x_initial[N_CITIES];
  unsigned int i;

  const gsl_rng * r = gsl_rng_alloc (gsl_rng_env_setup()) ;

  gsl_ieee_env_setup ();

  prepare_distance_matrix();

  /* set up a trivial initial route */
  printf("# initial order of cities:\n");
  for (i = 0; i < N_CITIES; ++i) {
    printf("# \"%s\"\n", cities[i].name);
    x_initial[i] = i;
  }

  printf("# distance matrix is:\n");
  print_distance_matrix();

  printf("# initial coordinates of cities (longitude and latitude)\n");
  /* this can be plotted with */
  /* ./siman_tsp > hhh ; grep city_coord hhh | awk '{print $2 "   " $3}' | xyplot -ps -d "xy" > c.eps */
  for (i = 0; i < N_CITIES+1; ++i) {
    printf("###initial_city_coord: %g %g \"%s\"\n",
	   -cities[x_initial[i % N_CITIES]].longitude,
	   cities[x_initial[i % N_CITIES]].lat,
	   cities[x_initial[i % N_CITIES]].name);
  }

/*   exhaustive_search(); */

  gsl_siman_solve(r, x_initial, Etsp, Stsp, Mtsp, Ptsp, NULL, NULL, NULL,
		  N_CITIES*sizeof(int), params);

  printf("# final order of cities:\n");
  for (i = 0; i < N_CITIES; ++i) {
    printf("# \"%s\"\n", cities[x_initial[i]].name);
  }

  printf("# final coordinates of cities (longitude and latitude)\n");
  /* this can be plotted with */
  /* ./siman_tsp > hhh ; grep city_coord hhh | awk '{print $2 "   " $3}' | xyplot -ps -d "xy" > c.eps */
  for (i = 0; i < N_CITIES+1; ++i) {
    printf("###final_city_coord: %g %g %s\n",
	   -cities[x_initial[i % N_CITIES]].longitude,
	   cities[x_initial[i % N_CITIES]].lat,
	   cities[x_initial[i % N_CITIES]].name);
  }

  printf("# ");
  fflush(stdout);
#if 0
  system("date");
#endif /* 0 */
  fflush(stdout);

  return 0;
}

void prepare_distance_matrix()
{
  unsigned int i, j;
  double dist;

  for (i = 0; i < N_CITIES; ++i) {
    for (j = 0; j < N_CITIES; ++j) {
      if (i == j) {
	dist = 0;
      } else {
	dist = city_distance(cities[i], cities[j]);
      }
      distance_matrix[i][j] = dist;
    }
  }
}

void print_distance_matrix()
{
  unsigned int i, j;

  for (i = 0; i < N_CITIES; ++i) {
    printf("# ");
    for (j = 0; j < N_CITIES; ++j) {
      printf("%15.8f   ", distance_matrix[i][j]);
    }
    printf("\n");
  }
}

/* [only works for 12] search the entire space for solutions */
static double best_E = 1.0e100, second_E = 1.0e100, third_E = 1.0e100;
static int best_route[N_CITIES];
static int second_route[N_CITIES];
static int third_route[N_CITIES];
static void do_all_perms(int *route, int n);

void exhaustive_search()
{
  static int initial_route[N_CITIES] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
  printf("\n# ");
  fflush(stdout);
#if 0
  system("date");
#endif
  fflush(stdout);
  do_all_perms(initial_route, 1);
  printf("\n# ");
  fflush(stdout);
#if 0
  system("date");
#endif /* 0 */
  fflush(stdout);

  printf("# exhaustive best route: ");
  Ptsp(best_route);
  printf("\n# its energy is: %g\n", best_E);

  printf("# exhaustive second_best route: ");
  Ptsp(second_route);
  printf("\n# its energy is: %g\n", second_E);

  printf("# exhaustive third_best route: ");
  Ptsp(third_route);
  printf("\n# its energy is: %g\n", third_E);
}

/* James Theiler's recursive algorithm for generating all routes */
static void do_all_perms(int *route, int n)
{
  if (n == (N_CITIES-1)) {
    /* do it! calculate the energy/cost for that route */
    double E;
    E = Etsp(route);		/* TSP energy function */
    /* now save the best 3 energies and routes */
    if (E < best_E) {
      third_E = second_E;
      memcpy(third_route, second_route, N_CITIES*sizeof(*route));
      second_E = best_E;
      memcpy(second_route, best_route, N_CITIES*sizeof(*route));
      best_E = E;
      memcpy(best_route, route, N_CITIES*sizeof(*route));
    } else if (E < second_E) {
      third_E = second_E;
      memcpy(third_route, second_route, N_CITIES*sizeof(*route));
      second_E = E;
      memcpy(second_route, route, N_CITIES*sizeof(*route));
    } else if (E < third_E) {
      third_E = E;
      memcpy(route, third_route, N_CITIES*sizeof(*route));
    }
  } else {
    int new_route[N_CITIES];
    unsigned int j;
    int swap_tmp;
    memcpy(new_route, route, N_CITIES*sizeof(*route));
    for (j = n; j < N_CITIES; ++j) {
      swap_tmp = new_route[j];
      new_route[j] = new_route[n];
      new_route[n] = swap_tmp;
      do_all_perms(new_route, n+1);
    }
  }
}


#pragma once

#include <stdint.h>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

#include "nn_math.h"
#include "nn_utils.h"

typedef struct {
    size_t size;
    uintmax_t *layers;
    gsl_vector **biases;
    gsl_matrix **weights;
} nn_network;

nn_network * nn_network_create(const size_t n, const uintmax_t *layers);
void nn_network_destroy(nn_network *network);
void nn_network_set_biases(nn_network *network, gsl_vector **biases);
void nn_network_get_biases(nn_network *network, gsl_vector **biases);
void nn_network_set_weights(nn_network *network, gsl_matrix **weights);
void nn_network_get_weights(nn_network *network, gsl_matrix **weights);
gsl_vector * nn_network_ff(nn_network *network, gsl_vector *x);


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>

double f  (double x,void *params);
double f  (double x,void *params)
{
        double n=* (double *) params;
        double f=exp (-x) * pow (x,n);
        return f;
}

void integral_gen (int nmin, int nmax, double vals[]);
void integral_gen (int nmin, int nmax, double vals[])
{
	double a=0.,b=1;
        double abserr=1.e-9,relerr=1.e-9;
        double result;
        double error;
        double n=10;
        size_t np=1000;
	 gsl_integration_workspace *w = gsl_integration_workspace_alloc (np);
                gsl_function F;
                F.function = &f;

	for (int i=nmin;i<=nmax;i++)
	{
        	F.params =&n;
        	gsl_integration_qag (&F, a, b, abserr, relerr,np,GSL_INTEG_GAUSS15, w, &result, &error);
		vals[i]=result;
	}
	gsl_integration_workspace_free (w);
}


#include "lah.h"

#ifdef HAVE_LAPACK /* Use a LAPACK package to do the heavy work */
/*#include <cblas.h>*/
#include <lapacke.h>
lah_Return lah_forwardSub(lah_mat *B, lah_mat const *L)
{
    if ( L == NULL ||  B == NULL || L->nR != B->nR
        || B->nR != L->nC)
    {
        return lahReturnParameterError;
    }

    /*
    matrix_layout = LAH_LAPACK_LAYOUT;
    if (matrix_layout == LAPACK_COL_MAJOR)
    {
        lda = L->nR;
        ldb = B->nR;
    }
    else
    {
        lda = L->nC;
        ldb = B->nC;
    }
    */

    return (!POTRS(LAH_LAPACK_LAYOUT, 'L', L->nR, B->nC,
                   L->data, LAH_LEADING_DIM(L), B->data, LAH_LEADING_DIM(B))) ? lahReturnOk : lahReturnExternError;
}

#else /* fallback to primitive linear algebra subroutines */
#include <stdlib.h>
#include <math.h>
lah_Return lah_forwardSub(lah_mat *B, const lah_mat  *L)
{
    lah_index i, j, k;
    lah_value *diag, *row, *Aij, *bi, *bj;

    if ( L == NULL ||  B == NULL || L->nR != B->nR
        || B->nR != L->nC)
    {
        return lahReturnParameterError;
    }

    /* loop over cols of x and b */
    for (k = 0; k < B->nC; k++)
    {
        /* initialize pointers for i-loop*/
        bi = B->data + k * B->incCol;
        diag = L->data;
        row = L->data;
        /* loop over rows of x*/
        for (i = 0; i < B->nR; i++)
        {
            /* initialize pointers for j-loop */
            bj = B->data + k * B->incCol;
            Aij = row;

            /* substitute up to (excluding) current x */
            for (j = 0; j < i; j++)
            {
                *bi -= *Aij * *bj;

                /* End of Loop: Increment Pointers */
                bj += B->incRow;
                Aij += L->incCol;
            }
            /* divide xi by diagonal element of L */
            if (fabs(*diag) > 0.00001)
                *bi /= *diag;
            else
                return lahReturnMathError;

            /* End of Loop: Increment Pointers */
            diag += L->incRow + L->incCol;
            bi += B->incRow;
            row += L->incRow;
        }
    }
    return lahReturnOk;
}

#endif /* endif primitive implementation */


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

typedef struct {
    PetscInt dim;
    PetscReal k;
    PetscReal gamma;
    PetscReal Rgas;
    PetscReal L;
} Constants;

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

static PetscReal ComputeTExact( PetscReal time, const PetscReal xyz[], Constants *constants, PetscReal rho){

    PetscReal cv = constants->gamma*constants->Rgas/(constants->gamma - 1) - constants->Rgas;

    PetscReal alpha = constants->k/(rho*cv);
    PetscReal Tinitial = 100.0;
    PetscReal T = 300.0;
    for(PetscReal n =1; n < 2000; n ++){
        PetscReal Bn = -Tinitial*2.0*(-1.0 + PetscPowReal(-1.0, n))/(n*PETSC_PI);
        T += Bn*PetscSinReal(n * PETSC_PI*xyz[0]/constants->L)*PetscExpReal(-n*n*PETSC_PI*PETSC_PI*alpha*time/(PetscSqr(constants->L)));
    }

    return T;
}

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

    Constants *constants = (Constants *)ctx;

    PetscReal T = ComputeTExact(time, xyz, constants, 1.0);

    PetscReal u = 0.0;
    PetscReal v = xyz[0];
    PetscReal rho = 1.0;
    PetscReal p= rho*constants->Rgas*T;
    PetscReal e = p/((constants->gamma - 1.0)*rho);
    PetscReal eT = e + 0.5*(u*u + v*v);

    node[RHO] = rho;
    node[RHOE] = rho*eT;
    node[RHOU + 0] = rho*u;
    node[RHOU + 1] = rho*v;

    PetscFunctionReturn(0);
}

static PetscReal computeTemperature(PetscInt dim, const PetscScalar* conservedValues, PetscReal gamma, PetscReal Rgas){
    PetscReal density = conservedValues[RHO];
    PetscReal totalEnergy = conservedValues[RHOE]/density;

    // Get the velocity in this direction
    PetscReal speedSquare = 0.0;
    for (PetscInt d =0; d < dim; d++){
        speedSquare += PetscSqr(conservedValues[RHOU + d]/density);
    }

    // assumed eos
    PetscReal internalEnergy = (totalEnergy) - 0.5 * speedSquare;
    PetscReal p = (gamma - 1.0)*density*internalEnergy;

    PetscReal T = p/(Rgas*density);

    return T;
}

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

    FlowData flowData = (FlowData)ctx;

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

    ierr = FlowViewFromOptions(flowData, "-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);

    ierr = PetscObjectSetName((PetscObject)exactVec, "exact");
    CHKERRQ(ierr);
    ierr = VecViewFromOptions(exactVec, NULL, "-sol_view");
    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);

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

    // And the infinity error
    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 = PetscObjectSetName((PetscObject)exactVec, "error");
    CHKERRQ(ierr);
    ierr = VecViewFromOptions(exactVec, NULL, "-sol_view");
    CHKERRQ(ierr);

    ierr = VecDestroy(&exactVec);
    CHKERRQ(ierr);
    PetscFunctionReturn(0);
}
//
//static PetscErrorCode FillBoundary(PetscInt dim, PetscInt dof, const PetscFVFaceGeom *faceGeom, const PetscFVCellGeom *cellGeom, const PetscFVCellGeom *cellGeomG, const PetscScalar *a_xI, const PetscScalar *a_xGradI, const PetscScalar *a_xG,  PetscScalar *a_xGradG, void *ctx){
//
//    for(PetscInt pd = 0; pd < dof; ++pd) {
//        PetscReal dPhidS = a_xG[pd] - a_xI[pd];
//
//        // over each direction
//        for (PetscInt dir = 0; dir < dim; dir++) {
//            PetscReal dx = (cellGeomG->centroid[dir] - faceGeom->centroid[dir]);
//
//            // If there is a contribution in this direction
//            if (PetscAbs(dx) > 1E-8) {
//                a_xGradG[pd*dim + dir] = dPhidS / (dx);
//            } else {
//                a_xGradG[pd*dim + dir] = 0.0;
//            }
//        }
//    }
//    return 0;
//}
//
///**
// * this function updates the boundaries with the gradient computed from the boundary cell value
// * @param dm
// * @param auxFvm
// * @param gradLocalVec
// * @return
// */
//static PetscErrorCode DMPlexFillGradientInBoundaryFVM(DM dm, PetscFV auxFvm, Vec localXVec, Vec gradLocalVec){
//    PetscFunctionBeginUser;
//    PetscErrorCode ierr;
//
//    // Get the dmGrad
//    DM dmGrad;
//    ierr = VecGetDM(gradLocalVec, &dmGrad);CHKERRQ(ierr);
//
//    // get the problem
//    PetscDS prob;
//    PetscInt nFields;
//    ierr = DMGetDS(dm, &prob);CHKERRQ(ierr);
//    ierr = PetscDSGetNumFields(prob, &nFields);CHKERRQ(ierr);
//    if (nFields > 1){
//        SETERRQ(PetscObjectComm((PetscObject) dm), PETSC_ERR_ARG_WRONG, "Cannot fill DMPlexFillGradientInBoundaryFVM with more than a single field." );
//    }
//    PetscInt field;
//    ierr = PetscDSGetFieldIndex(prob, (PetscObject) auxFvm, &field);CHKERRQ(ierr);
//    PetscInt dof;
//    ierr = PetscDSGetFieldSize(prob, field, &dof);CHKERRQ(ierr);
//
//    // Obtaining local cell ownership
//    PetscInt cellStart, cellEnd, cEndInterior;
//    ierr = DMPlexGetHeightStratum(dm, 0, &cellStart, &cellEnd);CHKERRQ(ierr);
//    ierr = DMPlexGetGhostCellStratum(dm, &cEndInterior, NULL);CHKERRQ(ierr);
//    cEndInterior = cEndInterior < 0 ? cellEnd: cEndInterior;
//
//    // Get the fvm face and cell geometry
//    Vec cellGeomVec = NULL;/* vector of structs related to cell geometry*/
//    Vec faceGeomVec = NULL;/* vector of structs related to face geometry*/
//    ierr = DMPlexGetGeometryFVM(dm, &faceGeomVec, &cellGeomVec, NULL);CHKERRQ(ierr);
//
//    // get the dm for each geom type
//    DM dmFaceGeom, dmCellGeom;
//    ierr = VecGetDM(faceGeomVec, &dmFaceGeom);CHKERRQ(ierr);
//    ierr = VecGetDM(cellGeomVec, &dmCellGeom);CHKERRQ(ierr);
//
//    // extract the gradLocalVec
//    PetscScalar * gradLocalArray;
//    ierr = VecGetArray(gradLocalVec, &gradLocalArray);CHKERRQ(ierr);
//
//    const PetscScalar* localArray;
//    ierr = VecGetArrayRead(localXVec, &localArray);CHKERRQ(ierr);
//
//    // get the dim
//    PetscInt dim;
//    ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);
//
//    // extract the arrays for the face and cell geom, along with their dm
//    const PetscScalar *cellGeomArray;
//    ierr = VecGetArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//
//    const PetscScalar *faceGeomArray;
//    ierr = VecGetArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    // March over each face
//    for (PetscInt cell = cellStart; cell < cEndInterior; ++cell) {
//        // Get the face information
//        PetscInt               numFaces;
//        const PetscInt        *faces;
//        ierr = DMPlexGetConeSize(dm, cell, &numFaces);CHKERRQ(ierr);
//        ierr = DMPlexGetCone(dm, cell, &faces);CHKERRQ(ierr);
//
//        //March over each face
//        for(PetscInt f =0 ; f < numFaces; f++){
//            PetscBool boundary;
//            ierr = DMIsBoundaryPoint(dm, faces[f], &boundary);CHKERRQ(ierr);
//
//            // if this is on the boundary
//            if(boundary){
//                // get the boundary cell index
//                const PetscInt        *fcells;
//                ierr  = DMPlexGetSupport(dm, faces[f], &fcells);CHKERRQ(ierr);
//                PetscInt side  = (cell != fcells[0]); /* c is on left=0 or right=1 of face */
//                PetscInt ncell = fcells[!side];    /* the neighbor */
//
//                // get the face geom
//                const PetscFVFaceGeom  *faceGeom;
//                ierr  = DMPlexPointLocalRead(dmFaceGeom, faces[f], faceGeomArray, &faceGeom);CHKERRQ(ierr);
//
//                // get the cell centroid information
//                const PetscFVCellGeom       *cellGeom;
//                const PetscFVCellGeom       *cellGeomGhost;
//                ierr  = DMPlexPointLocalRead(dmCellGeom, cell, cellGeomArray, &cellGeom);CHKERRQ(ierr);
//                ierr  = DMPlexPointLocalRead(dmCellGeom, ncell, cellGeomArray, &cellGeomGhost);CHKERRQ(ierr);
//
//                // Read the local point
//                PetscScalar* boundaryGradCellValues;
//                ierr  = DMPlexPointLocalRef(dmGrad, ncell, gradLocalArray, &boundaryGradCellValues);CHKERRQ(ierr);
//
//                const PetscScalar*  cellGradValues;
//                ierr  = DMPlexPointLocalRead(dmGrad, cell, gradLocalArray, &cellGradValues);CHKERRQ(ierr);
//
//                const PetscScalar* boundaryCellValues;
//                ierr  = DMPlexPointLocalRead(dm, ncell, localArray, &boundaryCellValues);CHKERRQ(ierr);
//                const PetscScalar* cellValues;
//                ierr  = DMPlexPointLocalRead(dm, cell, localArray, &cellValues);CHKERRQ(ierr);
//
//                // compute the gradient for the boundary node and pass in
//                //const PetscReal *cellGeom, const PetscReal *cellGeomG, const PetscScalar *a_xI, const PetscScalar *a_xGradI, PetscScalar *a_xG,  PetscScalar *a_xGradG, void *ctx
//                ierr = FillBoundary(dim, dof, faceGeom, cellGeom, cellGeomGhost, cellValues, cellGradValues, boundaryCellValues, boundaryGradCellValues, NULL);CHKERRQ(ierr);
//            }
//        }
//    }
//
//    ierr = VecRestoreArrayRead(localXVec, &localArray);CHKERRQ(ierr);
//    ierr = VecRestoreArray(gradLocalVec, &gradLocalArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    PetscFunctionReturn(0);
//}
//
//static PetscErrorCode DiffusionSource(DM dm, PetscReal time, Vec locXVec, Vec globFVec, void *ctx){
//    // Call the flux calculation
//    PetscErrorCode ierr;
//
//    ProblemSetup *setup = (ProblemSetup *)ctx;
//
//    // call the base rhs function eval
//    ierr = DMPlexTSComputeRHSFunctionFVM(dm, time, locXVec, globFVec, setup->flowData);CHKERRQ(ierr);
//
//    // update the aux fields
//    ierr = UpdateAuxFields(NULL, locXVec, setup->flowData);CHKERRQ(ierr);
//
//    Constants constants = setup->constants;
//
//    // get the fvm field assuming that it is the first
//    PetscFV auxFvm;
//    ierr = DMGetField(setup->flowData->auxDm,0, NULL, (PetscObject*)&auxFvm);CHKERRQ(ierr);
//
//    PetscInt dim;
//    ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);
//
//    // Get the locXArray
//    const PetscScalar *locXArray;
//    ierr = VecGetArrayRead(locXVec, &locXArray);CHKERRQ(ierr);
//
//    // Get the fvm face and cell geometry
//    Vec cellGeomVec = NULL;/* vector of structs related to cell geometry*/
//    Vec faceGeomVec = NULL;/* vector of structs related to face geometry*/
//
//    // extract the fvm data
//    ierr = DMPlexGetGeometryFVM(setup->flowData->dm, &faceGeomVec, &cellGeomVec, NULL);CHKERRQ(ierr);
//
//    // Get the needed auxDm
//    DM auxFieldGradDM = NULL; /* dm holding the grad information */
//    ierr = DMPlexGetDataFVM(setup->flowData->auxDm, auxFvm, NULL, NULL, &auxFieldGradDM);CHKERRQ(ierr);
//    if(!auxFieldGradDM){
//        SETERRQ(PetscObjectComm((PetscObject)setup->flowData->auxDm), PETSC_ERR_ARG_WRONGSTATE, "The FVM method for aux variables must support computing gradients.");
//    }
//
//    // get the dm for each geom type
//    DM dmFaceGeom, dmCellGeom;
//    ierr = VecGetDM(faceGeomVec, &dmFaceGeom);CHKERRQ(ierr);
//    ierr = VecGetDM(cellGeomVec, &dmCellGeom);CHKERRQ(ierr);
//
//    // extract the arrays for the face and cell geom, along with their dm
//    const PetscScalar *faceGeomArray, *cellGeomArray;
//    ierr = VecGetArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecGetArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    // Obtaining local cell and face ownership
//    PetscInt faceStart, faceEnd;
//    PetscInt cellStart, cellEnd;
//    ierr = DMPlexGetHeightStratum(dm, 1, &faceStart, &faceEnd);CHKERRQ(ierr);
//    ierr = DMPlexGetHeightStratum(dm, 0, &cellStart, &cellEnd);CHKERRQ(ierr);
//
//    // get the ghost label
//    DMLabel ghostLabel;
//    ierr = DMGetLabel(dm, "ghost", &ghostLabel);CHKERRQ(ierr);
//
//    // extract the localFArray from the locFVec
//    PetscScalar *fa;
//    Vec locFVec;
//    ierr = DMGetLocalVector(dm, &locFVec);CHKERRQ(ierr);
//    ierr = VecZeroEntries(locFVec);CHKERRQ(ierr);
//    ierr = VecGetArray(locFVec, &fa);CHKERRQ(ierr);
//
//    // create a global and local grad vector for the auxField
//    Vec gradGlobalVec, gradLocalVec;
//    ierr = DMCreateGlobalVector(auxFieldGradDM, &gradGlobalVec);CHKERRQ(ierr);
//    ierr = VecSet(gradGlobalVec, NAN);CHKERRQ(ierr);
//
//    // compute the global grad values
//    ierr = DMPlexReconstructGradientsFVM(setup->flowData->auxDm, setup->flowData->auxField, gradGlobalVec);CHKERRQ(ierr);
//
//    // Map to a local grad vector
//    ierr = DMCreateLocalVector(auxFieldGradDM, &gradLocalVec);CHKERRQ(ierr);
//    ierr = DMGlobalToLocalBegin(auxFieldGradDM, gradGlobalVec, INSERT_VALUES, gradLocalVec);CHKERRQ(ierr);
//    ierr = DMGlobalToLocalEnd(auxFieldGradDM, gradGlobalVec, INSERT_VALUES, gradLocalVec);CHKERRQ(ierr);
//
//    // fill the boundary conditions
//    ierr = DMPlexFillGradientInBoundaryFVM(setup->flowData->auxDm, auxFvm, setup->flowData->auxField,  gradLocalVec);CHKERRQ(ierr);
//
//    // access the local vector
//    const PetscScalar *localGradArray;
//    ierr = VecGetArrayRead(gradLocalVec,&localGradArray);CHKERRQ(ierr);
//
//    // march over each face
//    for (PetscInt face = faceStart; face < faceEnd; ++face) {
//        PetscFVFaceGeom       *fg;
//        PetscFVCellGeom       *cgL, *cgR;
//        const PetscScalar           *gradL;
//        const PetscScalar           *gradR;
//
//        // make sure that this is a valid face to check
//        PetscInt  ghost, nsupp, nchild;
//        ierr = DMLabelGetValue(ghostLabel, face, &ghost);CHKERRQ(ierr);
//        ierr = DMPlexGetSupportSize(dm, face, &nsupp);CHKERRQ(ierr);
//        ierr = DMPlexGetTreeChildren(dm, face, &nchild, NULL);CHKERRQ(ierr);
//        if (ghost >= 0 || nsupp > 2 || nchild > 0){
//            continue;// skip this face
//        }
//
//        // get the face geometry
//        ierr = DMPlexPointLocalRead(dmFaceGeom, face, faceGeomArray, &fg);CHKERRQ(ierr);
//
//        // Get the left and right cells for this face
//        const PetscInt        *faceCells;
//        ierr = DMPlexGetSupport(dm, face, &faceCells);CHKERRQ(ierr);
//
//        // get the cell geom for the left and right faces
//        ierr = DMPlexPointLocalRead(dmCellGeom, faceCells[0], cellGeomArray, &cgL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalRead(dmCellGeom, faceCells[1], cellGeomArray, &cgR);CHKERRQ(ierr);
//
//        // extract the cell grad
//        ierr = DMPlexPointLocalRead(auxFieldGradDM, faceCells[0], localGradArray, &gradL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalRead(auxFieldGradDM, faceCells[1], localGradArray, &gradR);CHKERRQ(ierr);
//
//        PetscInt f = 0;
//
//        // extract the field values
//        PetscScalar *xL, *xR,
//        ierr = DMPlexPointLocalFieldRead(dm, faceCells[0], f, locXArray, &xL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalFieldRead(dm, faceCells[1], f, locXArray, &xR);CHKERRQ(ierr);
//
//        // Compute the normal grad
//        PetscReal normalGrad = 0.0;
//        PetscInt dof = 0;
//        for (PetscInt d = 0; d < dim; ++d){
//            normalGrad += fg->normal[d]*0.5*(gradL[dof*dim + d] + gradR[dof*dim + d]);
//        }
//
//        // compute the normal heatFlux
//        PetscReal normalHeatFlux = -constants.k *normalGrad;
//
//        // Add to the source terms of f
//        PetscScalar    *fL = NULL, *fR = NULL;
//        ierr = DMLabelGetValue(ghostLabel,faceCells[0],&ghost);CHKERRQ(ierr);
//        if (ghost <= 0) {ierr = DMPlexPointLocalFieldRef(dm, faceCells[0], f, fa, &fL);CHKERRQ(ierr);}
//        ierr = DMLabelGetValue(ghostLabel,faceCells[1],&ghost);CHKERRQ(ierr);
//        if (ghost <= 0) {ierr = DMPlexPointLocalFieldRef(dm, faceCells[1], f, fa, &fR);CHKERRQ(ierr);}
//
//        if(fL){
//            fL[RHOE] -= normalHeatFlux/cgL->volume;
//        }
//        if(fR){
//            fR[RHOE] += normalHeatFlux/cgR->volume;
//        }
//    }
//
//    // Add the new locFVec to the globFVec
//    ierr = VecRestoreArray(locFVec, &fa);CHKERRQ(ierr);
//    ierr = DMLocalToGlobalBegin(dm, locFVec, INSERT_VALUES, globFVec);CHKERRQ(ierr);
//    ierr = DMLocalToGlobalEnd(dm, locFVec, INSERT_VALUES, globFVec);CHKERRQ(ierr);
//    ierr = DMRestoreLocalVector(dm, &locFVec);CHKERRQ(ierr);
//
//    // restore the arrays
//    ierr = VecRestoreArrayRead(gradLocalVec, &localGradArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(locXVec, &locXArray);CHKERRQ(ierr);
//
//    // destroy grad vectors
//    ierr = VecDestroy(&gradGlobalVec);CHKERRQ(ierr);
//    ierr = VecDestroy(&gradLocalVec);CHKERRQ(ierr);
//
////    VecView(globFVec, PETSC_VIEWER_STDOUT_WORLD);
//
//    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;

    // compute the centroid location of the real cell
    // 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;//TODO:zero boundary
        x[i] = c[i] + n[i]*0.5;//TODO:uniform grad
    }

    // compute the temperature
    PetscReal Tinside =   ComputeTExact(time, x, constants, 1.0);
    PetscReal boundaryValue = 300.0;

    PetscReal T = boundaryValue;//boundaryValue - (Tinside - boundaryValue);//TODO: fix uniform grad

//    PetscReal T = c[0] < constants->L/2.0 ? 300 : 400;

    PetscReal u = 0.0;
    PetscReal v = 0.0;
    PetscReal rho = 1.0;
    PetscReal p= rho*constants->Rgas*T;
    PetscReal e = p/((constants->gamma - 1.0)*rho);
    PetscReal eT = e + 0.5*(u*u + v*v);

    a_xG[RHO] = rho;
    a_xG[RHOE] = rho*eT;
    a_xG[RHOU + 0] = rho*u;
    a_xG[RHOU + 1] = rho*v;

    PetscFunctionReturn(0);
}

static PetscErrorCode PhysicsBoundary_Mirror(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
    for(PetscInt f =0; f < RHOU + constants->dim; f++){
        a_xG[f] = a_xI[f];
    }
    PetscFunctionReturn(0);
}

int main(int argc, char **argv)
{
    // Setup the problem
    Constants constants;

    // sub sonic
    constants.dim = 2;
    constants.L = 0.1;
    constants.gamma = 1.4;
    constants.k = 0.3;
    constants.Rgas = 1.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};
    PetscReal nxHeight = 3;
    PetscInt nx[] = {nxHeight, nxHeight};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, constants.dim, PETSC_FALSE, nx, start, end, NULL, 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;
    params[RGAS] = constants.Rgas;
    params[K] = constants.k;

    // set up the finite volume fluxes
    CompressibleFlow_StartProblemSetup(flowData, TOTAL_COMPRESSIBLE_FLOW_PARAMETERS, params);

    // Add in any boundary conditions
    PetscDS prob;
    ierr = DMGetDS(flowData->dm, &prob);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    const PetscInt idsLeft[]= {2, 4};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall left", "Face Sets", 0, 0, NULL, (void (*)(void))PhysicsBoundary_Euler, NULL, 2, idsLeft, &constants);CHKERRQ(ierr);

    const PetscInt idsTop[]= {1, 3};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "top/bottom", "Face Sets", 0, 0, NULL, (void (*)(void))PhysicsBoundary_Mirror, NULL, 2, idsTop, &constants);CHKERRQ(ierr);

//    ierr = FlowRegisterAuxField(flowData, "Temperature", "T", 1, FV);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, DiffusionSource, &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, problemSetup.flowData, NULL);CHKERRQ(ierr);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // set the initial conditions
    PetscErrorCode     (*func[2]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {InitialConditions};
    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, InitialConditions, &constants);CHKERRQ(ierr);

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

    // Compute the end time so it goes around once
    // compute dt
    PetscReal cv = constants.gamma*constants.Rgas/(constants.gamma - 1) - constants.Rgas;
    PetscReal dxMin = constants.L/(10 * PetscPowRealInt(2, 2));

    PetscReal alpha = PetscAbs(constants.k/ (cv * 1.0));
    double dt_conduc = 0.00000625;//.3*PetscSqr(dxMin) / alpha;

    PetscPrintf(PETSC_COMM_WORLD, "dt_conduc: %f\n", dt_conduc);
    TSSetTimeStep(ts, dt_conduc);
//    TSSetMaxTime(ts, 0.001);
    TSSetMaxSteps(ts, 600);

    PetscDSView(prob, PETSC_VIEWER_STDOUT_WORLD);

    DMView(flowData->auxDm, PETSC_VIEWER_STDOUT_WORLD);

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

    FlowDestroy(&flowData);
    TSDestroy(&ts);

    return PetscFinalize();

}