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

/* Author:  G. Jungman */

#ifndef __GSL_SF_GAMMA_H__
#define __GSL_SF_GAMMA_H__

#include <gsl/gsl_sf_result.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/* Log[Gamma(x)], x not a negative integer
 * Uses real Lanczos method.
 * Returns the real part of Log[Gamma[x]] when x < 0,
 * i.e. Log[|Gamma[x]|].
 *
 * exceptions: GSL_EDOM, GSL_EROUND
 */
int gsl_sf_lngamma_e(double x, gsl_sf_result * result);
double gsl_sf_lngamma(const double x);

/* Log[Gamma(x)], x not a negative integer
 * Uses real Lanczos method. Determines
 * the sign of Gamma[x] as well as Log[|Gamma[x]|] for x < 0.
 * So Gamma[x] = sgn * Exp[result_lg].
 *
 * exceptions: GSL_EDOM, GSL_EROUND
 */
int gsl_sf_lngamma_sgn_e(double x, gsl_sf_result * result_lg, double *sgn);

/* Gamma(x), x not a negative integer
 * Uses real Lanczos method.
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EROUND
 */
int gsl_sf_gamma_e(const double x, gsl_sf_result * result);
double gsl_sf_gamma(const double x);

/* Regulated Gamma Function, x > 0
 * Gamma^*(x) = Gamma(x)/(Sqrt[2Pi] x^(x-1/2) exp(-x))
 *            = (1 + 1/(12x) + ...),  x->Inf
 * A useful suggestion of Temme.
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_gammastar_e(const double x, gsl_sf_result * result);
double gsl_sf_gammastar(const double x);

/* 1/Gamma(x)
 * Uses real Lanczos method.
 *
 * exceptions: GSL_EUNDRFLW, GSL_EROUND
 */
int gsl_sf_gammainv_e(const double x, gsl_sf_result * result);
double gsl_sf_gammainv(const double x);

/* Log[Gamma(z)] for z complex, z not a negative integer
 * Uses complex Lanczos method. Note that the phase part (arg)
 * is not well-determined when |z| is very large, due
 * to inevitable roundoff in restricting to (-Pi,Pi].
 * This will raise the GSL_ELOSS exception when it occurs.
 * The absolute value part (lnr), however, never suffers.
 *
 * Calculates:
 *   lnr = log|Gamma(z)|
 *   arg = arg(Gamma(z))  in (-Pi, Pi]
 *
 * exceptions: GSL_EDOM, GSL_ELOSS
 */
int gsl_sf_lngamma_complex_e(double zr, double zi, gsl_sf_result * lnr, gsl_sf_result * arg);

/* x^n / n!
 *
 * x >= 0.0, n >= 0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
int gsl_sf_taylorcoeff_e(const int n, const double x, gsl_sf_result * result);
double gsl_sf_taylorcoeff(const int n, const double x);

/* n!
 *
 * exceptions: GSL_EDOM, GSL_OVRFLW
 */
int gsl_sf_fact_e(const unsigned int n, gsl_sf_result * result);
double gsl_sf_fact(const unsigned int n);

/* n!! = n(n-2)(n-4) ...
 *
 * exceptions: GSL_EDOM, GSL_OVRFLW
 */
int gsl_sf_doublefact_e(const unsigned int n, gsl_sf_result * result);
double gsl_sf_doublefact(const unsigned int n);

/* log(n!)
 * Faster than ln(Gamma(n+1)) for n < 170; defers for larger n.
 *
 * exceptions: none
 */
int gsl_sf_lnfact_e(const unsigned int n, gsl_sf_result * result);
double gsl_sf_lnfact(const unsigned int n);

/* log(n!!)
 *
 * exceptions: none
 */
int gsl_sf_lndoublefact_e(const unsigned int n, gsl_sf_result * result);
double gsl_sf_lndoublefact(const unsigned int n);

/* log(n choose m)
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_lnchoose_e(unsigned int n, unsigned int m, gsl_sf_result * result);
double gsl_sf_lnchoose(unsigned int n, unsigned int m);

/* n choose m
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_choose_e(unsigned int n, unsigned int m, gsl_sf_result * result);
double gsl_sf_choose(unsigned int n, unsigned int m);

/* Logarithm of Pochhammer (Apell) symbol
 *   log( (a)_x )
 *   where (a)_x := Gamma[a + x]/Gamma[a]
 *
 * a > 0, a+x > 0
 *
 * exceptions:  GSL_EDOM
 */
int gsl_sf_lnpoch_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_lnpoch(const double a, const double x);

/* Logarithm of Pochhammer (Apell) symbol, with sign information.
 *   result = log( |(a)_x| )
 *   sgn    = sgn( (a)_x )
 *   where (a)_x := Gamma[a + x]/Gamma[a]
 *
 * a != neg integer, a+x != neg integer
 *
 * exceptions:  GSL_EDOM
 */
int gsl_sf_lnpoch_sgn_e(const double a, const double x, gsl_sf_result * result, double * sgn);

/* Pochhammer (Apell) symbol
 *   (a)_x := Gamma[a + x]/Gamma[x]
 *
 * a != neg integer, a+x != neg integer
 *
 * exceptions:  GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_poch_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_poch(const double a, const double x);

/* Relative Pochhammer (Apell) symbol
 *   ((a,x) - 1)/x
 *   where (a,x) = (a)_x := Gamma[a + x]/Gamma[a]
 *
 * exceptions:  GSL_EDOM
 */
int gsl_sf_pochrel_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_pochrel(const double a, const double x);

/* Normalized Incomplete Gamma Function
 *
 * Q(a,x) = 1/Gamma(a) Integral[ t^(a-1) e^(-t), {t,x,Infinity} ]
 *
 * a >= 0, x >= 0
 *   Q(a,0) := 1
 *   Q(0,x) := 0, x != 0
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_gamma_inc_Q_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_gamma_inc_Q(const double a, const double x);

/* Complementary Normalized Incomplete Gamma Function
 *
 * P(a,x) = 1/Gamma(a) Integral[ t^(a-1) e^(-t), {t,0,x} ]
 *
 * a > 0, x >= 0
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_gamma_inc_P_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_gamma_inc_P(const double a, const double x);

/* Non-normalized Incomplete Gamma Function
 *
 * Gamma(a,x) := Integral[ t^(a-1) e^(-t), {t,x,Infinity} ]
 *
 * x >= 0.0
 *   Gamma(a, 0) := Gamma(a)
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_gamma_inc_e(const double a, const double x, gsl_sf_result * result);
double gsl_sf_gamma_inc(const double a, const double x);

/* Logarithm of Beta Function
 * Log[B(a,b)]
 *
 * a > 0, b > 0
 * exceptions: GSL_EDOM
 */
int gsl_sf_lnbeta_e(const double a, const double b, gsl_sf_result * result);
double gsl_sf_lnbeta(const double a, const double b);

int gsl_sf_lnbeta_sgn_e(const double x, const double y, gsl_sf_result * result, double * sgn);

/* Beta Function
 * B(a,b)
 *
 * a > 0, b > 0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
int gsl_sf_beta_e(const double a, const double b, gsl_sf_result * result);
double gsl_sf_beta(const double a, const double b);

/* Normalized Incomplete Beta Function
 * B_x(a,b)/B(a,b)
 *
 * a > 0, b > 0, 0 <= x <= 1
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
int gsl_sf_beta_inc_e(const double a, const double b, const double x, gsl_sf_result * result);
double gsl_sf_beta_inc(const double a, const double b, const double x);

/* The maximum x such that gamma(x) is not
 * considered an overflow.
 */
#define GSL_SF_GAMMA_XMAX  171.0

/* The maximum n such that gsl_sf_fact(n) does not give an overflow. */
#define GSL_SF_FACT_NMAX 170

/* The maximum n such that gsl_sf_doublefact(n) does not give an overflow. */
#define GSL_SF_DOUBLEFACT_NMAX 297

__END_DECLS

#endif /* __GSL_SF_GAMMA_H__ */


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

/* computes a!/b! =  a(a-1)(a-2)...(b+1) */
double factorial_frac(int a, int b)
{
    if(a<0 || b<0)
    {
        perror("factorial_frac: either a or b is negative!!\n");
        exit(1);
    }

    int recip = 0;
    if(a==b)
        return 1.0;
    else if(a<b)
    {
        int temp = b;
        b = a;
        a = temp;
        recip = 1;
    }

    double val = 1;

    while(a > b)
    {
        val = val * a;
        a--;
    }

    if(recip)
        val = 1.0/val;

    return val;

}

void scf_integral(double* mat_cos, double* mat_sin)
{
    long long k;
    double* rr = (double*)malloc(NumPart*sizeof(double));
    double* cos_tt = (double*)malloc(NumPart*sizeof(double));
    double* pp = (double*)malloc(NumPart*sizeof(double));

#ifdef USE_GSL_GEGENBAUER_HOST
    double* xxi = (double*)malloc(NumPart*sizeof(double));
    double this_Cna;
#endif

    /* cos_tt is calculated directly by cos(theta)=z/r because
    we never need theta by itself, but only cos(theta) in the
    legendre polynomials. Also, notice that cos_tt must be
    calculated in double precision because with single precision
    zz[k] can be greater than rr[k] by a tiny bit, which
    the plgndr routine will complain later on */

    for(k=0; k<NumPart; k++)
    {
        rr[k] = sqrt((double)xx[k]*(double)xx[k] +
                      (double)yy[k]*(double)yy[k] +
                      (double)zz[k]*(double)zz[k]);

        cos_tt[k] = (double)zz[k]/rr[k];

        pp[k] = atan2(yy[k], xx[k]);

#ifdef USE_GSL_GEGENBAUER_HOST
        xxi[k] = (rr[k]-1.0) / (rr[k]+1.0);
#endif
    }

    double N_lm;
    double K_nl, I_nl, A_nl;
    double gamm1, gamm2;

    float this_Phi_nl;
    double* P_lm_k = (double*)malloc(NumPart*sizeof(double));

    double m_Phinlk_Plm, Nlm_Anl;
    double sum_cos = 0.0;
    double sum_sin = 0.0;

    int n, l, m;
    for(l=0; l<=decompose_lmax; l++)
    {
        for(m=0; m<=l; m++)
        {
            N_lm = (2*l+1)/4.0/PI * 2.0 * factorial_frac(l-m,l+m);
            if(m==0)
                N_lm = 0.5*N_lm;

            for (k=0; k<NumPart; k++)
            {
                P_lm_k[k] = gsl_sf_legendre_Plm(l,m, cos_tt[k]);

                /* Note: This can be computed more efficiently using gsl_sf_legendre_Plm_array
                which computes a list of legendre polynomials at constant m with varying l.
                but this changes the code and I don't have time to deal with that for now.
                Numerical Recipes's plgndr is actually faster, but single precision. */
            }

            for(n=0; n<=decompose_nmax; n++)
            {
                K_nl = 0.5*n*(n+4*l+3)+(l+1)*(2*l+1);
                gamm1 = factorial_frac(n+4*l+2,n);
                gamm2 = factorial_frac(2*l+1, 2*(2*l+1));
                I_nl = -K_nl/(n+2*l+1.5) * gamm1 * gamm2 * gamm2;
                A_nl = 1.0/I_nl;

                sum_cos = 0.0;
                sum_sin = 0.0;

                for(k=0; k<NumPart; k++)
                {
#ifdef USE_GSL_GEGENBAUER_HOST
                    this_Cna = gsl_sf_gegenpoly_n(n, 2*l+1.5, xxi[k]);
                    this_Phi_nl = Phi_nl(this_Cna, l, rr[k]);
#else
                    this_Phi_nl = Phi_nl(n, l, rr[k]);
#endif

                    m_Phinlk_Plm = mm[k] * this_Phi_nl * P_lm_k[k];

                    sum_cos += m_Phinlk_Plm * cos(m*pp[k]);
                    sum_sin += m_Phinlk_Plm * sin(m*pp[k]);
                }

                Nlm_Anl = N_lm * A_nl;

                mat_cos[ind(n,l,m)] = sum_cos * Nlm_Anl;
                mat_sin[ind(n,l,m)] = sum_sin * Nlm_Anl;

            } // n loop

        } // m loop

    } // l loop

    free(rr);
    free(pp);
    free(cos_tt);
    free(P_lm_k);

}

void scf_integral_spherical(float* mat_cos)
{
    long long k;
    float* rr = (float*)malloc(NumPart*sizeof(float));
    double temp_r;

    for(k=0; k<NumPart; k++)
    {
        temp_r = sqrt(xx[k]*xx[k] + yy[k]*yy[k] + zz[k]*zz[k]);
        rr[k] = (float)temp_r;
    }

    double N00 = 0.07957747154; /* = 1/(4*pi) */
    double K_n0, I_n0, A_n0;

    float this_Phi_n0;

    double sum_cos = 0.0;

    int n;

    for(n=0; n<=decompose_nmax; n++)
    {
        K_n0 = 0.5*n*(n+3)+1;
        I_n0 = -K_n0/(n+1.5)/4.0 * (n+2) * (n+1);
        A_n0 = 1.0/I_n0;

        sum_cos = 0.0;

        for(k=0; k<NumPart; k++)
        {
            this_Phi_n0 = Phi_n0(n, rr[k]);
            sum_cos += mm[k] * this_Phi_n0;
        }

        mat_cos[n] = (float) (sum_cos * A_n0 * N00);

    } // n loop

    free(rr);

}


/* Calling DGELS using row-major order */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <string.h>
#include "magneticTrap.h"
//#include <sys/stat.h>
#define SIZE 3

//Function to calculate beta. The speed is then v=beta*c
double calculateBeta(double T) {
  double eMass = 9.10938356e-31;
  double c = 299792458.0;
  double factor = pow(T,2.0)/(pow(eMass,2.0)*pow(c,4.0)) + (2.0*T)/(eMass*pow(c,2.0)) + 1;
  double beta = sqrt(1.0 - 1.0/factor);
  return beta;
}

int main (int argc, const char * argv[]) {
    int i, j;
    //FILE *inputFile = fopen("simParms.dat","r");
    //for(int kk=0; kk<10; kk++){
    //}
    //
    double lambda,lambda0,Bx,By,Bz,q,dt,posX,posY,posZ;
    double kinE, mass,gamma,c,Ux,Uy,Uz,magV,magVComputed;
    double dirCosX, dirCosY, dirCosZ, rotationR, solenoidWidth;
    double coilRadius, current, mu, coilPosZMin1, coilPosZMax1, coilPosZMin2, coilPosZMax2;
    double Z_coil1_, Z_coil2_ ;
    int    nTsteps, nCoils;

    //double vecRHS[SIZE]  = {0.0, 0.0, 0.0};
    //double Bfield[SIZE]  = {0.0, 0.0, 0.0 };
    //double Bcgrnd[SIZE]  = {0.0, 0.0, 1.0 };
    double *vecRHS, *Bfield, *background;
    vecRHS = (double*)malloc(SIZE*sizeof(double));
    Bfield = (double*)malloc(SIZE*sizeof(double));
    background = (double*)malloc(SIZE*sizeof(double));
    background[0] = 0.0; background[1] = 0.0; background[2] = 1.0;
    lapack_int info,nRowsA,nColsB,nColsARowsB,ldA,ldB,ipvt[SIZE*SIZE];

    dt= 5.0e-12;
    nTsteps = 2.5e6;
    q = 1.602176634e-19;
    c = 299792458.0;
    mass = 9.10938356e-31;
    kinE = 18575.0*q;
    rotationR = 4.6375e-4;
    solenoidWidth = 0.2;
    coilRadius = 0.05;
    current = 20.0;
    nCoils = 11;
    mu = 1.25663706e-6;
    Z_coil1_ = -0.5;
    Z_coil2_ =  0.5;
    coilPosZMin1 = Z_coil1_ - solenoidWidth/2.0;
    coilPosZMax1 = Z_coil1_ + solenoidWidth/2.0;
    coilPosZMin2 = Z_coil2_ - solenoidWidth/2.0;
    coilPosZMax2 = Z_coil2_ + solenoidWidth/2.0;

    posX=rotationR;posY=0.0;posZ=0.0;
    bathTubFieldSolenoids(coilRadius, current, coilPosZMin1, coilPosZMax1, coilPosZMin2, coilPosZMax2, nCoils,
                           mu, posX, posY, posZ, background, Bfield);
    Bx = Bfield[0]; By = Bfield[1]; Bz = Bfield[2];
    //-----------
    magV = calculateBeta(kinE)*c;
    double pitchAngle = 85.0 * M_PI/180.0;
    dirCosX = 0.0; dirCosZ=cos(pitchAngle); dirCosY=1.0-dirCosZ;
    Ux = magV*dirCosX; Uy=magV*dirCosY; Uz=magV*dirCosZ;
    //double ufuture[SIZE] = {0.0, 0.0, 0.0};
	//double upast[SIZE]   = {Ux, Uy, Uz};
    double *ufuture, *upast;
    ufuture = (double*)malloc(SIZE*sizeof(double));
    upast = (double*)malloc(SIZE*sizeof(double));
    upast[0] = Ux; upast[1] = Uy; upast[2] = Uz;
    printf("# Starting simulation with U(x), U(y), U(z) = %8.5e , %8.5e , %8.5e \n", Ux, Uy, Uz);
    gamma = 1.0/(sqrt(1.0-(pow(magV,2.0))/(pow(c,2.0))));
    lambda0= (0.5*q*dt)/(2.0*mass*gamma); // Careful: this definition is only for the first time step
    lambda = (1.0*q*dt)/(2.0*mass*gamma); // Careful: this definition is only for the first time step

    //writing a and b as vectors, but they are in concept the matrices of the system of linear equations
    //double a[SIZE*SIZE]  = { 1.0, lambda0*Bz,-lambda0*By , -lambda0*Bz,1.0, lambda0*Bx ,  lambda0*By,-lambda0*Bx,1.0 };
    //double b[SIZE*SIZE]  = { 1.0,-lambda0*Bz, lambda0*By ,  lambda0*Bz,1.0,-lambda0*Bx , -lambda0*By, lambda0*Bx,1.0 };
    double *a, *b;
    a = (double*)malloc(SIZE*SIZE*sizeof(double));
    b = (double*)malloc(SIZE*SIZE*sizeof(double));

    a[0] = 1.0;         a[1] = lambda0*Bz;  a[2] = -lambda0*By;
    a[3] = -lambda0*Bz; a[4] = 1.0;         a[5] = lambda0*Bx;
    a[6] = lambda0*By;  a[7] = -lambda0*Bx; a[8] = 1.0;

    b[0] = 1.0;         b[1] = -lambda0*Bz; b[2] = lambda0*By;
    b[3] = lambda0*Bz;  b[4] = 1.0;         b[5] = -lambda0*Bx;
    b[6] = -lambda0*By; b[7] = lambda0*Bx;  b[8] = 1.0;

    nRowsA = SIZE;
    nColsB = 1;
    nColsARowsB = SIZE;
    ldA    = nRowsA;
    ldB    = nColsARowsB;

    // Printing LHS matrix
    /*printf("#LHS Matrix (Traspose -> Column major order -----------\t | \n");
    for(i=0;i<SIZE;i++)
    {
        printf("#| \t");
        for(j=0;j<SIZE;j++)
        {
           printf("%.5e \t",a[i*SIZE + j]);
        }
        printf("\t | \n");
    }
    printf("#------------------------------------------------------\n");
    printf("#RHS Matrix (Traspose -> Column major order -----------\t | \n");
    for(i=0;i<SIZE;i++)
    {
        printf("#| \t");
        for(j=0;j<SIZE;j++)
        {
           printf("%.5e \t",b[i*SIZE + j]);
        }
        printf("\t | \n");
    }
    printf("#----------------------------------------------------------\n");
    printf("# Time       | Ux \t        | Uy \t        | Uz \t        | magV \t        | PosX \t        | PosY \t        | PosZ \t        |\n");
    */
    double timeVal = 0.0;
    for (int tt = 0; tt < nTsteps; tt++) {
        timeVal = (double)tt * dt;
        //using header variables from LAPACK and openblas
        cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,
            nRowsA,nColsB,nColsARowsB,1.0,b,ldA,upast,ldB,0.0,vecRHS,nRowsA);
        for (i=0; i<SIZE; i++){ufuture[i] = vecRHS[i];}; // This is because dgesv rewrites ufuture
        info = LAPACKE_dgesv(LAPACK_COL_MAJOR, SIZE, 1, a, ldA, ipvt, ufuture, ldA);
        for (i=0; i<SIZE; i++){upast[i] = ufuture[i];};
        Ux = ufuture[0];
        Uy = ufuture[1];
        Uz = ufuture[2];
        magVComputed = sqrt(pow(Ux,2.0)+pow(Uy,2.0)+pow(Uz,2.0));
        magV = magVComputed;
        gamma = 1.0/(sqrt(1.0-(pow(magV,2.0))/(pow(c,2.0))));
        lambda = (q*dt)/(2.0*mass*gamma);

        bathTubFieldSolenoids(coilRadius, current, coilPosZMin1, coilPosZMax1,
                coilPosZMin2, coilPosZMax2, nCoils, mu, posX, posY, posZ, background, Bfield);
        Bx = Bfield[0]; By = Bfield[1]; Bz = Bfield[2];

        a[0] = 1.0;        a[1] = lambda*Bz;    a[2] = -lambda*By;
        a[3] = -lambda*Bz; a[4] = 1.0;          a[5] = lambda*Bx;
        a[6] = lambda*By;  a[7] = -lambda*Bx;   a[8] = 1.0;

        b[0] = 1.0;        b[1] = -lambda*Bz;   b[2] = lambda*By;
        b[3] = lambda*Bz;  b[4] = 1.0;          b[5] = -lambda*Bx;
        b[6] = -lambda*By; b[7] = lambda*Bx;    b[8] = 1.0;
        // -----------
        posX = posX + dt*Ux;
        posY = posY + dt*Uy;
        posZ = posZ + dt*Uz;
        if ( tt%1 == 0 ) {    //This alters the number of points written to the text file
            printf(" %7.4e \t %8.5e \t %8.5e \t %8.5e \t %8.5e \t %8.5e \t %8.5e \n ",
                timeVal, posX, posY, posZ, ufuture[0], ufuture[1], ufuture[2]);
        }
    }

    //free(a); free(b);
    free(ufuture); free(upast);
    free(vecRHS); free(Bfield); free(background);
    info = 0;
    return(info);
}


/*
 * GENANN - Minimal C Artificial Neural Network
 *
 * Copyright (c) 2015-2018 Lewis Van Winkle
 *
 * http://CodePlea.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgement in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 */

#include "genann.h"

#include <assert.h>
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <cblas.h>

#ifndef genann_act
#define genann_act_hidden genann_act_hidden_indirect
#define genann_act_output genann_act_output_indirect
#else
#define genann_act_hidden genann_act
#define genann_act_output genann_act
#endif

#define LOOKUP_SIZE 4096

double genann_act_hidden_indirect(const struct genann *ann, double a) {
    return ann->activation_hidden(ann, a);
}

double genann_act_output_indirect(const struct genann *ann, double a) {
    return ann->activation_output(ann, a);
}

const double sigmoid_dom_min = -15.0;
const double sigmoid_dom_max = 15.0;
double interval;
double lookup[LOOKUP_SIZE];

#ifdef __GNUC__
#define likely(x)       __builtin_expect(!!(x), 1)
#define unlikely(x)     __builtin_expect(!!(x), 0)
#define unused          __attribute__((unused))
#else
#define likely(x)       x
#define unlikely(x)     x
#define unused
#pragma warning(disable : 4996) /* For fscanf */
#endif

double genann_act_sigmoid(const genann *ann unused, double a) {
    if (a < -45.0) return 0;
    if (a > 45.0) return 1;
    return 1.0 / (1 + exp(-a));
}

void genann_init_sigmoid_lookup(const genann *ann) {
        const double f = (sigmoid_dom_max - sigmoid_dom_min) / LOOKUP_SIZE;
        int i;

        interval = LOOKUP_SIZE / (sigmoid_dom_max - sigmoid_dom_min);
        for (i = 0; i < LOOKUP_SIZE; ++i) {
            lookup[i] = genann_act_sigmoid(ann, sigmoid_dom_min + f * i);
        }
}

double genann_act_sigmoid_cached(const genann *ann unused, double a) {
    assert(!isnan(a));

    if (a < sigmoid_dom_min) return lookup[0];
    if (a >= sigmoid_dom_max) return lookup[LOOKUP_SIZE - 1];

    size_t j = (size_t)((a-sigmoid_dom_min)*interval+0.5);

    /* Because doubleing point... */
    if (unlikely(j >= LOOKUP_SIZE)) return lookup[LOOKUP_SIZE - 1];

    return lookup[j];
}

double genann_act_linear(const struct genann *ann unused, double a) {
    return a;
}

double genann_act_threshold(const struct genann *ann unused, double a) {
    return a > 0;
}

genann *genann_init(int inputs, int hidden_layers, int hidden, int outputs) {
    if (hidden_layers < 0) return 0;
    if (inputs < 1) return 0;
    if (outputs < 1) return 0;
    if (hidden_layers > 0 && hidden < 1) return 0;

    const int hidden_weights = hidden_layers ? (inputs+1) * hidden + (hidden_layers-1) * (hidden+1) * hidden : 0;
    const int output_weights = (hidden_layers ? (hidden+1) : (inputs+1)) * outputs;
    const int total_weights = (hidden_weights + output_weights);

    const int total_neurons = (inputs + hidden * hidden_layers + outputs);

    /* Allocate extra size for weights, outputs, and deltas. */
    const int size = sizeof(genann) + sizeof(double) * (total_weights + total_neurons + (total_neurons - inputs));
    genann *ret = malloc(size);
    if (!ret) return 0;

    ret->inputs = inputs;
    ret->hidden_layers = hidden_layers;
    ret->hidden = hidden;
    ret->outputs = outputs;

    ret->total_weights = total_weights;
    ret->total_neurons = total_neurons;

    /* Set pointers. */
    ret->weight = (double*)((char*)ret + sizeof(genann));
    ret->output = ret->weight + ret->total_weights;
    ret->delta = ret->output + ret->total_neurons;

    genann_randomize(ret);

    ret->activation_hidden = genann_act_sigmoid_cached;
    ret->activation_output = genann_act_sigmoid_cached;

    genann_init_sigmoid_lookup(ret);

    return ret;
}

genann *genann_read(FILE *in) {
    int inputs, hidden_layers, hidden, outputs;
    int rc;

    errno = 0;
    rc = fscanf(in, "%d %d %d %d", &inputs, &hidden_layers, &hidden, &outputs);
    if (rc < 4 || errno != 0) {
        perror("fscanf");
        return NULL;
    }

    genann *ann = genann_init(inputs, hidden_layers, hidden, outputs);

    int i;
    for (i = 0; i < ann->total_weights; ++i) {
        errno = 0;
        rc = fscanf(in, " %le", ann->weight + i);
        if (rc < 1 || errno != 0) {
            perror("fscanf");
            genann_free(ann);

            return NULL;
        }
    }

    return ann;
}

genann *genann_copy(genann const *ann) {
    const int size = sizeof(genann) + sizeof(double) * (ann->total_weights + ann->total_neurons + (ann->total_neurons - ann->inputs));
    genann *ret = malloc(size);
    if (!ret) return 0;

    memcpy(ret, ann, size);

    /* Set pointers. */
    ret->weight = (double*)((char*)ret + sizeof(genann));
    ret->output = ret->weight + ret->total_weights;
    ret->delta = ret->output + ret->total_neurons;

    return ret;
}

void genann_randomize(genann *ann) {
    int i;
    for (i = 0; i < ann->total_weights; ++i) {
        double r = GENANN_RANDOM();
        /* Sets weights from -0.5 to 0.5. */
        ann->weight[i] = r - 0.5;
    }
}

void genann_free(genann *ann) {
    /* The weight, output, and delta pointers go to the same buffer. */
    free(ann);
}

double const *genann_run(genann const *ann, double const *inputs) {
    //Copy the weights into a more convenient variable
    double const *w = ann->weight;
    //Initialize the output vector to point at the second layer of neurons
    double *o = ann->output + ann->inputs;
    //Initialize the input vector to point at the first layer of neurons
    double const *i = ann->output;

    /* Copy the inputs to the scratch area, where we also store each neuron's
     * output, for consistency. This way the first layer isn't a special case. */
    memcpy(ann->output, inputs, sizeof(double) * ann->inputs);

    int h, j, k;

    if (!ann->hidden_layers) {
        double *ret = o;
        for (j = 0; j < ann->outputs; ++j) {
            double sum = *w++ * -1.0;
            for (k = 0; k < ann->inputs; ++k) {
                sum += *w++ * i[k];
            }
            *o++ = genann_act_output(ann, sum);
        }

        return ret;
    }

    /* Figure input layer */
    for (j = 0; j < ann->hidden; ++j) {
        double sum = *w++ * -1.0;
        for (k = 0; k < ann->inputs; ++k) {
            sum += *w++ * i[k];
        }
        *o++ = genann_act_hidden(ann, sum);
    }

    i += ann->inputs;

    /////////////////////////////////
    // TODO: 1 GEMV using BLAS   //
    /////////////////////////////////

    /*
        Function description:

        Activates each neuron in the input layer based on an input vector.
        Propagates these activations through the network.

        Variables:
        ann:    The neural network
                ann->weights: The edge and neuron weights for all layers
                ann->inputs: The number of neurons in the input layer
                ann->hidden: The number of neurons in the hidden layers
                ann->hidden_layers: The number of hidden layers in the network
                ann->output: The neuron activations for the whole network, incluing input, hidden and final layer.
        inputs: The input vector
        w:      Points at ann->weights
        o:      Points at the layer of ann->output we are currently writing to
        i:      Points to the layer of ann->output we are currently using as input.
                i.e. the previous layer.

        When the function returns, ann->output contains the final result.
    */

    /* Comment from original source: Figure hidden layers, if any. */

    //These are the dimensions of the square weight matrix
    int m = ann->hidden;
    int n = ann->hidden;

    //In addition to n edge weights, each neuron has one value (bias) associated with it.
    //This value is *also* saved in w, meaning the complete matrix has (n+1)*m elements.
    //This value is always multiplied by -1, which we make room for in a copy of the input vector.
    double* temp_i = malloc( (ann->hidden+1) * sizeof(double) );
    double* sums = calloc((ann->hidden+1),  sizeof(double));

    for (h = 1; h < ann->hidden_layers; ++h) {
        //Copyyng the input vector and setting the first value to -1 as described above.
        temp_i[0] = -1.0;
        memcpy(temp_i+1, i, n*sizeof(double));

        ////////////////////////////////////////////////////////////
        // Decompose and replace this double for loop with GEMV call
        for (j = 0; j < ann->hidden; ++j) {
            for (k = 0; k < ann->hidden+1; ++k) {
                sums[j] += w[k + j*(ann->hidden+1)] * temp_i[k];
            }
            o[j] = genann_act_hidden(ann, sums[j]);
        }
        ////////////////////////////////////////////////////////////

        w += (n + 1) * m;
        o += m;
        i += m;
    }
    free(temp_i);
    free(sums);

    /////////////////////////////////
    // TODO 1 END               //
    /////////////////////////////////

    double const *ret = o;
    /* Figure output layer. */
    for (j = 0; j < ann->outputs; ++j) {
        double sum = *w++ * -1.0;
        for (k = 0; k < ann->hidden; ++k) {
            sum += *w++ * i[k];
        }
        *o++ = genann_act_output(ann, sum);
    }

    /* Sanity check that we used all weights and wrote all outputs. */
    assert(w - ann->weight == ann->total_weights);
    assert(o - ann->output == ann->total_neurons);

    return ret;
}

void genann_train(genann const *ann, double const *inputs, double const *desired_outputs, double learning_rate) {
    /* To begin with, we must run the network forward. */
    genann_run(ann, inputs);

    int h, j, k;

    /*
    TDT4200 comment:  This lonesome curly bracket starts a new scope.
                      That means no variables declared within will be
                      visible outside outside it (that is, o, d and t).
                      That also means the names can safely be re-used later.
    */
    {   /* First set the output layer deltas. */
        double const *o = ann->output + ann->inputs + ann->hidden * ann->hidden_layers; /* First output. */
        double *d = ann->delta + ann->hidden * ann->hidden_layers; /* First delta. */
        double const *t = desired_outputs; /* First desired output. */

        /* Set output layer deltas. */
        if (genann_act_output == genann_act_linear ||
                ann->activation_output == genann_act_linear) {
            for (j = 0; j < ann->outputs; ++j) {
                *d++ = *t++ - *o++;
            }
        } else {
            for (j = 0; j < ann->outputs; ++j) {
                *d++ = (*t - *o) * *o * (1.0 - *o);
                ++o; ++t;
            }
        }
    }

    /* Set hidden layer deltas, start on last layer and work backwards. */
    /* Note that loop is skipped in the case of hidden_layers == 0. */
    for (h = ann->hidden_layers - 1; h >= 0; --h) {

        /* Find first output and delta in this layer. */
        double const *o = ann->output + ann->inputs + (h * ann->hidden);
        double *d = ann->delta + (h * ann->hidden);

        /* Find first delta in following layer (which may be hidden or output). */
        double const * const dd = ann->delta + ((h+1) * ann->hidden);

        /* Find first weight in following layer (which may be hidden or output). */
        double const * const ww = ann->weight + ((ann->inputs+1) * ann->hidden) + ((ann->hidden+1) * ann->hidden * (h));

        /////////////////////////////////
        // TODO: 2 GEMV using BLAS   //
        /////////////////////////////////

        /*
        Function description:
        First, run an input through the network (see TODO 1).
        Then, starting at the output layer and running backwards, determine the
        deltas, the differences between the desired activation and actual
        activation of each neuron.

        Variables:
        ann: The artificial neural network, same as in TODO 1. \
             ann->delta: An area of memory as big as ann->output.
                        When function returns, stores difference between desired
                        output and actual output.
        h:  The index of the current layer. Starting at the output layer and
            going backwards.
        o:  The actual output of the current layer, produced by TODO 1 function.
        d:  The delta vector of the current layer, this is what we're calculating
        dd: The delta vector of the *previous* layer, at index (h+1).
        ww: The edge weights going from the previous layer and the current one.

        Note 1: We're playing fast and loose with the term "difference" here.
                It is not strictly a [desired]-[actual] calculation, but can be
                conceptually thought of as a difference.
        Note 2: We say "previous" layer meaning the layer involved in the
                previous calculation. Because we are going backwards, this is,
                strictly speaking, the "following" layer, which is what it is
                called in the source code comments.
        */

        // TODO 2.a: Define the m and n dimension of the delta matrix
        // Hint: Look at the double for loop
        int m = 0;
        int n = 0;

        //A temporary vector to store the propagated delta from the previuos layer.
        double* delta = calloc(ann->hidden, sizeof(double));

        // TODO 2.b: Decompose and implement GEMV BLAS call for the code
        // Hint: Think about how ww is offset from its original address.
        // You will need pointer arithmetic for the BLAS call
        for (j = 0; j < ann->hidden; ++j) {
            //We iterate up to the value ann->outputs if we are on the output layer,
            //h == ann->hidden_layers-1, and to ann->hidden otherwise.
            for (k = 0; k < (h == ann->hidden_layers-1 ? ann->outputs : ann->hidden); ++k) {
                //Similarly to TODO 1, we add 1 to ann->hidden and j here as a
                //consequence of how bias for each neuron is stored.
                const int windex = k * (ann->hidden + 1) + (j + 1);
                //Propagate the deltas from the previous layer backwards
                //Using the weights between the layers and storing the result in "delta"
                delta[j] += dd[k] * ww[windex];
            }
            //Calculate the actual new deltas for this layer
            d[j] = o[j] * (1.0-o[j]) * delta[j];
        }

        free(delta);

        /////////////////////////////////
        // TODO 2 END               //
        /////////////////////////////////
    }

    /* Train the outputs. */
    {
        /* Find first output delta. */
        double const *d = ann->delta + ann->hidden * ann->hidden_layers; /* First output delta. */

        /* Find first weight to first output delta. */
        double *w = ann->weight + (ann->hidden_layers
                ? ((ann->inputs+1) * ann->hidden + (ann->hidden+1) * ann->hidden * (ann->hidden_layers-1))
                : (0));

        /* Find first output in previous layer. */
        double const * const i = ann->output + (ann->hidden_layers
                ? (ann->inputs + (ann->hidden) * (ann->hidden_layers-1))
                : 0);

        /* Set output layer weights. */
        for (j = 0; j < ann->outputs; ++j) {
            *w++ += *d * learning_rate * -1.0;
            for (k = 1; k < (ann->hidden_layers ? ann->hidden : ann->inputs) + 1; ++k) {
                *w++ += *d * learning_rate * i[k-1];
            }

            ++d;
        }

        assert(w - ann->weight == ann->total_weights);
    }

    /* Train the hidden layers. */
    for (h = ann->hidden_layers - 1; h >= 0; --h) {

        /* Find first delta in this layer. */
        double const *d = ann->delta + (h * ann->hidden);

        /* Find first input to this layer. */
        double const *i = ann->output + (h
                ? (ann->inputs + ann->hidden * (h-1))
                : 0);

        /* Find first weight to this layer. */
        double *w = ann->weight + (h
                ? ((ann->inputs+1) * ann->hidden + (ann->hidden+1) * (ann->hidden) * (h-1))
                : 0);

    /////////////////////////////////
    // TODO: 3 (Optional) Optimize //
    /////////////////////////////////
        for (j = 0; j < ann->hidden; ++j) {
            *w++ += *d * learning_rate * -1.0;
            for (k = 1; k < (h == 0 ? ann->inputs : ann->hidden) + 1; ++k) {
                *w++ += *d * learning_rate * i[k-1];
            }
            ++d;
        }
    /////////////////////////////////
    // TODO 3 END               //
    /////////////////////////////////
    }

}

void genann_write(genann const *ann, FILE *out) {
    fprintf(out, "%d %d %d %d", ann->inputs, ann->hidden_layers, ann->hidden, ann->outputs);

    int i;
    for (i = 0; i < ann->total_weights; ++i) {
        fprintf(out, " %.20e", ann->weight[i]);
    }
}


/* Implementation of the interface in orbital_state_vector.h.  */

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_odeiv.h>

#include "basic_types.h"
#include "earth_constants.h"
#include "orbital_state_vector.h"

OrbitalStateVector *
orbital_state_vector_new (double x, double y, double z, double vx, double vy,
			  double vz)
{
  OrbitalStateVector *self = malloc (sizeof (OrbitalStateVector));

  self->position = vector_new (x, y, z);
  self->velocity = vector_new (vx, vy, vz);

  return self;
}

OrbitalStateVector *
orbital_state_vector_copy (OrbitalStateVector *a)
{
  OrbitalStateVector *new = malloc (sizeof (OrbitalStateVector));

  new->position = vector_copy (a->position);
  new->velocity = vector_copy (a->velocity);

  return new;
}

void
orbital_state_vector_get_keplerian_elements
(OrbitalStateVector *self, double *a, double *e, double *i,
 double *capital_omega, double *lower_case_omega, double *nu, double *E,
 double *M)
{
  /* Shorthand.  */
  static const double gm = EARTH_GRAVITATIONAL_CONSTANT;

  /* Fundamental vectors as described in "Satellite Geodesy".  */
  Vector *h = vector_cross (self->position, self->velocity);
  Vector *X = vector_new (1.0, 0.0, 0.0);
  Vector *Z = vector_new (0.0, 0.0, 1.0);
  Vector *n = vector_cross (Z, h);
  double r_dot_v = vector_dot (self->position, self->velocity);
  double r_mag = vector_magnitude (self->position);
  double v_mag = vector_magnitude (self->velocity);
  Vector *tmp1 = vector_copy (self->position);
  Vector *tmp2 = vector_copy (self->velocity);
  /* Fundamental vector 'e' ('_fv' is appended to dodge name clash).  */
  Vector *e_fv;
  double h_mag;
  double n_mag;

  vector_multiply (tmp1, pow (v_mag, 2) - gm / r_mag);
  vector_multiply (tmp2, r_dot_v);
  vector_subtract (tmp1, tmp2);
  vector_multiply (tmp1, (1.0 / gm));
  e_fv = vector_copy (tmp1);

  /* Compute keplerian elements.  */
  *e = vector_magnitude (e_fv);
  h_mag = vector_magnitude (h);
  *a = (pow (h_mag, 2) / gm) / (1 - pow (*e, 2));
  *i = acos (vector_dot (h, Z) / h_mag);
  n_mag = vector_magnitude (n);
  *capital_omega = acos (vector_dot (n, X) / n_mag);
  *lower_case_omega = acos (vector_dot (n, e_fv) / (n_mag * (*e)));
  *nu = acos (vector_dot (e_fv, self->position)
	      / ((*e) * vector_magnitude (self->position)));
  *E = acos ((*e + cos (*nu)) / (1.0 + (*e) * cos (*nu)));
  *M = *E - (*e) * sin (*E);

  /* Free fundamental and temporary vectors used in computation.  */
  free (h);
  free (Z);
  free (X);
  free (n);
  free (tmp1);
  free (tmp2);
  free (e_fv);
}

double
orbital_state_vector_height_above_ellipsoid (OrbitalStateVector *self)
{
  double r = vector_magnitude (self->position);

  /* Compute height of satellite above the WGS84 ellipsoid.  */
  static Vector z_unit = {0.0, 0.0, 1.0};
  /* Angle between position vector and equatorial plane.  */
  double eq_angle = M_PI / 2 - vector_angle (self->position, &z_unit);
  double ae = EARTH_SEMIMAJOR_AXIS;
  /* Earth semiminor axis.  */
  double be = sqrt (pow (ae, 2) * (1 - pow (EARTH_ECCENTRICITY, 2)));
  /* Radius of ellipsoid at point under satellite.  */
  double ellipsoid_radius = sqrt (pow (ae * cos (eq_angle), 2)
				  + pow (be * sin (eq_angle), 2));

  return r - ellipsoid_radius;
}

/* Compute derivatives f from functions y(t), t, and other
   miscellaneous parameters.  Return GSL_SUCCESS after successful
   computation.  This code mostly follows the example in the GNU
   Scientific Library documentation, which uses a y array for the
   function values.  The y[1] value is the only one that corresponds
   to the y coordinate, and the y[4] vale the only one that
   corresponds to the y velocity.  */
static int
func (double t, const double y[], double f[], void *params)
{
  /* Shorthand for some earth constants.  */
  static const double gm = EARTH_GRAVITATIONAL_CONSTANT;
  static const double ae = EARTH_SEMIMAJOR_AXIS;

  /* Create position and velocity vectors.  We use static vectors to
     avoid allocation overhead.  */
  static Vector p_st;
  static Vector v_st;
  static Vector *p = &p_st;
  static Vector *v = &v_st;

  double r;
  double j2;
  double a_j2_x, a_j2_y, a_j2_z;
  double ksx, ksy, ksz;

  vector_set (p, y[0], y[1], y[2]);
  vector_set (v, y[3], y[4], y[5]);

  /* Current range to satellife from center of earth.  */
  r = vector_magnitude (p);
  double r3 = r*r*r;

  /* The so-called first zonal harmonic is used to account for the
     effects of earth oblatedness (see below for references).  */
  j2 = 1082.63e-6;	/* First zonal harmonic coefficient.  */
  a_j2_x = (gm * p->x / r3) * j2 * (3.0 / 2) * (ae*ae / (r*r))
    * (5.0 * (p->z * p->z) / (r*r) - 1.0);
  a_j2_y = (p->y / p->x) * a_j2_x;
  a_j2_z = (-gm * p->z / r3) * j2 * (3.0 / 2)
    * (ae*ae / (r*r)) * (3.0 - 5.0 * (p->z * p->z) / (r*r));

  /* Total perturbations.  */
  ksx = a_j2_x;
  ksy = a_j2_y;
  ksz = a_j2_z;

  f[0] = v->x;
  f[1] = v->y;
  f[2] = v->z;
  f[3] = ksx - gm * (p->x / r3);
  f[4] = ksy - gm * (p->y / r3);
  f[5] = ksz - gm * (p->z / r3);

  return GSL_SUCCESS;
}

void
orbital_state_vector_propagate (OrbitalStateVector *self, double time)
{
  /* We need to solve the following six dimensional system of ordinary
     differential equations:

         dx/dt = vx
	 dy/dt = vy
	 dz/dt = vz

	 dvx/dt = ksx - (GM / r^3) * x
	 dvy/dt = ksy - (GM / r^3) * y
	 dvz/dt = ksz - (GM / r^3) * z

     where

         r = sqrt (x^2 + y^2 + z^2)

     with initial conditions given by the current state.  The values
     ksx, ksy, and ksz represent the perturbations due to earth
     oblatedness as described in section 3.2.2.3 respectively of
     "Satellite Geodesy", 2nd edition, by Gunter Seeber.  An ordinary
     differential equation solver from the GNU Scientific Library
     (GSL) is used to solve the system.  It is well described in the
     GSL documentation section "Ordinary Differential Equations".  */

  const int dimension = 6;
  /* Create some things the ODE solver uses.  */
  const gsl_odeiv_step_type *step_type = gsl_odeiv_step_rkf45;
  gsl_odeiv_step *ode_step = gsl_odeiv_step_alloc (step_type, dimension);
  /* Hold the absolute integration error for each coordinate for each
     step to within this value.  This value was chosen using the
     following sophisticated methodology: Keep tightening the
     tolerance until the answer no longer changes with further
     tightenings, for an short (~2.5 minute) propagation.  */
  double mae = 0.000001;
  gsl_odeiv_control *ode_control = gsl_odeiv_control_y_new (mae, 0.0);
  gsl_odeiv_evolve *ode_evolve = gsl_odeiv_evolve_alloc (dimension);
  gsl_odeiv_system ode_system;
  double t0 = 0.0, t1 = time;	/* Start and end times.  */
 /* Initial guess for step size.  */
  const double initial_step_size = GSL_SIGN (t1) * 1.0;
  double step_size = initial_step_size;
  double *y = malloc (dimension * sizeof (double));
  double t = t0;		/* Current time.  */

  ode_system.function = func;
  ode_system.jacobian = NULL;
  ode_system.dimension = dimension;
  ode_system.params = NULL;

  y[0] = self->position->x;
  y[1] = self->position->y;
  y[2] = self->position->z;
  y[3] = self->velocity->x;
  y[4] = self->velocity->y;
  y[5] = self->velocity->z;
  /* Here is the actual propagation.  */
  while ( fabs (t) < fabs (t1) ) {
    int status = gsl_odeiv_evolve_apply (ode_evolve, ode_control, ode_step,
					 &ode_system, &t, t1, &step_size, y);
    assert (status == GSL_SUCCESS);
  }
  self->position->x = y[0];
  self->position->y = y[1];
  self->position->z = y[2];
  self->velocity->x = y[3];
  self->velocity->y = y[4];
  self->velocity->z = y[5];

  free (y);
  gsl_odeiv_evolve_free (ode_evolve);
  gsl_odeiv_control_free (ode_control);
  gsl_odeiv_step_free (ode_step);
}

void
orbital_state_vector_propagate_interpolate (OrbitalStateVector *self,
					    double time,
					    OrbitalStateVector *other)
{
  /* Not implemented.  */
  assert (FALSE);
}


#include <stdio.h>
#include <stdlib.h>
#define COMPEARTH_PRIVATE_GEM3 1
#define COMPEARTH_PRIVATE_GEMT3 1
#include "compearth.h"
#ifdef DEBUG
#ifdef COMPEARTH_USE_MKL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#endif

/*!
 * @brief Transforms a set of 3 x 3 tensors using transformation matrix T.
 *
 * @param[in] nmt    Number of moment tensors.
 * @param[in] T      3 x 3 transformation matrix in column major order.
 * @param[in] Min    [3 x 3 x n] input matrices.  Each [3 x 3] matrix is in
 *                   column major order.
 * @param[out] Mout  [3 x 3 x n] output matrices.  Each [3 x 3] matrix is
 *                   in column major order.
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker.
 *
 */
int compearth_transform_mat(const int nmt,
                            const double *__restrict__ T,
                            const double *__restrict__ Min,
                            double *__restrict__ Mout)
{
    double TM[9];
    int imt; //i, j
    // Compute T*Min*T'
    for (imt=0; imt<nmt; imt++)
    {
#ifdef DEBUG
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                    3, 3, 3, 1.0, T, 3, &Min[9*imt], 3, 0.0, TM, 3);
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
                    3, 3, 3, 1.0, TM, 3, T, 3, 0.0, &Mout[9*imt], 3);
#else
        gemm3_colMajorNoTransTrans(T, &Min[9*imt], TM);
        gemm3_colMajorNoTransTrans(TM, T, &Mout[9*imt]);
#endif
    }
    return 0;
}


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

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_matrix_double.h>

int main() {
	int ret;
	gsl_matrix A;
	double data[9];
	int i, j;

	memset(&A, 0, sizeof(gsl_matrix));

	A.size1 = 3;
	A.size2 = 3;
	A.tda = 3;
	A.data = data;

	gsl_matrix_set(&A, 0, 0, 34.0);
	gsl_matrix_set(&A, 0, 1, 4.0);
	gsl_matrix_set(&A, 0, 2, 14.0);
	gsl_matrix_set(&A, 1, 0, 1.0);
	gsl_matrix_set(&A, 1, 1, 8.0);
	gsl_matrix_set(&A, 1, 2, 3.0);
	gsl_matrix_set(&A, 2, 0, 7.0);
	gsl_matrix_set(&A, 2, 1, 1.0);
	gsl_matrix_set(&A, 2, 2, 8.0);

	for (i=0; i<A.size1; i++) {
		printf((i==0) ? "A = (" : "    (");
		for (j=0; j<A.size2; j++) {
			printf(" %12.5g ", gsl_matrix_get(&A, i, j));
		}
		printf(")\n");
	}
	printf("\n");

	ret = gsl_linalg_cholesky_decomp(&A);

	for (i=0; i<A.size1; i++) {
		printf((i==0) ? "L = (" : "    (");
		for (j=0; j<A.size2; j++) {
			printf(" %12.5g ", (j <= i) ? gsl_matrix_get(&A, i, j) : 0.0);
		}
		printf(")\n");
	}
	printf("\n");

	return 0;
}


//Gets cepstral coefficients (CCs). This is the same as dct + lifter.
//This computes "the" DCT (discrete cosine transformation) along one dimension of matrix X.
//This uses fftw3 to compute the DCT-II.
//Then it applies a lifter (cepstral domain window) to the same dimension of X.
//The lifter has a single parameter, Q, which is default 22.0 in HTK, Kaldi and Ellis.
//Only the first K CCs are output.

#include <stdio.h>
#include <float.h>
#include <math.h>
#include <cblas.h>
#include <fftw3.h>

#ifndef M_PIf
   #define M_PIf 3.141592653589793238462643383279502884f
#endif

#ifndef M_PI
   #define M_PI 3.141592653589793238462643383279502884
#endif

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int get_ccs_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int ndct, const float Q, const int K);
int get_ccs_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int ndct, const double Q, const int K);

int get_ccs_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int ndct, const float Q, const int K)
{
    const float z = 0.0f;
    const float Q2 = 0.5f*Q, PQ = M_PIf/Q;
    int r, c, k;
    float *X1, *Y1; //*lift;
    fftwf_plan plan;
    //struct timespec tic, toc;

    //Checks
    if (R<1) { fprintf(stderr,"error in get_ccs_s: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in get_ccs_s: C (ncols X) must be positive\n"); return 1; }
    if (ndct<R && dim==0) { fprintf(stderr,"error in get_ccs_s: ndct must be >= R for dim==0\n"); return 1; }
    if (ndct<C && dim==1) { fprintf(stderr,"error in get_ccs_s: ndct must be >= C for dim==1\n"); return 1; }
    if (Q<0.0f) { fprintf(stderr,"error in get_ccs_s: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in get_ccs_s: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in get_ccs_s: K must be <= ndct>\n"); return 1; }

    //Initialize fftwf
    X1 = fftwf_alloc_real((size_t)ndct);
    Y1 = fftwf_alloc_real((size_t)ndct);
    plan = fftwf_plan_r2r_1d(ndct,X1,Y1,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in get_ccs_s: problem creating fftw plan\n"); return 1; }
    cblas_scopy(ndct,&z,0,&X1[0],1); //zero-pad

    //clock_gettime(CLOCK_REALTIME,&tic);

    //Initialize lifter
    //if (!(lift=(float *)malloc((size_t)K*sizeof(float)))) { fprintf(stderr,"error in get_ccs_s: problem with malloc for lifter. "); perror("malloc"); return 1; }
    //for (k=0; k<K; k++) { lift[k] = fmaf(Q2,sinf(k*PQ),1.0f); }

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c*R],1,&X1[0],1);
                fftwf_execute(plan);
                //cblas_ssbmv(CblasColMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1[0],1,0.0f,&Y[c*K],1);
                cblas_scopy(K,&Y1[0],1,&Y[c*K],1);
            }
            if (Q>FLT_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); }
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c],C,&X1[0],1);
                fftwf_execute(plan);
                //cblas_ssbmv(CblasRowMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1[0],1,0.0f,&Y[c],C);
                cblas_scopy(K,&Y1[0],1,&Y[c],C);
            }
            if (Q>FLT_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*C],1); }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r],R,&X1[0],1);
                fftwf_execute(plan);
                //cblas_ssbmv(CblasColMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1[0],1,0.0f,&Y[r],R);
                cblas_scopy(K,&Y1[0],1,&Y[r],R);
            }
            if (Q>FLT_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*R],1); }
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r*C],1,&X1[0],1);
                fftwf_execute(plan);
                //cblas_ssbmv(CblasRowMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1[0],1,0.0f,&Y[r*K],1);
                cblas_scopy(K,&Y1[0],1,&Y[r*K],1);
            }
            if (Q>FLT_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in get_ccs_s: dim must be 0 or 1.\n"); return 1;
    }
    //clock_gettime(CLOCK_REALTIME,&toc); fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);

    fftwf_destroy_plan(plan); fftwf_free(X1); fftwf_free(Y1);
    return 0;
}

int get_ccs_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int ndct, const double Q, const int K)
{
    const double z = 0.0;
    const double Q2 = 0.5*Q, PQ = M_PI/Q;
    int r, c, k;
    double *X1, *Y1;
    fftw_plan plan;

    //Checks
    if (R<1) { fprintf(stderr,"error in get_ccs_d: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in get_ccs_d: C (ncols X) must be positive\n"); return 1; }
    if (ndct<R && dim==0) { fprintf(stderr,"error in get_ccs_d: ndct must be >= R for dim==0\n"); return 1; }
    if (ndct<C && dim==1) { fprintf(stderr,"error in get_ccs_d: ndct must be >= C for dim==1\n"); return 1; }
    if (Q<0.0) { fprintf(stderr,"error in get_ccs_d: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in get_ccs_d: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in get_ccs_d: K must be <= ndct>\n"); return 1; }

    //Initialize fftw
    X1 = fftw_alloc_real((size_t)ndct);
    Y1 = fftw_alloc_real((size_t)ndct);
    plan = fftw_plan_r2r_1d(ndct,X1,Y1,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in get_ccs_s: problem creating fftw plan\n"); return 1; }
    cblas_dcopy(ndct,&z,0,&X1[0],1); //zero-pad

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c*R],1,&X1[0],1);
                fftw_execute(plan);
                cblas_dcopy(K,&Y1[0],1,&Y[c*K],1);
            }
            if (Q>DBL_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k],K); }
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c],C,&X1[0],1);
                fftw_execute(plan);
                cblas_dcopy(K,&Y1[0],1,&Y[c],C);
            }
            if (Q>DBL_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k*C],1); }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r],R,&X1[0],1);
                fftw_execute(plan);
                cblas_dcopy(K,&Y1[0],1,&Y[r],R);
            }
            if (Q>DBL_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k*R],1); }
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r*C],1,&X1[0],1);
                fftw_execute(plan);
                cblas_dcopy(K,&Y1[0],1,&Y[r*K],1);
            }
            if (Q>DBL_EPSILON)
            {
                for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k],K); }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in get_ccs_d: dim must be 0 or 1.\n"); return 1;
    }

    fftw_destroy_plan(plan); fftw_free(X1); fftw_free(Y1);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


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

#define DIM 2                   /* Geometric dimension */

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

typedef struct {
    PetscReal rho;
    PetscReal rhoU[2];//Hardcode for 2 for now
    PetscReal rhoE;
} EulerNode;

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

typedef struct {
    InitialConditions initialConditions;
    StarState starState;
} ProblemSetup;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    PetscFunctionReturn(0);
}

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

    PetscFunctionReturn(0);
}

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

    PetscFunctionReturn(0);
}

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

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

    // 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[] = {1.0, 1};
    PetscInt nx[] = {100, 1};
    DMBoundaryType bcType[] = {DM_BOUNDARY_NONE, DM_BOUNDARY_NONE};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, DIM, PETSC_FALSE, nx, start, end, bcType, PETSC_TRUE, &dm);CHKERRQ(ierr);

// Setup the problem
    ProblemSetup problem;

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

    // case 2
//    problem.initialConditions.rhoL=1.0;
//    problem.initialConditions.uL=-2.0;
//    problem.initialConditions.pL=0.4;
//    problem.initialConditions.rhoR=1.0;
//    problem.initialConditions.uR=2.0;
//    problem.initialConditions.pR=0.4;
//    problem.initialConditions.maxTime = 0.15;
//    problem.initialConditions.length = 1;
//    problem.initialConditions.gamma = 1.4;

    // case 5
//    problem.initialConditions.rhoL=5.99924;
//    problem.initialConditions.uL=19.5975;
//    problem.initialConditions.pL=460.894;
//    problem.initialConditions.rhoR=5.99242;
//    problem.initialConditions.uR=-6.19633;
//    problem.initialConditions.pR=46.0950;
//    problem.initialConditions.maxTime =  0.035;
//    problem.initialConditions.length = 1;
//    problem.initialConditions.gamma = 1.4;

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

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

    //Setup
    CompressibleFlow_SetupDiscretization(flowData, dm);

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

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

    // Complete the problem setup
    ierr = CompressibleFlow_CompleteProblemSetup(flowData, ts);
    CHKERRABORT(PETSC_COMM_WORLD, 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, &problem, NULL);CHKERRQ(ierr);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetMaxTime(ts,problem.initialConditions.maxTime);CHKERRQ(ierr);

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

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

    return PetscFinalize();

}


#pragma once

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

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

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

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

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

		/// @brief Creates a new `Vec2` with the given x and y components
		///
		/// @param x - The x component
		/// @param y - The y component
		constexpr Vec2(T x, T y) noexcept : elements{x, y} {
		}
		constexpr Vec2(const Vec2& vec) noexcept = default;
		constexpr Vec2(Vec2&& vec) noexcept = default;
		constexpr ~Vec2() 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
		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
		inline constexpr auto y() noexcept -> T& {
			return elements[Y];
		}

		/// @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 Vec2<TT>& vec) const noexcept -> TT {
			return narrow_cast<TT>(x()) * vec.x() + narrow_cast<TT>(y()) * vec.y();
		}

		/// @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 Vec2<TT>& vec) const noexcept -> T {
			return x() * narrow_cast<T>(vec.x()) + y() * narrow_cast<T>(vec.y());
		}

		/// @brief Performs the 2D equivalent to the cross product
		/// @note This is equivalent to the z component of a 3D cross product
		///
		/// @param vec The vector to perform the cross product with
		///
		/// @return The 2D cross product
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto cross_prod(const Vec2<TT>& vec) const noexcept -> TT {
			return narrow_cast<TT>(x()) * vec.y() - narrow_cast<TT>(y()) * vec.x();
		}

		/// @brief Performs the 2D equivalent to the cross product
		/// @note This is equivalent to the z component of a 3D cross product
		///
		/// @param vec The vector to perform the cross product with
		///
		/// @return The 2D cross product
		template<SignedIntegral TT = int>
		[[nodiscard]] inline constexpr auto cross_prod(const Vec2<TT>& vec) const noexcept -> T {
			return x() * narrow_cast<T>(vec.y()) - y() * narrow_cast<T>(vec.x());
		}

		/// @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
		template<FloatingPoint TT = float>
		[[nodiscard]] inline constexpr auto normal() const noexcept -> Vec2<TT> {
			constexpr auto _x = narrow_cast<TT>(1.0);
			const auto _y = (-_x * narrow_cast<TT>(x())) / narrow_cast<TT>(y());
			return {_x, _y};
		}

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

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

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

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

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

		template<FloatingPoint TT = float>
		inline constexpr auto operator==(const Vec2<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);
			return xEqual && yEqual;
		}

		template<SignedIntegral TT = int>
		inline constexpr auto operator==(const Vec2<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);
				return xEqual && yEqual;
			}
			else {
				return x() == narrow_cast<T>(vec.x()) && y() == narrow_cast<T>(vec.y());
			}
		}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

		/// @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();
		}

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

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

	template<SignedIntegral T>
	explicit Vec2(T, T) -> Vec2<T>;
} // namespace math


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

typedef struct {
    PetscInt dim;
    PetscReal rc;
    PetscReal xc;
    PetscReal yc;
    PetscReal L;
    PetscReal Tinf;
    PetscReal rhoInf;
    PetscReal Mac;
    PetscReal MaxInf;
    PetscReal MayInf;
    PetscReal Rgas;
    PetscReal gamma;
} Constants;

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

static PetscLogStage monitorStage;

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

    Constants *constants = (Constants *)ctx;

    // define the speed of sound and other variables in the infinity
    PetscReal aInf = PetscSqrtReal(constants->gamma*constants->Rgas*constants->Tinf);
    PetscReal pInf = constants->rhoInf*constants->Rgas*constants->Tinf;
    PetscReal uInf = constants->MaxInf*aInf;
    PetscReal vInf = constants->MayInf*aInf;

    // compute the values for the vortex
    PetscReal Tvort = constants->Tinf/(1.0 + 0.5*(constants->gamma - 1.0)*constants->Mac*constants->Mac);
    PetscReal aVort = PetscSqrtReal(constants->gamma*constants->Rgas*Tvort);

    // compute the values at each location based upon the time and the var field velocity
    PetscReal distTraveledX = time*uInf;
    PetscReal distTraveledY = time*vInf;
    // compute the updated center
    PetscReal xc = constants->xc + distTraveledX;
    PetscReal yc = constants->yc + distTraveledY;

    while(xc > constants->L*2){
        xc -= constants->L*2;
    }
    while(yc > constants->L){
        yc -= constants->L;
    }

    PetscReal x = xyz[0];
    PetscReal y = xyz[1];

    PetscReal xStar = (x- xc)/constants->rc;
    PetscReal yStar = (y - yc)/constants->rc;
    PetscReal rStar = PetscSqrtReal(PetscSqr((x - xc))+ PetscSqr((y - yc)))/constants->rc;

    PetscReal u = uInf - constants->Mac*aVort*yStar* PetscExpReal(0.5*(1.0 - PetscSqr(rStar)));
    PetscReal v = vInf + constants->Mac*aVort*xStar* PetscExpReal(0.5*(1.0 - PetscSqr(rStar)));
    PetscReal machTerm = 0.5*(constants->gamma-1.0)*PetscSqr(constants->Mac);
    PetscReal T = constants->Tinf*(1.0 - machTerm/(1+machTerm)*PetscSqr(rStar)*PetscExpReal(1-PetscSqr(rStar)) );
    PetscReal p = pInf* PetscPowReal(T/constants->Tinf, constants->gamma/(constants->gamma - 1.0));
    PetscReal rho = p/constants->Rgas/T;
    PetscReal e = p/((constants->gamma - 1.0)*rho);
    PetscReal eT = e + 0.5*(u*u + v*v);

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

    PetscFunctionReturn(0);
}

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

    PetscInt interval = 1;
    if(step % interval == 0) {
        // Get the DM
        DM dm;
        ierr = TSGetDM(ts, &dm);
        CHKERRQ(ierr);

        ierr = VecViewFromOptions(u, NULL, "-sol_view");
        CHKERRQ(ierr);

        // Open a vtk viewer
        //    PetscViewer viewer;
        //    char        filename[PETSC_MAX_PATH_LEN];
        //    ierr = PetscSNPrintf(filename,sizeof(filename),"/Users/mcgurn/chrestScratch/results/vortex/flow%.4D.vtu",step);CHKERRQ(ierr);
        //    ierr = PetscViewerVTKOpen(PetscObjectComm((PetscObject)dm),filename,FILE_MODE_WRITE,&viewer);CHKERRQ(ierr);
        //    ierr = VecView(u,viewer);CHKERRQ(ierr);
        //    ierr = PetscViewerDestroy(&viewer);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, "-exact_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, "-exact_view");
        CHKERRQ(ierr);
        ierr = VecDestroy(&exactVec);
        CHKERRQ(ierr);
    }
    PetscLogStagePop();
    PetscFunctionReturn(0);
}

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

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

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

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

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

    // sub sonic
    constants.dim = 2;
    constants.L = 1.0;
    constants.xc = constants.L/2.0;
    constants.yc = constants.L/2.0;
    constants.Tinf =298.0;
    constants.rhoInf = 1.0;
    constants.Rgas = 287.0;
    constants.gamma = 1.4;
    constants.rc = .1*constants.L;
    constants.gamma = 1.4;
    constants.Mac = 0.3;
    constants.MaxInf = 0.3;
    constants.MayInf = 0.0;

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

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

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

    PetscInt lengthFactor = 2;
    ierr =  PetscOptionsGetInt(NULL, NULL, "-lengthFactor", &lengthFactor, NULL);CHKERRQ(ierr);
    PetscPrintf(PETSC_COMM_WORLD, "LengthFactor %d\n", lengthFactor);

    // Create a mesh
    // hard code the problem setup
    PetscReal start[] = {0.0, 0.0};
    PetscReal end[] = {constants.L*lengthFactor, constants.L};
    PetscReal nxHeight = 10;
    PetscInt nx[] = {lengthFactor*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;

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

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

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

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

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

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

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

    // Compute the end time so it goes around once
    PetscReal aInf = PetscSqrtReal(constants.gamma*constants.Rgas*constants.Tinf);
    PetscReal u_x = constants.MaxInf*aInf;
    PetscReal endTime = constants.L/u_x;

    TSSetMaxTime(ts, endTime);
    TSSetMaxSteps(ts, 50);
    PetscDSView(prob, PETSC_VIEWER_STDOUT_WORLD);
    PetscLogStage solveStage;
    PetscLogStageRegister("TSSolve",&solveStage);
    PetscLogStageRegister("Monitor", &monitorStage);

    PetscLogStagePush(solveStage);
    ierr = TSSolve(ts,flowData->flowField);CHKERRQ(ierr);
    PetscLogStagePop();
    PetscPrintf(PETSC_COMM_WORLD, "Done");
    FlowDestroy(&flowData);
    TSDestroy(&ts);

    return PetscFinalize();

}