#include"pluto.h"
#define MAX_ITER  20
#define COUNT_FAILURES  NO

typedef struct RIEMANN_STATE{
  int    fail;
  double vx, vy, vz;
  double Bx, By, Bz;
  double Kx, Ky, Kz, K2;
  double w, sw, p, rho;
  double u[NFLX], R[NVAR];
  double S, Sa;
/*
  double fun1, fun2, fun3; 
  double denL, denR;
*/
} Riemann_State;


static double Sc, Bx;
static double Fstar (Riemann_State *PaL, Riemann_State *PaR, double p);

static int GET_RIEMANN_STATE (Riemann_State *Pv, double p, int);
static void GET_ASTATE (Riemann_State *Pa,  double p);
static void GET_CSTATE (Riemann_State *PaL, Riemann_State *PaR, double p, double *Uc);

static double PTOT  (double *V);

static void PRINT_STATES(double *vL, double *vR);
static void PRINT_WHATSWRONG(Riemann_State *PaL, Riemann_State *PaR, 
                      double phll, double *vL, double *vR);

#define DEBUG  NO

/* ********************************************************************* */
void HLLD_Solver (const State_1D *state, int beg, int end, 
                  real *cmax, Grid *grid)
/*
 *
 * NAME
 *
 *   HLLD_SOLVER
 *
 *
 * PURPOSE
 *
 *   Solve riemann problem for the relativistic MHD equations 
 *   using th HLLD solver;
 * 
 *   Reference:  "A five wave Harte-Lax-van Leer Riemann solver 
 *                for relativistic magnetohydrodynamics"
 *                Mignone et al, MNRAS (2009) 393,1141.
 *   
 *
 * LAST_MODIFIED
 *
 *   Feb 15/2010, by Andrea Mignone  (mignone@ph.unito.it)
 *              
 *
 *
 **************************************************************************** */
{
  int    nv, i, k;
  int    switch_to_hll;
  real   scrh;
  static real **fluxL, **fluxR;
  static real *pL, *pR, *a2L, *a2R, *hL, *hR;
  static real **Uhll, **Fhll, **Vhll;
  double *vL, *vR, *fL, *fR, *uL, *uR, *SL, *SR;
  static double **VL, **VR, **UL, **UR;
 
  double p0, f0, p, f, dp, dS_1;
  double Uc[NVAR];
  Riemann_State PaL, PaR;
double pguess;

/* -------------------------------------------------------
     for information purposes, show how many failures
     have been encountered in every step
   ------------------------------------------------------- */

  #if COUNT_FAILURES == YES
   static double totfail=0.0,totzones=1.0; 
   static int last_step = -1;
   FILE *fp;   

   if (g_stepNumber == 0 && prank == 0) {
     fp = fopen("fails.dat","w");
     fprintf (fp, "#  Step    Failures (%) \n");
     fprintf (fp, "# -----------------\n");
     fclose(fp);
   }
   if (last_step != g_stepNumber){ /* -- at the beginning of new step -- */
     #ifdef PARALLEL
      MPI_Allreduce (&totfail, &scrh, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
      totfail = scrh;
      MPI_Allreduce (&totzones, &scrh, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
      totzones = scrh;
     #endif
     if (prank == 0){
       fp = fopen("fails.dat","a+");
       fprintf (fp, "    %d    %f\n",g_stepNumber, totfail/totzones*100.);
       fclose(fp);
     }
     totfail = totzones = 0.0; 
     last_step = g_stepNumber;
   } 
  #endif
 
/* ----------------------------------------------------------- */

  if (fluxL == NULL){
    fluxL = ARRAY_2D(NMAX_POINT, NFLX, double);
    fluxR = ARRAY_2D(NMAX_POINT, NFLX, double);
    pL    = ARRAY_1D(NMAX_POINT, double);
    pR    = ARRAY_1D(NMAX_POINT, double);
    Uhll = ARRAY_2D(NMAX_POINT, NVAR, double);
    Fhll = ARRAY_2D(NMAX_POINT, NVAR, double);
    Vhll = ARRAY_2D(NMAX_POINT, NVAR, double);

    #ifdef GLM_MHD
     VL = ARRAY_2D(NMAX_POINT, NVAR, double);
     VR = ARRAY_2D(NMAX_POINT, NVAR, double);
     UL = ARRAY_2D(NMAX_POINT, NVAR, double);
     UR = ARRAY_2D(NMAX_POINT, NVAR, double);
    #endif

    a2L = ARRAY_1D(NMAX_POINT, double);
    a2R = ARRAY_1D(NMAX_POINT, double);
    hL  = ARRAY_1D(NMAX_POINT, double);
    hR  = ARRAY_1D(NMAX_POINT, double);
  }
/*
  #if MHD_FORMULATION == EIGHT_WAVES
   print ("! hlld Riemann solver does not work with Powell's 8-wave\n");
   QUIT_PLUTO(1);
  #endif
*/

  #ifdef GLM_MHD
   GLM_Solve (state, VL, VR, beg, end, grid);
   PrimToCons (VL, UL, beg, end);
   PrimToCons (VR, UR, beg, end);
  #else
   VL = state->vL; UL = state->uL;
   VR = state->vR; UR = state->uR;
  #endif

/* ----------------------------------------------------
     compute sound speed & fluxes at zone interfaces
   ---------------------------------------------------- */

  SoundSpeed2 (VL, a2L, hL, beg, end, FACE_CENTER, grid);
  SoundSpeed2 (VR, a2R, hR, beg, end, FACE_CENTER, grid);

  Flux (UL, VL, hL, fluxL, pL, beg, end);
  Flux (UR, VR, hR, fluxR, pR, beg, end);

/* -- compute speeds -- */

  SL = state->SL; SR = state->SR;
  HLL_Speed (VL, VR, a2L, a2R, hL, hR, SL, SR, beg, end);

/* -- begin main loop -- */

  for (i = beg; i <= end; i++) {

#if DEBUG == YES
    if (!(grid[IDIR].x[i] < 0.5 && grid[IDIR].x[i+1] > 0.5)) continue;
#endif

    #if COUNT_FAILURES == YES
     totzones += 1.0;
    #endif

    scrh  = MAX(fabs(SL[i]), fabs(SR[i]));
    cmax[i] = scrh;

    vL = VL[i]; vR = VR[i]; fR = fluxR[i];
    uL = UL[i]; uR = UR[i]; fL = fluxL[i];

    if (SL[i] >= 0.0){

      for (nv = NFLX; nv--; ) state->flux[i][nv] = fL[nv];
      state->press[i] = pL[i];

    }else if (SR[i] <= 0.0){

      for (nv = NFLX; nv--; ) state->flux[i][nv] = fR[nv];
      state->press[i] = pR[i];

    }else{

  /* ---- build the HLL average state ---- */

      dS_1 = 1.0/(SR[i] - SL[i]);
      for (nv = NFLX; nv--;  ){  /* -- we use NVAR and not NFLX  since 
                                       ConsToPrim may need entropy  -- */
        Uhll[i][nv]  = SR[i]*uR[nv] - SL[i]*uL[nv]  + fL[nv] - fR[nv];
        Uhll[i][nv] *= dS_1;

        Fhll[i][nv]  = SR[i]*fL[nv] - SL[i]*fR[nv] + SL[i]*SR[i]*(uR[nv] - uL[nv]);
        Fhll[i][nv] *= dS_1;
      }
      Uhll[i][MXn] += (pL[i] - pR[i])*dS_1;
      #if NSCL > 0 
       for (nv = NFLX; nv < NVAR; nv++){
         Uhll[i][nv]  = (SR[i] - vR[VXn])*uR[nv] - (SL[i] - vL[VXn])*uL[nv];
         Uhll[i][nv] *= dS_1;

         Fhll[i][nv]  = SR[i]*uL[nv]*vL[VXn] - SL[i]*uR[nv]*vR[VXn] + SL[i]*SR[i]*(uR[nv] - uL[nv]);
         Fhll[i][nv] *= dS_1;
       }
      #endif

  /* ---- revert to HLL in proximity of strong shocks ---- */

      #if SHOCK_FLATTENING == MULTID
       if (CheckZone(i, FLAG_HLL) || CheckZone(i+1, FLAG_HLL)){
         for (nv = NFLX; nv--; ){
           state->flux[i][nv] = Fhll[i][nv];
         }
         state->press[i] = (SR[i]*pL[i] - SL[i]*pR[i])*dS_1;
         continue;
       }
      #endif

  /* ---- proceed normally otherwise ---- */

      PaL.S = SL[i];
      PaR.S = SR[i];
      Bx    = Uhll[i][BXn];
      for (nv = 0; nv < NFLX; nv++){
        PaL.R[nv] = SL[i]*uL[nv] - fL[nv];
        PaR.R[nv] = SR[i]*uR[nv] - fR[nv];
      }
      PaL.R[MXn] -= pL[i];
      PaR.R[MXn] -= pR[i];
      #if SUBTRACT_DENSITY == YES
       PaL.R[ENG] += PaL.R[RHO];
       PaR.R[ENG] += PaR.R[RHO];
      #endif

  /* ---- provide an initial guess ---- */

      scrh = MAX(pL[i], pR[i]);
      if (Bx*Bx/scrh < 0.01) { /* -- try the B->0 limit -- */

        double a,b,c;
        a = SR[i] - SL[i];
        b = PaR.R[ENG] - PaL.R[ENG] + SR[i]*PaL.R[MXn] - SL[i]*PaR.R[MXn];
        c = PaL.R[MXn]*PaR.R[ENG] - PaR.R[MXn]*PaL.R[ENG];
        scrh = b*b - 4.0*a*c;
        scrh = MAX(scrh,0.0);
        p0 = 0.5*(- b + sqrt(scrh))*dS_1;

      }else{  /* ----  use HLL average ---- */
                         
        ConsToPrim(Uhll, Vhll, i, i, state->flag);
        p0    = PTOT(Vhll[i]);
      }
      
  /* ---- check if guess makes sense ---- */

      pguess = p0;
      switch_to_hll = 0;
      f0 = Fstar(&PaL, &PaR, p0);
      if (f0 != f0 || PaL.fail) switch_to_hll = 1;

  /* ---- Root finder ---- */

      k = 0;
      if (fabs(f0) > 1.e-12 && !switch_to_hll){
        p  = 1.025*p0; f  = f0;
        for (k = 1; k < MAX_ITER; k++){

#if DEBUG == YES
 printf ("k = %d, p = %12.6e,  f = %12.6e\n",k,p,f);
#endif

          f  = Fstar(&PaL, &PaR, p);
          if ( f != f  || PaL.fail || (k > 7) || 
               (fabs(f) > fabs(f0) && k > 4)) {
            switch_to_hll = 1;
            break;
          }

          dp = (p - p0)/(f - f0)*f;

          p0 = p; f0 = f;
          p -= dp;
          if (p < 0.0) p = 1.e-6;
          if (fabs(dp) < 1.e-5*p || fabs(f) < 1.e-6) break;
        }
      }else p = p0;

#if DEBUG == YES
PRINT_WHATSWRONG(&PaL, &PaR, phll, phll, p0Bx, p, vL, vR);
#endif

    /* ----  too many iter ? --> use HLL ---- */

      if (PaL.fail) switch_to_hll = 1;
      if (switch_to_hll){

        #if COUNT_FAILURES == YES
         totfail += 1.0;
        #endif

        for (nv = NFLX; nv--; ) state->flux[i][nv] = Fhll[i][nv];
        state->press[i]  = (SR[i]*pL[i] - SL[i]*pR[i])*dS_1;
        continue;
      }

  /* -- ok, solution should be reliable -- */

      g_maxRiemannIter = MAX(g_maxRiemannIter, k);

      #ifdef GLM_MHD
       PaL.u[PSI_GLM] = PaR.u[PSI_GLM] = Uc[PSI_GLM] = uL[PSI_GLM];
      #endif

      if (PaL.Sa >= -1.e-6){      

        GET_ASTATE (&PaL, p);
        #if SUBTRACT_DENSITY == YES
         PaL.u[ENG] -= PaL.u[RHO];
        #endif
        for (nv = NFLX; nv--;   ) {
          state->flux[i][nv] = fL[nv] + SL[i]*(PaL.u[nv] - uL[nv]);
        }
        state->press[i] = pL[i];

      }else if (PaR.Sa <= 1.e-6){

        GET_ASTATE (&PaR, p);
        #if SUBTRACT_DENSITY == YES
         PaR.u[ENG] -= PaR.u[RHO];
        #endif
        for (nv = NFLX; nv--;   ) {
          state->flux[i][nv] = fR[nv] + SR[i]*(PaR.u[nv] - uR[nv]);
        }
        state->press[i] = pR[i];

      }else{

        GET_CSTATE (&PaL, &PaR, p, Uc);
        if (Sc > 0.0){
          #if SUBTRACT_DENSITY == YES
           PaL.u[ENG] -= PaL.u[RHO];
           Uc[ENG]    -= Uc[RHO];
          #endif
          for (nv = NFLX; nv--;   ) {
            state->flux[i][nv] = fL[nv] + SL[i]*(PaL.u[nv] - uL[nv]) 
                                        + PaL.Sa*(Uc[nv] - PaL.u[nv]);
          }
          state->press[i] = pL[i];

        }else{
          #if SUBTRACT_DENSITY == YES
           PaR.u[ENG] -= PaR.u[RHO];
           Uc[ENG]    -= Uc[RHO];
          #endif
          for (nv = NFLX; nv--;   ) {
            state->flux[i][nv] = fR[nv] + SR[i]*(PaR.u[nv] - uR[nv]) 
                                        + PaR.Sa*(Uc[nv] - PaR.u[nv]);
          }
          state->press[i] = pR[i];
        }  
      }
    } /* --- end if on SL, SR -- */
  } /* --- end loop on points -- */

/* --------------------------------------------------------
              initialize source term
   -------------------------------------------------------- */
 
  #if MHD_FORMULATION == EIGHT_WAVES
   HLL_DIVB_SOURCE (state, Uhll, beg + 1, end, grid);
  #endif

}
#undef MAX_ITER
/* *********************************************************************** */
double Fstar (Riemann_State *PaL, Riemann_State *PaR, double p)
/*
 *
 *
 *
 ************************************************************************* */
{
  int    success = 1;
  double dK, Bxc, Byc, Bzc;
  double sBx, fun;
  double vxcL, KLBc;
  double vxcR, KRBc;

  sBx = Bx > 0.0 ? 1.0:-1.0;

  success *= GET_RIEMANN_STATE (PaL, p, -1);
  success *= GET_RIEMANN_STATE (PaR, p,  1);

/* -- compute B from average state -- */

  dK  = PaR->Kx - PaL->Kx + 1.e-12;

  EXPAND(Bxc = Bx*dK;                            ,

         Byc =   PaR->By*(PaR->Kx - PaR->vx) 
               - PaL->By*(PaL->Kx - PaL->vx)
               + Bx*(PaR->vy - PaL->vy);         ,

         Bzc =   PaR->Bz*(PaR->Kx - PaR->vx) 
               - PaL->Bz*(PaL->Kx - PaL->vx)
               + Bx*(PaR->vz - PaL->vz);)
  
  KLBc = EXPAND(PaL->Kx*Bxc, + PaL->Ky*Byc, + PaL->Kz*Bzc);
  KRBc = EXPAND(PaR->Kx*Bxc, + PaR->Ky*Byc, + PaR->Kz*Bzc);

  vxcL = PaL->Kx - dK*Bx*(1.0 - PaL->K2)/(PaL->sw*dK - KLBc);
  vxcR = PaR->Kx - dK*Bx*(1.0 - PaR->K2)/(PaR->sw*dK - KRBc);

  PaL->Sa = PaL->Kx;
  PaR->Sa = PaR->Kx;
  Sc      = 0.5*(vxcL + vxcR);
  fun     = vxcL - vxcR;

/*
fun = dK*(1.0 - Bx*(  (1.0 - PaR->K2)/(PaR->sw*dK - KRBc)
                    - (1.0 - PaL->K2)/(PaL->sw*dK - KLBc)) );
*/

  /* -- check if state makes physically sense -- */

  success  = (vxcL - PaL->Kx)  > -1.e-6;
  success *= (PaR->Kx  - vxcR) > -1.e-6;

  success *= (PaL->S - PaL->vx) < 0.0;
  success *= (PaR->S - PaR->vx) > 0.0;

  success *= (PaR->w - p) > 0.0;
  success *= (PaL->w - p) > 0.0;
  success *= (PaL->Sa - PaL->S)  > -1.e-6;
  success *= (PaR->S  - PaR->Sa) > -1.e-6;

  PaL->fail = !success;

/*
scrh  = (1.0 - PaR->K2)*(PaL->sw*dK - KLBc);
scrh -= (1.0 - PaL->K2)*(PaR->sw*dK - KRBc);

PaL->fun1 = (PaR->sw*dK - KRBc)*(PaL->sw*dK - KLBc) - Bx*scrh; 
PaL->fun2 = scrh;
PaL->denL = (PaL->sw*dK - KLBc);
PaL->denR = (PaR->sw*dK - KRBc);
*/
  return (fun);
}

/* *********************************************************************** */
int GET_RIEMANN_STATE (Riemann_State *Pv, double p, int side)
/*
 *
 *  
 *  Return 1 if succesfull, 0 if w < 0 is encountered.
 *  side = -1 : means left
 *  side =  1 : means right
 *
 ************************************************************************* */
{
  double A, C, G, X, s;
  double vx, vy, vz, scrh, S, *R;

  S = Pv->S;
  R = Pv->R;

  A = R[MXn] + p*(1.0 - S*S) - S*R[ENG];
  C = EXPAND(0.0, + R[BXt]*R[MXt], + R[BXb]*R[MXb]);
  G = EXPAND(0.0, + R[BXt]*R[BXt], + R[BXb]*R[BXb]);
  X = Bx*(A*S*Bx + C) - (A + G)*(S*p + R[ENG]);

  EXPAND(vx = ( Bx*(A*Bx + C*S) - (R[MXn] + p)*(G + A) );  ,

         vy = ( - (A + G - Bx*Bx*(1.0 - S*S))*R[MXt]     
                  + R[BXt]*(C + Bx*(S*R[MXn] - R[ENG])) );   ,
 
         vz = ( - (A + G - Bx*Bx*(1.0 - S*S))*R[MXb]     
                 + R[BXb]*(C + Bx*(S*R[MXn] - R[ENG])) );)

  scrh = EXPAND(vx*R[MXn], + vy*R[MXt], + vz*R[MXb]);
  scrh = X*R[ENG] - scrh;
  Pv->w = p + scrh/(X*S - vx);

  if (Pv->w < 0.0) return(0);  /* -- failure -- */

  EXPAND(Pv->vx = vx/X;  , 
         Pv->vy = vy/X;  ,
         Pv->vz = vz/X;)
/*
  EXPAND(Pv->Bx = Bx;                            , 
         Pv->By = (R[BXt]*X - Bx*vy)/(X*S - vx);  ,
         Pv->Bz = (R[BXb]*X - Bx*vz)/(X*S - vx);)
*/
  EXPAND(Pv->Bx = Bx;                                   , 
         Pv->By = -(R[BXt]*(S*p + R[ENG]) - Bx*R[MXt])/A;  ,
         Pv->Bz = -(R[BXb]*(S*p + R[ENG]) - Bx*R[MXb])/A;)
  
  s  = Bx > 0.0 ? 1.0:-1.0;
  if (side < 0) s *= -1.0;

  Pv->sw = s*sqrt(Pv->w);

  scrh = 1.0/(S*p +  R[ENG] + Bx*Pv->sw);
  EXPAND(Pv->Kx = scrh*(R[MXn] + p + R[BXn]*Pv->sw);  ,
         Pv->Ky = scrh*(R[MXt]     + R[BXt]*Pv->sw);  ,
         Pv->Kz = scrh*(R[MXb]     + R[BXb]*Pv->sw);)

  Pv->K2 = EXPAND(Pv->Kx*Pv->Kx, + Pv->Ky*Pv->Ky, + Pv->Kz*Pv->Kz);
  return(1); /* -- success -- */
}
/* ************************************************************* */
void GET_ASTATE (Riemann_State *Pa,  double p)
/*
 *
 *        Compute states aL and aR (behind fast waves)
 *
 *************************************************************** */
{
  double vB, *ua, *R, S;
  double scrh;
  
  ua = Pa->u;
  S  = Pa->S;
  R  = Pa->R;

  scrh = 1.0/(S - Pa->vx);

  ua[RHO] =  R[RHO]*scrh;
  EXPAND(ua[BXn] =  Bx;                       ,
         ua[BXt] = (R[BXt] - Bx*Pa->vy)*scrh;  ,
         ua[BXb] = (R[BXb] - Bx*Pa->vz)*scrh;)

  vB     = EXPAND(Pa->vx*ua[BXn], + Pa->vy*ua[BXt], + Pa->vz*ua[BXb]);
  ua[ENG] = (R[ENG] + p*Pa->vx - vB*Bx)*scrh;

  EXPAND(ua[MXn] = (ua[ENG] + p)*Pa->vx - vB*ua[BXn];  ,
         ua[MXt] = (ua[ENG] + p)*Pa->vy - vB*ua[BXt];  ,
         ua[MXb] = (ua[ENG] + p)*Pa->vz - vB*ua[BXb];)
}

/* ************************************************************* */
void GET_CSTATE (Riemann_State *PaL, Riemann_State *PaR, double p,
                double *Uc)
/*
 *
 *     Compute state  cL and cR across contact mode
 *
 *************************************************************** */
{
  double dK, *ua;
  double vxcL, vycL, vzcL, KLBc;
  double vxcR, vycR, vzcR, KRBc;
  double vxc, vyc, vzc, vBc;
  double Bxc, Byc, Bzc, Sa, vxa;
  double scrhL, scrhR;

  GET_ASTATE (PaL, p);
  GET_ASTATE (PaR, p);
  dK = (PaR->Kx - PaL->Kx) + 1.e-12;

  EXPAND(Bxc = Bx*dK;                          ,

         Byc =   PaR->By*(PaR->Kx - PaR->vx) 
               - PaL->By*(PaL->Kx - PaL->vx)
                    + Bx*(PaR->vy - PaL->vy);  ,

         Bzc =   PaR->Bz*(PaR->Kx - PaR->vx) 
               - PaL->Bz*(PaL->Kx - PaL->vx)
                    + Bx*(PaR->vz - PaL->vz);)
   
  EXPAND(Bxc  = Bx;  ,
         Byc /= dK;  ,
         Bzc /= dK;)

  EXPAND(Uc[BXn] = Bxc;  ,
         Uc[BXt] = Byc;  ,
         Uc[BXb] = Bzc;)

  KLBc = EXPAND(PaL->Kx*Bxc, + PaL->Ky*Byc, + PaL->Kz*Bzc);
  KRBc = EXPAND(PaR->Kx*Bxc, + PaR->Ky*Byc, + PaR->Kz*Bzc);

  scrhL = (1.0 - PaL->K2)/(PaL->sw - KLBc);
  scrhR = (1.0 - PaR->K2)/(PaR->sw - KRBc);

  EXPAND(vxcL = PaL->Kx - Uc[BXn]*scrhL;  
         vxcR = PaR->Kx - Uc[BXn]*scrhR;  ,

         vycL = PaL->Ky - Uc[BXt]*scrhL;  
         vycR = PaR->Ky - Uc[BXt]*scrhR;  ,

         vzcL = PaL->Kz - Uc[BXb]*scrhL;
         vzcR = PaR->Kz - Uc[BXb]*scrhR;)

  EXPAND(vxc = 0.5*(vxcL + vxcR);  ,
         vyc = 0.5*(vycL + vycR);  ,
         vzc = 0.5*(vzcL + vzcR);)

  if (vxc > 0.0) {
    GET_ASTATE (PaL, p);
    ua  = PaL->u;
    Sa  = PaL->Sa;
    vxa = PaL->vx;
  } else {
    GET_ASTATE (PaR, p);
    ua  = PaR->u;
    Sa  = PaR->Sa;
    vxa = PaR->vx;
  }
  
  vBc = EXPAND(vxc*Uc[BXn], + vyc*Uc[BXt], + vzc*Uc[BXb]);

  Uc[RHO] = ua[RHO]*(Sa - vxa)/(Sa - vxc);
  Uc[ENG] = (Sa*ua[ENG] - ua[MXn] + p*vxc - vBc*Bx)/(Sa - vxc);

  EXPAND(Uc[MXn] = (Uc[ENG] + p)*vxc - vBc*Bx;      ,
         Uc[MXt] = (Uc[ENG] + p)*vyc - vBc*Uc[BXt];  ,
         Uc[MXb] = (Uc[ENG] + p)*vzc - vBc*Uc[BXb];)
}

/* ************************************************************* */
double PTOT (double *V)
/*
 *
 * Compute total pressure
 *
 *************************************************************** */
{
  double vel2, Bmag2, vB;
  double pt;

  vel2  = EXPAND(V[VX1]*V[VX1], + V[VX2]*V[VX2], + V[VX3]*V[VX3]);
  Bmag2 = EXPAND(V[BX1]*V[BX1], + V[BX2]*V[BX2], + V[BX3]*V[BX3]);
  vB    = EXPAND(V[VX1]*V[BX1], + V[VX2]*V[BX2], + V[VX3]*V[BX3]);
          
  pt = V[PRS] + 0.5*(Bmag2*(1.0 - vel2) + vB*vB);
  return(pt);  
}



void PRINT_STATES(double *vL, double *vR)
{
  printf ("RHO_LEFT  %18.9e\n",vL[RHO]);
  printf ("VX_LEFT   %18.9e\n",vL[VXn]);
  printf ("VY_LEFT   %18.9e\n",vL[VXt]);
  #if COMPONENTS == 3
   printf ("VZ_LEFT   %18.9e \n" ,vL[VXb]);
  #else
   printf ("VZ_LEFT   %18.9e \n" ,0.0);
  #endif
  printf ("BY_LEFT   %18.9e\n",vL[BXt]);
  #if COMPONENTS == 3
   printf ("BZ_LEFT   %18.9e \n" ,vL[BXb]);
  #else
   printf ("BZ_LEFT   %18.9e \n" ,0.0);
  #endif
  printf ("PR_LEFT   %18.9e \n" ,vL[PRS]);

  printf ("RHO_RIGHT  %18.9e\n",vR[RHO]);
  printf ("VX_RIGHT   %18.9e\n",vR[VXn]);
  printf ("VY_RIGHT   %18.9e\n",vR[VXt]);
  #if COMPONENTS == 3
   printf ("VZ_RIGHT   %18.9e \n", vR[VXb]);
  #else
   printf ("VZ_RIGHT   %18.9e \n", 0.0);
  #endif
  printf ("BY_RIGHT   %18.9e\n", vR[BXt]);
  #if COMPONENTS == 3
   printf ("BZ_RIGHT   %18.9e\n", vR[BXb]);
  #else
   printf ("BZ_RIGHT   %18.9e\n",0.0);
  #endif
  printf ("PR_RIGHT   %18.9e \n",vR[PRS]);
  printf ("BX_CONST   %18.9e \n",vR[BXn]);
  printf ("GAMMA_EOS  %18.9e\n",g_gamma);
}


void PRINT_WHATSWRONG(Riemann_State *PaL, Riemann_State *PaR, 
                      double pguess, double *vL, double *vR)
{
  double f,p;
  FILE *fp;

  PRINT_STATES(vL, vR);

  GET_ASTATE (PaL, pguess);
  GET_ASTATE (PaR, pguess);

  printf ("pguess  = %f, F = %f\n",pguess, Fstar(PaL, PaR, pguess));
  printf ("S = %f  %f  %f  %f  %f\n", PaL->S, PaL->Sa, Sc, PaR->Sa, PaR->S);

  fp = fopen("new.dat","w");
  for (p = 0.01*pguess; p <= 10.0*pguess; p += 1.e-4*pguess){
    f = Fstar(PaL, PaR, p);
    fprintf (fp,"%f  %f %f  %f\n", p, f, Sc - PaL->Sa, PaR->Sa - Sc);
  }
  fclose(fp);
  exit(1);
}