MODULE CommonData
USE types
IMPLICIT NONE

    !file for input and output
    character(200) :: outfile,molfile,specref
    character(*), PARAMETER :: radat='./data/'
    character(*), PARAMETER :: version = '30nov2011'
    character(*), PARAMETER :: logfile = './radex.log'

    !Escape probability method (uncomment your choice)
    INTEGER :: method=1 !1=uniform sphere,2=LVG, 3=slab

    !---------------------------------------------------------
    !Physical and astronomical constants (CODATA 2002)
    REAL(dp), PARAMETER :: clight  = 2.99792458d10  ! speed of light     (cm/s)
    REAL(dp), PARAMETER :: hplanck = 6.6260963d-27  ! Planck constant    (erg/Hz)
    REAL(dp), PARAMETER :: kboltz  = 1.3806505d-16  ! Boltzmann constant (erg/K)
    REAL(dp), PARAMETER :: pi      = 3.14159265d0  ! pi
    REAL(dp), PARAMETER :: amu     = 1.67262171d-24 ! atomic mass unit   (g)
    REAL(dp), PARAMETER :: tcmb = 2.725 ! CMB background temperature (K)
    !---------------------------------------------------------

    !Array sizes
    INTEGER, PARAMETER :: maxpart = 9     ! maximum no. of collision partners (seven defined)
    INTEGER, PARAMETER :: maxtemp = 99    ! maximum no. of collision temperatures
    INTEGER, PARAMETER :: maxlev  = 2999  ! maximum no. of energy levels
    INTEGER, PARAMETER :: maxline = 99999  ! maximum no. of radiative transitions
    INTEGER, PARAMETER :: maxcoll = 99999 ! maximum no. of collisional transitions

    !---------------------------------------------------------
    !   Molecular data
    !---------------------------------------------------------
    INTEGER :: nlev,nline,ncoll,npart,ntemp,iupp(maxline),ilow(maxline)
    !nlev:  actual number of levels
    !nline: actual number of lines
    !ncoll: actual number of transitions
    !npart: actual number of partners
    !ntemp: actual number of collision temperatures

    !iupp(i): upper level of line i
    !ilow(i): lower level of line i

    REAL(dp) :: amass,eterm(maxlev),gstat(maxlev),aeinst(maxline)&
         &,eup(maxline)
    ! amass:  molecular mass              (amu)
    ! eterm:  energy levels               (1/cm)
    ! gstat:  statistical weights
    ! aeinst: Einstein A coefficients     (1/s)
    ! eup:    line upper level energy     (K)
    ! colld:  downward rate coefficients  (cm^3 /s)
    ! xpop:   level populations

    !---------------------------------------------------------
    !Physical conditions
    !---------------------------------------------------------
    REAL(dp) :: density(maxpart),tkin,tbg,cdmol,deltav,totdens

    ! density:  number densities of collision partners  (cm^-3)
    ! totdens:  total number density of all partners    (cm^-3)
    ! tkin:     kinetic temperature                     (K)
    ! tbg:      temperature of background radiation     (K)
    ! cdmol:    molecular column density                (cm^-2)
    ! deltav:   FWHM line width                         (cm/s)

    !---------------------------------------------------------
    !  Numerical parameters
    !---------------------------------------------------------
    INTEGER, PARAMETER :: miniter=10  ! minimum number of iterations
    INTEGER, PARAMETER :: maxiter=9999 ! maximum number of iterations

    REAL(dp) :: fmin,fmax !minimum/maximum output frequency
    REAL(dp), PARAMETER :: ccrit=1.0e-6   ! relative tolerance on solution
    REAL(dp), PARAMETER :: eps=1.0d-30    ! round-off error
    REAL(dp), PARAMETER :: minpop=1.0d-20 ! minimum level population

    !---------------------------------------------------------
    ! Radiative quantities
    !---------------------------------------------------------
    REAL(dp) :: taul(maxline),tex(maxline),backi(maxline),xnu(maxline)
    REAL(dp) :: trj(maxline),totalb(maxline),spfreq(maxline),antennaTemp(maxline)
    REAL(dp) :: upperPops(maxline),lowerPops(maxline),wavelength(maxline)
    REAL(dp) :: intensityKkms(maxline),intensityErgs(maxline)

    CHARACTER(6) :: qnum(maxlev),lowQNum(maxlev),upperQNum(maxlev)
    REAL(dp), PARAMETER :: fk    = hplanck*clight/kboltz
    REAL(dp), PARAMETER :: thc   = 2.d0*hplanck*clight
    REAL(dp), PARAMETER :: fgaus = 1.0645*8.0*pi

    ! xnu:    line frequency (cm^-1)
    ! taul:   line optical depth
    ! tex:    line excitation temperature

    ! trj:    background brightness (RJ)
    ! backi:  background intensity [erg s-1 cm-2 Hz-1 sr-1]
    ! totalb: background temperature (BB)

    ! fk,thc: help to calculate intensities
    ! fgaus:  accounts for Gaussian line shape

    ! spfreq: spectroscopic line frequency (GHz), not used in
    !       calculation but only to print output
    ! qnum:   quantum numbers of levels

    !---------------------------------------------------------
    !   Collisional quantities
    !---------------------------------------------------------
    REAL(dp) :: ctot(maxlev),crate(maxlev,maxlev),xpop(maxlev)

    ! crate: collision rate matrix (density * rate coefficient)
    ! ctot:  total collision rate
    ! xpop:  level populations

    !For development / maintenance purposes:
    LOGICAL, PARAMETER :: debug =.False.
END MODULE CommonData


      SUBROUTINE DCOSQF1 (N,X,W,XH)
	IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION       X(*)       ,W(*)       ,XH(*)
      NS2 = (N+1)/2
      NP2 = N+2
      DO 101 K=2,NS2
         KC = NP2-K
         XH(K) = X(K)+X(KC)
         XH(KC) = X(K)-X(KC)
  101 CONTINUE
      MODN = MOD(N,2)
      IF (MODN .EQ. 0) XH(NS2+1) = X(NS2+1)+X(NS2+1)
      DO 102 K=2,NS2
         KC = NP2-K
         X(K) = W(K-1)*XH(KC)+W(KC-1)*XH(K)
         X(KC) = W(K-1)*XH(K)-W(KC-1)*XH(KC)
  102 CONTINUE
      IF (MODN .EQ. 0) X(NS2+1) = W(NS2)*XH(NS2+1)
      CALL DFFTF (N,X,XH)
      DO 103 I=3,N,2
         XIM1 = X(I-1)-X(I)
         X(I) = X(I-1)+X(I)
         X(I-1) = XIM1
  103 CONTINUE
      RETURN
      END


module advance_module

  use shared_data
  use boundary_conditions
  use diagnostics
!  use gauss_seidel
  use multigrid

  implicit none

  private

  public :: advance_dt

  type(mg_input) :: input

  contains

  subroutine advance_dt

    call set_dt

    call step_1 ! Calculate time-centered normal velocities on the interfaces

    call step_2 ! Step 2 : MAC Projection

    call step_3 ! Step 3: Reconstruct interface states consistent with constraint

    call advect_dens ! consv. update of density based on mac vels

    call step_4 ! Step 4: Provisional update for full dt (star state)

    call step_5 ! Step 5: Project provisional field to constraint

    time = time + dt

  end subroutine advance_dt

  subroutine step_1

    real(num) :: du, dv
    real(num) :: transv

    ! 1 - Calculate the advective velocities

    ! 1A -  calculate  time-centered interface states for the normal
    ! velocities

    ! be careful with indicies (conversion between i+1/2 type notation
    ! and separate indicies for xc and xb values)

    ! x faced data

    call velocity_bcs(arr_cc = u,di = 1)
    call velocity_bcs(arr_cc = v,di = 2)

    do iy = 1, ny
    do ix = 0, nx  !xb counts from 0 to nx, <0 and >nx are ghosts

      if (use_minmod) then
        du = minmod((u(ix,iy)-u(ix-1,iy))/dx,(u(ix+1,iy)-u(ix,iy))/dx)
      else
        du = ( u(ix+1,iy) - u(ix-1,iy) ) /2.0_num / dx
      endif

      uhxl(ix,iy) = u(ix,iy) + &
        0.5_num * (1.0_num - dt * u(ix,iy) / dx ) * du * dx

      if (use_minmod) then
        du = minmod((u(ix+1,iy)-u(ix,iy))/dx,(u(ix+2,iy)-u(ix+1,iy))/dx)
      else
        du= ( u(ix+2,iy)-u(ix,iy) ) / 2.0_num / dx
      endif

      uhxr(ix,iy) = u(ix+1,iy) - &
        0.5_num * (1.0_num + dt * u(ix+1,iy) / dx ) * du * dx

      if (use_minmod) then
        dv = minmod((v(ix,iy)-v(ix-1,iy))/dx, (v(ix+1,iy)-v(ix,iy))/dx)
      else
        dv = (v(ix+1,iy) - v(ix-1,iy))/2.0_num/dx
      endif
      vhxl(ix,iy) = v(ix,iy) + &
        & 0.5_num * (1.0_num - dt * u(ix,iy) / dx ) * dv * dx

      if (use_minmod) then
        dv = minmod((v(ix+1,iy)-v(ix,iy))/dx, (v(ix+2,iy)-v(ix+1,iy))/dx)
      else
        dv = (v(ix+2,iy)-v(ix,iy)) /2.0_num/dx
      endif
      vhxr(ix,iy) = v(ix+1,iy) - &
        & 0.5_num * (1.0_num + dt * u(ix+1,iy) / dx) * dv * dx

    enddo
    enddo

    ! y faced data

    do iy = 0, ny
    do ix = 1, nx

      if (use_minmod) then
        du = minmod( (u(ix,iy)-u(ix,iy-1))/dy, (u(ix,iy+1)-u(ix,iy))/dy)
      else
        du = (u(ix,iy+1) - u(ix,iy-1)) / 2.0_num / dy
      endif

      uhyl(ix,iy) = u(ix,iy) + &
        & 0.5_num * (1.0_num - dt * v(ix,iy) / dy) * du * dy

      if (use_minmod) then
        du = minmod( (u(ix,iy+1)-u(ix,iy))/dy, (u(ix,iy+2)-u(ix,iy+1))/dy)
      else
        du = (u(ix,iy+2)-u(ix,iy)) / 2.0_num/dy
      endif

      uhyr(ix,iy) = u(ix,iy+1) - &
        & 0.5_num * (1.0_num + dt * v(ix,iy+1) / dy) * du * dy

      if (use_minmod) then
        dv = minmod( (v(ix,iy)-v(ix,iy-1))/dy, (v(ix,iy+1)-v(ix,iy))/dy)
      else
        dv = (v(ix,iy+1)-v(ix,iy-1))/2.0_num/dy
      endif
      vhyl(ix,iy) = v(ix,iy) + &
        & 0.5_num * (1.0_num - dt * v(ix,iy) / dy ) * dv * dy

      if (use_minmod) then
        dv = minmod( (v(ix,iy+1)-v(ix,iy))/dy, (v(ix,iy+2)-v(ix,iy+1))/dy)
      else
        dv = (v(ix,iy+2)-v(ix,iy))/2.0_num/dy
      endif
      vhyr(ix,iy) = v(ix,iy+1) - &
        & 0.5_num * (1.0_num + dt * v(ix,iy+1) / dy) * dv * dy

    enddo
    enddo

    ! 1B - Use the normal velocities to calculate the advective vel
    ! by solving a Riemann problem

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        uha(ix,iy) = riemann(uhxl(ix,iy),uhxr(ix,iy))
      endif
      if (ix /= 0) then !can do the yface stuff
        vha(ix,iy) = riemann(vhyl(ix,iy), vhyr(ix,iy))
      endif
    enddo
    enddo

    ! 1C - Upwind the hat states (normal velocity predictions)
    ! using the advective vels

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        uhx(ix,iy) = upwind(uha(ix,iy),uhxl(ix,iy),uhxr(ix,iy))
        vhx(ix,iy) = upwind(uha(ix,iy),vhxl(ix,iy),vhxr(ix,iy))
      endif
      if (ix /= 0) then !can do the yface stuff
        uhy(ix,iy) = upwind(vha(ix,iy),uhyl(ix,iy),uhyr(ix,iy))
        vhy(ix,iy) = upwind(vha(ix,iy),vhyl(ix,iy),vhyr(ix,iy))
      endif
    enddo
    enddo

    ! 1D construct the full left and right predictions of normal
    ! velocities on the interfaces

    ! (actually get them on all interfaces, as needed later steps)

    call velocity_bcs(arr_xface = uha, di = 1)
    call velocity_bcs(arr_yface = vha, di = 2)
    call velocity_bcs(arr_xface = uhx, arr_yface = uhy, di = 1)
    call velocity_bcs(arr_xface = vhx, arr_yface = vhy, di = 2)

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        ! normal components

        ! left
        transv = -0.5_num * dt * 0.5_num * (vha(ix,iy-1) + vha(ix,iy)) &
          & * (uhy(ix,iy)-uhy(ix,iy-1)) / dy
        uxl(ix,iy) = uhxl(ix,iy)  + transv + 0.5_num * dt * get_force_cc(ix,iy,1)

        ! right
        transv = -0.5_num * dt * 0.5_num *(vha(ix+1,iy-1)+vha(ix+1,iy))&
          & * (uhy(ix+1,iy)-uhy(ix+1,iy-1 )) /dy
        uxr(ix,iy) = uhxr(ix,iy)  + transv + 0.5_num * dt * get_force_cc(ix+1,iy,1)

        ! also calc the tangential vel states for step 3

        ! left
        transv = -0.5_num * dt * 0.5_num * (vha(ix,iy-1) + vha(ix,iy)) &
          & * (vhy(ix,iy)-vhy(ix,iy-1)) / dy
        vxl(ix,iy) = vhxl(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix,iy,2)

        ! right
        transv = -0.5_num * dt * 0.5_num *(vha(ix+1,iy-1)+vha(ix+1,iy))&
          & * (vhy(ix+1,iy)-vhy(ix+1,iy-1 )) /dy
        vxr(ix,iy) = vhxr(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix+1,iy,2)
      endif
      if (ix /= 0) then !can do the yface stuff
        ! normal components
        transv = -0.5_num * dt * 0.5_num * (uha(ix-1,iy) + uha(ix,iy)) &
          & * (vhx(ix,iy) - vhx(ix-1,iy)) / dx
        vyl(ix,iy) = vhyl(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix,iy,2)

        transv = -0.5_num * dt * 0.5_num *(uha(ix-1,iy+1)+uha(ix,iy+1))&
          & * (vhx(ix,iy+1) - vhx(ix-1,iy+1)) / dx
        vyr(ix,iy) = vhyr(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix,iy+1,2)

        ! also calc the tangential vel states for step 3
        transv = -0.5_num * dt * 0.5_num * (uha(ix-1,iy) + uha(ix,iy)) &
          & * (uhx(ix,iy) - uhx(ix-1,iy)) / dx
        uyl(ix,iy) = uhyl(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix,iy,1)

        transv = -0.5_num * dt * 0.5_num *(uha(ix-1,iy+1)+uha(ix,iy+1))&
          & * (uhx(ix,iy+1) - uhx(ix-1,iy+1)) / dx
        uyr(ix,iy) = uhyr(ix,iy) + transv + 0.5_num * dt * get_force_cc(ix,iy+1,1)

     endif
    enddo
    enddo

    ! 1E Final riemann solve + upwinding for full normal velocities
    ! (sometimes AKA the MAC velocities)

    ! if you find bugs here check 3E also

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        ua(ix,iy) = riemann(uxl(ix,iy),uxr(ix,iy))
      endif
      if (ix /= 0) then !can do the yface stuff
        va(ix,iy) = riemann(vyl(ix,iy), vyr(ix,iy))
      endif
    enddo
    enddo

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        macu(ix,iy) = upwind(ua(ix,iy),uxl(ix,iy),uxr(ix,iy))
      endif
      if (ix /= 0) then !can do the yface stuff
        macv(ix,iy) = upwind(va(ix,iy),vyl(ix,iy),vyr(ix,iy))
      endif
    enddo
    enddo

    print *, 'Step #1 completed normally'

  end subroutine step_1

  subroutine step_2

    real(num) :: correction

    print *, 'Step #2'
    print *, '*** start'

    ! calc divU at cc using the MAC velocities
    do iy = 1, ny
    do ix = 1, nx
      divu(ix,iy) = (macu(ix,iy) - macu(ix-1,iy) ) /dx &
        & + (macv(ix,iy) - macv(ix,iy-1))/dy
    enddo
    enddo

!call plot_divergence_now ! debug
!if (step /=0) call plot_divergence_now ! debug

    call rho_bcs(arr_cc = rho) !needed for correction

!    call solve_variable_elliptic(phigs = phi, f = divu(1:nx,1:ny), &
!      & eta= 1.0_num / rho(0:nx+1,0:ny+1), &
!      & use_old_phi = .false., tol = 1e-18_num)

      input = mg_input(tol = 1e-12_num, nx = nx, ny = ny, dx = dx, dy = dy, &
            & f = divu(1:nx,1:ny), phi=phi, &
            & bc_xmin = mg_bc_xmin, bc_xmax = mg_bc_xmax, &
            & bc_ymin = mg_bc_ymin, bc_ymax = mg_bc_ymax, &
            & eta = 1.0_num / rho(1:nx,1:ny), eta_present = .true., &
            & eta_bc_xmin = mg_etabc_xmin, eta_bc_xmax = mg_etabc_xmax, &
            & eta_bc_ymin = mg_etabc_ymin, eta_bc_ymax = mg_etabc_ymax, &
            & etaval_bc_xmin = mg_etaval_bc_xmin, etaval_bc_ymin = mg_etaval_bc_ymin, &
            & etaval_bc_xmax = mg_etaval_bc_xmax, etaval_bc_ymax = mg_etaval_bc_ymax )

      call mg_interface(input)
      phi = input%phi

    print *, '*** max divu before cleaning',maxval(abs(divu))

    do ix = 0, nx
    do iy = 0, ny
      if (iy /= 0) then !can do the xface stuff
        correction = (phi(ix+1,iy) - phi(ix,iy))/dx
        correction = correction / &
            (0.5_num * (rho(ix,iy) + rho(ix+1,iy)))
        macu(ix,iy) = macu(ix,iy) - correction
      endif
      if (ix /= 0) then !can do the yface stuff
        correction = (phi(ix,iy+1)-phi(ix,iy))/dy
        correction = correction / &
            (0.5_num * (rho(ix,iy) + rho(ix,iy+1)))
        macv(ix,iy) = macv(ix,iy) - correction
      endif
    enddo
    enddo

    ! calculate the new divergence
    ! calc divU at cc using the MAC velocities

    do iy = 1, ny
    do ix = 1, nx
      divu(ix,iy) = (macu(ix,iy) - macu(ix-1,iy) ) /dx &
        & + (macv(ix,iy) - macv(ix,iy-1))/dy
    enddo
    enddo

    print *, '*** max divu after cleaning',maxval(abs(divu))
    print *, '*** complete'

!if (step /=0) call plot_divergence_now ! debug
!call plot_divergence_now ! debug
  end subroutine step_2

  subroutine step_3

    ! reconstruct the interface states using the MAC velocities
    ! for consistency
    ! (redo some of step 1 but use mac velocities  for upwinding)

    ! Because you haven't been overwriting or deallocating
    ! such arrays, most of it doesnt have to be recalculated

    ! We only need to re-do E

    ! Step 3E Upwind face components based upon MAC vels

    do iy = 0, ny
    do ix = 0, nx
      if (iy /= 0) then !can do the xface stuff
        ux(ix,iy) = upwind(macu(ix,iy),uxl(ix,iy),uxr(ix,iy))
        vx(ix,iy) = upwind(macu(ix,iy),vxl(ix,iy),vxr(ix,iy))
      endif
      if (ix /= 0) then !can do the yface stuff
        uy(ix,iy) = upwind(macv(ix,iy),uyl(ix,iy),uyr(ix,iy))
        vy(ix,iy) = upwind(macv(ix,iy),vyl(ix,iy),vyr(ix,iy))
      endif
    enddo
    enddo

    print *, 'Step #3 completed normally'
  end subroutine step_3

  subroutine step_4

    real(num) :: Au, Av ! evaluation of advection term

    do iy = 1, ny
    do ix = 1, nx
      Au = get_Au(ix,iy)
      Av = get_Av(ix,iy)
      ustar(ix,iy) = u(ix,iy) - dt * Au + dt * get_force_cc(ix,iy,1)
      vstar(ix,iy) = v(ix,iy) - dt * Av + dt * get_force_cc(ix,iy,2)
    enddo
    enddo

    print *, 'Step #4 completed normally'
  end subroutine step_4

  subroutine step_5

    real(num) :: correction, gpsi

    print *,'Step #5'
    print *, '*** start'

    ! calc divU at cc using the star velocities which themselves are cc
    ! (this differs to step two which uses face vars to get a CC var)

    call velocity_bcs(arr_cc = ustar, di = 1)
    call velocity_bcs(arr_cc = vstar, di = 2)

    do iy = 1, ny
    do ix = 1, nx
      divu(ix,iy) = (ustar(ix+1,iy) - ustar(ix-1,iy))/dx/2.0_num &
        & + (vstar(ix,iy+1) - vstar(ix,iy-1))/dy/2.0_num
    enddo
    enddo

!if (step /=0) call plot_divergence_now
!call plot_divergence_now

    divu = divu/dt

!    call solve_variable_elliptic(phigs = phi, f = divu(1:nx,1:ny), &
!      & eta= 1.0_num / rho(0:nx+1,0:ny+1), &
!      & use_old_phi = .true., tol = 1e-18_num)

      input = mg_input(tol = 1e-12_num, nx = nx, ny = ny, dx = dx, dy = dy, &
            & f = divu(1:nx,1:ny), phi=phi, &
            & bc_xmin = mg_bc_xmin, bc_xmax = mg_bc_xmax, &
            & bc_ymin = mg_bc_ymin, bc_ymax = mg_bc_ymax, &
            & eta = 1.0_num / rho(1:nx,1:ny), eta_present = .true., &
            & eta_bc_xmin = mg_etabc_xmin, eta_bc_xmax = mg_etabc_xmax, &
            & eta_bc_ymin = mg_etabc_ymin, eta_bc_ymax = mg_etabc_ymax, &
            & etaval_bc_xmin = mg_etaval_bc_xmin, etaval_bc_ymin = mg_etaval_bc_ymin, &
            & etaval_bc_xmax = mg_etaval_bc_xmax, etaval_bc_ymax = mg_etaval_bc_ymax )

      call mg_interface(input)
      phi = input%phi

    print *, '*** max divu before cleaning',maxval(abs(divu)*dt)

    call phi_bcs

    do iy = 1, ny
    do ix = 1, nx

      gpsi = (phi(ix+1,iy) - phi(ix-1,iy))/dx/2.0_num
      correction = dt * gpsi
      correction = correction/rho(ix,iy) !cc here
      u(ix,iy) = ustar(ix,iy) - correction

      gpsi = (phi(ix,iy+1)-phi(ix,iy-1))/dy/2.0_num
      correction = dt * gpsi
      correction = correction/rho(ix,iy)
      v(ix,iy) = vstar(ix,iy) - correction

    enddo
    enddo

    ! calculate the divergence of the updated velocity field

    call velocity_bcs(arr_cc = u, di = 1)
    call velocity_bcs(arr_cc = v, di = 2)

    do iy = 1, ny
    do ix = 1, nx
      divu(ix,iy) = (u(ix+1,iy) - u(ix-1,iy))/dx/2.0_num &
        & + (v(ix,iy+1) - v(ix,iy-1))/dy/2.0_num
    enddo
    enddo

    print *, '*** max divu after cleaning',maxval(abs(divu))

!if (step /=0) call plot_divergence_now ! debug
!call plot_divergence_now ! debug
!call plot_vel_now ! debug

    ! update the pressure gradient

    do ix = 0, nx+1
    do iy = 0, ny+1

      gpsi = (phi(ix+1,iy) - phi(ix-1,iy))/dx/2.0_num
      gradp_x(ix,iy) = gradp_x(ix,iy) + gpsi

      gpsi = (phi(ix,iy+1)-phi(ix,iy-1))/dy/2.0_num
      gradp_y(ix,iy) = gradp_y(ix,iy) + gpsi

    enddo
    enddo

!call plot_gradp_now ! debug

  end subroutine step_5

  subroutine advect_dens

    real(num) :: drho

    call rho_bcs(arr_cc = rho)

    ! calculate rhohat states on faces

    do ix = 0, nx
    do iy = 0, ny
      if (iy /= 0) then ! xface vars

        if (use_minmod) then
          drho = minmod((rho(ix,iy)-rho(ix-1,iy))/dx,(rho(ix+1,iy)-rho(ix,iy))/dx)
        else
          drho = ( rho(ix+1,iy) - rho(ix-1,iy) ) /2.0_num / dx
        endif

        rhohxl(ix,iy) = rho(ix,iy) + &
          & 0.5_num * (1.0_num - dt * macu(ix,iy) / dx ) * drho * dx

        if (use_minmod) then
          drho = minmod((rho(ix+1,iy)-rho(ix,iy))/dx,(rho(ix+2,iy)-rho(ix+1,iy))/dx)
        else
          drho= ( rho(ix+2,iy)-rho(ix,iy) ) / 2.0_num / dx
        endif

        rhohxr(ix,iy) = rho(ix+1,iy) - &
          & 0.5_num * (1.0_num + dt * macu(ix,iy) / dx) * drho * dx

      endif

      if (ix /= 0) then !y face vars

        if (use_minmod) then
          drho = minmod( (rho(ix,iy)-rho(ix,iy-1))/dy, (rho(ix,iy+1)-rho(ix,iy))/dy)
        else
          drho = (rho(ix,iy+1) - rho(ix,iy-1)) / 2.0_num / dy
        endif

        rhohyl(ix,iy) = rho(ix,iy) + &
          & 0.5_num * (1.0_num - dt * macv(ix,iy) / dy ) * drho * dy

        if (use_minmod) then
          drho = minmod( (rho(ix,iy+1)-rho(ix,iy))/dy, (rho(ix,iy+2)-rho(ix,iy+1))/dy)
        else
          drho = (rho(ix,iy+2)-rho(ix,iy)) / 2.0_num/dy
        endif

        rhohyr(ix,iy) = rho(ix,iy+1) - &
          & 0.5_num * (1.0_num + dt * macv(ix,iy) / dy) * drho * dy

      endif
    enddo
    enddo

    ! upwind using the MAC velocities

    do ix = 0, nx
    do iy = 0, ny
      if (iy /= 0) then ! xface vars
        rhohx(ix,iy) = upwind(macu(ix,iy),rhohxl(ix,iy),rhohxr(ix,iy))
      endif
      if (ix /= 0) then !y face vars
        rhohy(ix,iy) = upwind(macv(ix,iy),rhohyl(ix,iy),rhohyr(ix,iy))
      endif
    enddo
    enddo

    ! calculate full states with transverse terms

    call rho_bcs(arr_xface = rhohx, arr_yface = rhohy)
    call velocity_bcs(arr_xface = macu, di = 1)
    call velocity_bcs(arr_yface = macv, di = 2)

    do ix = 0, nx
    do iy = 0, ny
      if (iy /= 0) then ! xface vars
        rhoxl(ix,iy) = rhohxl(ix,iy) &
          & - 0.5_num * dt * rho(ix,iy) * (macu(ix,iy) - macu(ix-1,iy)) / dx &
          & - 0.5_num * dt / dy * ( rhohy(ix,iy)*macv(ix,iy) - &
                                      & rhohy(ix,iy-1)*macv(ix,iy-1) )
        rhoxr(ix,iy) = rhohxr(ix,iy) &
          & - 0.5_num * dt * rho(ix+1,iy) * (macu(ix+1,iy) - macu(ix,iy)) / dx &
          & - 0.5_num * dt / dy * ( rhohy(ix+1,iy)*macv(ix+1,iy) - &
                                    & rhohy(ix+1,iy-1)*macv(ix+1,iy-1) )
      endif
      if (ix /= 0) then ! yface vars
         rhoyl(ix,iy) = rhohyl(ix,iy) &
          & - 0.5_num * dt * rho(ix,iy) * (macv(ix,iy) - macv(ix,iy-1)) / dy &
          & - 0.5_num * dt / dx * ( rhohx(ix,iy)*macu(ix,iy) - &
                                      & rhohx(ix-1,iy)*macu(ix-1,iy) )
        rhoyr(ix,iy) = rhohyr(ix,iy) &
          & - 0.5_num * dt * rho(ix,iy+1) * (macv(ix,iy+1) - macv(ix,iy)) / dy &
          & - 0.5_num * dt / dx * ( rhohx(ix,iy+1)*macu(ix,iy+1) - &
                                    & rhohx(ix-1,iy+1)*macu(ix-1,iy+1) )
      endif
    enddo
    enddo

    ! resolve states via upwind

    do ix = 0, nx
    do iy = 0, ny
      if (iy /= 0) then ! xface vars
        rhox(ix,iy) = upwind(macu(ix,iy),rhoxl(ix,iy),rhoxr(ix,iy))
      endif
      if (ix /= 0) then !y face vars
        rhoy(ix,iy) = upwind(macv(ix,iy),rhoyl(ix,iy),rhoyr(ix,iy))
      endif
    enddo
    enddo

    ! simple conservative update to new time level

    do ix = 1, nx
    do iy = 1, ny
      rho(ix,iy) = rho(ix,iy) &
        & - dt / dx * (rhox(ix,iy)*macu(ix,iy) - rhox(ix-1,iy)*macu(ix-1,iy)) &
        & - dt / dy * (rhoy(ix,iy)*macv(ix,iy) - rhoy(ix,iy-1)*macv(ix,iy-1))
    enddo
    enddo

  end subroutine advect_dens

  subroutine set_dt

    real(num) :: dtx, dty
    real(num) :: dtf

    ! need to call bcs to capture velocities on driven boundaries
    call velocity_bcs(arr_cc = u, di = 1)
    call velocity_bcs(arr_cc = v, di = 2)

    dtx = 1e6_num
    dty = dtx

    if (maxval(abs(u)) > 1e-16_num) dtx = CFL * dx / maxval(abs(u))
    if (maxval(abs(v)) > 1e-16_num) dty = CFL * dy / maxval(abs(v))
    dt = MIN(dtx,dty)

    if (abs(grav_x) > 1e-16_num) then
      dtf = CFL * sqrt(2.0_num * dx / maxval(abs(gradp_x-grav_x*rho)))
      dt = MIN(dt,dtf)
    endif

    if (abs(grav_y) > 1e-16_num) then
      dtf = CFL * sqrt(2.0_num * dy / maxval(abs(gradp_y-grav_y*rho)))
      dt = MIN(dt,dtf)
    endif

!    if (sqrt(grav_x**2 + grav_y**2) > 1e-16_num) then
!      if (use_vardens) then
!        dtf = CFL * sqrt(2.0_num * dx / maxval(abs(gradp_x-grav_x*rho)))
!        dt = MIN(dt,dtf)
!        dtf = CFL * sqrt(2.0_num * dy / maxval(abs(gradp_y-grav_y*rho)))
!        dt = MIN(dt,dtf)
!      else
!        dtf = CFL * sqrt(2.0_num * dx / maxval(abs(gradp_x-grav_x)))
!        dt = MIN(dt,dtf)
!        dtf = CFL * sqrt(2.0_num * dy / maxval(abs(gradp_y-grav_y)))
!        dt = MIN(dt,dtf)
!      endif
!    endif

    print *, 'hydro dt = ',dt

  end subroutine set_dt

  real(num) function minmod(a,b)
    real(num), intent(in) :: a, b
    if ( (abs(a) < abs(b)) .and. (a*b > 0.0_num) ) then
      minmod = a
    else if ( (abs(a) > abs(b)) .and. (a*b > 0.0_num) ) then
      minmod = b
    else
      minmod = 0.0_num
    endif
  end function minmod

  real(num) function riemann(a,b)
    real(num), intent(in) :: a, b
    if ( (a > 0.0_num) .and. ( (a+b)>0.0_num) ) then
      riemann = a
    else if ( (a <= 0.0_num) .and. (b >= 0.0_num) ) then
      riemann = 0.0_num
    else
      riemann = b
    endif
  end function riemann

  real(num) function upwind(sadv, a, b)
    real(num), intent(in) :: sadv,a,b
    if (sadv > 0.0_num) then
      upwind = a
    else if (sadv < 0.0_num) then
      upwind = b
    else
      upwind = 0.5_num * (a + b)
    endif
  end function upwind

  real(num) function get_force_cc(ix,iy,di)
    integer,intent(in) :: ix,iy, di
    real(num) :: grav_tmp_x
    real(num) :: grav_tmp_y

    grav_tmp_x = grav_x
    grav_tmp_y = grav_y

! debug: uncomment to turn grav off at the closest cc's to the edges
!    grav_tmp_x = 0.0_num
!    grav_tmp_y = 0.0_num
!    if ( (ix > 1) .and. (ix < nx) ) grav_tmp_x = grav_x
!    if ( (iy > 1) .and. (iy < ny) ) grav_tmp_y = grav_y

    if (di==1) then
      get_force_cc = -gradp_x(ix,iy)/rho(ix,iy) + grav_tmp_x
    else if (di==2) then
      get_force_cc = -gradp_y(ix,iy)/rho(ix,iy) + grav_tmp_y
    else
      print *,'error get_force_cc given invalid dimension'
      print *,'di = 1(x) or =2(y)'
      print *,'STOP'
      STOP
    endif

  end function get_force_cc

  real(num) function get_Au(ix,iy)

    integer, intent(in) :: ix,iy
    get_Au = 0.5_num * (macu(ix-1,iy)+macu(ix,iy)) * (ux(ix,iy)-ux(ix-1,iy))/dx &
       &+ 0.5_num * (macv(ix,iy-1)+macv(ix,iy)) * (uy(ix,iy)-uy(ix,iy-1))/dy

  endfunction get_Au

  real(num) function get_Av(ix,iy)

    integer, intent(in) :: ix,iy
    get_Av = 0.5_num * (macu(ix-1,iy)+macu(ix,iy)) * (vx(ix,iy)-vx(ix-1,iy))/dx &
     &+ 0.5_num * (macv(ix,iy-1)+macv(ix,iy)) * (vy(ix,iy)-vy(ix,iy-1))/dy

  endfunction get_Av

end module advance_module

! Old subroutines

!!!! Currently unused - was used in the viscous code. Keep here, might prove useful.
!!!! (but if you use it, double check is still standard 5 point
!!!!
!!!!  real(num) function get_LU_cc(ix,iy,di) ! calculate vector laplacian at a coordinate
!!!!    integer, intent(in) :: ix, iy, di
!!!!
!!!!    if (di == 1) then
!!!!
!!!!      get_LU_cc = (u(ix+1,iy) - 2.0_num*u(ix,iy) + u(ix-1,iy)) / dx**2 + &
!!!!         (u(ix,iy+1) - 2.0_num*u(ix,iy) + u(ix,iy-1)) / dy**2
!!!!
!!!!    else if (di == 2) then
!!!!
!!!!      get_LU_cc = (v(ix+1,iy) - 2.0_num*v(ix,iy) + v(ix-1,iy)) / dx**2 + &
!!!!         (v(ix,iy+1) - 2.0_num*v(ix,iy) + v(ix,iy-1)) / dy**2
!!!!    else
!!!!
!!!!      print *,'error: get_LU_cc (calc vector laplacian) not given valid dimension'
!!!!      print *,'di = 1 (x) or = 2 (y)'
!!!!      print *,'Terminating early'
!!!!      STOP
!!!!
!!!!    endif
!!!!
!!!!  endfunction get_LU_cc


      subroutine tvcorr(cv,x0,al,n)
      use macmath
      implicit none
      integer*4 n,i
      real*8 x,w,tgauss,s,x1,al,x0
      complex*16 cv(n),cd
      if(x0 .eq. 0.d0)then
        cv(1)=(0.d0,0.d0)
        w=sqrt(al/n)
        do 1010 i=2,n
          cv(i)=cv(i-1)+dcmplx(w*tgauss(),w*tgauss())
1010    continue
        cd=cv(n)/n
        do 1020 i=2,n
          cv(i)=cv(i)-cd*(i-1)
1020    continue
        return
      endif
      x=x0/al
      s=0.d0
      x1=.25d0*(x*pi2)**2
      do 10 i=2,n/2
        w=exp(-(i-1)**2*x1)
        cv(i)=dcmplx(w*tgauss(),w*tgauss())
        cv(n-i+1)=dcmplx(w*tgauss(),w*tgauss())
        s=s+w**2
10    continue
      cv(1)=dcmplx(tgauss(),tgauss())
      s=1.d0/sqrt(4.d0*s+2.d0)
      call tcftr(cv,n,.false.)
      do 20 i=1,n
        cv(i)=cv(i)*s
20    continue
      return
      end


      subroutine normal_vector(u, v)
      use stel_kinds
      USE torlines_background
      USE torlines_runtime, ONLY: lvessel, vsurf
      USE wall_mod
      implicit none
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      real(rprec) :: v, u
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      real(rprec), parameter :: zero = 0, one = 1
      integer :: mn, m, n, ik
      real(rprec) :: rbn, zbn, rub, rvb, zub, zvb,
     1    cosnv, sinnv, cosmn1, sinmn1, cosmu, sinmu, snx, sny
      real(rprec) :: x2, y2, v2, norm, twopi, dist
      REAL(rprec) :: surf_norm(3)
C-----------------------------------------------

      twopi = 8*atan(one)

      rbn = zero;  rub = zero;   rvb = zero
      zbn = zero;  zub = zero;   zvb = zero

      do mn = 1, mnmax_m
         m = ixm_m(mn)
         n = ixn_m(mn)                          !!nfp factored out already, nescoil convention
         cosmu = cos(m*u)
         sinmu = sin(m*u)
         cosnv = cos(n*v)
         sinnv = sin(n*v)
         cosmn1 = cosmu*cosnv - sinmu*sinnv   !!cos(mu+nv), nescoil convention
         sinmn1 = sinmu*cosnv + cosmu*sinnv
         rbn = rbn + rmnc_m(mn) * cosmn1
     1             + rmns_m(mn) * sinmn1
         rub = rub - ixm_m(mn) * rmnc_m(mn) * sinmn1
     1             + ixm_m(mn) * rmns_m(mn) * cosmn1
         rvb = rvb - nfp*ixn_m(mn) * rmnc_m(mn) * sinmn1
     1             + nfp*ixn_m(mn) * rmns_m(mn) * cosmn1
         zbn = zbn + zmns_m(mn) * sinmn1
     1             + zmnc_m(mn) * cosmn1
         zub = zub + ixm_m(mn) * zmns_m(mn) * cosmn1
     1             - ixm_m(mn) * zmnc_m(mn) * sinmn1
         zvb = zvb + nfp*ixn_m(mn) * zmns_m(mn) * cosmn1
     1             - nfp*ixn_m(mn) * zmnc_m(mn) * sinmn1
      end do

!
!     un-normalized r,phi,z components of outward normal vector
!
      surf_norm(1) =  rbn * zub
      surf_norm(2) =  (rub * zvb - rvb * zub)
      surf_norm(3) = -rbn * rub

      surf_norm = isgn * surf_norm

      norm = sqrt(sum(surf_norm*surf_norm))

      surf_norm(:) = surf_norm(:) / norm

      cosnv = cos(v/nfp)
      sinnv = sin(v/nfp)

      snx = surf_norm(1)*cosnv - surf_norm(2)*sinnv
      sny = surf_norm(1)*sinnv + surf_norm(2)*cosnv

      x2    = rbn*cosnv + dr_m*snx
      y2    = rbn*sinnv + dr_m*sny
      zb_ws = zbn+dr_m*surf_norm(3)

      v2    = atan2(y2,x2)
      !if(v2 .lt. zero .and. x2 .lt. zero) v2 = v2 + twopi   !Cut at v = +- pi
      v2 = v2*nfp
      vb_ws = v2

      rb_ws = sqrt (x2*x2 + y2*y2)
      RETURN

      end subroutine normal_vector


!-----------------------------------------------------------------------------------
! Name: rpn3(ixyz,maxm,meqn,mwaves,maux,mbc,mx,ql,qr,auxl,auxr,wave,s,amdq,apdq)
!
! Description: Roe-solver for the 3D Euler Equations
!
! waves: 3
! equations: 5
!
! Conserved quantities:
!       1 density
!       2 x_momentum
!       3 y_momentum
!       4 z_momentum
!       5 energy
!
! Inputs:
!         ixyz <INTEGER>   : input values are a slice along: x-direction if ixyz=1;
!                                                            y-direction if ixyz=2;
!                                                            z-direction if ixyz=3.
!         maxm <INTEGER>   : max number of grid cells (excluding ghost cells)
!         meqn <INTEGER>   : number of equations in the system (=5)
!         mwaves <INTEGER> : number of waves in approximate Riemann solution (=3)
!                            This solver ignores the zero-velocity shear waves
!                            since they don't affect the cell updates
!         maux <INTEGER>   : number of auxilary variables (not used)
!         mbc <INTEGER>    : number of ghost cells at each boundary
!         mx <INTEGER>     : number of elements in slice
!         ql <REAL>        : state vector at left edge of each cell
!                            Note that the i'th Riemann problem has left state qr(:,i-1)
!         qr <REAL>        : state vector at right edge of each cell
!                            Note that the i'th Riemann problem has right state ql(:,i)
!         auxl <REAL>      : state of auxiliary variable on left edge of cell
!         auxr <REAL>      : state of auxiliary variable on right egde of cell
!
! Outputs:
!          wave <REAL>     : q-wave vectors (eigenvectors of Roe matrix)
!          s <REAL>        : wave speeds (eigenvalues of Roe matrix)
!          amdq <REAL>     : left-going fluctuations
!          apdq <REAL>     : right-going fluctuations
!
! Adapted from rpn3_euler.f90 in $CLAWHOME/riemann/src
!-----------------------------------------------------------------------------------
SUBROUTINE rpn3(ixyz,maxm,meqn,mwaves,maux,mbc,mx,ql,qr,auxl,auxr,wave,s,amdq,apdq)

    IMPLICIT NONE

    ! Input
    INTEGER, INTENT(IN) :: ixyz,maxm,meqn,mwaves,maux,mbc,mx
    REAL(kind=8), DIMENSION(meqn,1-mbc:maxm+mbc), INTENT(IN) :: ql,qr
    REAL(kind=8), DIMENSION(maux,1-mbc:maxm+mbc), INTENT(IN) :: auxl,auxr

    ! Output
    REAL(kind=8), DIMENSION(meqn,mwaves,1-mbc:maxm+mbc), INTENT(INOUT) :: wave
    REAL(kind=8), DIMENSION(mwaves,1-mbc:maxm+mbc), INTENT(INOUT) :: s
    REAL(kind=8), DIMENSION(meqn,1-mbc:maxm+mbc), INTENT(INOUT) :: amdq,apdq

    ! Local Storage
    INTEGER, PARAMETER :: maxmrp = 1002
    REAL(kind=8), DIMENSION(5) :: delta,alpha
    REAL(kind=8), DIMENSION(-1:maxmrp) :: u2v2w2,u,v,w,enth,a,g1a2,euv
    REAL(kind=8) :: asqrd,c1,c2,ci,cim1,en1,en2,p1,p2,pi,pim1,pl,pr,&
                    rho1,rho2,rhoi,rhoim1,rhou1,rhou2,rhov1,rhov2,rhow1,rhow2,&
                    rhsq2,rhsqrtl,rhsqrtr,s0,s1,s2,s3,sfract,df
    INTEGER :: i,j,m,mu,mv,mw,mws

    ! Use entropy fix for transonic rarefactions
    LOGICAL, PARAMETER :: efix = .TRUE.

    REAL(kind=8) :: gamma,gamma1
    COMMON /cparam/ gamma         ! ideal gas constant

    gamma1 = gamma - 1.d0

    IF (maxmrp < maxm+2*mbc)THEN
        WRITE(6,*) 'need to increase maxmrp in rpn3_euler.f90'
        WRITE(6,*) 'maxmrp: ',maxmrp,' maxm: ',maxm,' mbc: ',mbc
        WRITE(6,*) 'maxm+mbc=',maxm+mbc
        STOP
    ENDIF

    IF (mwaves /= 3) THEN
        WRITE(6,*) '*** Must set mwaves=3 for this Riemann solver'
        STOP
    ENDIF

    ! Set mu to point to  the component of the system that corresponds to momentum in
    !  the direction of this slice, mv and mw to the orthogonal momentum components:
    IF(ixyz == 1)THEN
        mu = 2
        mv = 3
        mw = 4
    ELSE IF(ixyz == 2)THEN
        mu = 3
        mv = 4
        mw = 2
    ELSE
        mu = 4
        mv = 2
        mw = 3
    ENDIF

    ! Note that notation for u,v, and w reflects assumption that the
    !   Riemann problems are in the x-direction with u in the normal
    !   direction and v and w in the orthogonal directions, but with the
    !   above definitions of mu, mv, and mw the routine also works with
    !   ixyz = 2 and ixyz = 3
    !   and returns, for example, f0 as the Godunov flux g0 for the
    !   Riemann problems u_t + g(u)_y = 0 in the y-direction.

    ! Initialize waves and speeds
    wave = 0.d0
    s = 0.d0

    ! Compute the Roe-averaged variables needed in the Roe solver.

    ! Loop over grid cell interfaces
    DO i = 2-mbc, mx+mbc
        IF (qr(1,i-1) <= 0.d0 .OR. ql(1,i) <= 0.d0) THEN
            WRITE(*,*) i, mu, mv, mw
            WRITE(*,'(5e12.4)') (qr(j,i-1),j=1,5)
            WRITE(*,'(5e12.4)') (ql(j,i),j=1,5)
            IF (ixyz == 1) &
                WRITE(6,*) '*** rho <= 0 in x-sweep at ',i
            IF (ixyz == 2) &
                WRITE(6,*) '*** rho <= 0 in y-sweep at ',i
            IF (ixyz == 3) &
                WRITE(6,*) '*** rho <= 0 in z-sweep at ',i
            WRITE(6,*) 'stopped with rho <= 0...'
            STOP
        ENDIF
        rhsqrtl = SQRT(qr(1,i-1))
        rhsqrtr = SQRT(ql(1,i))
        pl = gamma1*(qr(5,i-1) - 0.5d0*(qr(mu,i-1)**2 &
             + qr(mv,i-1)**2 + qr(mw,i-1)**2)/qr(1,i-1))
        pr = gamma1*(ql(5,i) - 0.5d0*(ql(mu,i)**2 &
             + ql(mv,i)**2 + ql(mw,i)**2)/ql(1,i))
        rhsq2 = rhsqrtl + rhsqrtr
        u(i) = (qr(mu,i-1)/rhsqrtl + ql(mu,i)/rhsqrtr) / rhsq2
        v(i) = (qr(mv,i-1)/rhsqrtl + ql(mv,i)/rhsqrtr) / rhsq2
        w(i) = (qr(mw,i-1)/rhsqrtl + ql(mw,i)/rhsqrtr) / rhsq2
        enth(i) = (((qr(5,i-1)+pl)/rhsqrtl &
                  + (ql(5,i)+pr)/rhsqrtr)) / rhsq2
        u2v2w2(i) = u(i)**2 + v(i)**2 + w(i)**2
        asqrd = gamma1*(enth(i) - .5d0*u2v2w2(i))

        IF (i>=0 .AND. i<=mx .AND. asqrd <= 0.d0) THEN
            IF (ixyz == 1) &
                WRITE(6,*) '*** a**2 <= 0 in x-sweep at ',i
            IF (ixyz == 2) &
                WRITE(6,*) '*** a**2 <= 0 in y-sweep at ',i
            IF (ixyz == 3) &
                WRITE(6,*) '*** a**2 <= 0 in z-sweep at ',i
            WRITE(6,*) 'stopped with a**2 < 0...'
            STOP
        ENDIF
        a(i) = SQRT(asqrd)
        g1a2(i) = gamma1 / asqrd
        euv(i) = enth(i) - u2v2w2(i)
    END DO

    ! Now split the jump in q1d at each interface into waves

    ! Find alpha(1) thru alpha(5), the coefficients of the 5 eigenvectors:
    DO i = 2-mbc, mx+mbc
        delta(1) = ql(1,i) - qr(1,i-1)
        delta(2) = ql(mu,i) - qr(mu,i-1)
        delta(3) = ql(mv,i) - qr(mv,i-1)
        delta(4) = ql(mw,i) - qr(mw,i-1)
        delta(5) = ql(5,i) - qr(5,i-1)
        alpha(4) = g1a2(i) * (euv(i)*delta(1) &
                  + u(i)*delta(2) + v(i)*delta(3) + w(i)*delta(4) - delta(5))
        alpha(2) = delta(3) - v(i)*delta(1)
        alpha(3) = delta(4) - w(i)*delta(1)
        alpha(5) = (delta(2) + (a(i)-u(i))*delta(1) - a(i)*alpha(4)) / (2.d0*a(i))
        alpha(1) = delta(1) - alpha(4) - alpha(5)

        ! Compute the waves.
        !   Note that the 2-wave, 3-wave and 4-wave travel at the same speed
        !   and are lumped together in wave(.,2,.).  The 5-wave is then stored
        !   in wave(.,3,.).
        wave(1,1,i)  = alpha(1)
        wave(mu,1,i) = alpha(1)*(u(i)-a(i))
        wave(mv,1,i) = alpha(1)*v(i)
        wave(mw,1,i) = alpha(1)*w(i)
        wave(5,1,i)  = alpha(1)*(enth(i) - u(i)*a(i))
        s(1,i) = u(i)-a(i)

        wave(1,2,i)  = alpha(4)
        wave(mu,2,i) = alpha(4)*u(i)
        wave(mv,2,i) = alpha(4)*v(i) + alpha(2)
        wave(mw,2,i) = alpha(4)*w(i) + alpha(3)
        wave(5,2,i)  = alpha(4)*0.5d0*u2v2w2(i)  + alpha(2)*v(i) + alpha(3)*w(i)
        s(2,i) = u(i)

        wave(1,3,i)  = alpha(5)
        wave(mu,3,i) = alpha(5)*(u(i)+a(i))
        wave(mv,3,i) = alpha(5)*v(i)
        wave(mw,3,i) = alpha(5)*w(i)
        wave(5,3,i)  = alpha(5)*(enth(i)+u(i)*a(i))
        s(3,i) = u(i)+a(i)
    END DO

    amdq = 0.d0
    apdq = 0.d0

    IF (.NOT.efix) THEN
        !  No entropy fix

        ! compute fluctuations amdq and apdq
        !  amdq = SUM s*wave   over left-going waves
        !  apdq = SUM s*wave   over right-going waves

        DO i=2-mbc, mx+mbc
            DO mws=1,mwaves
                IF (s(mws,i) < 0.d0) THEN
                    amdq(:,i) = amdq(:,i) + s(mws,i)*wave(:,mws,i)
                ELSE
                    apdq(:,i) = apdq(:,i) + s(mws,i)*wave(:,mws,i)
                ENDIF
            END DO
        END DO

    ELSE
    ! With entropy fix

    ! Compute flux differences amdq and apdq.
    !   First compute amdq as sum of s*wave for left going waves.
    !   Incorporate entropy fix by adding a modified fraction of
    !   transonic wave if s should change sign.

    DO i = 2-mbc, mx+mbc

        ! Check if 1-wave is transonic
        rhoim1 = qr(1,i-1)
        pim1 = gamma1*(qr(5,i-1) - 0.5d0*(qr(mu,i-1)**2 &
               + qr(mv,i-1)**2 + qr(mw,i-1)**2) / rhoim1)
        cim1 = SQRT(gamma*pim1/rhoim1)
        s0 = qr(mu,i-1)/rhoim1 - cim1  ! u-c in left state (cell i-1)

        ! Check for fully supersonic case:
        IF (s0 >= 0.d0 .AND. s(1,i) > 0.d0) CYCLE ! everything is right-going

        rho1 = qr(1,i-1) + wave(1,1,i)
        rhou1 = qr(mu,i-1) + wave(mu,1,i)
        rhov1 = qr(mv,i-1) + wave(mv,1,i)
        rhow1 = qr(mw,i-1) + wave(mw,1,i)
        en1 = qr(5,i-1) + wave(5,1,i)
        p1 = gamma1*(en1 - 0.5d0*(rhou1**2 + rhov1**2 + rhow1**2)/rho1)
        c1 = SQRT(gamma*p1/rho1)
        s1 = rhou1/rho1 - c1  ! u-c to right of 1-wave
        IF (s0 < 0.d0 .AND. s1 > 0.d0) THEN
            ! transonic rarefaction in the 1-wave
            sfract = s0 * (s1-s(1,i)) / (s1-s0)
        ELSE IF (s(1,i) < 0.d0) THEN
            ! 1-wave is leftgoing
            sfract = s(1,i)
        ELSE
            ! 1-wave is rightgoing
            sfract = 0.d0   ! this shouldn't happen since s0 < 0
        ENDIF
        amdq(:,i) = sfract*wave(:,1,i)

        ! Check if 2-wave is transonic
        IF (s(2,i) >= 0.d0) CYCLE ! 2- and 3-waves are right-going
        amdq(:,i) = amdq(:,i) + s(2,i)*wave(:,2,i)

        ! Check if 3-wave is transonic
        rhoi = ql(1,i)
        pi = gamma1*(ql(5,i) - 0.5d0*(ql(mu,i)**2+ql(mv,i)**2+ql(mw,i)**2)/rhoi)
        ci = SQRT(gamma*pi/rhoi)
        s3 = ql(mu,i)/rhoi + ci     ! u+c in right state  (cell i)

        rho2 = ql(1,i) - wave(1,3,i)
        rhou2 = ql(mu,i) - wave(mu,3,i)
        rhov2 = ql(mv,i) - wave(mv,3,i)
        rhow2 = ql(mw,i) - wave(mw,3,i)
        en2 = ql(5,i) - wave(5,3,i)
        p2 = gamma1*(en2 - 0.5d0*(rhou2**2 + rhov2**2 + rhow2**2)/rho2)
        c2 = SQRT(gamma*p2/rho2)
        s2 = rhou2/rho2 + c2   !# u+c to left of 3-wave
        IF (s2 < 0.d0 .AND. s3 > 0.d0 ) THEN
            ! transonic rarefaction in the 3-wave
            sfract = s2 * (s3-s(3,i)) / (s3-s2)
        ELSE IF (s(3,i) < 0.d0) THEN
            ! 3-wave is left-going
            sfract = s(3,i)
        ELSE
            ! 3-wave is right-going
           CYCLE
        ENDIF

        amdq(:,i) = amdq(:,i) + sfract*wave(:,3,i)
    END DO

    ! Compute the remaining right-going fluctuations:
    ! df = SUM s*wave   is the total flux difference and apdq = df - amdq
    DO m=1,meqn
        DO i = 2-mbc, mx+mbc
            df = 0.d0
            DO mws=1,mwaves
                df = df + s(mws,i)*wave(m,mws,i)
            END DO
            apdq(m,i) = df - amdq(m,i)
        END DO
    END DO

    END IF ! Entropy fix

END SUBROUTINE rpn3


!================================================================
!================================================================
!================================================================
!================================================================
subroutine condinit(x,u,dx,nn)
  use amr_parameters
  use hydro_parameters
  use poisson_parameters, ONLY: gravity_params
  implicit none
  integer ::nn                            ! Number of cells
  real(dp)::dx                            ! Cell size
  real(dp),dimension(1:nvector,1:nvar+3)::u ! Conservative variables
  real(dp),dimension(1:nvector,1:ndim)::x ! Cell center position.
  !================================================================
  ! This routine generates initial conditions for RAMSES.
  ! Positions are in user units:
  ! x(i,1:3) are in [0,boxlen]**ndim.
  ! U is the conservative variable vector. Conventions are here:
  ! U(i,1): d, U(i,2:4): d.u,d.v,d.w, U(i,5): E, U(i,6:8): Bleft,
  ! U(i,nvar+1:nvar+3): Bright
  ! Q is the primitive variable vector. Conventions are here:
  ! Q(i,1): d, Q(i,2:4):u,v,w, Q(i,5): P, Q(i,6:8): Bleft,
  ! Q(i,nvar+1:nvar+3): Bright
  ! If nvar > 8, remaining variables (9:nvar) are treated as passive
  ! scalars in the hydro solver.
  ! U(:,:) and Q(:,:) are in user units.
  !================================================================
  integer::ivar,i,it,nticks
  real(dp),dimension(1:nvector,1:nvar+3),save::q   ! Primitive variables
  real(dp)::rx,ry,rz,rr,x0,y0,z0,eps,rcut,lr,lrmax,lrcut
  real(dp)::pi,v200,spin,c,fgas,dmax,mmax,Bnfw,Bz,dxmin
  real(dp)::fcut,vcirc,vrot,pnfw,dnfw,mnfw,fc,cosphi
  real(dp)::Axll,Axlr,Axrl,Axrr
  real(dp)::Ayll,Aylr,Ayrl,Ayrr
  real(dp)::xl,xr,yl,yr,zl,zr,xc,yc,zc
  real(dp)::xxl,xxr,yyl,yyr,zzl,zzr,xx,yy

  ! Add here, if you wish, some user-defined initial conditions
  eps=2.*boxlen*0.5**nlevelmax
  x0=boxlen/2.0
  y0=boxlen/2.0
  z0=boxlen/2.0
  rcut=boxlen/2.0*0.75

  v200=gravity_params(1)
  c   =gravity_params(2)
  spin=gravity_params(3)
  fgas=gravity_params(4)
  dmax=1d0/eps/(1d0+eps)**2
  pi  =acos(-1d0)

  mmax=4d0*pi*(log(1d0+rcut)-rcut/(1d0+rcut))
  vcirc=sqrt(4.*pi*(log(1d0+c)-c/(1d0+c))/c)
  fc=c*(0.5-0.5/(1d0+c)**2-log(1d0+c)/(1d0+c))/(log(1d0+c)-c/(1d0+c))**2
  Bnfw=gravity_params(5)*vcirc ! Scaled magnetic field

  dxmin=boxlen*0.5d0**nlevelmax
  nticks=max(ceiling(dx/dxmin),1)

  lrmax=log(10*boxlen)
  lrcut=log(rcut)
  do i=1,nn

     xl=x(i,1)-0.5*dx-x0
     xr=x(i,1)+0.5*dx-x0
     xc=x(i,1)-x0
     yl=x(i,2)-0.5*dx-y0
     yr=x(i,2)+0.5*dx-y0
     yc=x(i,2)-y0
     zl=x(i,3)-0.5*dx-z0
     zr=x(i,3)+0.5*dx-z0
     zc=x(i,3)-z0

     rr=sqrt(xc**2+yc**2+zc**2)
     fcut=exp(-(rr/rcut)**50)
     dnfw=1d0/rr/(1d0+rr)**2
     dnfw=MIN(dmax,dnfw)
     dnfw=MAX(dnfw*fcut,1d-6)
     mnfw=(log(1d0+rr)-rr/(1d0+rr))/(log(1d0+c)-c/(1d0+c))
     vrot=2d0*sqrt(2d0/fc)*spin*vcirc*c*mnfw/rr*fcut
     cosphi=sqrt(xc**2+yc**2)/rr
     lr=MIN(log(rr),lrcut)
     pnfw=romberg(lr,lrcut,1d-4)+mmax/MAX(rr,rcut)*1d-6

     ! Euler variables
     q(i,1)=fgas*dnfw
     q(i,2)=-vrot/rr*yc
     q(i,3)=+vrot/rr*xc
     q(i,4)=0.0
     q(i,5)=fgas*pnfw

     ! Vector potential along X
     Axrr=0d0; Axlr=0d0; Axrl=0d0; Axll=0d0
     do it=1,nticks
        xx=xl+(dble(it)-0.5d0)*dxmin
        rr=sqrt(xx**2+yr**2+zr**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Axrr=Axrr+dnfw**(2./3.)*Bnfw*(-yr)/dble(nticks)

        rr=sqrt(xx**2+yl**2+zr**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Axlr=Axlr+dnfw**(2./3.)*Bnfw*(-yl)/dble(nticks)

        rr=sqrt(xx**2+yr**2+zl**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Axrl=Axrl+dnfw**(2./3.)*Bnfw*(-yr)/dble(nticks)

        rr=sqrt(xx**2+yl**2+zl**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Axll=Axll+dnfw**(2./3.)*Bnfw*(-yl)/dble(nticks)
     end do

     ! Vector potential along Y
     Ayrr=0d0; Aylr=0d0; Ayrl=0d0; Ayll=0d0
     do it=1,nticks
        yy=yl+(dble(it)-0.5d0)*dxmin
        rr=sqrt(xr**2+yy**2+zr**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Ayrr=Ayrr+dnfw**(2./3.)*Bnfw*(+xr)/dble(nticks)

        rr=sqrt(xl**2+yy**2+zr**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Aylr=Aylr+dnfw**(2./3.)*Bnfw*(+xl)/dble(nticks)

        rr=sqrt(xr**2+yy**2+zl**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Ayrl=Ayrl+dnfw**(2./3.)*Bnfw*(+xr)/dble(nticks)

        rr=sqrt(xl**2+yy**2+zl**2)
        dnfw=1d0/rr/(1d0+rr)**2
        dnfw=MIN(dmax,dnfw)
        Ayll=Ayll+dnfw**(2./3.)*Bnfw*(+xl)/dble(nticks)
     end do

     ! B left
     q(i,6)=(-(Aylr-Ayll)/dx)
     q(i,7)=(+(Axlr-Axll)/dx)
     q(i,8)=(+(Ayrl-Ayll)/dx-(Axrl-Axll)/dx)

     ! B right
     q(i,nvar+1)=(-(Ayrr-Ayrl)/dx)
     q(i,nvar+2)=(+(Axrr-Axrl)/dx)
     q(i,nvar+3)=(+(Ayrr-Aylr)/dx-(Axrr-Axlr)/dx)

     ! Passive scalar
     if(metal)q(i,9)=0.

  enddo

  ! Convert primitive to conservative variables
  ! density -> density
  u(1:nn,1)=q(1:nn,1)
  ! velocity -> momentum
  u(1:nn,2)=q(1:nn,1)*q(1:nn,2)
  u(1:nn,3)=q(1:nn,1)*q(1:nn,3)
  u(1:nn,4)=q(1:nn,1)*q(1:nn,4)
  ! kinetic energy
  u(1:nn,5)=0.0d0
  u(1:nn,5)=u(1:nn,5)+0.5*q(1:nn,1)*q(1:nn,2)**2
  u(1:nn,5)=u(1:nn,5)+0.5*q(1:nn,1)*q(1:nn,3)**2
  u(1:nn,5)=u(1:nn,5)+0.5*q(1:nn,1)*q(1:nn,4)**2
  ! pressure -> total fluid energy
  u(1:nn,5)=u(1:nn,5)+q(1:nn,5)/(gamma-1.0d0)
  ! magnetic energy -> total fluid energy
  u(1:nn,5)=u(1:nn,5)+0.125d0*(q(1:nn,6)+q(1:nn,nvar+1))**2
  u(1:nn,5)=u(1:nn,5)+0.125d0*(q(1:nn,7)+q(1:nn,nvar+2))**2
  u(1:nn,5)=u(1:nn,5)+0.125d0*(q(1:nn,8)+q(1:nn,nvar+3))**2
  ! Magnetic field components
  u(1:nn,6:8)=q(1:nn,6:8)
  u(1:nn,nvar+1:nvar+3)=q(1:nn,nvar+1:nvar+3)
  ! passive scalars
  do ivar=9,nvar
     u(1:nn,ivar)=q(1:nn,1)*q(1:nn,ivar)
  end do

contains
  !ccccccccccccccccccccccccccccccccccccccccccccccccccccc
  function fffy(rint)
    implicit none
    !      Computes the integrand
    real(dp)::fffy
    real(dp)::rint,rrr,M,rho,vphi
    rrr=exp(rint)
    rho=1d0/rrr/(1d0+rrr)**2
    rho=MIN(rho,dmax)
    rho=MAX(rho*exp(-(rrr/rcut)**50),1d-6)
    M=4d0*pi*(log(1d0+rrr)-rrr/(1d0+rrr))

    vphi=2d0*sqrt(2d0/fc)*spin*vcirc*c/rrr &
         & *exp(-(rrr/rcut)**50) &
         & *(log(1d0+rrr)-rrr/(1d0+rrr))/(log(1d0+c)-c/(1d0+c))
    vphi=vphi*cosphi

    fffy=M/rrr*rho-vphi**2*rho
    return
  end function fffy
  !cccccccccccccccccccccccccccccccccccccccccccccccccccc
  function romberg(a,b,tol)
    implicit none
    real(dp)::romberg
    !
    !     Romberg returns the integral from a to b of f(x)dx using Romberg
    !     integration. The method converges provided that f(x) is continuous
    !     in (a,b). The function f must be double precision and must be
    !     declared external in the calling routine.
    !     tol indicates the desired relative accuracy in the integral.
    !
    integer::maxiter=16,maxj=5
    real(dp),dimension(100):: g
    real(dp)::a,b,tol,fourj
    real(dp)::h,error,gmax,g0,g1
    integer::nint,i,j,k,jmax

    h=0.5d0*(b-a)
    gmax=h*(fffy(a)+fffy(b))
    g(1)=gmax
    nint=1
    error=1.0d20
    i=0
10  i=i+1
    if(.not.  (i>maxiter.or.(i>5.and.abs(error)<tol)))then
       !	Calculate next trapezoidal rule approximation to integral.

       g0=0.0d0
       do k=1,nint
          g0=g0+fffy(a+(k+k-1)*h)
       end do
       g0=0.5d0*g(1)+h*g0
       h=0.5d0*h
       nint=nint+nint
       jmax=min(i,maxj)
       fourj=1.0d0

       do j=1,jmax
          ! Use Richardson extrapolation.
          fourj=4.0d0*fourj
          g1=g0+(g0-g(j))/(fourj-1.0d0)
          g(j)=g0
          g0=g1
       enddo
       if (abs(g0).gt.tol) then
          error=1.0d0-gmax/g0
       else
          error=gmax
       end if
       gmax=g0
       g(jmax+1)=g0
       go to 10
    end if
    romberg=g0

    if (i>maxiter.and.abs(error)>tol) &
         &    write(*,*) 'Romberg failed to converge; integral, error=', &
         &    romberg,error

    return
  end function romberg

end subroutine condinit

subroutine velana(x,v,dx,t,ncell)
  use amr_parameters
  use hydro_parameters
  implicit none
  integer ::ncell                         ! Size of input arrays
  real(dp)::dx                            ! Cell size
  real(dp)::t                             ! Current time
  real(dp),dimension(1:nvector,1:3)::v    ! Velocity field
  real(dp),dimension(1:nvector,1:ndim)::x ! Cell center position.
  !================================================================
  ! This routine computes the user defined velocity fields.
  ! x(i,1:ndim) are cell center position in [0,boxlen] (user units).
  ! v(i,1:3) is the imposed 3-velocity in user units.
  !================================================================
  integer::i
  real(dp)::xx,yy,zz,vx,vy,vz,rr,tt,omega,aa,twopi

  ! Add here, if you wish, some user-defined initial conditions
  aa=1.0
  twopi=2d0*ACOS(-1d0)
  do i=1,ncell

     xx=x(i,1)
     yy=x(i,2)
     zz=x(i,3)

     ! ABC
     vx=aa*(cos(twopi*yy)+sin(twopi*zz))
     vy=aa*(sin(twopi*xx)+cos(twopi*zz))
     vz=aa*(cos(twopi*xx)+sin(twopi*yy))

!!$     ! 1D advection test
!!$     vx=1.0_dp
!!$     vy=0.0_dp
!!$     vz=0.0_dp

!!$     ! Ponomarenko
!!$     xx=xx-boxlen/2.0
!!$     yy=yy-boxlen/2.0
!!$     rr=sqrt(xx**2+yy**2)
!!$     if(yy>0)then
!!$        tt=acos(xx/rr)
!!$     else
!!$        tt=-acos(xx/rr)+twopi
!!$     endif
!!$     if(rr<1.0)then
!!$        omega=0.609711
!!$        vz=0.792624
!!$     else
!!$        omega=0.0
!!$        vz=0.0
!!$     endif
!!$     vx=-sin(tt)*rr*omega
!!$     vy=+cos(tt)*rr*omega

     v(i,1)=vx
     v(i,2)=vy
     v(i,3)=vz

  end do

end subroutine velana


*     ----------------------------------------------------------------
*     L S J F A C T
*     ----------------------------------------------------------------
*
*     This subroutine calculates the LSJ dependent angular factors
*     for the orbital,spindipolar,contact and quadrupol contribution and
*     saves them in vectors ORBF(K),DIPF(K),CONTF(K),QUADF(K).
*     K is a variable to order the different combinations of J and J'.
*     Note on the phase; MCHF_ASP assumes S + L coupling while the
*     decoupling formulas uses the L + S coupling of Cowan.
*     The needed (-1)**(L+S-J) faktors for l.h.s and r.h.s have to be
*     included for consistency.

      SUBROUTINE LSJFACT(JI,JF,NJQ,IDIAG,ndens)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      INTEGER SS1,SS2,ALPHJ(24)
      COMMON/COEF/ORBF(20),DIPF(20),CONTF(20),QUADF(20),J(20),JP(20),
     :JJMAX,JJMIN,JJ1MAX,JJ1MIN,JJ2MAX,JJ2MIN,LL1,SS1,LL2,SS2,VOLF(20)
*                       DATA STATEMENTS
      COMMON/CONSTS/ZERO,TENTH,HALF,ONE,TWO,THREE,FOUR,SEVEN,ELEVEN,EPS
      DATA ALPHJ/4H  0 ,4H 1/2,4H  1 ,4H 3/2,4H  2 ,4H 5/2,4H  3 ,4H 7/2
     :          ,4H  4 ,4H 9/2,4H  5 ,4H11/2,4H  6 ,4H13/2,4H  7 ,4H15/2
     :          ,4H  8 ,4H17/2,4H  9 ,4H19/2,4H  10,4H21/2,4H  11,4H23/2
     :/
      DATA GS/2.002319D0/
      NJQ=0
Cwww
      NPH2=(LL2+SS2-LL1-SS1)/2
      PHASE=DFLOAT((-1)**NPH2)
Cwww
      DO 10 JJ=JJMIN,JJMAX,2
         IF (JJ.EQ.0) GO TO 10
         JJP=JJ
         NJQ=NJQ+1

*        J'=J

         IF (((JJ.GE.JJ1MIN).AND.(JJ.LE.JJ1MAX)).AND.
     :   ((JJ.GE.JJ2MIN).AND.(JJ.LE.JJ2MAX))) THEN
	    CALL SIXJ1(LL1,JJ,SS1,JJ,LL2,1,W61)
	    IF(MOD(LL1+SS1+2+JJ,4).NE.0) W61=-W61
	    CALL NINELS(LL1,SS1,JJ,LL2,SS2,JJ,4,2,2,1,INGG,S9J)
	    IF(INGG.EQ.0) THEN
	      S9J=ZERO
	    ELSE
	      CALL NINE12(LL1,SS1,JJ,LL2,SS2,4,S9J)
	    ENDIF
	    CALL SIXJ1(SS1,JJ,LL1,JJ,SS2,1,W62)
	    IF(MOD(LL1+SS1+2+JJ,4).NE.0)W62=-W62
            CALL SIXJ2(LL1,JJ,SS1,JJ,LL2,1,W63)
	    IF(MOD(LL1+SS1+4+JJ,4).NE.0)W63=-W63
            IF(NDENS.EQ.1) THEN
	      CALL SIXJ(LL1,SS1,JJ,JJ,0,LL2,1,W64)
	      IF(MOD(LL1+SS1+JJ,4).NE.0)W64=-W64
	    ENDIF
            SQRT1=DSQRT(DFLOAT((JJ+1)*4)/DFLOAT(JJ*(JJ+2)))
            SQRT2=DSQRT(DFLOAT(JJ*(JJ+1)*(JJ-1))/DFLOAT((JJ+2)*(JJ+3)))

            if (ndens.eq.1) sqrt3 = dsqrt(jj + 1.d0)

            NPH1=(SS2-SS1)/2
            ORBF(NJQ)=SQRT1*W61*2.D0*PHASE
            DIPF(NJQ)=(-1.D0)*DSQRT(30.D0)*SQRT1*S9J*GS*PHASE
         CONTF(NJQ)=DFLOAT((-1)**(NPH1))*SQRT1*W62*2.D0*GS*PHASE/3.D0

            if (ndens.eq.1) volf(njq) = sqrt3*w64*phase

            QUADF(NJQ)=(-1.D0)*SQRT2*W63*2.D0*PHASE
         ELSE
            ORBF(NJQ)=0.D0
            DIPF(NJQ)=0.D0
            CONTF(NJQ)=0.D0

            if (ndens.eq.1) volf(njq) = 0.d0

            QUADF(NJQ)=0.D0
         ENDIF

         J(NJQ)=ALPHJ(JJ+1)
         JP(NJQ)=ALPHJ(JJP+1)

*        J'=J-1

Cwww
Cwww     Observe that A(LS;J,J-1)=(-1)A(SL;J,J-1) therefore
Cwww     we multiply with PHASE and not PHASE*(-1)
Cwww
         IF (JJP.EQ.JJMIN.OR.IDIAG.EQ.1) GO TO 10
         JJP=JJ-2
         NJQ=NJQ+1

         if (ndens.eq.1) volf(njq) = 0.d0

         IF (((JJ.GE.JJ1MIN).AND.(JJ.LE.JJ1MAX)).AND.
     :   ((JJP.GE.JJ2MIN).AND.(JJP.LE.JJ2MAX))) THEN
	    CALL SIXJ1(LL1,JJ,SS1,JJP,LL2,1,W61)
	    IF(MOD(LL1+SS1+2+JJP,4).NE.0)W61=-W61
	    CALL NINELS(LL1,SS1,JJ,LL2,SS2,JJP,4,2,2,1,INGG,S9J1)
	    IF(INGG.EQ.0) THEN
	      S9J1=ZERO
	    ELSE
	      CALL NINE13(LL1,SS1,JJ,LL2,SS2,4,S9J1)
	    ENDIF
	    CALL SIXJ1(SS1,JJ,LL1,JJP,SS2,1,W62)
	    IF(MOD(LL1+SS1+2+JJP,4).NE.0)W62=-W62
	    CALL SIXJ2(LL1,JJ,SS1,JJP,LL2,1,W63)
	    IF(MOD(LL1+SS1+4+JJP,4).NE.0)W63=-W63
            SQRT1=DSQRT(DFLOAT(2)/DFLOAT(JJ))
            SQRT2=DSQRT(DFLOAT(JJ*(JJ-2))/DFLOAT((JJ+2)))
            NPH1=(SS2-SS1+2)/2
            ORBF(NJQ)=SQRT1*W61*2.D0*PHASE
            DIPF(NJQ)=(-1.D0)*DSQRT(30.D0)*SQRT1*S9J1*GS*PHASE
            CONTF(NJQ)=DFLOAT((-1)**(NPH1))*SQRT1*W62*2.D0*GS*PHASE
     :      /3.D0
            QUADF(NJQ)=(-1.D0)*SQRT2*W63*PHASE/(DSQRT(2.D0)*2.D0)
         ELSE
             ORBF(NJQ)=0.D0
             DIPF(NJQ)=0.D0
             CONTF(NJQ)=0.D0
             QUADF(NJQ)=0.D0
         ENDIF

         J(NJQ)=ALPHJ(JJ+1)
         JP(NJQ)=ALPHJ(JJP+1)

*        J'=J-2

         IF (JJP.EQ.JJMIN) GO TO 10
         JJP=JJ-4
         NJQ=NJQ+1

         if (ndens.eq.1) volf(njq) = 0.d0

         IF (((JJ.GE.JJ1MIN).AND.(JJ.LE.JJ1MAX)).AND.
     :   ((JJP.GE.JJ2MIN).AND.(JJP.LE.JJ2MAX))) THEN
	    CALL SIXJ2(LL1,JJ,SS1,JJP,LL2,1,W61)
	    IF(MOD(LL1+SS1+4+JJP,4).NE.0)W61=-W61
            SQRT2=DSQRT(DFLOAT((JJ-2)*JJ*(JJ-1)))
            QUADF(NJQ)=(-1.D0)*SQRT2*W61*PHASE/8.D0
         ELSE
            ORBF(NJQ)=0.D0
            DIPF(NJQ)=0.D0
            CONTF(NJQ)=0.D0
         ENDIF

40       J(NJQ)=ALPHJ(JJ+1)
         JP(NJQ)=ALPHJ(JJP+1)
10    CONTINUE
      RETURN
      END


      DOUBLE PRECISION FUNCTION rlog(x)
C     -------------------
C     COMPUTATION OF  X - 1 - LN(X)
C     -------------------
C     .. Scalar Arguments ..
      DOUBLE PRECISION x
C     ..
C     .. Local Scalars ..
      DOUBLE PRECISION a,b,p0,p1,p2,q1,q2,r,t,u,w,w1
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC dble,dlog
C     ..
C     .. Data statements ..
C     -------------------
      DATA a/.566749439387324D-01/
      DATA b/.456512608815524D-01/
      DATA p0/.333333333333333D+00/,p1/-.224696413112536D+00/,
     +     p2/.620886815375787D-02/
      DATA q1/-.127408923933623D+01/,q2/.354508718369557D+00/
C     ..
C     .. Executable Statements ..
C     -------------------
      IF (x.LT.0.61D0 .OR. x.GT.1.57D0) GO TO 40
      IF (x.LT.0.82D0) GO TO 10
      IF (x.GT.1.18D0) GO TO 20
C
C              ARGUMENT REDUCTION
C
      u = (x-0.5D0) - 0.5D0
      w1 = 0.0D0
      GO TO 30
C
   10 u = dble(x) - 0.7D0
      u = u/0.7D0
      w1 = a - u*0.3D0
      GO TO 30
C
   20 u = 0.75D0*dble(x) - 1.D0
      w1 = b + u/3.0D0
C
C               SERIES EXPANSION
C
   30 r = u/ (u+2.0D0)
      t = r*r
      w = ((p2*t+p1)*t+p0)/ ((q2*t+q1)*t+1.0D0)
      rlog = 2.0D0*t* (1.0D0/ (1.0D0-r)-r*w) + w1
      RETURN
C
C
   40 r = (x-0.5D0) - 0.5D0
      rlog = r - dlog(x)
      RETURN

      END


subroutine pressz

!
! Compute the reference pressure at height levels from temperature and
! moisture sounding. Mostly the effect of surface pressure change is
! taken into account here, which can be important for CRM's long runs.

use vars
use params
implicit none
integer k

real presr(nz)

presr(1)=(pres0/1000.)**(rgas/cp)
presi(1)=pres0

do k=1,nzm
 prespot(k)=(1000./pres(k))**(rgas/cp)
 tv0(k)=tabs0(k)*prespot(k)*(1.+epsv*q0(k))
 presr(k+1)=presr(k)-ggr/cp/tv0(k)*(zi(k+1)-zi(k))
 presi(k+1)=1000.*presr(k+1)**(cp/rgas)
 pres(k) = exp(log(presi(k))+log(presi(k+1)/presi(k))* &
              (z(k)-zi(k))/(zi(k+1)-zi(k)))
 prespot(k)=(1000./pres(k))**(rgas/cp)
end do

end subroutine pressz


      SUBROUTINE XYZPLH(X)
C
      IMPLICIT REAL*8 (A-H,O-Z)
      REAL*8 J2,J3,J4,J22
C
C     SUBROUTINE TO CONVERT INERTIAL XYZ COORDINATES TO LATITUDE,
C     LONGITUDE AND HEIGHT
C
      COMMON/CNBODY/ J2,J3,J4,J22,ZJ20,ZMU,WWO,FLAT,AEARTH
C
      COMMON/CONSTS/ PI,TWOPI,RADIAN
C
      COMMON/ECNSTS/ THETA1,THETGO(12)
C
      COMMON/IDATE1/ IY,IM,JWDAY
C
      COMMON/RPOOL1/ RHOK(10),TIME,SA(3,3),FM1(3,3),ZLK(10),OMEG(3),
     .               ZLKP(10),ZLKDP(10),CMAT(3,3),GBAR(3,3),YBCM(3),
     .               ZBZK(3,10),FCM(3,3),DTO,PHID,PHI1
C
      COMMON/SUBPOS/ ALAT,ALONG,HGT
C
      DIMENSION X(3)
C
      CALL TCNVRT(YY,ZM,DAORSC,TLAST,TIME,2)
      DAY=JWDAY
      IY1=IY-67
      THGO=THETGO(IY1)*RADIAN
      THE1=THETA1*RADIAN
      THE2=THE1 + TWOPI
      THETAG=THGO+DAY*THE1+DAORSC*THE2/8.64D4
      SINTH=DSIN(THETAG)
      COSTH=DCOS(THETAG)
C                       CALCULATE EARTH FIXED COORDINATES
      XE=COSTH*X(1) + SINTH*X(2)
      YE=COSTH*X(2) - SINTH*X(1)
      ZE=X(3)
C     **** CALCULATE ECCENTRICITY OF CENTRAL BODY (USUALLY EARTH) ******
      FL=1.D0/FLAT
      ECC=2.D0*FL-FL**2
C     **** CALCULATE LATITUDE,LONGITUDE,AND HEIGHT ****
      REQ=DSQRT(XE*XE+YE*YE)
      ALONG=0.0D0
      IF(XE.NE.0.0D0.OR.YE.NE.0.0D0) ALONG=DATAN2(YE,XE)
      IF(ALONG.LT.0.0D0) ALONG=ALONG+TWOPI
   10 ESQ1=1.0D0-ECC
      ALAT=DATAN2(ZE,REQ)
      SINPHI=DSIN(ALAT)
      RSAT=DSQRT(ZE*ZE+REQ*REQ)
      DEN=DSQRT(ESQ1+SINPHI*SINPHI*ECC)
      HGT=RSAT-AEARTH*(1.0D0-FL)/DEN
      RETURN
      END


      module e3if_dc_m
c
c------------------------------------------------------------------------------
c  calculating the discontinuity capturing (DC) operator for the interface
c  macro-elements
c------------------------------------------------------------------------------
        implicit none
c
c
      contains
        subroutine calc_projector(proj, nv)
c..............................................................................
c  calculating the projector which project a quanlity into its tangential
c  components as :
c  \bm{P} =  \bm{I} - \bm{n} \otimes \bm{n}
c..............................................................................
          use number_def_m
          use propar_m, only: npro
          use global_const_m, only: nsd
          implicit none
c
          real*8,  dimension(npro,nsd), intent(in) :: nv ! normal vector
          real*8,  dimension(npro,nsd,nsd), intent(out) :: proj ! projector
c
          proj = zero
c
          proj(:,1,1) = one - nv(:,1)*nv(:,1)
          proj(:,1,2) =     - nv(:,1)*nv(:,2)
          proj(:,1,3) =     - nv(:,1)*nv(:,3)
c
          proj(:,2,1) =     - nv(:,2)*nv(:,1)
          proj(:,2,2) = one - nv(:,2)*nv(:,2)
          proj(:,2,3) =     - nv(:,2)*nv(:,3)
c
          proj(:,3,1) =     - nv(:,3)*nv(:,1)
          proj(:,3,2) =     - nv(:,3)*nv(:,2)
          proj(:,3,3) = one - nv(:,3)*nv(:,3)

c
        end subroutine calc_projector
c
        subroutine calc_ch(ch0, ch1, f_jump)
c..............................................................................
c  calculating the c^h of the DC operator for the interface, which is analogous
c  to the nu^h for the volumn elements.
c  Unit of c^h ~ [Length]/ [Time]
c..............................................................................
          use number_def_m
          use propar_m, only: npro
          use conpar_m, only: nflow
          use dgifinp_m, only: if_e_dc
          implicit none
c
          real*8, dimension(npro),intent(out) :: ch0, ch1
          real*8, dimension(npro,nflow),intent(in) :: f_jump ! flux jump
c
          real*8, dimension(npro,nflow) :: u_ref_0, u_ref_1 ! reference conservative
                                                            ! variables
          real*8, dimension(npro,nflow) :: temp0, temp1 ! local temporary array
          integer :: iel
c
          u_ref_0(:,1) = 1.000000000000000d-2 ! hacking, rho_ref, gas phase
          u_ref_0(:,2) = 1.000000000000000d0 ! hacking, rho_ref*v_ref
          u_ref_0(:,3) = 1.000000000000000d0 ! hacking, rho_ref*v_ref
          u_ref_0(:,4) = 1.000000000000000d0 ! hacking, rho_ref*v_ref
          u_ref_0(:,5) = 2.114165517241379d3 ! hacking, rho_ref*(ei + 0.5*v_ref^2)
c
          u_ref_1(:,1) = 1.000000000000000d0 ! hacking, rho_ref, liquid phase
          u_ref_1(:,2) = 1.000000000000000d0  ! hacking, rho_ref*v_ref
          u_ref_1(:,3) = 1.000000000000000d0 ! hacking, rho_ref*v_ref
          u_ref_1(:,4) = 1.000000000000000d0 ! hacking, rho_ref*v_ref
          u_ref_1(:,5) = 4.204805000000000d5! hacking, rho_ref*(ei + 0.5*v_ref^2)
c... diag(U1_ref ... U5_ref) * flux_jump
          temp0(:,1) = ( one/u_ref_0 (:,1) )*f_jump(:,1)
          temp0(:,2) = ( one/u_ref_0 (:,2) )*f_jump(:,2)
          temp0(:,3) = ( one/u_ref_0 (:,3) )*f_jump(:,3)
          temp0(:,4) = ( one/u_ref_0 (:,4) )*f_jump(:,4)
          temp0(:,5) = ( one/u_ref_0 (:,5) )*f_jump(:,5)
c
          temp1(:,1) = ( one/u_ref_1 (:,1) )*f_jump(:,1)
          temp1(:,2) = ( one/u_ref_1 (:,2) )*f_jump(:,2)
          temp1(:,3) = ( one/u_ref_1 (:,3) )*f_jump(:,3)
          temp1(:,4) = ( one/u_ref_1 (:,4) )*f_jump(:,4)
          temp1(:,5) = ( one/u_ref_1 (:,5) )*f_jump(:,5)
c... c^h for each phase
          do iel = 1,npro
            ch0(iel) = if_e_dc
     &               * sqrt(dot_product(temp0(iel,:),temp0(iel,:)))
            ch1(iel) = if_e_dc
     &               * sqrt(dot_product(temp1(iel,:),temp1(iel,:)))
          enddo
c
        end subroutine calc_ch
c
        subroutine e3if_dc_res(ri, var, A0, ch, pt_g_p)
c..............................................................................
c  calculation of the contribution of DC operator to the integrand of residual
c  for both phases
c  G_b = c^h g^{km}_{I} N_{a,k} A0 Y_{,m} d \Gama
c..............................................................................
          use propar_m, only: npro
          use conpar_m, only: nflow
          use global_const_m, only: nsd
          use e3if_param_m, only: var_t
          implicit none
c
          real*8, dimension(npro,nflow*(nsd+1)), intent(inout) :: ri
          real*8, dimension(npro,nflow,nflow), intent(in) :: A0
          type(var_t), dimension(npro), intent(in) :: var
          real*8, dimension(npro), intent(in) :: ch
          real*8, dimension(npro,nsd,nsd),intent(in) :: pt_g_p
c
          real*8, dimension(npro,nflow,nsd) :: A0_grad_y
          integer :: iel, k, k0
c
          do iel = 1,npro
c... A0 Y_{,m}
            A0_grad_y(iel,:,:) = matmul( A0(iel,:,:),
     &                                   transpose(var(iel)%grad_y(:,:)))
c... c^h g^{km}_{I} A0 Y_{,m}
            do k = 1,nsd
              k0 = (k-1)*nflow !k0+1, starting index in ri for kth direction
                               !k0+nflow, ending index in ri for kth direction
              ri(iel,k0+1:k0+nflow) = ri(iel,k0+1:k0+nflow)
     &        + ch(iel)
     &        * matmul( A0_grad_y(iel,:,:), pt_g_p(iel,k,:) )
            enddo
          enddo
c
        end subroutine e3if_dc_res
c
        subroutine e3if_dc_egmass(egmass,nshl, shg,A0, ch,gI, WdetJ)
c..............................................................................
c  calculation of the contribution of DC operator to the local stiffness matrix
c  for both phases
c  \partial G_b / \partial Y_{a} =
c  c^h g^{km}_{I} N_{a,k} A0 N_{b,m} d \Gama
c..............................................................................
          use propar_m, only: npro
          use conpar_m, only: nflow
          use global_const_m, only: nsd
          implicit none
c
          real*8, dimension(:,:,:),   intent(inout) :: egmass
          real*8, dimension(:,:,:),   intent(in) :: shg
          real*8, dimension(npro,nflow,nflow), intent(in) :: A0
          real*8, dimension(npro), intent(in) :: ch
          real*8, dimension(npro,nsd,nsd),intent(in) :: gI !g^{ij}_{I} =
                                                           ! proj^T g^{ij} proj
          real*8, dimension(npro),intent(in) :: WdetJ
          integer, intent(in)  :: nshl
c
          real*8, dimension(npro,nsd) :: shga_g
          real*8, dimension(npro) :: shga_g_shgb
          integer :: iel, ia, ib, a_row, b_col
c
          do iel = 1,npro
            do ia = 1,nshl
              a_row = (ia -1)*nflow ! a_row+1: starting row# for dof ia
                                    ! a_row+nflow: ending row# for dof ia
c... g^{km}_{I} N_{ia,k}
              shga_g(iel,:) = matmul( shg(iel,ia,:), gI(iel,:,:) )
              do ib = 1,nshl
                b_col = (ib -1)*nflow ! b_col+1: starting column# for dof ib
                                      ! b_col+nflow: ending column# for dof ib
c... g^{km}_{I} N_{ia,k} N_{ib,m}
                shga_g_shgb(iel) = dot_product( shga_g(iel,:),shg(iel,ib,:))
c
                egmass(iel, a_row+1:a_row+nflow, b_col+1:b_col+nflow) =
     &          egmass(iel, a_row+1:a_row+nflow, b_col+1:b_col+nflow)
     &        + ch(iel) * shga_g_shgb(iel) * A0(iel,:, :) * WdetJ(iel)
              enddo
            enddo
          enddo
c
        end subroutine e3if_dc_egmass

        subroutine e3if_dc
c..............................................................................
c  calculating the discontinuity capturing (DC) operator
c..............................................................................
          use e3if_param_m
          use e3gij_m, only: e3giju
          implicit none
c
c
          real*8, dimension(npro) :: ch0, ch1 ! nu^h for DC on interface
          real*8, dimension(npro,nsd,nsd) :: proj  ! tangential projector
          real*8, dimension(npro,6) :: giju0, giju1 ! g^{ij} vector form
          real*8, dimension(npro,nsd,nsd) :: giju_f0, giju_f1 ! g^{ij} full
          real*8, dimension(npro,nsd,nsd) :: pt_g0, pt_g1 ! proj_{ik} g^ij
                                                          ! proj is symetric
          real*8, dimension(npro,nsd,nsd) :: pt_g_p0, pt_g_p1 ! proj_{ik} g^ij proj_{jm}
          integer :: iel
c... get the tangential projector
          call calc_projector(proj, nv0) ! get the tangential projector
c... get the c^h
          call calc_ch(ch0, ch1, f_jump)
c
          call e3giju (giju0, dxidx0, npro, nsd, lcsyst0) ! get g^{ij}, 0 side
          call e3giju (giju1, dxidx1, npro, nsd, lcsyst1) ! get g^{ij}, 1 side
c.... get the full g^ij matrix
          giju_f0(:,1,1) = giju0(:,1)
          giju_f0(:,1,2) = giju0(:,4)
          giju_f0(:,1,3) = giju0(:,5)
          giju_f0(:,2,1) = giju0(:,4)
          giju_f0(:,2,2) = giju0(:,2)
          giju_f0(:,2,3) = giju0(:,6)
          giju_f0(:,3,1) = giju0(:,5)
          giju_f0(:,3,2) = giju0(:,6)
          giju_f0(:,3,3) = giju0(:,3)
c
          giju_f1(:,1,1) = giju1(:,1)
          giju_f1(:,1,2) = giju1(:,4)
          giju_f1(:,1,3) = giju1(:,5)
          giju_f1(:,2,1) = giju1(:,4)
          giju_f1(:,2,2) = giju1(:,2)
          giju_f1(:,2,3) = giju1(:,6)
          giju_f1(:,3,1) = giju1(:,5)
          giju_f1(:,3,2) = giju1(:,6)
          giju_f1(:,3,3) = giju1(:,3)
c... calculate proj_{ik} g^ij, notice proj is symetric
          do iel = 1,npro
            pt_g0(iel,:,:) = matmul(proj(iel,:,:), giju_f0(iel,:,:))
            pt_g1(iel,:,:) = matmul(proj(iel,:,:), giju_f1(iel,:,:))
c... calculate proj_{ik} g^ij proj_{jm}
            pt_g_p0(iel,:,:) = matmul(pt_g0(iel,:,:),proj(iel,:,:))
            pt_g_p1(iel,:,:) = matmul(pt_g1(iel,:,:),proj(iel,:,:))
          enddo
c... calculate the local residual
          call e3if_dc_res(ri0, var0, A0_0, ch0,pt_g_p0)
          call e3if_dc_res(ri1, var1, A0_1, ch1,pt_g_p1)
c... calculate the local stiffness matrix
          if (lhs_dg .eq. 1) then
            call e3if_dc_egmass(egmass00, nshl0, shg0,A0_0, ch0, pt_g_p0,
     &                          WdetJif0)
            call e3if_dc_egmass(egmass11, nshl1, shg1,A0_1, ch1, pt_g_p1,
     &                          WdetJif1)
          endif
c

c
        end subroutine e3if_dc
c
      end module e3if_dc_m


program tr

  ! based on Pacheco's 'Parallel Programming With MPI' chapter 4

  use mpi

  implicit none

  integer :: i        ! loop variable
  integer :: ln       ! number of trapezoids for this task
  integer :: me       ! my mpi rank
  integer :: mpistat  ! mpi status
  integer :: n=0      ! number of trapezoids
  integer :: p        ! number of mpi tasks
  integer :: source   ! mpi task sending integral
  integer :: status   ! io status
  real    :: a=0,b=0  ! left and right interval endpoints
  real    :: h        ! trapezoid base width
  real    :: integral ! integral over my integral
  real    :: la,lb    ! local left and right interval endpoints
  real    :: total    ! total integral
  real    :: x        ! generic real temp

  character(len=5),parameter :: infile='tr.nl'
  character(len=4),parameter :: outfile1='out1'
  character(len=4),parameter :: outfile2='out2'
  integer,parameter          :: lun=77

  namelist /config/ a,b,n
  call mpi_init(mpistat)
  call mpi_comm_rank(mpi_comm_world,me,mpistat)
  call mpi_comm_size(mpi_comm_world,p,mpistat)
  if (me.eq.0) then
    open (lun,file=infile,status='old',iostat=status)
    if (status.ne.0) then
      write (*,'(a,a)') 'Error opening config file ',infile
      stop
    end if
    read (lun,nml=config)
    close (lun)
  end if
  call mpi_bcast(a,1,mpi_real,   0,mpi_comm_world,mpistat)
  call mpi_bcast(b,1,mpi_real,   0,mpi_comm_world,mpistat)
  call mpi_bcast(n,1,mpi_integer,0,mpi_comm_world,mpistat)
  if (n.lt.1) then
    write (*,'(a)') "Configuration parameter 'n' must be > 0"
    call mpi_finalize(mpistat)
    stop
  end if
  h=(b-a)/n;
  ln=n/p;
  la=a+me*ln*h
  lb=la+ln*h
  integral=trap(la,lb,ln,h)
  if (me.eq.0) then
    total=integral
    do source=1,p-1
      call mpi_recv(integral,1,mpi_real,source,0,mpi_comm_world,mpi_status_ignore,mpistat)
      total=total+integral
    end do
  else
    call mpi_send(integral,1,mpi_real,0,0,mpi_comm_world,mpistat)
  end if
  if (me.eq.0) then
    ! output file containing integral
    open (lun,file=outfile1,status='new',iostat=status)
    if (status.ne.0) then
      write (*,'(a,a)') 'Error opening output file ',outfile1
      stop
    end if
    write (lun,'(f4.2)') total
    close (lun)
    ! output file containing a constant
    open (lun,file=outfile2,status='new',iostat=status)
    if (status.ne.0) then
      write (*,'(a,a)') 'Error opening output file ',outfile2
      stop
    end if
    write (lun,'(i0)') 88
    close (lun)
  end if
  call mpi_finalize(mpistat)
  write (*,'(a)') 'SUCCESS'

contains

  real function trap(la,lb,ln,h)

    integer,intent(in) :: ln
    integer            :: i
    real,intent(in)    :: la,lb,h
    real               :: integral,x

    integral=(f(la)+f(lb))/2.0
    x=la
    do i=1,ln-1
      x=x+h
      integral=integral+f(x)
    end do
    integral=integral*h
    trap=integral

  end function trap

  real function f(x)

    real,intent(in)::x

    f=sin(x)

  end function f

end program tr


C     Scalar replacement in the presence of conditional control flow
C
C     Steve Carr and Ken Kennedy
C
C     Software - Practive and Experience, Vol. 24, No. 1, pp. 51-77, 1994
C
C     Sixth example, page 24: Livermore Loop 5 cannot be handled by PIPS
C     because we do not try to carry a value from one iteration to the next

      subroutine scalarization35(x, y, z, n)
      real x(n), y(n), z(n)

      do i = 2, n
         x(i) = z(i)*(y(i)-x(i-1))
      enddo

      print *, x

      end


! =====================================================
SUBROUTINE clawpack46_rpn2_manifold(ixy,maxm,meqn,mwaves, &
     mbc,mx,ql,qr,auxl,auxr,wave,s,amdq,apdq)
! =====================================================
! Riemann solver for the acoustics equations in 2d
! on general quadrilateral grid, with variable coefficients

! waves: 2
! equations: 3

! Conserved quantities:
!       1 pressure
!       2 x_velocity
!       3 y_velocity

! Auxiliary quantities:
!       1 a_x
!       2 a_y   where (a_x,a_y) is unit normal to left face
!       3 length_ratio_left ratio of length of left face to dyc
!       4 b_x
!       5 b_y   where (b_x,b_y) is unit normal to bottom face
!       6 length_ratio_bottom   ratio of length of bottom face to dxc
!       7 cell_area   ratio of cell area to dxc*dyc
!         (approximately Jacobian of mapping function)
!       8 Z (impedance)
!       9 c (sound speed)
!
! Rotate velocity and then call standard Riemann solver.
! The resulting waves and flux differences are then rotated
! back to x-y.

! solve Riemann problems along one slice of data.
!
! On input, ql contains the state vector at the left edge of each cell
! qr contains the state vector at the right edge of each cell
!
! This data is along a slice in the x-direction if ixy=1
! or the y-direction if ixy=2.
! On output, wave contains the waves, s the speeds,
! and amdq, apdq the decomposition of the flux difference
! f(qr(i-1)) - f(ql(i))
! into leftgoing and rightgoing parts respectively.
! With the Roe solver we have
!     amdq  =  A^- \Delta q    and    apdq  =  A^+ \Delta q
! where A is the Roe matrix.  An entropy fix can also be incorporated
! into the flux differences.
!
! Note that the i'th Riemann problem has left state qr(:,i-1)
!                                   and right state ql(:,i)
! From the basic clawpack routines, this routine is called with ql = qr.

  IMPLICIT NONE
!
  INTEGER, INTENT(in) :: maxm, meqn, mwaves, mbc, mx
  DOUBLE PRECISION, INTENT(out) :: wave(1-mbc:maxm+mbc,meqn, mwaves)
  DOUBLE PRECISION, INTENT(out) :: s(1-mbc:maxm+mbc, mwaves)
  DOUBLE PRECISION, INTENT(in)  ::   ql(1-mbc:maxm+mbc, meqn)
  DOUBLE PRECISION, INTENT(in)  ::   qr(1-mbc:maxm+mbc, meqn)
  DOUBLE PRECISION, INTENT(out) :: apdq(1-mbc:maxm+mbc, meqn)
  DOUBLE PRECISION, INTENT(out) :: amdq(1-mbc:maxm+mbc, meqn)
  DOUBLE PRECISION, INTENT(in)  :: auxl(1-mbc:maxm+mbc, *)
  DOUBLE PRECISION, INTENT(in)  :: auxr(1-mbc:maxm+mbc, *)
!
!   ------------
  DOUBLE PRECISION :: delta(3)
  DOUBLE PRECISION :: unorl, unorr, a1, a2
  DOUBLE PRECISION :: alpha, beta, zi, zim, ci, cim
  INTEGER :: ixy, inx, iny, ilenrat, i, m
!
! Rotate the velocity vector (q(2), q(3)) so that it is aligned with the face
! normal.  The normal vector for the face at the i'th Riemann problem
! is stored in the aux array
! in locations (1,2) if ixy=1 or (4,5) if ixy=2.  The ratio of the
! length of the cell side to the length of the computational cell
! is stored in aux(3) or aux(6), respectively.

  IF (ixy.EQ.1) THEN
     inx = 1
     iny = 2
     ilenrat = 3
  ELSE
     inx = 4
     iny = 5
     ilenrat = 6
  ENDIF

! Determine rotation matrix:
!               [ alpha  beta ]
!               [-beta  alpha ]
! Note that this reduces to the identity on a standard cartesian grid.

! Determine normal velocity components at this edge:
  DO i=2-mbc,mx+mbc
     alpha = auxl(i,inx)
     beta  = auxl(i,iny)
     unorl = alpha*ql(i,2) + beta*ql(i,3)
     unorr = alpha*qr(i-1,2) + beta*qr(i-1,3)

     delta(1) = ql(i,1) - qr(i-1,1)
     delta(2) = unorl - unorr

     zi  = auxl(i,8)
     zim = auxl(i-1,8)
     ci  = auxl(i,9)
     cim = auxl(i-1,9)

     a1 = (-delta(1) + zi*delta(2)) / (zim + zi)
     a2 =  (delta(1) + zim*delta(2)) / (zim + zi)

!       Scale the velocities by the length ratios on each side.

     wave(i,1,1) = -a1*zim
     wave(i,2,1) = a1 * alpha
     wave(i,3,1) = a1 * beta
     s(i,1) = -cim * auxl(i,ilenrat)

     wave(i,1,2) = a2*zi
     wave(i,2,2) = a2 * alpha
     wave(i,3,2) = a2 * beta
     s(i,2) = ci * auxl(i,ilenrat)
  ENDDO

!   Compute the left-going and right-going fluctuations.
!   Note that s(1,i) < 0   and   s(2,i) > 0.

  DO m=1,meqn
     DO i = 2-mbc, mx+mbc
        amdq(i,m) = s(i,1)*wave(i,m,1)
        apdq(i,m) = s(i,2)*wave(i,m,2)
     ENDDO
  ENDDO

  RETURN
END SUBROUTINE clawpack46_rpn2_manifold


      SUBROUTINE CLARFY( UPLO, N, V, INCV, TAU, C, LDC, WORK )
*
*  -- LAPACK auxiliary test routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            INCV, LDC, N
      COMPLEX            TAU
*     ..
*     .. Array Arguments ..
      COMPLEX            C( LDC, * ), V( * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  CLARFY applies an elementary reflector, or Householder matrix, H,
*  to an n x n Hermitian matrix C, from both the left and the right.
*
*  H is represented in the form
*
*     H = I - tau * v * v'
*
*  where  tau  is a scalar and  v  is a vector.
*
*  If  tau  is  zero, then  H  is taken to be the unit matrix.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          Specifies whether the upper or lower triangular part of the
*          Hermitian matrix C is stored.
*          = 'U':  Upper triangle
*          = 'L':  Lower triangle
*
*  N       (input) INTEGER
*          The number of rows and columns of the matrix C.  N >= 0.
*
*  V       (input) COMPLEX array, dimension
*                  (1 + (N-1)*abs(INCV))
*          The vector v as described above.
*
*  INCV    (input) INTEGER
*          The increment between successive elements of v.  INCV must
*          not be zero.
*
*  TAU     (input) COMPLEX
*          The value tau as described above.
*
*  C       (input/output) COMPLEX array, dimension (LDC, N)
*          On entry, the matrix C.
*          On exit, C is overwritten by H * C * H'.
*
*  LDC     (input) INTEGER
*          The leading dimension of the array C.  LDC >= max( 1, N ).
*
*  WORK    (workspace) COMPLEX array, dimension (N)
*
*  =====================================================================
*
*     .. Parameters ..
      COMPLEX            ONE, ZERO, HALF
      PARAMETER          ( ONE = ( 1.0E+0, 0.0E+0 ),
     $                   ZERO = ( 0.0E+0, 0.0E+0 ),
     $                   HALF = ( 0.5E+0, 0.0E+0 ) )
*     ..
*     .. Local Scalars ..
      COMPLEX            ALPHA
*     ..
*     .. External Subroutines ..
      EXTERNAL           CAXPY, CHEMV, CHER2
*     ..
*     .. External Functions ..
      COMPLEX            CDOTC
      EXTERNAL           CDOTC
*     ..
*     .. Executable Statements ..
*
      IF( TAU.EQ.ZERO )
     $   RETURN
*
*     Form  w:= C * v
*
      CALL CHEMV( UPLO, N, ONE, C, LDC, V, INCV, ZERO, WORK, 1 )
*
      ALPHA = -HALF*TAU*CDOTC( N, WORK, 1, V, INCV )
      CALL CAXPY( N, ALPHA, V, INCV, WORK, 1 )
*
*     C := C - v * w' - w * v'
*
      CALL CHER2( UPLO, N, -TAU, V, INCV, WORK, 1, C, LDC )
*
      RETURN
*
*     End of CLARFY
*
      END


PROGRAM driver
use num_types
use derivative_structure_generator
use io_utils
implicit none

integer nMin, nMax ! Numbers of various things
integer k ! Number of colors/label types (i.e., binary, ternary, etc.)
integer LatDim ! 2D or 3D parent lattice?
integer nD ! Number of sites in the basis (i.e., number of points in the multilattice)
real(dp), allocatable :: d(:,:)
character(1) :: latTyp
real(dp)  eps
real(dp) :: parLV(3,3)

character(80) title, fname, arg2
logical fullLab,concCheck
logical :: origCrossOutAlgorithm = .false.
integer, pointer :: cRange(:,:)
integer, allocatable :: label(:,:)
integer, allocatable :: digit(:)
integer, allocatable :: equivalencies(:)

if (iargc()>=2) then
    call getarg(2,arg2)
    read(arg2, '(L1)') origCrossOutAlgorithm
endif

if (iargc()>=1) then
   call getarg(1,fname)
else
   fname = "struct_enum.in"
endif
call read_input(title,LatDim,parLV,nD,d,k,equivalencies,nMin,nMax,eps&
     &,fullLab,label,digit,fname,cRange,concCheck) ! Read in parent lattice vectors, etc.
if (LatDim==3) then; latTyp='b';else;latTyp='s';endif
call gen_multilattice_derivatives(title, parLV,nD,d,k,nMin,nMax,latTyp,eps,fullLab,&
         label,digit,equivalencies,concCheck,cRange, polya=.false.,&
         origCrossOutAlgorithm=origCrossOutAlgorithm)

END PROGRAM driver


!
! Copyright (C) 2001-2007 Quantum ESPRESSO group
! This file is distributed under the terms of the
! GNU General Public License. See the file `License'
! in the root directory of the present distribution,
! or http://www.gnu.org/copyleft/gpl.txt .
!
!-----------------------------------------------------------------------
SUBROUTINE kpoint_grid ( nrot, time_reversal, skip_equivalence, s, t_rev, &
                         bg, npk, k1,k2,k3, nk1,nk2,nk3, nks, xk, wk)
!-----------------------------------------------------------------------
!
!  Automatic generation of a uniform grid of k-points
!
  USE kinds, ONLY: DP
  IMPLICIT NONE
  !
  INTEGER, INTENT(in):: nrot, npk, k1, k2, k3, nk1, nk2, nk3, &
                        t_rev(48), s(3,3,48)
  LOGICAL, INTENT(in):: time_reversal, skip_equivalence
  real(DP), INTENT(in):: bg(3,3)
  !
  INTEGER, INTENT(out) :: nks
  real(DP), INTENT(out):: xk(3,npk)
  real(DP), INTENT(out):: wk(npk)
  ! LOCAL:
  real(DP), PARAMETER :: eps=1.0d-5
  real(DP) :: xkr(3), fact, xx, yy, zz
  real(DP), ALLOCATABLE:: xkg(:,:), wkk(:)
  INTEGER :: nkr, i,j,k, ns, n, nk
  INTEGER, ALLOCATABLE :: equiv(:)
  LOGICAL :: in_the_list
  !
  nkr=nk1*nk2*nk3
  ALLOCATE (xkg( 3,nkr),wkk(nkr))
  ALLOCATE (equiv( nkr))
  !
  DO i=1,nk1
     DO j=1,nk2
        DO k=1,nk3
           !  this is nothing but consecutive ordering
           n = (k-1) + (j-1)*nk3 + (i-1)*nk2*nk3 + 1
           !  xkg are the components of the complete grid in crystal axis
           xkg(1,n) = dble(i-1)/nk1 + dble(k1)/2/nk1
           xkg(2,n) = dble(j-1)/nk2 + dble(k2)/2/nk2
           xkg(3,n) = dble(k-1)/nk3 + dble(k3)/2/nk3
        ENDDO
     ENDDO
  ENDDO

  !  equiv(nk) =nk : k-point nk is not equivalent to any previous k-point
  !  equiv(nk)!=nk : k-point nk is equivalent to k-point equiv(nk)

  DO nk=1,nkr
     equiv(nk)=nk
  ENDDO

  IF ( skip_equivalence ) THEN
    CALL infomsg('kpoint_grid', 'ATTENTION: skip check of k-points equivalence')
    wkk = 1.d0
  ELSE
    DO nk=1,nkr
    !  check if this k-point has already been found equivalent to another
      IF (equiv(nk) == nk) THEN
        wkk(nk)   = 1.0d0
        !  check if there are equivalent k-point to this in the list
        !  (excepted those previously found to be equivalent to another)
        !  check both k and -k
        DO ns=1,nrot
           DO i=1,3
              xkr(i) = s(i,1,ns) * xkg(1,nk) &
                     + s(i,2,ns) * xkg(2,nk) &
                     + s(i,3,ns) * xkg(3,nk)
              xkr(i) = xkr(i) - nint( xkr(i) )
           ENDDO
           IF(t_rev(ns)==1) xkr = -xkr
           xx = xkr(1)*nk1 - 0.5d0*k1
           yy = xkr(2)*nk2 - 0.5d0*k2
           zz = xkr(3)*nk3 - 0.5d0*k3
           in_the_list = abs(xx-nint(xx))<=eps .and. &
                         abs(yy-nint(yy))<=eps .and. &
                         abs(zz-nint(zz))<=eps
           IF (in_the_list) THEN
              i = mod ( nint ( xkr(1)*nk1 - 0.5d0*k1 + 2*nk1), nk1 ) + 1
              j = mod ( nint ( xkr(2)*nk2 - 0.5d0*k2 + 2*nk2), nk2 ) + 1
              k = mod ( nint ( xkr(3)*nk3 - 0.5d0*k3 + 2*nk3), nk3 ) + 1
              n = (k-1) + (j-1)*nk3 + (i-1)*nk2*nk3 + 1
              IF (n>nk .and. equiv(n)==n) THEN
                 equiv(n) = nk
                 wkk(nk)=wkk(nk)+1.0d0
              ELSE
                 IF (equiv(n)/=nk .or. n<nk ) CALL errore('kpoint_grid', &
                    'something wrong in the checking algorithm',1)
              ENDIF
           ENDIF
           IF ( time_reversal ) THEN
              xx =-xkr(1)*nk1 - 0.5d0*k1
              yy =-xkr(2)*nk2 - 0.5d0*k2
              zz =-xkr(3)*nk3 - 0.5d0*k3
              in_the_list=abs(xx-nint(xx))<=eps.and.abs(yy-nint(yy))<=eps &
                                                 .and. abs(zz-nint(zz))<=eps
              IF (in_the_list) THEN
                 i = mod ( nint (-xkr(1)*nk1 - 0.5d0 * k1 + 2*nk1), nk1 ) + 1
                 j = mod ( nint (-xkr(2)*nk2 - 0.5d0 * k2 + 2*nk2), nk2 ) + 1
                 k = mod ( nint (-xkr(3)*nk3 - 0.5d0 * k3 + 2*nk3), nk3 ) + 1
                 n = (k-1) + (j-1)*nk3 + (i-1)*nk2*nk3 + 1
                 IF (n>nk .and. equiv(n)==n) THEN
                    equiv(n) = nk
                    wkk(nk)=wkk(nk)+1.0d0
                 ELSE
                    IF (equiv(n)/=nk.or.n<nk) CALL errore('kpoint_grid', &
                    'something wrong in the checking algorithm',2)
                 ENDIF
              ENDIF
           ENDIF
        ENDDO
      ENDIF
    ENDDO
  ENDIF

  !  count irreducible points and order them

  nks=0
  fact=0.0d0
  DO nk=1,nkr
     IF (equiv(nk)==nk) THEN
        nks=nks+1
        IF (nks>npk) CALL errore('kpoint_grid','too many k-points',1)
        wk(nks) = wkk(nk)
        fact    = fact+wk(nks)
        !  bring back into to the first BZ
        DO i=1,3
           xk(i,nks) = xkg(i,nk)-nint(xkg(i,nk))
        ENDDO
     ENDIF
  ENDDO
  !  go to cartesian axis (in units 2pi/a0)
  CALL cryst_to_cart(nks,xk,bg,1)
  !  normalize weights to one
  DO nk=1,nks
     wk(nk) = wk(nk)/fact
  ENDDO

  DEALLOCATE(equiv)
  DEALLOCATE(xkg,wkk)

  RETURN
END SUBROUTINE kpoint_grid
!
!-----------------------------------------------------------------------
SUBROUTINE tetrahedra ( nsym, s, time_reversal, t_rev, at, bg, npk, &
     k1,k2,k3, nk1,nk2,nk3, nks, xk, wk, ntetra, tetra )
  !-----------------------------------------------------------------------
  !
  ! Tetrahedron method according to P. E. Bloechl et al, PRB49, 16223 (1994)
  !
  USE kinds, ONLY: DP
  IMPLICIT NONE
  !
  INTEGER, INTENT(IN):: nks, nsym, t_rev(48), s(3,3,48), npk, &
                        k1, k2, k3, nk1, nk2, nk3, ntetra
  LOGICAL, INTENT (IN) :: time_reversal
  real(DP), INTENT(IN) :: at(3,3), bg(3,3), xk(3,npk), wk(npk)
  !
  INTEGER, INTENT(OUT) :: tetra(4,ntetra)
  !
  real(DP) :: xkr(3), deltap(3), deltam(3)
  real(DP), PARAMETER:: eps=1.0d-5
  real(DP), ALLOCATABLE :: xkg(:,:)
  INTEGER :: nkr, i,j,k, ns, n, nk, ip1,jp1,kp1, &
       n1,n2,n3,n4,n5,n6,n7,n8
  INTEGER, ALLOCATABLE:: equiv(:)
  !
  ! Re-generate a uniform grid of k-points xkg
  !
  nkr=nk1*nk2*nk3
  !      ntetra=6*nkr
  ALLOCATE (xkg( 3,nkr))
  ALLOCATE (equiv( nkr))
!
  DO i=1,nk1
     DO j=1,nk2
        DO k=1,nk3
           !  this is nothing but consecutive ordering
           n = (k-1) + (j-1)*nk3 + (i-1)*nk2*nk3 + 1
           !  xkg are the components of the complete grid in crystal axis
           xkg(1,n) = dble(i-1)/nk1 + dble(k1)/2/nk1
           xkg(2,n) = dble(j-1)/nk2 + dble(k2)/2/nk2
           xkg(3,n) = dble(k-1)/nk3 + dble(k3)/2/nk3
        ENDDO
     ENDDO
  ENDDO

  !  locate k-points of the uniform grid in the list of irreducible k-points
  !  that was previously calculated

  !  bring irreducible k-points to crystal axis
  CALL cryst_to_cart (nks,xk,at,-1)
  !
  DO nk=1,nkr
     DO n=1,nks
        DO ns=1,nsym
           DO i=1,3
              xkr(i) = s(i,1,ns) * xk(1,n) + &
                       s(i,2,ns) * xk(2,n) + &
                       s(i,3,ns) * xk(3,n)
           ENDDO
           IF(t_rev(ns)==1) xkr = -xkr
           !  xkr is the n-th irreducible k-point rotated wrt the ns-th symmetry
           DO i=1,3
              deltap(i) = xkr(i)-xkg(i,nk) - nint (xkr(i)-xkg(i,nk) )
              deltam(i) = xkr(i)+xkg(i,nk) - nint (xkr(i)+xkg(i,nk) )
           ENDDO
           !  deltap is the difference vector, brought back in the first BZ
           !  deltam is the same but with k => -k (for time reversal)
           IF ( sqrt ( deltap(1)**2 + &
                       deltap(2)**2 + &
                       deltap(3)**2 ) < eps .or. ( time_reversal .and. &
                sqrt ( deltam(1)**2 +  &
                       deltam(2)**2 +  &
                       deltam(3)**2 ) < eps ) ) THEN
              !  equivalent irreducible k-point found
              equiv(nk) = n
              GOTO 15
           ENDIF
        ENDDO
     ENDDO
     !  equivalent irreducible k-point found - something wrong
     CALL errore('tetrahedra','cannot locate  k point',nk)
15   CONTINUE
  ENDDO

  DO n=1,nks
     DO nk=1,nkr
        IF (equiv(nk)==n) GOTO 20
     ENDDO
     !  this failure of the algorithm may indicate that the displaced grid
     !  (with k1,k2,k3.ne.0) does not have the full symmetry of the lattice
     CALL errore('tetrahedra','cannot remap grid on k-point list',n)
20   CONTINUE
  ENDDO

  !  bring irreducible k-points back to cartesian axis
  CALL cryst_to_cart (nks,xk,bg, 1)

  !  construct tetrahedra

  DO i=1,nk1
     DO j=1,nk2
        DO k=1,nk3
           !  n1-n8 are the indices of k-point 1-8 forming a cube
           ip1 = mod(i,nk1)+1
           jp1 = mod(j,nk2)+1
           kp1 = mod(k,nk3)+1
           n1 = (  k-1) + (  j-1)*nk3 + (  i-1)*nk2*nk3 + 1
           n2 = (  k-1) + (  j-1)*nk3 + (ip1-1)*nk2*nk3 + 1
           n3 = (  k-1) + (jp1-1)*nk3 + (  i-1)*nk2*nk3 + 1
           n4 = (  k-1) + (jp1-1)*nk3 + (ip1-1)*nk2*nk3 + 1
           n5 = (kp1-1) + (  j-1)*nk3 + (  i-1)*nk2*nk3 + 1
           n6 = (kp1-1) + (  j-1)*nk3 + (ip1-1)*nk2*nk3 + 1
           n7 = (kp1-1) + (jp1-1)*nk3 + (  i-1)*nk2*nk3 + 1
           n8 = (kp1-1) + (jp1-1)*nk3 + (ip1-1)*nk2*nk3 + 1
           !  there are 6 tetrahedra per cube (and nk1*nk2*nk3 cubes)
           n  = 6 * ( (k-1) + (j-1)*nk3 + (i-1)*nk3*nk2 )

           tetra (1,n+1) = equiv(n1)
           tetra (2,n+1) = equiv(n2)
           tetra (3,n+1) = equiv(n3)
           tetra (4,n+1) = equiv(n6)

           tetra (1,n+2) = equiv(n2)
           tetra (2,n+2) = equiv(n3)
           tetra (3,n+2) = equiv(n4)
           tetra (4,n+2) = equiv(n6)

           tetra (1,n+3) = equiv(n1)
           tetra (2,n+3) = equiv(n3)
           tetra (3,n+3) = equiv(n5)
           tetra (4,n+3) = equiv(n6)

           tetra (1,n+4) = equiv(n3)
           tetra (2,n+4) = equiv(n4)
           tetra (3,n+4) = equiv(n6)
           tetra (4,n+4) = equiv(n8)

           tetra (1,n+5) = equiv(n3)
           tetra (2,n+5) = equiv(n6)
           tetra (3,n+5) = equiv(n7)
           tetra (4,n+5) = equiv(n8)

           tetra (1,n+6) = equiv(n3)
           tetra (2,n+6) = equiv(n5)
           tetra (3,n+6) = equiv(n6)
           tetra (4,n+6) = equiv(n7)
        ENDDO
     ENDDO
  ENDDO

  !  check

  DO n=1,ntetra
     DO i=1,4
        IF ( tetra(i,n)<1 .or. tetra(i,n)>nks ) &
             CALL errore ('tetrahedra','something wrong',n)
     ENDDO
  ENDDO

  DEALLOCATE(equiv)
  DEALLOCATE(xkg)

  RETURN
END SUBROUTINE tetrahedra