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

#include "cctk.h"
#include "cctk_Parameters.h"
#include "cctk_Arguments.h"
#include "cctk_Functions.h"
#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))


// some prototypes
CCTK_INT GRHydro_UseGeneralCoordinates(const cGH * cctkGH);
static inline double SpatialDeterminantC(double gxx, double gxy,
					double gxz, double gyy,
					double gyz, double gzz);

static inline __attribute__((always_inline)) void prim2conC(double *w, double *dens, double *sx, 
			     double *sy, double *sz, double *tau, 
			     const double gxx, const double gxy,
			     const double gxz, const double gyy, const double gyz, 
			     const double gzz, const double sdet, 
			     const double rho, const double vx,
			     const double vy, const double vz, 
			     const double eps, const double press); 




void Primitive2ConservativeC(CCTK_ARGUMENTS);

CCTK_FCALL void CCTK_FNAME(Primitive2ConservativeCforF)(cGH ** p_cctkGH) {
  Primitive2ConservativeC(*p_cctkGH);
}


void Primitive2ConservativeC(CCTK_ARGUMENTS)
{
   DECLARE_CCTK_ARGUMENTS;
   DECLARE_CCTK_PARAMETERS;

   // save memory when multipatch is not used
   CCTK_REAL * restrict g11, * restrict g12, * restrict g13;
   CCTK_REAL * restrict g22, * restrict g23, * restrict g33;
    
   if(GRHydro_UseGeneralCoordinates(cctkGH)) {
     g11 = gaa;
     g12 = gab;
     g13 = gac;
     g22 = gbb;
     g23 = gbc;
     g33 = gcc;
   } else {
     g11 = gxx;
     g12 = gxy;
     g13 = gxz;
     g22 = gyy;
     g23 = gyz;
     g33 = gzz;
   }

   // if padding is used the the "extra" vector elements could contain junk
   // which we do not know how to handle
   assert(cctk_lsh[0] == cctk_ash[0]);
   assert(cctk_lsh[1] == cctk_ash[1]);
   assert(cctk_lsh[2] == cctk_ash[2]);

   // EOS calls (now GF-wide)
   if(!*evolve_temper) {
     int n = cctk_ash[0]*cctk_ash[1]*cctk_ash[2];
     int *keyerr = malloc(sizeof(*keyerr)*n);
     int anyerr = 0;
     int keytemp = 0;

     // don't need special error handling for analytic EOS
     EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		    rhominus,epsminus,NULL,NULL,pressminus,keyerr,&anyerr);

     EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		    rhoplus,epsplus,NULL,NULL,pressplus,keyerr,&anyerr);
     
     free(keyerr);
   } else {
     if(reconstruct_temper) {
       int n = cctk_ash[0]*cctk_ash[1]*cctk_ash[2];
       int *keyerr = malloc(sizeof(*keyerr)*n);
       int anyerr = 0;
       int keytemp = 1;

       // ensure Y_e and temperature within bounds
#pragma omp parallel for
       for(int i=0;i<n;i++) {
	 Y_e_minus[i] = MAX(MIN(Y_e_minus[i],GRHydro_Y_e_max),GRHydro_Y_e_min);
	 Y_e_plus[i] = MAX(MIN(Y_e_plus[i],GRHydro_Y_e_max),GRHydro_Y_e_min);
	 tempminus[i] = MIN(MAX(tempminus[i],GRHydro_hot_atmo_temp),GRHydro_max_temp);
	 tempplus[i] = MIN(MAX(tempplus[i],GRHydro_hot_atmo_temp),GRHydro_max_temp);
        }

       EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		      rhominus,epsminus,tempminus,Y_e_minus,pressminus,keyerr,&anyerr);

       if(anyerr!=0) {
#pragma omp parallel for
	 for(int i=0;i<n;i++) {
	   if(keyerr[i] != 0) {
#pragma omp critical
	     {
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d i,x,y,z: %d %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, i, x[i], y[i], z[i], keyerr[i]);
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d r,t,ye: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, rhominus[i], tempminus[i], Y_e_minus[i], keyerr[i]);
	     }
	   }
	 } // for i, i<n
	 CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
		    "Aborting!");
       }

       EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		      rhoplus,epsplus,tempplus,Y_e_plus,pressplus,keyerr,&anyerr);

       if(anyerr!=0) {
#pragma omp parallel for
	 for(int i=0;i<n;i++) {
	   if(keyerr[i] != 0) {
#pragma omp critical
	     {
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d i,x,y,z: %d %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, i, x[i], y[i], z[i], keyerr[i]);
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d r,t,ye: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, rhoplus[i], tempplus[i], Y_e_plus[i], keyerr[i]);
	     }
	   }
	 } // for i, i<n
	 CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
		    "Aborting!");
       }
       free(keyerr);
     } else {
       // ******************** EPS RECONSTRUCTION BRANCH ******************
       int n = cctk_ash[0]*cctk_ash[1]*cctk_ash[2];
       int *keyerr = malloc(sizeof(*keyerr)*n);
       int anyerr = 0;
       int keytemp = 0;

       // ensure Y_e and temperature within bounds
#pragma omp parallel for
       for(int i=0;i<n;i++) {
	 Y_e_minus[i] = MAX(MIN(Y_e_minus[i],GRHydro_Y_e_max),GRHydro_Y_e_min);
	 Y_e_plus[i] = MAX(MIN(Y_e_plus[i],GRHydro_Y_e_max),GRHydro_Y_e_min);
	 tempminus[i] = MIN(MAX(tempminus[i],GRHydro_hot_atmo_temp),GRHydro_max_temp);
	 tempplus[i] = MIN(MAX(tempplus[i],GRHydro_hot_atmo_temp),GRHydro_max_temp);
	 temperature[i] = MIN(MAX(temperature[i],GRHydro_hot_atmo_temp),GRHydro_max_temp);
        }

       EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		      rhominus,epsminus,tempminus,Y_e_minus,pressminus,keyerr,&anyerr);

       if(anyerr!=0) {
#pragma omp parallel for
	 for(int i=0;i<n;i++) {
	   if(keyerr[i] != 0) {
#pragma omp critical
	     {
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d x,y,z: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, x[i], y[i], z[i], keyerr[i]);
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d r,t,ye: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, rhominus[i], tempminus[i], 
			  Y_e_minus[i], keyerr[i]);
	       if(keyerr[i] == 668) {
		 // This means the temperature came back negative.
		 // We'll try using piecewise constant for the temperature
		 tempminus[i] = temperature[i];
		 const int ln=1;
		 int lkeyerr[1];
		 int lanyerr = 0;
		 int lkeytemp = 1;
		 EOS_Omni_press(*GRHydro_eos_handle,lkeytemp,GRHydro_eos_rf_prec,ln,
				&rhominus[i],&epsminus[i],&tempminus[i],
				&Y_e_minus[i],&pressminus[i],lkeyerr,&lanyerr);
		 if(lanyerr !=0) {
		   CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
			      "Aborting! keyerr=%d, r=%15.6E, t=%15.6E, ye=%15.6E",
			      lkeyerr[0],rhominus[i],tempminus[i],Y_e_minus[i]);
		 }		 
	       } else {
		 CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
			    "Aborting!");
	       }
	     }
	   }
	 } // for i, i<n
       }

       EOS_Omni_pressOMP(*GRHydro_eos_handle,keytemp,GRHydro_eos_rf_prec,n,
		      rhoplus,epsplus,tempplus,Y_e_plus,pressplus,keyerr,&anyerr);

       if(anyerr!=0) {
#pragma omp parallel for
	 for(int i=0;i<n;i++) {
	   if(keyerr[i] != 0) {
#pragma omp critical
	     {
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d x,y,z: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, x[i], y[i], z[i], keyerr[i]);
	       CCTK_VWarn(1, __LINE__, __FILE__, CCTK_THORNSTRING,
			  "rl: %d r,t,ye: %15.6E %15.6E %15.6E, keyerr: %d",
			  *GRHydro_reflevel, rhoplus[i], tempplus[i], Y_e_plus[i], keyerr[i]);
	       if(keyerr[i] == 668) {
		 // This means the temperature came back negative.
		 // We'll try using piecewise constant for the temperature
		 tempplus[i] = temperature[i];
		 const int ln=1;
		 int lkeyerr[1];
		 int lanyerr = 0;
		 int lkeytemp = 1;
		 EOS_Omni_press(*GRHydro_eos_handle,lkeytemp,GRHydro_eos_rf_prec,ln,
				&rhoplus[i],&epsplus[i],&tempplus[i],
				&Y_e_plus[i],&pressplus[i],lkeyerr,&lanyerr);
		 if(lanyerr !=0) {
		   CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
			      "Aborting! keyerr=%d, r=%15.6E, t=%15.6E, ye=%15.6E",
			      lkeyerr[0],rhoplus[i],tempplus[i],Y_e_plus[i]);
		 }		 
	       } else {
		 CCTK_VWarn(0, __LINE__, __FILE__, CCTK_THORNSTRING,
			    "Aborting!");
	       }
	     } // end critical
	   }
	 } // for i, i<n error checking
       }
       free(keyerr);
     } // end branch for no temp reconsturction
   } // end of evolve temper branch


#pragma omp parallel for
     for(int k = GRHydro_stencil-1; k < cctk_lsh[2]-GRHydro_stencil+1; k++)
       for(int j = GRHydro_stencil-1; j < cctk_lsh[1]-GRHydro_stencil+1; j++) 
#pragma ivdep // force compiler to vectorize the goddamn loop
	 for(int i = GRHydro_stencil-1; i < cctk_lsh[0]-GRHydro_stencil+1; i++) {

	   const int idx = CCTK_GFINDEX3D(cctkGH,i,j,k);

	   const int idxl = CCTK_GFINDEX3D(cctkGH,i-*xoffset,j-*yoffset,k-*zoffset);
	   const int idxr = CCTK_GFINDEX3D(cctkGH,i+*xoffset,j+*yoffset,k+*zoffset);

	   const double g11l = 0.5 * (g11[idx] + g11[idxl]);
	   const double g12l = 0.5 * (g12[idx] + g12[idxl]);
	   const double g13l = 0.5 * (g13[idx] + g13[idxl]);
	   const double g22l = 0.5 * (g22[idx] + g22[idxl]);
	   const double g23l = 0.5 * (g23[idx] + g23[idxl]);
	   const double g33l = 0.5 * (g33[idx] + g33[idxl]);

	   const double g11r = 0.5 * (g11[idx] + g11[idxr]);
	   const double g12r = 0.5 * (g12[idx] + g12[idxr]);
	   const double g13r = 0.5 * (g13[idx] + g13[idxr]);
	   const double g22r = 0.5 * (g22[idx] + g22[idxr]);
	   const double g23r = 0.5 * (g23[idx] + g23[idxr]);
	   const double g33r = 0.5 * (g33[idx] + g33[idxr]);

	   const double savg_detl = 
	     sqrt(SpatialDeterminantC(g11l,g12l,g13l,g22l,g23l,g33l));
	   const double savg_detr = 
	     sqrt(SpatialDeterminantC(g11r,g12r,g13r,g22r,g23r,g33r));

	   // minus call to p2c
	   prim2conC(&w_lorentzminus[idx], &densminus[idx], &sxminus[idx],
		     &syminus[idx], &szminus[idx], &tauminus[idx],
		     g11l,g12l,g13l,g22l,g23l,g33l,
		     savg_detl,rhominus[idx], velxminus[idx], velyminus[idx],
		     velzminus[idx], epsminus[idx], pressminus[idx]);


	   // plus call to p2c
	   prim2conC(&w_lorentzplus[idx], &densplus[idx], &sxplus[idx],
		     &syplus[idx], &szplus[idx], &tauplus[idx],
		     g11r,g12r,g13r,g22r,g23r,g33r,
		     savg_detr,rhoplus[idx], velxplus[idx], velyplus[idx],
		     velzplus[idx], epsplus[idx], pressplus[idx]);
	 }       

} // end function Conservative2PrimitiveC

static inline double SpatialDeterminantC(double gxx, double gxy,
					 double gxz, double gyy,
					 double gyz, double gzz) 
{
  return -gxz*gxz*gyy + 2.0*gxy*gxz*gyz - gxx*gyz*gyz 
    - gxy*gxy*gzz + gxx*gyy*gzz;
}


static inline __attribute__((always_inline)) void prim2conC(double *w, double *dens, double *sx, 
			     double *sy, double *sz, double *tau,
			     const double gxx, const double gxy,
			     const double gxz, const double gyy, 
			     const double gyz, 
			     const double gzz, const double sdet, 
			     const double rho, const double vx,
			     const double vy, const double vz, 
			     const double eps, const double press)
{

  // local helpers
  const double wtemp = 1.0 / sqrt(1.0 - (gxx*vx*vx + gyy*vy*vy + 
				    gzz*vz*vz + 2.0*gxy*vx*vy 
				    + 2.0*gxz*vx*vz + 2.0*gyz*vy*vz));

  const double vlowx = gxx*vx + gxy*vy + gxz*vz;
  const double vlowy = gxy*vx + gyy*vy + gyz*vz;
  const double vlowz = gxz*vx + gyz*vy + gzz*vz;
  
  const double hrhow2 = (rho*(1.0+eps)+press)*(wtemp)*(wtemp);
  const double denstemp = sdet*rho*(wtemp);

  *w = wtemp;
  *dens = denstemp;
  *sx = sdet*hrhow2 * vlowx;
  *sy = sdet*hrhow2 * vlowy;
  *sz = sdet*hrhow2 * vlowz;
  *tau = sdet*( hrhow2 - press) - denstemp;

}