!> Various definitions and tools for running an NGA2 simulation
module simulation
   use string,            only: str_medium
   use precision,         only: WP
   use geometry,          only: cfg1,cfg2
   use geometry,          only: xinj_dist,inj_norm_diam,rli0,rlo0,rgi0
   use incomp_class,      only: incomp
   use tpns_class,        only: tpns
   use vfs_class,         only: vfs
   use coupler_class,     only: coupler
   use sgsmodel_class,    only: sgsmodel
   use timetracker_class, only: timetracker
   use ensight_class,     only: ensight
   use event_class,       only: event
   use datafile_class,    only: datafile
   use monitor_class,     only: monitor
   implicit none
   private

   !> Single-phase incompressible flow solver, and corresponding time tracker and sgs model
   type(incomp),      public :: fs1
   type(timetracker), public :: time1
   type(sgsmodel),    public :: sgs1

   ! Couplers between 1 and 2
   type(coupler),     public :: cpl12x,cpl12y,cpl12z

   !> Two-phase incompressible flow solver, VF solver, and corresponding time tracker and sgs model
   type(tpns),        public :: fs2
   type(vfs),         public :: vf2
   type(timetracker), public :: time2
   type(sgsmodel),    public :: sgs2

   !> Provide two datafiles and an event tracker for saving restarts
   type(event)    :: save_evt
   type(datafile) :: df1,df2
   character(len=str_medium) :: irl_file
   logical :: restarted

   !> Ensight postprocessing
   type(ensight) :: ens_out1
   type(ensight) :: ens_out2
   type(event)   :: ens_evt1
   type(event)   :: ens_evt2

   !> Simulation monitor file
   type(monitor) :: mfile1,cflfile1
   type(monitor) :: mfile2,cflfile2

   public :: simulation_init,simulation_run,simulation_final

   !> Private work arrays
   real(WP), dimension(:,:,:), allocatable :: resU1,resV1,resW1
   real(WP), dimension(:,:,:), allocatable :: Ui1,Vi1,Wi1
   real(WP), dimension(:,:,:,:), allocatable :: SR1
   real(WP), dimension(:,:,:), allocatable :: resU2,resV2,resW2
   real(WP), dimension(:,:,:), allocatable :: Ui2,Vi2,Wi2
   real(WP), dimension(:,:,:,:), allocatable :: SR2

contains

   !> Function that localizes the right domain boundary
   function right_boundary(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      isIn=.false.
      if (i.eq.pg%imax+1) isIn=.true.
   end function right_boundary

   !> Function that localizes injector at -y
   function inj1(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      real(WP) ::hyp,rise,run
      logical :: isIn
      isIn=.false.
      ! Check injector x-z plane
      hyp =norm2([pg%xm(i),pg%ym(j),pg%zm(k)]-[xinj_dist,0.0_WP,0.0_WP])
      rise=abs(pg%ym(j))
      run =sqrt(hyp**2-rise**2)
      if (run.le.inj_norm_diam/2.0_WP.and.j.eq.pg%jmin) isIn=.true.
   end function inj1

   !> Function that localizes injector at +y
   function inj2(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      real(WP) ::hyp,rise,run
      logical :: isIn
      isIn=.false.
      ! Check injector x-z plane
      hyp =norm2([pg%xm(i),pg%ym(j),pg%zm(k)]-[xinj_dist,0.0_WP,0.0_WP])
      rise=abs(pg%ym(j))
      run =sqrt(hyp**2-rise**2)
      if (run.le.inj_norm_diam/2.0_WP.and.j.eq.pg%jmax+1) isIn=.true.
   end function inj2

   !> Function that localizes injector at -z
   function inj3(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      real(WP) ::hyp,rise,run
      logical :: isIn
      isIn=.false.
      ! Check injector x-y plane
      hyp =norm2([pg%xm(i),pg%ym(j),pg%zm(k)]-[xinj_dist,0.0_WP,0.0_WP])
      rise=abs(pg%zm(k))
      run =sqrt(hyp**2-rise**2)
      if (run.le.inj_norm_diam/2.0_WP.and.k.eq.pg%kmin) isIn=.true.
   end function inj3

   !> Function that localizes injector at +z
   function inj4(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      real(WP) ::hyp,rise,run
      logical :: isIn
      isIn=.false.
      ! Check injector x-y plane
      hyp =norm2([pg%xm(i),pg%ym(j),pg%zm(k)]-[xinj_dist,0.0_WP,0.0_WP] )
      rise=abs(pg%zm(k))
      run =sqrt(hyp**2-rise**2)
      if (run.le.inj_norm_diam/2.0_WP.and.k.eq.pg%kmax+1) isIn=.true.
   end function inj4

   !> Function that localizes liquid stream at -x
   function liq_inj(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      real(WP) :: rad
      isIn=.false.
      rad=sqrt(pg%ym(j)**2+pg%zm(k)**2)
      if (rad.lt.rli0.and.i.eq.pg%imin) isIn=.true.
   end function liq_inj

   !> Function that localizes gas stream at -x
   function gas_inj(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      real(WP) :: rad
      isIn=.false.
      rad=sqrt(pg%ym(j)**2+pg%zm(k)**2)
      if (rad.ge.rlo0.and.rad.lt.rgi0.and.i.eq.pg%imin) isIn=.true.
   end function gas_inj

   !> Function that localizes the top (y+) of the domain
   function yp_locator(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      implicit none
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      isIn=.false.
      if (j.eq.pg%jmax+1) isIn=.true.
   end function yp_locator

   !> Function that localizes the bottom (y-) of the domain
   function ym_locator(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      implicit none
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      isIn=.false.
      if (j.eq.pg%jmin) isIn=.true.
   end function ym_locator

   !> Function that localizes the top (z+) of the domain
   function zp_locator(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      implicit none
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      isIn=.false.
      if (k.eq.pg%kmax+1) isIn=.true.
   end function zp_locator

   !> Function that localizes the bottom (z-) of the domain
   function zm_locator(pg,i,j,k) result(isIn)
      use pgrid_class, only: pgrid
      implicit none
      class(pgrid), intent(in) :: pg
      integer, intent(in) :: i,j,k
      logical :: isIn
      isIn=.false.
      if (k.eq.pg%kmin) isIn=.true.
   end function zm_locator

   !> Initialization of problem solver
   subroutine simulation_init
      use param, only: param_read
      implicit none

      ! Handle restart/saves here
      restart_and_save: block
         character(len=str_medium) :: dir_restart
         ! CAREFUL - WE NEED TO CREATE THE TIMETRACKER BEFORE THE EVENT !
         time2=timetracker(cfg2%amRoot,name='nozzle_exterior')
         ! Create event for saving restart files
         save_evt=event(time2,'Restart output')
         call param_read('Restart output period',save_evt%tper)
         ! Check if we are restarting
         call param_read(tag='Restart from',val=dir_restart,short='r',default='')
         restarted=.false.; if (len_trim(dir_restart).gt.0) restarted=.true.
         if (restarted) then
            ! If we are, read the name of the directory
            call param_read('Restart from',dir_restart,'r')
            ! Read the two datafiles and the name of the IRL file to read later
            df1=datafile(pg=cfg1,fdata=trim(adjustl(dir_restart))//'/'//'data.1')
            df2=datafile(pg=cfg2,fdata=trim(adjustl(dir_restart))//'/'//'data.2')
            irl_file=trim(adjustl(dir_restart))//'/'//'data.irl'
         else
            ! If we are not restarting, we will still need datafiles for saving restart files
            df1=datafile(pg=cfg1,filename=trim(cfg1%name),nval=2,nvar=9)
            df1%valname(1)='t'
            df1%valname(2)='dt'
            df1%varname(1)='U'
            df1%varname(2)='V'
            df1%varname(3)='W'
            df1%varname(4)='P'
            df1%varname(5)='Uold'
            df1%varname(6)='Vold'
            df1%varname(7)='Wold'
            df1%varname(8)='LM'
            df1%varname(9)='MM'
            df2=datafile(pg=cfg2,filename=trim(cfg2%name),nval=2,nvar=10)
            df2%valname(1)='t'
            df2%valname(2)='dt'
            df2%varname(1)='U'
            df2%varname(2)='V'
            df2%varname(3)='W'
            df2%varname(4)='P'
            df2%varname(5)='Pjx'
            df2%varname(6)='Pjy'
            df2%varname(7)='Pjz'
            df2%varname(8)='LM'
            df2%varname(9)='MM'
            df2%varname(10)='VF'
         end if
      end block restart_and_save

      ! *****************************************************************************
      ! **************************** INITIALIZE SOLVER 2 ****************************
      ! *****************************************************************************

      ! Allocate work arrays for cfg2
      allocate_work_arrays2: block
         allocate(resU2(cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(resV2(cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(resW2(cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(Ui2  (cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(Vi2  (cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(Wi2  (cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
         allocate(SR2(6,cfg2%imino_:cfg2%imaxo_,cfg2%jmino_:cfg2%jmaxo_,cfg2%kmino_:cfg2%kmaxo_))
      end block allocate_work_arrays2

      ! Initialize time tracker
      initialize_timetracker2: block
         !time2=timetracker(cfg2%amRoot,name='nozzle_exterior')   !< This is moved up for restarts!
         call param_read('2 Max timestep size',time2%dtmax)
         call param_read('2 Max cfl number',time2%cflmax)
         time2%dt=time2%dtmax
         time2%itmax=2
         ! Handle restart
         if (restarted) then
            call df2%pullval(name='t' ,val=time2%t )
            call df2%pullval(name='dt',val=time2%dt)
            time2%told=time2%t-time2%dt
         end if
      end block initialize_timetracker2

      ! Initialize our VOF solver and field
      create_and_initialize_vof2: block
         use vfs_class, only: lvira,r2p
         integer :: i,j,k
         real(WP) :: xloc,rad
         ! Create a VOF solver with LVIRA
         vf2=vfs(cfg=cfg2,reconstruction_method=lvira,name='VOF')
         ! Create a VOF solver with R2P
         !vf2=vfs(cfg=cfg2,reconstruction_method=r2p,name='VOF')
         !vf2%VFflot =1.0e-4_WP !< Enables flotsam removal
         !vf2%VFsheet=1.0e-2_WP !< Enables sheet removal
         ! Initialize to flat interface in liquid needle
         xloc=-0.001_WP !< Interface initially just inside the nozzle
         do k=vf2%cfg%kmino_,vf2%cfg%kmaxo_
            do j=vf2%cfg%jmino_,vf2%cfg%jmaxo_
               do i=vf2%cfg%imino_,vf2%cfg%imaxo_
                  rad=sqrt(vf2%cfg%ym(j)**2+vf2%cfg%zm(k)**2)
                  if (vf2%cfg%xm(i).lt.xloc.and.rad.le.rli0) then
                     vf2%VF(i,j,k)=1.0_WP
                  else
                     vf2%VF(i,j,k)=0.0_WP
                  end if
                  vf2%Lbary(:,i,j,k)=[vf2%cfg%xm(i),vf2%cfg%ym(j),vf2%cfg%zm(k)]
                  vf2%Gbary(:,i,j,k)=[vf2%cfg%xm(i),vf2%cfg%ym(j),vf2%cfg%zm(k)]
               end do
            end do
         end do
         ! Handle restart - using IRL data - tested but not working
         !if (restarted) then
         !   ! Get the IRL interface
         !   call vf2%read_interface(filename=trim(irl_file))
         !   ! Reset moments to guarantee compatibility with interface reconstruction
         !   call vf2%reset_volume_moments()
         !end if
         ! Handle restart - using VF data
         if (restarted) call df2%pullvar(name='VF',var=vf2%VF)
         ! Update the band
         call vf2%update_band()
         ! Perform interface reconstruction from VOF field
         call vf2%build_interface()
         ! Set interface planes at the boundaries
         call vf2%set_full_bcond()
         ! Create discontinuous polygon mesh from IRL interface
         call vf2%polygonalize_interface()
         ! Calculate distance from polygons
         call vf2%distance_from_polygon()
         ! Calculate subcell phasic volumes
         call vf2%subcell_vol()
         ! Calculate curvature
         call vf2%get_curvature()
         ! Reset moments to guarantee compatibility with interface reconstruction
         call vf2%reset_volume_moments()
      end block create_and_initialize_vof2

      ! Create a two-phase flow solver with bconds
      create_solver2: block
         use tpns_class, only: dirichlet,clipped_neumann,neumann
         use ils_class,  only: pcg_pfmg
         use mathtools,  only: Pi
         ! Create a two-phase flow solver
         fs2=tpns(cfg=cfg2,name='Two-phase NS')
         ! Assign constant viscosity to each phase
         call param_read('Liquid dynamic viscosity',fs2%visc_l)
         call param_read('Gas dynamic viscosity'   ,fs2%visc_g)
         ! Assign constant density to each phase
         call param_read('Liquid density',fs2%rho_l)
         call param_read('Gas density'   ,fs2%rho_g)
         ! Read in surface tension coefficient
         call param_read('Surface tension coefficient',fs2%sigma)
         call param_read('Static contact angle',fs2%contact_angle)
         fs2%contact_angle=fs2%contact_angle*Pi/180.0_WP
         ! Define direction gas/liquid stream boundary conditions
         call fs2%add_bcond(name='gas_inj',type=dirichlet      ,face='x',dir=-1,canCorrect=.false.,locator=gas_inj)
         call fs2%add_bcond(name='liq_inj',type=dirichlet      ,face='x',dir=-1,canCorrect=.false.,locator=liq_inj)
         ! Neumann on the side
         call fs2%add_bcond(name='bc_yp'  ,type=clipped_neumann,face='y',dir=+1,canCorrect=.true. ,locator=yp_locator)
         call fs2%add_bcond(name='bc_ym'  ,type=clipped_neumann,face='y',dir=-1,canCorrect=.true. ,locator=ym_locator)
         call fs2%add_bcond(name='bc_zp'  ,type=clipped_neumann,face='z',dir=+1,canCorrect=.true. ,locator=zp_locator)
         call fs2%add_bcond(name='bc_zm'  ,type=clipped_neumann,face='z',dir=-1,canCorrect=.true. ,locator=zm_locator)
         ! Outflow on the right
         call fs2%add_bcond(name='outflow',type=clipped_neumann,face='x',dir=+1,canCorrect=.true. ,locator=right_boundary)
         ! Configure pressure solver
         call param_read('Pressure iteration',fs2%psolv%maxit)
         call param_read('Pressure tolerance',fs2%psolv%rcvg)
         ! Configure implicit velocity solver
         call param_read('Implicit iteration',fs2%implicit%maxit)
         call param_read('Implicit tolerance',fs2%implicit%rcvg)
         ! Setup the solver
         fs2%psolv%maxlevel=16
         call fs2%setup(pressure_ils=pcg_pfmg,implicit_ils=pcg_pfmg)
      end block create_solver2

      ! Initialize our velocity field
      initialize_velocity2: block
         use mathtools,  only: pi
         use tpns_class, only: bcond
         type(bcond), pointer :: mybc
         integer  :: n,i,j,k
         real(WP) :: rad,Uports
         real(WP) :: Q_SLPM,Q_SI,Aport
         real(WP), parameter :: SLPM2SI=1.66667E-5_WP
         ! Zero initial field
         fs2%U=0.0_WP; fs2%V=0.0_WP; fs2%W=0.0_WP
         ! Handle restart
         if (restarted) then
            call df2%pullvar(name='U'  ,var=fs2%U  )
            call df2%pullvar(name='V'  ,var=fs2%V  )
            call df2%pullvar(name='W'  ,var=fs2%W  )
            call df2%pullvar(name='P'  ,var=fs2%P  )
            call df2%pullvar(name='Pjx',var=fs2%Pjx)
            call df2%pullvar(name='Pjy',var=fs2%Pjy)
            call df2%pullvar(name='Pjz',var=fs2%Pjz)
         end if
         ! Apply Dirichlet at liquid injector port
         call param_read('Liquid flow rate (SLPM)',Q_SLPM)
         Q_SI=Q_SLPM*SLPM2SI
         Aport=pi*rli0**2
         Uports=Q_SI/Aport
         call fs2%get_bcond('liq_inj',mybc)
         do n=1,mybc%itr%no_
            i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
            fs2%U(i,j,k)=Uports
         end do
         ! Apply all other boundary conditions
         call fs2%apply_bcond(time2%t,time2%dt)
         ! Compute MFR through all boundary conditions
         call fs2%get_mfr()
         ! Adjust MFR for global mass balance
         call fs2%correct_mfr()
         ! Compute cell-centered velocity
         call fs2%interp_vel(Ui2,Vi2,Wi2)
         ! Compute divergence
         call fs2%get_div()
      end block initialize_velocity2

      ! Create an LES model
      create_sgs2: block
         sgs2=sgsmodel(cfg=fs2%cfg,umask=fs2%umask,vmask=fs2%vmask,wmask=fs2%wmask)
         ! Handle restart
         if (restarted) then
            call df2%pullvar(name='LM',var=sgs2%LM)
            call df2%pullvar(name='MM',var=sgs2%MM)
         end if
      end block create_sgs2

      ! Add Ensight output
      create_ensight2: block
         ! Create Ensight output from cfg2
         ens_out2=ensight(cfg2,'atom')
         ! Create event for Ensight output
         ens_evt2=event(time2,'Ensight output')
         call param_read('2 Ensight output period',ens_evt2%tper)
         ! Add variables to output
         call ens_out2%add_vector('velocity',Ui2,Vi2,Wi2)
         call ens_out2%add_scalar('VOF',vf2%VF)
         call ens_out2%add_scalar('curvature',vf2%curv)
         call ens_out2%add_scalar('visc_t',sgs2%visc)
         ! Output to ensight
         if (ens_evt2%occurs()) call ens_out2%write_data(time2%t)
      end block create_ensight2

      ! Create a monitor file
      create_monitor2: block
         ! Prepare some info about fields
         call fs2%get_cfl(time2%dt,time2%cfl)
         call fs2%get_max()
         call vf2%get_max()
         ! Create simulation monitor
         mfile2=monitor(fs2%cfg%amRoot,'simulation2')
         call mfile2%add_column(time2%n,'Timestep number')
         call mfile2%add_column(time2%t,'Time')
         call mfile2%add_column(time2%dt,'Timestep size')
         call mfile2%add_column(time2%cfl,'Maximum CFL')
         call mfile2%add_column(fs2%Umax,'Umax')
         call mfile2%add_column(fs2%Vmax,'Vmax')
         call mfile2%add_column(fs2%Wmax,'Wmax')
         call mfile2%add_column(fs2%Pmax,'Pmax')
         call mfile2%add_column(vf2%VFmax,'VOF maximum')
         call mfile2%add_column(vf2%VFmin,'VOF minimum')
         call mfile2%add_column(vf2%VFint,'VOF integral')
         call mfile2%add_column(fs2%divmax,'Maximum divergence')
         call mfile2%add_column(fs2%psolv%it,'Pressure iteration')
         call mfile2%add_column(fs2%psolv%rerr,'Pressure error')
         call mfile2%write()
         ! Create CFL monitor
         cflfile2=monitor(fs2%cfg%amRoot,'cfl2')
         call cflfile2%add_column(time2%n,'Timestep number')
         call cflfile2%add_column(time2%t,'Time')
         call cflfile2%add_column(fs2%CFLc_x,'Convective xCFL')
         call cflfile2%add_column(fs2%CFLc_y,'Convective yCFL')
         call cflfile2%add_column(fs2%CFLc_z,'Convective zCFL')
         call cflfile2%add_column(fs2%CFLv_x,'Viscous xCFL')
         call cflfile2%add_column(fs2%CFLv_y,'Viscous yCFL')
         call cflfile2%add_column(fs2%CFLv_z,'Viscous zCFL')
         call cflfile2%write()
      end block create_monitor2

      ! *****************************************************************************
      ! **************************** INITIALIZE SOLVER 1 ****************************
      ! *****************************************************************************

      ! Allocate work arrays for cfg1 solver
      allocate_work_arrays1: block
         allocate(resU1(cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(resV1(cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(resW1(cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(Ui1  (cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(Vi1  (cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(Wi1  (cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
         allocate(SR1(6,cfg1%imino_:cfg1%imaxo_,cfg1%jmino_:cfg1%jmaxo_,cfg1%kmino_:cfg1%kmaxo_))
      end block allocate_work_arrays1

      ! Initialize time tracker
      initialize_timetracker1: block
         time1=timetracker(cfg1%amRoot,name='nozzle_interior')
         call param_read('1 Max timestep size',time1%dtmax)
         call param_read('1 Max cfl number',time1%cflmax)
         time1%dt=time1%dtmax
         time1%itmax=2
         ! Handle restart
         if (restarted) then
            call df1%pullval(name='t' ,val=time1%t )
            call df1%pullval(name='dt',val=time1%dt)
            time1%told=time1%t-time1%dt
         end if
      end block initialize_timetracker1

      ! Create an incompressible flow solver with bconds
      create_solver1: block
         use incomp_class, only: dirichlet,clipped_neumann
         use ils_class,    only: pcg_pfmg,pcg_amg
         ! Create flow solver
         fs1=incomp(cfg=cfg1,name='Incompressible NS')
         ! Set the flow properties
         fs1%rho=fs2%rho_g
         fs1%visc=fs2%visc_g
         ! Define gas port boundary conditions
         call fs1%add_bcond(name='inj1',type=dirichlet,face='y',dir=-1,canCorrect=.false.,locator=inj1)
         call fs1%add_bcond(name='inj2',type=dirichlet,face='y',dir=+1,canCorrect=.false.,locator=inj2)
         call fs1%add_bcond(name='inj3',type=dirichlet,face='z',dir=-1,canCorrect=.false.,locator=inj3)
         call fs1%add_bcond(name='inj4',type=dirichlet,face='z',dir=+1,canCorrect=.false.,locator=inj4)
         ! Outflow on the right
         call fs1%add_bcond(name='outflow',type=clipped_neumann,face='x',dir=+1,canCorrect=.true. ,locator=right_boundary)
         ! Configure pressure solver
         call param_read('Pressure iteration',fs1%psolv%maxit)
         call param_read('Pressure tolerance',fs1%psolv%rcvg)
         ! Configure implicit velocity solver
         call param_read('Implicit iteration',fs1%implicit%maxit)
         call param_read('Implicit tolerance',fs1%implicit%rcvg)
         ! Setup the solver
         fs1%psolv%maxlevel=16
         call fs1%setup(pressure_ils=pcg_amg,implicit_ils=pcg_pfmg)
      end block create_solver1

      ! Initialize our velocity field
      initialize_velocity1: block
         use mathtools, only: pi
         use incomp_class, only: bcond
         type(bcond), pointer :: mybc
         integer  :: n,i,j,k
         real(WP) :: rad,Uports
         real(WP) :: Q_SLPM,Q_SI,Aport
         real(WP), parameter :: SLPM2SI=1.66667E-5_WP
         ! Zero initial field
         fs1%U=0.0_WP; fs1%V=0.0_WP; fs1%W=0.0_WP
         ! Handle restart
         if (restarted) then
            call df1%pullvar(name='U'   ,var=fs1%U   )
            call df1%pullvar(name='V'   ,var=fs1%V   )
            call df1%pullvar(name='W'   ,var=fs1%W   )
            call df1%pullvar(name='P'   ,var=fs1%P   )
            call df1%pullvar(name='Uold',var=fs1%Uold)
            call df1%pullvar(name='Vold',var=fs1%Vold)
            call df1%pullvar(name='Wold',var=fs1%Wold)
         end if
         ! Read in mean gas
         call param_read('Gas flow rate (SLPM)',Q_SLPM)
         Q_SI=Q_SLPM*SLPM2SI
         Aport=pi/4.0_WP*inj_norm_diam**2
         Uports=Q_SI/(4.0_WP*Aport) ! fix here conversion
         ! Apply Dirichlet at 4 injector ports
         call fs1%get_bcond('inj1',mybc)
         do n=1,mybc%itr%no_
            i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
            fs1%V(i,j,k)=+Uports
         end do
         call fs1%get_bcond('inj2',mybc)
         do n=1,mybc%itr%no_
            i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
            fs1%V(i,j,k)=-Uports
         end do
         call fs1%get_bcond('inj3',mybc)
         do n=1,mybc%itr%no_
            i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
            fs1%W(i,j,k)=+Uports
         end do
         call fs1%get_bcond('inj4',mybc)
         do n=1,mybc%itr%no_
            i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
            fs1%W(i,j,k)=-Uports
         end do
         ! Apply all other boundary conditions
         call fs1%apply_bcond(time1%t,time1%dt)
         ! Compute MFR through all boundary conditions
         call fs1%get_mfr()
         ! Adjust MFR for global mass balance
         call fs1%correct_mfr()
         ! Compute cell-centered velocity
         call fs1%interp_vel(Ui1,Vi1,Wi1)
         ! Compute divergence
         call fs1%get_div()
      end block initialize_velocity1

      ! Create an LES model
      create_sgs1: block
         sgs1=sgsmodel(cfg=fs1%cfg,umask=fs1%umask,vmask=fs1%vmask,wmask=fs1%wmask)
         ! Handle restart
         if (restarted) then
            call df1%pullvar(name='LM',var=sgs1%LM)
            call df1%pullvar(name='MM',var=sgs1%MM)
         end if
      end block create_sgs1

      ! Add Ensight output
      create_ensight1: block
         ! Create Ensight output from cfg
         ens_out1=ensight(cfg1,'nozzle')
         ! Create event for Ensight output
         ens_evt1=event(time1,'Ensight output')
         call param_read('1 Ensight output period',ens_evt1%tper)
         ! Add variables to output
         call ens_out1%add_vector('velocity',Ui1,Vi1,Wi1)
         call ens_out1%add_scalar('visc_t',sgs1%visc)
         ! Output to ensight
         if (ens_evt1%occurs()) call ens_out1%write_data(time1%t)
      end block create_ensight1

      ! Create a monitor file
      create_monitor1: block
         ! Prepare some info about fields
         call fs1%get_cfl(time1%dt,time1%cfl)
         call fs1%get_max()
         ! Create simulation monitor
         mfile1=monitor(fs1%cfg%amRoot,'simulation1')
         call mfile1%add_column(time1%n,'Timestep number')
         call mfile1%add_column(time1%t,'Time')
         call mfile1%add_column(time1%dt,'Timestep size')
         call mfile1%add_column(time1%cfl,'Maximum CFL')
         call mfile1%add_column(fs1%Umax,'Umax')
         call mfile1%add_column(fs1%Vmax,'Vmax')
         call mfile1%add_column(fs1%Wmax,'Wmax')
         call mfile1%add_column(fs1%Pmax,'Pmax')
         call mfile1%add_column(fs1%divmax,'Maximum divergence')
         call mfile1%add_column(fs1%psolv%it,'Pressure iteration')
         call mfile1%add_column(fs1%psolv%rerr,'Pressure error')
         call mfile1%write()
         ! Create CFL monitor
         cflfile1=monitor(fs1%cfg%amRoot,'cfl1')
         call cflfile1%add_column(time1%n,'Timestep number')
         call cflfile1%add_column(time1%t,'Time')
         call cflfile1%add_column(fs1%CFLc_x,'Convective xCFL')
         call cflfile1%add_column(fs1%CFLc_y,'Convective yCFL')
         call cflfile1%add_column(fs1%CFLc_z,'Convective zCFL')
         call cflfile1%add_column(fs1%CFLv_x,'Viscous xCFL')
         call cflfile1%add_column(fs1%CFLv_y,'Viscous yCFL')
         call cflfile1%add_column(fs1%CFLv_z,'Viscous zCFL')
         call cflfile1%write()
      end block create_monitor1

      ! ###############################################
      ! ######## PREPARE SOLVER COUPLING HERE #########
      ! ###############################################
      coupler_prep: block
         use parallel, only: group
         cpl12x=coupler(src_grp=group,dst_grp=group,name='nozzle_to_atom_x'); call cpl12x%set_src(cfg1,'x'); call cpl12x%set_dst(cfg2,'x'); call cpl12x%initialize()
         cpl12y=coupler(src_grp=group,dst_grp=group,name='nozzle_to_atom_y'); call cpl12y%set_src(cfg1,'y'); call cpl12y%set_dst(cfg2,'y'); call cpl12y%initialize()
         cpl12z=coupler(src_grp=group,dst_grp=group,name='nozzle_to_atom_z'); call cpl12z%set_src(cfg1,'z'); call cpl12z%set_dst(cfg2,'z'); call cpl12z%initialize()
      end block coupler_prep

   end subroutine simulation_init

   !> Perform an NGA2 simulation
   subroutine simulation_run
      use parallel,   only: MPI_REAL_WP
      use mpi_f08,    only: MPI_ALLREDUCE,MPI_SUM
      use tpns_class, only: bcond,static_contact
      implicit none
      integer :: n,i,j,k,ierr
      type(bcond), pointer :: mybc
      character(len=str_medium) :: dirname,timestamp

      ! Perform time integration - the second solver is the main driver here
      do while (.not.time2%done())

         ! Increment time
         call fs2%get_cfl(time2%dt,time2%cfl)
         call time2%adjust_dt()
         call time2%increment()

         ! ###############################################
         ! ############ ADVANCE SOLVER 1 HERE ############
         ! ###############################################

         ! Advance until we've caught up
         do while (time1%t.lt.time2%t)

            ! Increment time
            call fs1%get_cfl(time1%dt,time1%cfl)
            call time1%adjust_dt()
            call time1%increment()

            ! Remember old velocity
            fs1%Uold=fs1%U
            fs1%Vold=fs1%V
            fs1%Wold=fs1%W

            ! Apply time-varying Dirichlet conditions
            ! This is where time-dpt Dirichlet would be enforced

            ! Reset here fluid properties
            fs1%visc=fs2%visc_g

            ! Turbulence modeling
            call fs1%get_strainrate(Ui=Ui1,Vi=Vi1,Wi=Wi1,SR=SR1)
            resU1=fs1%rho
            call sgs1%get_visc(dt=time1%dtold,rho=resU1,Ui=Ui1,Vi=Vi1,Wi=Wi1,SR=SR1)
            where (sgs1%visc.lt.-fs1%visc)
               sgs1%visc=-fs1%visc
            end where
            fs1%visc=fs1%visc+sgs1%visc

            ! Perform sub-iterations
            do while (time1%it.le.time1%itmax)

               ! Build mid-time velocity
               fs1%U=0.5_WP*(fs1%U+fs1%Uold)
               fs1%V=0.5_WP*(fs1%V+fs1%Vold)
               fs1%W=0.5_WP*(fs1%W+fs1%Wold)

               ! Explicit calculation of drho*u/dt from NS
               call fs1%get_dmomdt(resU1,resV1,resW1)

               ! Assemble explicit residual
               resU1=-2.0_WP*(fs1%rho*fs1%U-fs1%rho*fs1%Uold)+time1%dt*resU1
               resV1=-2.0_WP*(fs1%rho*fs1%V-fs1%rho*fs1%Vold)+time1%dt*resV1
               resW1=-2.0_WP*(fs1%rho*fs1%W-fs1%rho*fs1%Wold)+time1%dt*resW1

               ! Form implicit residuals
               call fs1%solve_implicit(time1%dt,resU1,resV1,resW1)

               ! Apply these residuals
               fs1%U=2.0_WP*fs1%U-fs1%Uold+resU1
               fs1%V=2.0_WP*fs1%V-fs1%Vold+resV1
               fs1%W=2.0_WP*fs1%W-fs1%Wold+resW1

               ! Apply other boundary conditions on the resulting fields
               call fs1%apply_bcond(time1%t,time1%dt)

               ! Solve Poisson equation
               call fs1%correct_mfr()
               call fs1%get_div()
               fs1%psolv%rhs=-fs1%cfg%vol*fs1%div*fs1%rho/time1%dt
               fs1%psolv%sol=0.0_WP
               call fs1%psolv%solve()

               ! Correct velocity
               call fs1%get_pgrad(fs1%psolv%sol,resU1,resV1,resW1)
               fs1%P=fs1%P+fs1%psolv%sol
               fs1%U=fs1%U-time1%dt*resU1/fs1%rho
               fs1%V=fs1%V-time1%dt*resV1/fs1%rho
               fs1%W=fs1%W-time1%dt*resW1/fs1%rho

               ! Increment sub-iteration counter
               time1%it=time1%it+1

            end do

            ! Recompute interpolated velocity and divergence
            call fs1%interp_vel(Ui1,Vi1,Wi1)
            call fs1%get_div()

            ! Output to ensight
            if (ens_evt1%occurs()) call ens_out1%write_data(time1%t)

            ! Perform and output monitoring
            call fs1%get_max()
            call mfile1%write()
            call cflfile1%write()

         end do

         ! ###############################################
         ! ######## PERFORM ONE-WAY COUPLING HERE ########
         ! ###############################################
         do_coupling: block
            real(WP) :: wt
            ! At this point, we have ensured that t1>t2, therefore t2 is in [t1old,t1], so interpolate solver 1's velocity in time to t2
            wt=(time2%t-time1%told)/time1%dt
            resU1=wt*fs1%U+(1.0_WP-wt)*fs1%Uold
            resV1=wt*fs1%V+(1.0_WP-wt)*fs1%Vold
            resW1=wt*fs1%W+(1.0_WP-wt)*fs1%Wold
            ! Exchange data using cpl12x/y/z couplers
            call cpl12x%push(resU1); call cpl12x%transfer(); call cpl12x%pull(resU2)
            call cpl12y%push(resV1); call cpl12y%transfer(); call cpl12y%pull(resV2)
            call cpl12z%push(resW1); call cpl12z%transfer(); call cpl12z%pull(resW2)
            ! Apply time-varying Dirichlet conditions
            call fs2%get_bcond('gas_inj',mybc)
            do n=1,mybc%itr%no_
               i=mybc%itr%map(1,n); j=mybc%itr%map(2,n); k=mybc%itr%map(3,n)
               fs2%U(i  ,j,k)=resU2(i,j,k)
               fs2%V(i-1,j,k)=resV2(i,j,k)
               fs2%W(i-1,j,k)=resW2(i,j,k)
            end do
         end block do_coupling

         ! ###############################################
         ! ############ ADVANCE SOLVER 2 HERE ############
         ! ###############################################

         ! Remember old VOF
         vf2%VFold=vf2%VF

         ! Remember old velocity
         fs2%Uold=fs2%U
         fs2%Vold=fs2%V
         fs2%Wold=fs2%W

         ! Prepare old staggered density (at n)
         call fs2%get_olddensity(vf=vf2)

         ! VOF solver step
         call vf2%advance(dt=time2%dt,U=fs2%U,V=fs2%V,W=fs2%W)

         ! Prepare new staggered viscosity (at n+1)
         call fs2%get_viscosity(vf=vf2)

         ! Turbulence modeling - only work with gas properties here
         call fs2%get_strainrate(Ui=Ui2,Vi=Vi2,Wi=Wi2,SR=SR2)
         resU2=fs2%rho_g
         call sgs2%get_visc(dt=time2%dtold,rho=resU2,Ui=Ui2,Vi=Vi2,Wi=Wi2,SR=SR2)
         where (sgs2%visc.lt.-fs2%visc_g)
            sgs2%visc=-fs2%visc_g
         end where
         do k=fs2%cfg%kmino_+1,fs2%cfg%kmaxo_
            do j=fs2%cfg%jmino_+1,fs2%cfg%jmaxo_
               do i=fs2%cfg%imino_+1,fs2%cfg%imaxo_
                  fs2%visc(i,j,k)   =fs2%visc(i,j,k)   +sgs2%visc(i,j,k)
                  fs2%visc_xy(i,j,k)=fs2%visc_xy(i,j,k)+sum(fs2%itp_xy(:,:,i,j,k)*sgs2%visc(i-1:i,j-1:j,k))
                  fs2%visc_yz(i,j,k)=fs2%visc_yz(i,j,k)+sum(fs2%itp_yz(:,:,i,j,k)*sgs2%visc(i,j-1:j,k-1:k))
                  fs2%visc_zx(i,j,k)=fs2%visc_zx(i,j,k)+sum(fs2%itp_xz(:,:,i,j,k)*sgs2%visc(i-1:i,j,k-1:k))
               end do
            end do
         end do

         ! Perform sub-iterations
         do while (time2%it.le.time2%itmax)

            ! Build mid-time velocity
            fs2%U=0.5_WP*(fs2%U+fs2%Uold)
            fs2%V=0.5_WP*(fs2%V+fs2%Vold)
            fs2%W=0.5_WP*(fs2%W+fs2%Wold)

            ! Preliminary mass and momentum transport step at the interface
            call fs2%prepare_advection_upwind(dt=time2%dt)

            ! Explicit calculation of drho*u/dt from NS
            call fs2%get_dmomdt(resU2,resV2,resW2)

            ! Assemble explicit residual
            resU2=-2.0_WP*fs2%rho_U*fs2%U+(fs2%rho_Uold+fs2%rho_U)*fs2%Uold+time2%dt*resU2
            resV2=-2.0_WP*fs2%rho_V*fs2%V+(fs2%rho_Vold+fs2%rho_V)*fs2%Vold+time2%dt*resV2
            resW2=-2.0_WP*fs2%rho_W*fs2%W+(fs2%rho_Wold+fs2%rho_W)*fs2%Wold+time2%dt*resW2

            ! Form implicit residuals
            call fs2%solve_implicit(time2%dt,resU2,resV2,resW2)

            ! Apply these residuals
            fs2%U=2.0_WP*fs2%U-fs2%Uold+resU2
            fs2%V=2.0_WP*fs2%V-fs2%Vold+resV2
            fs2%W=2.0_WP*fs2%W-fs2%Wold+resW2

            ! Apply other boundary conditions
            call fs2%apply_bcond(time2%t,time2%dt)

            ! Solve Poisson equation
            call fs2%update_laplacian()
            call fs2%correct_mfr()
            call fs2%get_div()
            call fs2%add_surface_tension_jump(dt=time2%dt,div=fs2%div,vf=vf2,contact_model=static_contact)
            fs2%psolv%rhs=-fs2%cfg%vol*fs2%div/time2%dt
            fs2%psolv%sol=0.0_WP
            call fs2%psolv%solve()
            call fs2%shift_p(fs2%psolv%sol)

            ! Correct velocity
            call fs2%get_pgrad(fs2%psolv%sol,resU2,resV2,resW2)
            fs2%P=fs2%P+fs2%psolv%sol
            fs2%U=fs2%U-time2%dt*resU2/fs2%rho_U
            fs2%V=fs2%V-time2%dt*resV2/fs2%rho_V
            fs2%W=fs2%W-time2%dt*resW2/fs2%rho_W

            ! Increment sub-iteration counter
            time2%it=time2%it+1

         end do

         ! Recompute interpolated velocity and divergence
         call fs2%interp_vel(Ui2,Vi2,Wi2)
         call fs2%get_div()

         ! Output to ensight
         if (ens_evt2%occurs()) call ens_out2%write_data(time2%t)

         ! Perform and output monitoring
         call fs2%get_max()
         call vf2%get_max()
         call mfile2%write()
         call cflfile2%write()

         ! After we're done clip all VOF at the exit area and along the sides
         do k=fs2%cfg%kmino_,fs2%cfg%kmaxo_
            do j=fs2%cfg%jmino_,fs2%cfg%jmaxo_
               do i=fs2%cfg%imino_,fs2%cfg%imaxo_
                  if (i.ge.vf2%cfg%imax-5) vf2%VF(i,j,k)=0.0_WP
                  if (j.ge.vf2%cfg%jmax-5) vf2%VF(i,j,k)=0.0_WP
                  if (j.le.vf2%cfg%jmin+5) vf2%VF(i,j,k)=0.0_WP
                  if (k.ge.vf2%cfg%kmax-5) vf2%VF(i,j,k)=0.0_WP
                  if (k.le.vf2%cfg%kmin+5) vf2%VF(i,j,k)=0.0_WP
               end do
            end do
         end do

         ! Finally, see if it's time to save restart files
         if (save_evt%occurs()) then
            ! Prefix for files
            dirname='restart_'; write(timestamp,'(es12.5)') time2%t
            ! Prepare a new directory
            if (fs1%cfg%amRoot) call execute_command_line('mkdir -p '//trim(adjustl(dirname))//trim(adjustl(timestamp)))
            ! Populate df1 and write it
            call df1%pushval(name=   't',val=time1%t )
            call df1%pushval(name=  'dt',val=time1%dt)
            call df1%pushvar(name=   'U',var=fs1%U   )
            call df1%pushvar(name=   'V',var=fs1%V   )
            call df1%pushvar(name=   'W',var=fs1%W   )
            call df1%pushvar(name=   'P',var=fs1%P   )
            call df1%pushvar(name='Uold',var=fs1%Uold)
            call df1%pushvar(name='Vold',var=fs1%Vold)
            call df1%pushvar(name='Wold',var=fs1%Wold)
            call df1%pushvar(name=  'LM',var=sgs1%LM )
            call df1%pushvar(name=  'MM',var=sgs1%MM )
            call df1%write(fdata=trim(adjustl(dirname))//trim(adjustl(timestamp))//'/'//'data.1')
            ! Populate df2 and write it
            call df2%pushval(name=  't',val=time2%t )
            call df2%pushval(name= 'dt',val=time2%dt)
            call df2%pushvar(name=  'U',var=fs2%U   )
            call df2%pushvar(name=  'V',var=fs2%V   )
            call df2%pushvar(name=  'W',var=fs2%W   )
            call df2%pushvar(name=  'P',var=fs2%P   )
            call df2%pushvar(name='Pjx',var=fs2%Pjx )
            call df2%pushvar(name='Pjy',var=fs2%Pjy )
            call df2%pushvar(name='Pjz',var=fs2%Pjz )
            call df2%pushvar(name= 'LM',var=sgs2%LM )
            call df2%pushvar(name= 'MM',var=sgs2%MM )
            call df2%pushvar(name= 'VF',var=vf2%VF  )
            call df2%write(fdata=trim(adjustl(dirname))//trim(adjustl(timestamp))//'/'//'data.2')
            ! Also output IRL interface
            call vf2%write_interface(filename=trim(adjustl(dirname))//trim(adjustl(timestamp))//'/'//'data.irl')
         end if

      end do

   end subroutine simulation_run

   !> Finalize the NGA2 simulation
   subroutine simulation_final
      implicit none

      ! Get rid of all objects - need destructors
      ! monitor
      ! ensight
      ! bcond
      ! timetracker

      ! Deallocate work arrays
      deallocate(resU1,resV1,resW1,Ui1,Vi1,Wi1,SR1)
      deallocate(resU2,resV2,resW2,Ui2,Vi2,Wi2,SR2)

   end subroutine simulation_final

end module simulation


c-----------------------------------------------------------------------
c
c     To do:
c
c        Differing BC's imposed for ophinv, incomprn, etc.
c
c        1-shot Fast solver for Helmholtz and pressure
c
c
c-----------------------------------------------------------------------
      subroutine induct (igeom)
c
c     Solve the convection-diffusion equation for the B-field, with
c     projection onto a div-free space.
c
c
      include 'SIZE'
      include 'INPUT'
      include 'EIGEN'
      include 'SOLN'
      include 'TSTEP'
      include 'MASS'

      common /scrns/  resv1 (lx1,ly1,lz1,lelv)
     $ ,              resv2 (lx1,ly1,lz1,lelv)
     $ ,              resv3 (lx1,ly1,lz1,lelv)
     $ ,              dv1   (lx1,ly1,lz1,lelv)
     $ ,              dv2   (lx1,ly1,lz1,lelv)
     $ ,              dv3   (lx1,ly1,lz1,lelv)
      common /scrvh/  h1    (lx1,ly1,lz1,lelv)
     $ ,              h2    (lx1,ly1,lz1,lelv)

      ifield = ifldmhd

      if (igeom.eq.1) then  ! old geometry, old velocity

         call makebsource_mhd

      else

         call lagbfield
         call lagvel

         call elsasserh(igeom)

         call vol_flow        ! check for fixed flow rate

      endif
c
      return
      end
c--------------------------------------------------------------------
      subroutine lagbfield
c
c     Keep old B-field(s)
c
      include 'SIZE'
      include 'INPUT'
      include 'SOLN'
      include 'TSTEP'
c
      do ilag=nbdinp-1,2,-1
         call opcopy
     $    ( bxlag(1,ilag  ),bylag(1,ilag  ),bzlag(1,ilag  )
     $    , bxlag(1,ilag-1),bylag(1,ilag-1),bzlag(1,ilag-1) )
      enddo
      call opcopy (bxlag,bylag,bzlag,bx,by,bz)
c
      return
      end
c--------------------------------------------------------------------
      subroutine makebsource_mhd
c
c     Make rhs for induction equation
c
      include 'SIZE'
      include 'SOLN'
      include 'MASS'
      include 'INPUT'
      include 'TSTEP'
      include 'CTIMER'

      if (icalld.eq.0) tbmhd=0.0
      icalld = icalld+1
      nbmhd  = icalld
      etime1 = dnekclock()
c
      ifield = 1
                                      call makeuf
c
      ifield = ifldmhd
                                      call makeufb
      if (ifaxis) then
c        do ifield = 2,3
         do ifield = 2,npscal+1
                                      call makeuq  ! nonlinear terms
         enddo
         ifield = ifldmhd
      endif

      if (ifnav.and.(.not.ifchar)) then
         call advab_elsasser_fast
      endif
      if (ifchar) then
         write(6,*) 'No IFCHAR for MHD, yet.'
         call exitt
      endif

      ifield = 1
      if (iftran)                     call makeabf
                                      call makebdf
c
      ifield = ifldmhd
      if (iftran)                     call makextb
                                      call makebdfb
c
      tbmhd=tbmhd+(dnekclock()-etime1)
      return
      end
c--------------------------------------------------------------------
      subroutine makeufb
c
c     Compute and add: (1) user specified forcing function (FX,FY,FZ)
c
      include 'SIZE'
      include 'SOLN'
      include 'MASS'
      include 'TSTEP'
C
      time = time-dt
      call nekuf   (bmx,bmy,bmz)
      call opcolv2 (bmx,bmy,bmz,vtrans(1,1,1,1,ifield),bm1)
      time = time+dt
c
      return
      end
c--------------------------------------------------------------------
      subroutine makextb
c
c     Add extrapolation terms to magnetic source terms
c
c     (nek5 equivalent for velocity is "makeabf")
c
      include 'SIZE'
      include 'INPUT'
      include 'SOLN'
      include 'MASS'
      include 'TSTEP'
C
      common /scrns/ ta1 (lx1,ly1,lz1,lelv)
     $ ,             ta2 (lx1,ly1,lz1,lelv)
     $ ,             ta3 (lx1,ly1,lz1,lelv)
c
      ntot1 = nx1*ny1*nz1*nelv
c
      ab0 = ab(1)
      ab1 = ab(2)
      ab2 = ab(3)
      call add3s2 (ta1,bbx1,bbx2,ab1,ab2,ntot1)
      call add3s2 (ta2,bby1,bby2,ab1,ab2,ntot1)
      call copy   (bbx2,bbx1,ntot1)
      call copy   (bby2,bby1,ntot1)
      call copy   (bbx1,bmx,ntot1)
      call copy   (bby1,bmy,ntot1)
      call add2s1 (bmx,ta1,ab0,ntot1)
      call add2s1 (bmy,ta2,ab0,ntot1)
      if (ndim.eq.3) then
         call add3s2 (ta3,bbz1,bbz2,ab1,ab2,ntot1)
         call copy   (bbz2,bbz1,ntot1)
         call copy   (bbz1,bmz,ntot1)
         call add2s1 (bmz,ta3,ab0,ntot1)
      endif
c
      return
      end
c--------------------------------------------------------------------
      subroutine makebdfb
C
C     Add contributions to magnetic source from lagged BD terms.
C
      include 'SIZE'
      include 'SOLN'
      include 'MASS'
      include 'GEOM'
      include 'INPUT'
      include 'TSTEP'
C
      COMMON /SCRNS/ TA1(LX1,LY1,LZ1,LELV)
     $ ,             TA2(LX1,LY1,LZ1,LELV)
     $ ,             TA3(LX1,LY1,LZ1,LELV)
     $ ,             TB1(LX1,LY1,LZ1,LELV)
     $ ,             TB2(LX1,LY1,LZ1,LELV)
     $ ,             TB3(LX1,LY1,LZ1,LELV)
     $ ,             H2 (LX1,LY1,LZ1,LELV)
C
      NTOT1 = NX1*NY1*NZ1*NELV
      CONST = 1./DT
      CALL CMULT2(H2,vtrans(1,1,1,1,ifield),CONST,NTOT1)
      CALL OPCOLV3c (TB1,TB2,TB3,BX,BY,BZ,BM1,bd(2))
C
      DO 100 ILAG=2,NBD
         IF (IFGEOM) THEN
            CALL OPCOLV3c(TA1,TA2,TA3,BXLAG (1,ILAG-1),
     $                                BYLAG (1,ILAG-1),
     $                                BZLAG (1,ILAG-1),
     $                                BM1LAG(1,1,1,1,ILAG-1),bd(ilag+1))
         ELSE
            CALL OPCOLV3c(TA1,TA2,TA3,BXLAG (1,ILAG-1),
     $                                BYLAG (1,ILAG-1),
     $                                BZLAG (1,ILAG-1),
     $                                BM1                   ,bd(ilag+1))
         ENDIF
         CALL OPADD2  (TB1,TB2,TB3,TA1,TA2,TA3)
 100  CONTINUE
      CALL OPADD2col (BMX,BMY,BMZ,TB1,TB2,TB3,h2)
c
      return
      end
c--------------------------------------------------------------------
      subroutine cresvib(resv1,resv2,resv3,h1,h2)
c
c     Account for inhomogeneous Dirichlet boundary contributions
c     in rhs of induction eqn.
c                                               n
c     Also, subtract off best estimate of grad p
c
      include 'SIZE'
      include 'TOTAL'
      real           resv1 (lx1,ly1,lz1,1)
      real           resv2 (lx1,ly1,lz1,1)
      real           resv3 (lx1,ly1,lz1,1)
      real           h1    (lx1,ly1,lz1,1)
      real           h2    (lx1,ly1,lz1,1)
      common /scruz/ w1    (lx1,ly1,lz1,lelv)
     $ ,             w2    (lx1,ly1,lz1,lelv)
     $ ,             w3    (lx1,ly1,lz1,lelv)
c
c
      call bcdirvc (bx,by,bz,b1mask,b2mask,b3mask)
      call extrapp (pm,pmlag)
      call opgradt (resv1,resv2,resv3,pm)
      call opadd2  (resv1,resv2,resv3,bmx,bmy,bmz)
c     call opcopy  (resv1,resv2,resv3,bmx,bmy,bmz)
      call ophx    (w1,w2,w3,bx,by,bz,h1,h2)
      call opsub2  (resv1,resv2,resv3,w1,w2,w3)
c
      return
      end
c--------------------------------------------------------------------
      subroutine cresvibp(resv1,resv2,resv3,h1,h2)
c
c     Account for inhomogeneous Dirichlet boundary contributions
c     in rhs of momentum eqn.
c                                               n
c     Also, subtract off best estimate of grad p
c
      include 'SIZE'
      include 'TOTAL'
      real           resv1 (lx1,ly1,lz1,1)
      real           resv2 (lx1,ly1,lz1,1)
      real           resv3 (lx1,ly1,lz1,1)
      real           h1    (lx1,ly1,lz1,1)
      real           h2    (lx1,ly1,lz1,1)
      common /scruz/ w1    (lx1,ly1,lz1,lelv)
     $ ,             w2    (lx1,ly1,lz1,lelv)
     $ ,             w3    (lx1,ly1,lz1,lelv)
c
      call bcdirvc (vx,vy,vz,v1mask,v2mask,v3mask)
      if (ifstrs)  call bcneutr
      call extrapp (pr,prlag)
      call opgradt (resv1,resv2,resv3,pr)
      call opadd2  (resv1,resv2,resv3,bfx,bfy,bfz)
c     call opcopy  (resv1,resv2,resv3,bfx,bfy,bfz)
      call ophx    (w1,w2,w3,vx,vy,vz,h1,h2)
      call opsub2  (resv1,resv2,resv3,w1,w2,w3)
c
      return
      end
c--------------------------------------------------------------------
      subroutine incomprn (ux,uy,uz,up)
c
c     Project U onto the closest incompressible field
c
c     Input:  U     := (ux,uy,uz)
c
c     Output: updated values of U, iproj, proj; and
c             up    := pressure currection req'd to impose div U = 0
c
c
c     Dependencies: ifield ==> which "density" (vtrans) is used.
c
c     Notes  1.  up is _not_ scaled by bd(1)/dt.  This should be done
c                external to incompr().
c
c            2.  up accounts _only_ for the perturbation pressure,
c                not the current pressure derived from extrapolation.
c
c
      include 'SIZE'
      include 'TOTAL'
      include 'CTIMER'
c
      common /scrns/ w1    (lx1,ly1,lz1,lelv)
     $ ,             w2    (lx1,ly1,lz1,lelv)
     $ ,             w3    (lx1,ly1,lz1,lelv)
     $ ,             dv1   (lx1,ly1,lz1,lelv)
     $ ,             dv2   (lx1,ly1,lz1,lelv)
     $ ,             dv3   (lx1,ly1,lz1,lelv)
     $ ,             dp    (lx2,ly2,lz2,lelv)
      common /scrvh/ h1    (lx1,ly1,lz1,lelv)
     $ ,             h2    (lx1,ly1,lz1,lelv)
      common /scrhi/ h2inv (lx1,ly1,lz1,lelv)

      parameter(nset = 1 + lbelv/lelv)
      common /orthov/ pset(lx2*ly2*lz2*lelv*mxprev,nset)
      common /orthbi/ nprv(2)
      logical ifprjp

      ifprjp=.false.    ! Project out previous pressure solutions?
      istart=param(95)
      if (istep.ge.istart.and.istart.ne.0) ifprjp=.true.

      if (icalld.eq.0) tpres=0.0
      icalld = icalld+1
      npres  = icalld
      etime1 = dnekclock()

      ntot1  = nx1*ny1*nz1*nelv
      ntot2  = nx2*ny2*nz2*nelv
      intype = 1

      call rzero   (h1,ntot1)
      call copy    (h2,vtrans(1,1,1,1,ifield),ntot1)
      call invers2 (h2inv,h2,ntot1)

      call opdiv   (dp,ux,uy,uz)

      bdti = -bd(1)/dt
      call cmult   (dp,bdti,ntot2)

      call add2col2(dp,bm2,usrdiv,ntot2) ! User-defined divergence.

      call ortho   (dp)

      i = 1 + ifield/ifldmhd
      if (ifprjp)   call setrhsp  (dp,h1,h2,h2inv,pset(1,i),nprv(i))
                    scaledt = dt/bd(1)
                    scaledi = 1./scaledt
                    call cmult(dp,scaledt,ntot2)        ! scale for tol
                    call esolver  (dp,h1,h2,h2inv,intype)
                    call cmult(dp,scaledi,ntot2)
      if (ifprjp)   call gensolnp (dp,h1,h2,h2inv,pset(1,i),nprv(i))

      call add2(up,dp,ntot2)

      call opgradt  (w1 ,w2 ,w3 ,dp)
      call opbinv   (dv1,dv2,dv3,w1 ,w2 ,w3 ,h2inv)
      dtb  = dt/bd(1)
      call opadd2cm (ux ,uy ,uz ,dv1,dv2,dv3, dtb )

      if (ifmhd)  call chkptol	! to avoid repetition

      tpres=tpres+(dnekclock()-etime1)

      return
      end
c-----------------------------------------------------------------------
      subroutine extrapp(p,plag)
C
C     Pressure extrapolation
C
      INCLUDE 'SIZE'
      INCLUDE 'SOLN'
      INCLUDE 'TSTEP'

      real  p    (lx2,ly2,lz2,1)
     $     ,plag (lx2,ly2,lz2,1)

      common /cgeom/ igeom

      ntot2 = nx2*ny2*nz2*nelv

      if (nbd.eq.3.and.igeom.le.2) then

         const = dtlag(1)/dtlag(2)

         do i=1,ntot2
            pnm1          = p   (i,1,1,1)
            pnm2          = plag(i,1,1,1)
            p   (i,1,1,1) = pnm1 + const*(pnm1-pnm2)
            plag(i,1,1,1) = pnm1
         enddo

      elseif (nbd.gt.3) then
         WRITE (6,*) 'Pressure extrapolation cannot be completed'
         WRITE (6,*) 'Try a lower-order temporal scheme'
         call exitt
      endif
      return
      end
c-----------------------------------------------------------------------
      subroutine opzero(ux,uy,uz)
      include 'SIZE'
      include 'TOTAL'
      real ux(1),uy(1),uz(1)
c
      n = nx1*ny1*nz1*nelfld(ifield)
      call rzero(ux,n)
      call rzero(uy,n)
      if (if3d) call rzero(uz,n)
c
      return
      end
c-----------------------------------------------------------------------
      subroutine opnorm(unorm,ux,uy,uz,type3)
      include 'SIZE'
      include 'TOTAL'
      character*3 type3
      real ux(1),uy(1),uz(1)
      real un(3),wn(3)
c
      n = nx1*ny1*nz1*nelfld(ifield)
      if (type3.eq.'L2 ') then
         if (if3d) then
            un(1) = vlsc3(ux,ux,bm1,n)
            un(2) = vlsc3(uy,uy,bm1,n)
            un(3) = vlsc3(uz,uz,bm1,n)
            un(1) = un(1) + un(2) + un(3)
            unorm = glsum(un(1),1)
            if (unorm.gt.0) unorm = sqrt(unorm/volvm1)
         else
            un(1) = vlsc3(ux,ux,bm1,n)
            un(2) = vlsc3(uy,uy,bm1,n)
            un(1) = un(1) + un(2)
            unorm = glsum(un(1),1)
            if (unorm.gt.0) unorm = sqrt(unorm/volvm1)
         endif
      endif
c
      return
      end
c-----------------------------------------------------------------------
      subroutine lorentz_force (lf,b1,b2,b3,w1,w2)
c
c     Compute Lorentz force
c
c     Input:  B     := (b1,b2,b3)
c
c     Output: lf(1,ldim)
c
c     Work arrays: w1(ltot) and w2(ltot)
c
c     The output will not be continuous.  However, it will be in
c     the form appropriate for incorporation as a body force term
c     in the variational formulation of the Navier-Stokes equations.
c
c     (i.e.,   rhs(NS) = rhs(NS) + B*lf,  where B is the mass matrix)
c
      include 'SIZE'
c
      real lf(lx1*ly1*lz1*lelv,ldim)
      real b1(lx1*ly1*lz1*lelv)
      real b2(lx1*ly1*lz1*lelv)
      real b3(lx1*ly1*lz1*lelv)
c
      call curl(lf,b1,b2,b3,.false.,w1,w2)
c
      ntot = nx1*ny1*nz1*nelv
c
      do i=1,ntot
         c1 = lf(i,2)*b3(i) - lf(i,3)*b2(i)
         c2 = lf(i,3)*b1(i) - lf(i,1)*b3(i)
         c3 = lf(i,1)*b2(i) - lf(i,2)*b1(i)
         lf(i,1) = c1
         lf(i,2) = c2
         lf(i,3) = c3
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine curl(vort,u,v,w,ifavg,work1,work2)
c
      include 'SIZE'
      include 'TOTAL'
c
      logical ifavg
c
      parameter(lt=lx1*ly1*lz1*lelv)
      real vort(lt,3),work1(1),work2(1),u(1),v(1),w(1)
c
      ntot  = nx1*ny1*nz1*nelv
      if (if3d) then
c        work1=dw/dy ; work2=dv/dz
           call dudxyz(work1,w,rym1,sym1,tym1,jacm1,1,2)
           call dudxyz(work2,v,rzm1,szm1,tzm1,jacm1,1,3)
           call sub3(vort(1,1),work1,work2,ntot)
c        work1=du/dz ; work2=dw/dx
           call dudxyz(work1,u,rzm1,szm1,tzm1,jacm1,1,3)
           call dudxyz(work2,w,rxm1,sxm1,txm1,jacm1,1,1)
           call sub3(vort(1,2),work1,work2,ntot)
c        work1=dv/dx ; work2=du/dy
           call dudxyz(work1,v,rxm1,sxm1,txm1,jacm1,1,1)
           call dudxyz(work2,u,rym1,sym1,tym1,jacm1,1,2)
           call sub3(vort(1,3),work1,work2,ntot)
      else
c        work1=dv/dx ; work2=du/dy
           call dudxyz(work1,v,rxm1,sxm1,txm1,jacm1,1,1)
           call dudxyz(work2,u,rym1,sym1,tym1,jacm1,1,2)
           call sub3(vort(1,3),work1,work2,ntot)
      endif
c
c    Avg at bndry
c
      if (ifavg) then
         ifielt = ifield
         ifield = 1
         if (if3d) then
            do idim=1,ndim
               call col2  (vort(1,idim),bm1,ntot)
               call dssum (vort(1,idim),nx1,ny1,nz1)
               call col2  (vort(1,idim),binvm1,ntot)
            enddo
         else
            call col2  (vort(1,3),bm1,ntot)    ! NOTE:  This differs from
            call dssum (vort(1,3),nx1,ny1,nz1) ! "comp_vort", which returns
            call col2  (vort(1,3),binvm1,ntot) ! vorticity as 1st entry in vort
         endif
         ifield = ifielt
      endif
c
      return
      end
c-----------------------------------------------------------------------
      subroutine lorentz_force2(lf,b1,b2,b3)
c
c     Compute Lorentz force
c
c     Input:  B     := (b1,b2,b3)
c
c     Output: lf(1,ldim)
c
c     Work arrays: w1(ltot) and w2(ltot)
c
c     The output will not be continuous.  However, it will be in
c     the form appropriate for incorporation as a body force term
c     in the variational formulation of the Navier-Stokes equations.
c
c     (i.e.,   rhs(NS) = rhs(NS) + B*lf,  where B is the mass matrix)
c
      include 'SIZE'
c
      real lf(lx1*ly1*lz1*ldim,lelt)
      real b1(lx1*ly1*lz1,lelt)
      real b2(lx1*ly1*lz1,lelt)
      real b3(lx1*ly1*lz1,lelt)
c
      integer e
c
      do e = 1,nelt   ! NOTE:  the order is different from v. 1
         call lorentz_force_e(lf(1,e),b1(1,e),b2(1,e),b3(1,e),e)
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine lorentz_force_e(lf,b1,b2,b3,e)
c
c     Compute Lorentz force field for a single element
c
c     Input:  B     := (b1,b2,b3)
c
c     Output: lf(1,ldim)
c
c     Work arrays: cb(lxyzd) and w2(lxyzd)
c
c     The output will not be continuous.  However, it will be in
c     the form appropriate for incorporation as a body force term
c     in the variational formulation of the Navier-Stokes equations.
c     (i.e.,   rhs(NS) = rhs(NS) + B*lf,  where B is the mass matrix)
c
c     Dealiasing strategy:
c
c       e      e  -1   ~ T  ~e   ~ ~
c     lf  = ( B  )     I    B  ( I j x I b )
c       i              ~                    i
c
c            ~
c     Here,  I is the interpolant from N to M, where M = 3/2 N.
c
c            ~                                  -1
c            j is a special curl  (sans Jacobian   )
c
c            b is the B-field on the N-points
c
c            ~e
c            B  is the local mass matrix on the M-points (sans Jacabian)
c
c             e
c            B  is the local mass matrix on the N-points (with Jacabian)
c
c     The last B is req'd to compensate for the subsequent multiplication
c     by B that takes place once the rhs is formed.
c
c
c
      include 'SIZE'
      include 'DEALIAS'
      include 'GEOM'
      include 'WZ'
c
      real lf(lx1*ly1*lz1,3)
      real b1(lx1*ly1*lz1)
      real b2(lx1*ly1*lz1)
      real b3(lx1*ly1*lz1)
      integer d,e
c
      integer icalld
      save    icalld
      data    icalld /0/
c
      common /ctmp1x/ lfd(lxd*lyd*lzd,3)
     $             ,  bd (lxd*lyd*lzd,3)
     $             ,  cb (lx1*ly1*lz1,3)
     $             ,  cbd(lxd*lyd*lzd,3)
      real lfd

      if (icalld .eq. 0) then
          write(6,*) 'CALL SET PROJ',nx1,nxd
          call setmap (nx1,nxd)   ! Set up interpolation operators
          call setproj(nx1,nxd)   ! Set up interpolation operators
          icalld = icalld + 1
      endif
c
      call spec_curl_e (cb,b1,b2,b3                  !local curl, w/o Jacobian
     $                 , rxm1(1,1,1,e),rym1(1,1,1,e),rzm1(1,1,1,e)
     $                 , sxm1(1,1,1,e),sym1(1,1,1,e),szm1(1,1,1,e)
     $                 , txm1(1,1,1,e),tym1(1,1,1,e),tzm1(1,1,1,e) )
c
      do d=1,ndim                                   ! interpolate to M points
         call specmp(cbd(1,d),nxd,cb(1,d),nx1,im1d,im1dt,bd)
      enddo
      call specmp(bd(1,1),nxd,b1,nx1,im1d,im1dt,lfd)
      call specmp(bd(1,2),nxd,b2,nx1,im1d,im1dt,lfd)
      call specmp(bd(1,3),nxd,b3,nx1,im1d,im1dt,lfd)
c
      nxyzd = nxd*nyd*nzd
      do i=1,nxyzd
         lfd(i,1) = cbd(i,2)*bd(i,3) - cbd(i,3)*bd(i,2) ! Curl B x B
         lfd(i,2) = cbd(i,3)*bd(i,1) - cbd(i,1)*bd(i,3)
         lfd(i,3) = cbd(i,1)*bd(i,2) - cbd(i,2)*bd(i,1)
      enddo
c
c     Project back and simultaneous collocate with local quadrature weights
c
c                                 ~        ~        ~
c        P := Pz x Py x Px  =  Iz*B  x  Iy*B  x  Ix*B
c
c
c           ~            M
c     where B = diag (rho  )
c                        i
c
      call specmp(lf(1,1),nx1,lfd(1,1),nxd,pmd1,pmd1t,cbd)
      call specmp(lf(1,2),nx1,lfd(1,2),nxd,pmd1,pmd1t,cbd)
      call specmp(lf(1,3),nx1,lfd(1,3),nxd,pmd1,pmd1t,cbd)
c
c     Finally, divide by local mass matrix in anticipation of subsequent
c     multiply by BM1.
c
      nxyz = nx1*ny1*nz1
      do i=1,nxyz
c        scale = 1./(w3m1(i,1,1)*jacm1(i,1,1,e))
         scale = 1./jacm1(i,1,1,e)
         lf(i,1) = scale*lf(i,1)
         lf(i,2) = scale*lf(i,2)
         lf(i,3) = scale*lf(i,3)
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine spec_curl_e (cb,b1,b2,b3,rx,ry,rz,sx,sy,sz,tx,ty,tz)
c
c     local curl, multiplied by Jacobian
c
      include 'SIZE'
      include 'DXYZ'
c
      real cb(lx1*ly1*lz1,3)  ! Output J*curl B  (J:=Jacobian)
      real b1(1),b2(1),b3(1)  ! Input B-field
c
      real rx(1),ry(1),rz(1)  ! Metrics
      real sx(1),sy(1),sz(1)
      real tx(1),ty(1),tz(1)
c
      common /ctmp0x/ br(lx1*ly1*lz1),bs(lx1*ly1*lz1),bt(lx1*ly1*lz1)
c
c              / db3     db2 \
c     cb1 = J  | ---  -  --- |    ! Keep J ( J:=Jacobian)
c              \ dy      dz  /
c
c
c              / db1     db3 \
c     cb2 = J  | ---  -  --- |    ! Keep J ( J:=Jacobian)
c              \ dz      dx  /
c
c
c              / db2     db1 \
c     cb3 = J  | ---  -  --- |    ! Keep J ( J:=Jacobian)
c              \ dx      dy  /
c
c
c     Note:
c
c      db2      db2   dr     db2   ds     db2   dt
c    J --- =    --- J --  +  --- J --  +  --- J --
c      dz       dr    dz     ds    dz     dt    dz
c
c              etc.
c
c
c
      nxyz = nx1*ny1*nz1
c
      N=nx1-1
      call local_grad3(br,bs,bt,b1,N,1,dxm1,dxtm1)
      do i=1,nxyz
         cb(i,2) =  (br(i)*rz(i)+bs(i)*sz(i)+bt(i)*tz(i))
         cb(i,3) = -(br(i)*ry(i)+bs(i)*sy(i)+bt(i)*ty(i))
      enddo
c
      call local_grad3(br,bs,bt,b2,N,1,dxm1,dxtm1)
      do i=1,nxyz
         cb(i,1) = -(br(i)*rz(i)+bs(i)*sz(i)+bt(i)*tz(i))
         cb(i,3) =  (br(i)*rx(i)+bs(i)*sx(i)+bt(i)*tx(i))
     $           +  cb(i,3)
      enddo
c
      call local_grad3(br,bs,bt,b3,N,1,dxm1,dxtm1)
      do i=1,nxyz
         cb(i,1) =  (br(i)*ry(i)+bs(i)*sy(i)+bt(i)*ty(i))
     $           +  cb(i,1)
         cb(i,2) = -(br(i)*rx(i)+bs(i)*sx(i)+bt(i)*tx(i))
     $           +  cb(i,2)
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine specx(b,nb,a,na,ba,ab,w)
c
      include 'SIZE'
      include 'INPUT'
      real b(1),a(1)
      real w(1)
c
      n=na*na*na
      do i=1,n
         b(i) = a(i)
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine phys_to_elsasser(u1,u2,u3,b1,b2,b3,n)
c
      real u1(1),u2(1),u3(1),b1(1),b2(1),b3(1)
c
      do i=1,n
c
         zpx = u1(i) + b1(i)
         zpy = u2(i) + b2(i)
         zpz = u3(i) + b3(i)
c
         zmx = u1(i) - b1(i)
         zmy = u2(i) - b2(i)
         zmz = u3(i) - b3(i)
c
         u1(i) = zpx
         u2(i) = zpy
         u3(i) = zpz
c
         b1(i) = zmx
         b2(i) = zmy
         b3(i) = zmz
c
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine elsasser_to_phys(u1,u2,u3,b1,b2,b3,n)
c
      real u1(1),u2(1),u3(1),b1(1),b2(1),b3(1)
c
      do i=1,n
c
         zpx = 0.5*( u1(i) + b1(i) )
         zpy = 0.5*( u2(i) + b2(i) )
         zpz = 0.5*( u3(i) + b3(i) )
c
         zmx = 0.5*( u1(i) - b1(i) )
         zmy = 0.5*( u2(i) - b2(i) )
         zmz = 0.5*( u3(i) - b3(i) )
c
         u1(i) = zpx
         u2(i) = zpy
         u3(i) = zpz
c
         b1(i) = zmx
         b2(i) = zmy
         b3(i) = zmz
c
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine phys_to_elsasser2(u1,b1,n)
c
      real u1(1),b1(1)
c
      do i=1,n
         zpx = u1(i) + b1(i)
         zmx = u1(i) - b1(i)
         u1(i) = zpx
         b1(i) = zmx
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine elsasser_to_phys2(u1,b1,n)
c
      real u1(1),b1(1)
c
      do i=1,n
         zpx = 0.5*( u1(i) + b1(i) )
         zmx = 0.5*( u1(i) - b1(i) )
         u1(i) = zpx
         b1(i) = zmx
      enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine elsasserh(igeom)
c
c
c     Solve MHD in Elsasser variables
c
c
      include 'SIZE'
      include 'INPUT'
      include 'EIGEN'
      include 'SOLN'
      include 'TSTEP'
      include 'MASS'
      include 'GEOM'
C
      common /scrnt/  besv1 (lbx1,lby1,lbz1,lbelv)
     $ ,              besv2 (lbx1,lby1,lbz1,lbelv)
     $ ,              besv3 (lbx1,lby1,lbz1,lbelv)
      COMMON /SCRNS/  RESV1 (LX1,LY1,LZ1,LELV)
     $ ,              RESV2 (LX1,LY1,LZ1,LELV)
     $ ,              RESV3 (LX1,LY1,LZ1,LELV)
     $ ,              DV1   (LX1,LY1,LZ1,LELV)
     $ ,              DV2   (LX1,LY1,LZ1,LELV)
     $ ,              DV3   (LX1,LY1,LZ1,LELV)
      COMMON /SCRVH/  H1    (LX1,LY1,LZ1,LELV)
     $ ,              H2    (LX1,LY1,LZ1,LELV)
c
      n  = nx1*ny1*nz1*nelv
c
c     New geometry, new velocity
c
      intype = -1
c
      ifield = 1
      call sethlm   (h1,h2,intype)
      call cresvibp (resv1,resv2,resv3,h1,h2)
c
      ifield = ifldmhd
      call sethlm   (h1,h2,intype)
      call cresvib  (besv1,besv2,besv3,h1,h2)
c
c
      ifield = 1
      call sethlm   (h1,h2,intype)

      call ophinv_pr(dv1,dv2,dv3,resv1,resv2,resv3,h1,h2,tolhv,nmxh)

      call opadd2   (vx,vy,vz,dv1,dv2,dv3)

      if (param(103).gt.0) alpha_filt=param(103)      ! Optional Filtering
      if (param(103).gt.0) call q_filter(alpha_filt)

      call incomprn (vx,vy,vz,pr) ! project U onto div-free space

      ifield = ifldmhd
      call sethlm   (h1,h2,intype)

      call ophinv_pr(dv1,dv2,dv3,besv1,besv2,besv3,h1,h2,tolhv,nmxh)
      call opadd2   (bx,by,bz,dv1,dv2,dv3)

c     if (param(103).gt.0) call q_filter(alpha_filt)

      call incomprn (bx,by,bz,pm) ! project B onto div-free space

      return
      end
c--------------------------------------------------------------------
      subroutine compute_cfl(cfl,u,v,w,dt)
c
c     Given velocity field (u,v,w) and dt, compute current CFL number.
c
      include 'SIZE'
      include 'GEOM'
      include 'INPUT'
      include 'WZ'
      include 'SOLN'
c
      real u(nx1,ny1,nz1,nelv),v(nx1,ny1,nz1,nelv),w(nx1,ny1,nz1,nelv)
c
c     Store the inverse jacobian to speed up this operation
c
      common /cfldx/ dri(lx1),dsi(ly1),dti(lz1)
c
      integer e
c
      integer icalld
      save    icalld
      data    icalld /0/
c
      if (icalld.eq.0) then
         icalld=1
         call getdr(dri,zgm1(1,1),nx1)
         call getdr(dsi,zgm1(1,2),ny1)
         if (if3d) call getdr(dti,zgm1(1,3),nz1)
      endif

      cfl = 0.
      l   = 0

      if (if3d) then
         nxyz = nx1*ny1*nz1
         do e=1,nelv
            do k=1,nz1
            do j=1,ny1
            do i=1,nx1
               l = l+1
               ur = ( u(i,j,k,e)*rxm1(i,j,k,e)
     $            +   v(i,j,k,e)*rym1(i,j,k,e)
     $            +   w(i,j,k,e)*rzm1(i,j,k,e) ) * jacmi(l,1)
               us = ( u(i,j,k,e)*sxm1(i,j,k,e)
     $            +   v(i,j,k,e)*sym1(i,j,k,e)
     $            +   w(i,j,k,e)*szm1(i,j,k,e) ) * jacmi(l,1)
               ut = ( u(i,j,k,e)*txm1(i,j,k,e)
     $            +   v(i,j,k,e)*tym1(i,j,k,e)
     $            +   w(i,j,k,e)*tzm1(i,j,k,e) ) * jacmi(l,1)

               cflr = abs(dt*ur*dri(i))
               cfls = abs(dt*us*dsi(j))
               cflt = abs(dt*ut*dti(k))

               cflm = cflr + cfls + cflt
               cfl  = max(cfl,cflm)

               cflf(i,j,k,e) = cflm

            enddo
            enddo
            enddo
         enddo
      else
         nxyz = nx1*ny1
         do e=1,nelv
            do j=1,ny1
            do i=1,nx1
               l = l+1
               ur = ( u(i,j,1,e)*rxm1(i,j,1,e)
     $            +   v(i,j,1,e)*rym1(i,j,1,e) ) * jacmi(l,1)
               us = ( u(i,j,1,e)*sxm1(i,j,1,e)
     $            +   v(i,j,1,e)*sym1(i,j,1,e) ) * jacmi(l,1)

               cflr = abs(dt*ur*dri(i))
               cfls = abs(dt*us*dsi(j))

               cflm = cflr + cfls
               cfl  = max(cfl,cflm)

               cflf(i,j,1,e) = cflm

            enddo
            enddo
         enddo
      endif
c
      cfl = glmax(cfl,1)
c
      return
      end
c-----------------------------------------------------------------------
      subroutine getdr(dri,zgm1,nx1)
      real dri(nx1),zgm1(nx1)
c
      dri(1) = zgm1(2) - zgm1(1)   !  Compute 1/Dx
      do i=2,nx1-1
         dri(i) = 0.5*( zgm1(i+1) - zgm1(i-1) )
      enddo
      dri(nx1) = zgm1(nx1) - zgm1(nx1-1)
c
      call invcol1(dri,nx1)
c
      return
      end
c-----------------------------------------------------------------------
      subroutine ophinv_pr(o1,o2,o3,i1,i2,i3,h1,h2,tolh,nmxhi)
C
C     Ok = (H1*A+H2*B)-1 * Ik  (implicit)
C
      include 'SIZE'
      include 'INPUT'
      include 'MASS'
      include 'SOLN'
      include 'ORTHOV'
      include 'VPROJ'
      include 'TSTEP'
c
      real o1 (lx1,ly1,lz1,1) , o2 (lx1,ly1,lz1,1) , o3 (lx1,ly1,lz1,1)
      real i1 (lx1,ly1,lz1,1) , i2 (lx1,ly1,lz1,1) , i3 (lx1,ly1,lz1,1)
      real h1 (lx1,ly1,lz1,1) , h2 (lx1,ly1,lz1,1)
c
      ifproj = .false.
      if (param(94).gt.0) ifproj = .true.
c
      if (.not.ifproj .or. .not.if3d) then
         if (ifield.eq.1) call ophinvm
     $      (o1,o2,o3,i1,i2,i3,v1mask,v2mask,v3mask,h1,h2,tolh,nmxhi)
         if (ifield.eq.ifldmhd) call ophinvm
     $      (o1,o2,o3,i1,i2,i3,b1mask,b2mask,b3mask,h1,h2,tolh,nmxhi)
         return
      endif
c
      do i=1,2*ndim
         mtmp        = param(93)
         ivproj(1,i) = min(mxprev,mtmp) - 1
      enddo
c
      imesh = 1
c
      if (ifstrs) then
         matmod = 0
         call hmhzsf  ('nomg',o1,o2,o3,i1,i2,i3,h1,h2,
     $                  v1mask,v2mask,v3mask,vmult,
     $                  tolh,nmxhi,matmod)
      else
         if (ifield.eq.1) then
            call hsolve ('velx',o1,i1,h1,h2,v1mask,vmult
     $                         ,imesh,tolh,nmxhi,1
     $                         ,vproj(1,1),ivproj(1,1),binvm1)
            call hsolve ('vely',o2,i2,h1,h2,v2mask,vmult
     $                         ,imesh,tolh,nmxhi,2
     $                         ,vproj(1,2),ivproj(1,2),binvm1)
            if (if3d)
     $      call hsolve ('velz',o3,i3,h1,h2,v3mask,vmult
     $                         ,imesh,tolh,nmxhi,3
     $                         ,vproj(1,3),ivproj(1,3),binvm1)
         else  ! B-field
            call hsolve (' Bx ',o1,i1,h1,h2,b1mask,vmult
     $                         ,imesh,tolh,nmxhi,1
     $                         ,vproj(1,4),ivproj(1,4),binvm1)
            call hsolve (' By ',o2,i2,h1,h2,b2mask,vmult
     $                         ,imesh,tolh,nmxhi,2
     $                         ,vproj(1,5),ivproj(1,5),binvm1)
            if (if3d)
     $      call hsolve (' Bz ',o3,i3,h1,h2,b3mask,vmult
     $                         ,imesh,tolh,nmxhi,3
     $                         ,vproj(1,6),ivproj(1,6),binvm1)
         endif
      endif
C
      return
      end
c--------------------------------------------------------------------
      subroutine ophinvm(o1,o2,o3,i1,i2,i3,m1,m2,m3,h1,h2,tolh,nmxhi)
c
c     Ok  = (H1*A+H2*B)-1 * Ik   (implicit)
c
      include 'SIZE'
      include 'INPUT'
      include 'SOLN'
      include 'TSTEP'
      real o1(lx1,ly1,lz1,1), o2(lx1,ly1,lz1,1), o3(lx1,ly1,lz1,1)
      real i1(lx1,ly1,lz1,1), i2(lx1,ly1,lz1,1), i3(lx1,ly1,lz1,1)
      real m1(lx1,ly1,lz1,1), m2(lx1,ly1,lz1,1), m3(lx1,ly1,lz1,1)
      real h1(lx1,ly1,lz1,1), h2(lx1,ly1,lz1,1)
c
      imesh = 1
c
      if (ifstrs) then
         matmod = 0
         call hmhzsf  ('NOMG',o1,o2,o3,i1,i2,i3,h1,h2,
     $                  m1,m2,m3,vmult,tolh,nmxhi,matmod)
      elseif (ifield.eq.1) then
         call hmholtz ('VELX',o1,i1,h1,h2,m1,vmult,imesh,tolh,nmxhi,1)
         call hmholtz ('VELY',o2,i2,h1,h2,m2,vmult,imesh,tolh,nmxhi,2)
         if (ndim.eq.3)
     $   call hmholtz ('VELZ',o3,i3,h1,h2,m3,vmult,imesh,tolh,nmxhi,3)
      elseif (ifield.eq.ifldmhd) then
         call hmholtz (' BX ',o1,i1,h1,h2,m1,vmult,imesh,tolh,nmxhi,1)
         call hmholtz (' BY ',o2,i2,h1,h2,m2,vmult,imesh,tolh,nmxhi,2)
         if (ndim.eq.3)
     $   call hmholtz (' BZ ',o3,i3,h1,h2,m3,vmult,imesh,tolh,nmxhi,3)
      endif

      return
      end
c--------------------------------------------------------------------
      subroutine set_ifbcor
      include 'SIZE'
      include 'GEOM'
      include 'INPUT'
c     include 'TSTEP'   ! ifield?

      common  /nekcb/ cb
      character cb*3

      ifbcor = .true.
      ifield = ifldmhd

      nface  = 2*ndim
      do iel=1,nelv
      do ifc=1,nface
         cb = cbc(ifc,iel,ifield)
         if  (cb.eq.'ndd' .or. cb.eq.'dnd' .or. cb.eq.'ddn')
     $           ifbcor = .false.
      enddo
      enddo

      call gllog(ifbcor , .false.)

      if (nid.eq.0)  write (6,*) 'IFBCOR   =',ifbcor

      return
      end
c--------------------------------------------------------------------
c      subroutine set_ifbcor_old(ifbcor)
cc
cc     This is a quick hack for the rings problem - it is not general,
cc     but will also work fine for the periodic in Z problem
cc
c      include 'SIZE'
c      include 'TOTAL'
c
c      logical ifbcor
c
c      integer e,f
c      character*1 cb1(3)
c
c      itest = 0
c
c      do e=1,nelv
c      do f=1,2*ndim
c         call chcopy(cb1,cbc(f,e,ifldmhd),3)
c         if (cb1(3).eq.'n'.or.cb1(3).eq.'N') itest = 1
c      enddo
c      enddo
c
c      itest = iglmax(itest,1)
c
c      ifbcor = .true.  ! adjust mean pressure to rmv hydrostatic mode
c
c
c      if (itest.eq.1) ifbcor = .false.  ! do not adjust mean pressure
c
c      return
c      end
c--------------------------------------------------------------------
      subroutine setrhsp(p,h1,h2,h2inv,pset,nprev)
C
C     Project soln onto best fit in the "E" norm.
C
      include 'SIZE'
      include 'INPUT'
      include 'MASS'
      include 'SOLN'
      include 'TSTEP'

      real p    (lx2,ly2,lz2,lelv)
      real h1   (lx1,ly1,lz1,lelv)
      real h2   (lx1,ly1,lz1,lelv)
      real h2inv(lx1,ly1,lz1,lelv)
      real pset (lx2*ly2*lz2*lelv,mxprev)

      parameter (ltot2=lx2*ly2*lz2*lelv)
      common /orthox/ pbar(ltot2),pnew(ltot2)
      common /orthos/ alpha(mxprev),work(mxprev)

      if (nprev.eq.0) return

c     Diag to see how much reduction in the residual is attained.
      ntot2  = nx2*ny2*nz2*nelv
      alpha1 = glsc3(p,p,bm2inv,ntot2)
      if (alpha1.gt.0) then
         alpha1 = sqrt(alpha1/volvm2)
      else
         return
      endif

c     CALL UPDRHSE(P,H1,H2,H2INV,ierr) ! update rhs's if E-matrix has changed
c     if (ierr.eq.1) Nprev=0           ! Doesn't happen w/ new formulation

      do i=1,nprev  ! Perform Gram-Schmidt for previous soln's.
         alpha(i) = vlsc2(p,pset(1,i),ntot2)
      enddo
      call gop(alpha,work,'+  ',nprev)

      call rzero(pbar,ntot2)
      do i=1,nprev
         call add2s2(pbar,pset(1,i),alpha(i),ntot2)
      enddo
C
      intetype = 1
      call cdabdtp(pnew,pbar,h1,h2,h2inv,intetype)
      call sub2   (p,pnew,ntot2)

c    ................................................................
      alpha2 = glsc3(p,p,bm2inv,ntot2) ! Diagnostics
      if (alpha2.gt.0) then
         alpha2 = sqrt(alpha2/volvm2)
         ratio  = alpha1/alpha2
         n10=min(10,nprev)
         if (nid.eq.0) write(6,11) istep,nprev,(alpha(i),i=1,n10)
         if (nid.eq.0) write(6,12) istep,nprev,alpha1,alpha2,ratio
   11    format(2i5,' alpha:',1p10e12.4)
   12    format(i6,i4,1p3e12.4,' alph12')
      endif
c    ................................................................

      return
      end
c-----------------------------------------------------------------------
      subroutine gensolnp(p,h1,h2,h2inv,pset,nprev)
C
C     Reconstruct the solution to the original problem by adding back
C     the previous solutions
C
      include 'SIZE'
      include 'INPUT'

      real p    (lx2,ly2,lz2,lelv)
      real h1   (lx1,ly1,lz1,lelv)
      real h2   (lx1,ly1,lz1,lelv)
      real h2inv(lx1,ly1,lz1,lelv)
      real pset (lx2*ly2*lz2*lelv,mxprev)

      parameter (ltot2=lx2*ly2*lz2*lelv)
      common /orthox/ pbar(ltot2),pnew(ltot2)
      common /orthos/ alpha(mxprev),work(mxprev)

      mprev=param(93)
      mprev=min(mprev,mxprev)

      ntot2=nx2*ny2*nz2*nelv

      if (nprev.lt.mprev) then
         nprev = nprev+1
         call copy  (pset(1,nprev),p,ntot2)        ! Save current solution
         call add2  (p,pbar,ntot2)                 ! Reconstruct solution.
         call econjp(pset,nprev,h1,h2,h2inv,ierr)  ! Orthonormalize set
      else                                         !          (uses pnew).
         nprev = 1
         call add2  (p,pbar,ntot2)                 ! Reconstruct solution.
         call copy  (pset(1,nprev),p,ntot2)        ! Save current solution
         call econjp(pset,nprev,h1,h2,h2inv,ierr)  !   and orthonormalize.
      endif

      return
      end
c-----------------------------------------------------------------------
      subroutine econjp(pset,nprev,h1,h2,h2inv,ierr)

c     Orthogonalize the soln wrt previous soln's for which we already
c     know the soln.

      include 'SIZE'
      include 'INPUT'

      real p    (lx2,ly2,lz2,lelv)
      real h1   (lx1,ly1,lz1,lelv)
      real h2   (lx1,ly1,lz1,lelv)
      real h2inv(lx1,ly1,lz1,lelv)
      real pset (lx2*ly2*lz2*lelv,mxprev)

      parameter (ltot2=lx2*ly2*lz2*lelv)
      common /orthox/ pbar(ltot2),pnew(ltot2)
      common /orthos/ alpha(mxprev),work(mxprev)

      ierr  = 0

      ntot2=nx2*ny2*nz2*nelv

C
C     Gram Schmidt, w re-orthogonalization
C
      npass=1
c     if (abs(param(105)).eq.2) npass=2
      do ipass=1,npass

         intetype=1
         call cdabdtp(pnew,pset(1,nprev),h1,h2,h2inv,intetype)
         alphad = glsc2(pnew,pset(1,nprev),ntot2) ! compute part of the norm

         nprev1 = nprev-1
         do i=1,nprev1   !   Gram-Schmidt
            alpha(i) = vlsc2(pnew,pset(1,i),ntot2)
         enddo
         if (nprev1.gt.0) call gop(alpha,work,'+  ',nprev1)

         do i=1,nprev1
            alpham = -alpha(i)
            call add2s2(pset(1,nprev),pset(1,i),alpham,ntot2)
            alphad = alphad - alpha(i)**2
         enddo
      enddo
C
C    .Normalize new element in P~
C
      if (alphad.le.0) then
         write(6,*) 'ERROR:  alphad .le. 0 in econjp',alphad,nprev
         ierr = 1
         return
      endif
      alphad = 1./sqrt(alphad)
      call cmult(pset(1,nprev),alphad,ntot2)

      return
      end
c-----------------------------------------------------------------------
      subroutine advab_elsasser_fast
C
C     Eulerian scheme, add convection term to forcing function
C     at current time step.
C
      include 'SIZE'
      include 'INPUT'
      include 'GEOM'
      include 'SOLN'
      include 'MASS'
      include 'TSTEP'
c
      parameter (lxy=lx1*ly1*lz1,ltd=lxd*lyd*lzd)
      common /scrns/ wk(2*ltd)
     $             , fx(lxy),fy(lxy),fz(lxy)
     $             , gx(lxy),gy(lxy),gz(lxy)
     $             , zr(ltd),zs(ltd),zt(ltd)
     $             , tr(ltd,3),zp(ltd,3),zm(ltd,3)

      integer e
      integer icalld
      save    icalld
      data    icalld /0/

      if (icalld.eq.0) call set_dealias_rx
      icalld = icalld + 1

      nxyz1 = nx1*ny1*nz1
      nxyzd = nxd*nyd*nzd

      if (icalld.eq.1.and.nid.eq.0) write(6,*) 'inside fast',nxyz1,nxyzd

      if (if3d) then
c
         do e=1,nelv

c           Interpolate z+ and z- into fine mesh, translate to r-s-t coords

c           write(6,*) nid,' inside fast',e,nxyz1

            call add3(wk,vx(1,1,1,e),bx(1,1,1,e),nxyz1)
            call intp_rstd(zp(1,1),wk,nx1,nxd,if3d,0) ! 0 --> forward

            call add3(wk,vy(1,1,1,e),by(1,1,1,e),nxyz1)
            call intp_rstd(zp(1,2),wk,nx1,nxd,if3d,0)

            call add3(wk,vz(1,1,1,e),bz(1,1,1,e),nxyz1)
            call intp_rstd(zp(1,3),wk,nx1,nxd,if3d,0)

            call sub3(wk,vx(1,1,1,e),bx(1,1,1,e),nxyz1)
            call intp_rstd(zm(1,1),wk,nx1,nxd,if3d,0)

            call sub3(wk,vy(1,1,1,e),by(1,1,1,e),nxyz1)
            call intp_rstd(zm(1,2),wk,nx1,nxd,if3d,0)

            call sub3(wk,vz(1,1,1,e),bz(1,1,1,e),nxyz1)
            call intp_rstd(zm(1,3),wk,nx1,nxd,if3d,0)

            do i=1,nxyzd  ! Convert convector (zm) to r-s-t coordinates
               tr(i,1)=
     $            rx(i,1,e)*zm(i,1)+rx(i,2,e)*zm(i,2)+rx(i,3,e)*zm(i,3)
               tr(i,2)=
     $            rx(i,4,e)*zm(i,1)+rx(i,5,e)*zm(i,2)+rx(i,6,e)*zm(i,3)
               tr(i,3)=
     $            rx(i,7,e)*zm(i,1)+rx(i,8,e)*zm(i,2)+rx(i,9,e)*zm(i,3)
            enddo

            call grad_rst(zr,zs,zt,zp(1,1),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(fx,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zp(1,2),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(fy,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zp(1,3),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(fz,wk,nx1,nxd,if3d,1) ! Project back to coarse

            do i=1,nxyzd  ! Convert convector (zp) to r-s-t coordinates
               tr(i,1)=
     $            rx(i,1,e)*zp(i,1)+rx(i,2,e)*zp(i,2)+rx(i,3,e)*zp(i,3)
               tr(i,2)=
     $            rx(i,4,e)*zp(i,1)+rx(i,5,e)*zp(i,2)+rx(i,6,e)*zp(i,3)
               tr(i,3)=
     $            rx(i,7,e)*zp(i,1)+rx(i,8,e)*zp(i,2)+rx(i,9,e)*zp(i,3)
            enddo

            call grad_rst(zr,zs,zt,zm(1,1),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(gx,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zm(1,2),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(gy,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zm(1,3),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)+tr(i,3)*zt(i)
            enddo
            call intp_rstd(gz,wk,nx1,nxd,if3d,1) ! Project back to coarse

            tmp = -0.5 ! vtrans() assumed to be 1.0 !
            do i=1,nxyz1
               bfx(i,1,1,e) = bfx(i,1,1,e) + tmp*( fx(i) + gx(i) )
               bmx(i,1,1,e) = bmx(i,1,1,e) + tmp*( fx(i) - gx(i) )

               bfy(i,1,1,e) = bfy(i,1,1,e) + tmp*( fy(i) + gy(i) )
               bmy(i,1,1,e) = bmy(i,1,1,e) + tmp*( fy(i) - gy(i) )

               bfz(i,1,1,e) = bfz(i,1,1,e) + tmp*( fz(i) + gz(i) )
               bmz(i,1,1,e) = bmz(i,1,1,e) + tmp*( fz(i) - gz(i) )
            enddo

         enddo

      else ! 2D
c
         do e=1,nelv

c           Interpolate z+ and z- into fine mesh, translate to r-s-t coords

            call add3(wk,vx(1,1,1,e),bx(1,1,1,e),nxyz1)
            call intp_rstd(zp(1,1),wk,nx1,nxd,if3d,0) ! 0 --> forward

            call add3(wk,vy(1,1,1,e),by(1,1,1,e),nxyz1)
            call intp_rstd(zp(1,2),wk,nx1,nxd,if3d,0)

            call sub3(wk,vx(1,1,1,e),bx(1,1,1,e),nxyz1)
            call intp_rstd(zm(1,1),wk,nx1,nxd,if3d,0)

            call sub3(wk,vy(1,1,1,e),by(1,1,1,e),nxyz1)
            call intp_rstd(zm(1,2),wk,nx1,nxd,if3d,0)

            do i=1,nxyzd  ! Convert convector (zm) to r-s-t coordinates
               tr(i,1)=
     $            rx(i,1,e)*zm(i,1)+rx(i,2,e)*zm(i,2)
               tr(i,2)=
     $            rx(i,3,e)*zm(i,1)+rx(i,4,e)*zm(i,2)
            enddo

            call grad_rst(zr,zs,zt,zp(1,1),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)
            enddo
            call intp_rstd(fx,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zp(1,2),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)
            enddo
            call intp_rstd(fy,wk,nx1,nxd,if3d,1) ! Project back to coarse

            do i=1,nxyzd  ! Convert convector (zp) to r-s-t coordinates
               tr(i,1)=
     $            rx(i,1,e)*zp(i,1)+rx(i,2,e)*zp(i,2)
               tr(i,2)=
     $            rx(i,3,e)*zp(i,1)+rx(i,4,e)*zp(i,2)
            enddo

            call grad_rst(zr,zs,zt,zm(1,1),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)
            enddo
            call intp_rstd(gx,wk,nx1,nxd,if3d,1) ! Project back to coarse

            call grad_rst(zr,zs,zt,zm(1,2),nxd,if3d)
            do i=1,nxyzd ! mass matrix included, per DFM (4.8.5)
               wk(i) = tr(i,1)*zr(i)+tr(i,2)*zs(i)
            enddo
            call intp_rstd(gy,wk,nx1,nxd,if3d,1) ! Project back to coarse

            tmp = -0.5 ! vtrans() assumed to be 1.0 !
            do i=1,nxyz1
               bfx(i,1,1,e) = bfx(i,1,1,e) + tmp*( fx(i) + gx(i) )
               bmx(i,1,1,e) = bmx(i,1,1,e) + tmp*( fx(i) - gx(i) )

               bfy(i,1,1,e) = bfy(i,1,1,e) + tmp*( fy(i) + gy(i) )
               bmy(i,1,1,e) = bmy(i,1,1,e) + tmp*( fy(i) - gy(i) )
            enddo
         enddo
      endif

      return
      end
c-----------------------------------------------------------------------
      subroutine set_dealias_rx
C
C     Eulerian scheme, add convection term to forcing function
C     at current time step.
C
      include 'SIZE'
      include 'INPUT'
      include 'GEOM'
      include 'TSTEP' ! for istep

      common /dealias1/ zd(lxd),wd(lxd)
      integer e

      integer ilstep
      save    ilstep
      data    ilstep /-1/

      if (.not.ifgeom.and.ilstep.gt.1) return  ! already computed
      if (ifgeom.and.ilstep.eq.istep)  return  ! already computed
      ilstep = istep

      nxyz1 = nx1*ny1*nz1
      nxyzd = nxd*nyd*nzd

      call zwgl (zd,wd,nxd)  ! zwgl -- NOT zwgll!

      if (if3d) then
c
         do e=1,nelv

c           Interpolate z+ and z- into fine mesh, translate to r-s-t coords

            call intp_rstd(rx(1,1,e),rxm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,2,e),rym1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,3,e),rzm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,4,e),sxm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,5,e),sym1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,6,e),szm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,7,e),txm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,8,e),tym1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,9,e),tzm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd

            l = 0
            do k=1,nzd
            do j=1,nyd
            do i=1,nxd
               l = l+1
               w = wd(i)*wd(j)*wd(k)
               do ii=1,9
                  rx(l,ii,e) = w*rx(l,ii,e)
               enddo
            enddo
            enddo
            enddo
         enddo

      else ! 2D
c
         do e=1,nelv

c           Interpolate z+ and z- into fine mesh, translate to r-s-t coords

            call intp_rstd(rx(1,1,e),rxm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,2,e),rym1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,3,e),sxm1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd
            call intp_rstd(rx(1,4,e),sym1(1,1,1,e),nx1,nxd,if3d,0) ! 0 --> fwd

            l = 0
            do j=1,nyd
            do i=1,nxd
               l = l+1
               w = wd(i)*wd(j)
               do ii=1,4
                  rx(l,ii,e) = w*rx(l,ii,e)
               enddo
            enddo
            enddo
         enddo

      endif

      return
      end
c-----------------------------------------------------------------------
      subroutine cfl_check
c
      include 'SIZE'
      include 'INPUT'
      include 'SOLN'
      include 'MASS'
      include 'TSTEP'
c
      common /scrns/ ta1 (lx1,ly1,lz1,lelv)
     $ ,             ta2 (lx1,ly1,lz1,lelv)
     $ ,             ta3 (lx1,ly1,lz1,lelv)
     $ ,             tb1 (lx1,ly1,lz1,lelv)
     $ ,             tb2 (lx1,ly1,lz1,lelv)
     $ ,             tb3 (lx1,ly1,lz1,lelv)

      call opcopy(ta1,ta2,ta3,vx,vy,vz)
      call opcopy(tb1,tb2,tb3,bx,by,bz)

      ntot1 = nx1*ny1*nz1*nelv
      call phys_to_elsasser(ta1,ta2,ta3,tb1,tb2,tb3,ntot1) ! crude, but effective

      call compute_cfl(cflp,ta1,ta2,ta3,dt)   !  vx = U+B
      call compute_cfl(cflm,tb1,tb2,tb3,dt)   !  bx = U-B

      courno = max(cflp,cflm)

      if (nid.eq.0) write(6,1) istep,time,dt,cflp,cflm
    1 format(i9,1p4e15.7,' CFL')

      return
      end
c--------------------------------------------------------------------


!!  Copyright (C)  Stichting Deltares, 2012-2016.
!!
!!  This program is free software: you can redistribute it and/or modify
!!  it under the terms of the GNU General Public License version 3,
!!  as published by the Free Software Foundation.
!!
!!  This program is distributed in the hope that it will be useful,
!!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!!  GNU General Public License for more details.
!!
!!  You should have received a copy of the GNU General Public License
!!  along with this program. If not, see <http://www.gnu.org/licenses/>.
!!
!!  contact: delft3d.support@deltares.nl
!!  Stichting Deltares
!!  P.O. Box 177
!!  2600 MH Delft, The Netherlands
!!
!!  All indications and logos of, and references to registered trademarks
!!  of Stichting Deltares remain the property of Stichting Deltares. All
!!  rights reserved.

      subroutine dlwqm2( idt    , noseg  , volnew , nobnd  , noq    ,
     &                   ipoint , flowtot, disptot, theta  , diag   ,
     &                   iscale , diagcc , nomat  , mat    , rowpnt ,
     &                   fmat   , tmat   , iexseg )

!     Deltares - Delft Software Department

!     Created   :          2007 by Pauline van Slingerland

!     Function  : fills the matrix

!     Modified  : July     2009 by Leo Postma : double precission version
!     Modified  : November 2009 by Leo Postma : imat and jmat introduced

      use timers
      implicit none

!     Arguments           :

!     Kind        Function         Name                  Description

      integer(4), intent(in   ) :: idt                   ! time step in scu's
      integer(4), intent(in   ) :: noseg                 ! number of segments
      real   (4), intent(in   ) :: volnew(  noseg)       ! segment volumes
      integer(4), intent(in   ) :: nobnd                 ! number of boundary segments
      integer(4), intent(in   ) :: noq                   ! number of exchanges
      integer(4), intent(in   ) :: ipoint(  4,noq)       ! exchange pointers (dim: 4 x noq)
      real   (4), intent(in   ) :: flowtot( noq)         ! flows plus additional velos. (dim: noq)
      real   (4), intent(in   ) :: disptot( noq)         ! dispersion plus additional dipers. (dim: noq)
      real   (4), intent(in   ) :: theta (  noq)         ! variable theta coefficients
      real   (8), intent(  out) :: diag  (  noseg+nobnd) ! (scaled) diagonal matrix elements
      integer(4), intent(in   ) :: iscale                ! 0: no diagonal scaling
                                                         ! 1: diagonal scaling
      real   (8), intent(  out) :: diagcc(  noseg+nobnd) ! copy of the unscaled diagonal, needed to scale the rhs later
      integer(4), intent(in   ) :: nomat                 ! number of nonzero offdiagonal matrix elements
      real   (8), intent(  out) :: mat   (  nomat)       ! (scaled) nonzero offdiagonal matrix elements (elsewhere: amat)
      integer(4), intent(in   ) :: rowpnt(0:noseg+nobnd) ! row pointer, contains row lengths of mat (elsewhere: itrac)
      integer(4), intent(in   ) :: fmat  (  noq  )       ! pointer from(iq) in matrix
      integer(4), intent(in   ) :: tmat  (  noq  )       ! pointer to  (iq) in matrix
      integer(4), intent(in   ) :: iexseg(  noseg+nobnd) ! zero if explicit

!     Local declarations

      integer(4)                :: iseg                  ! current volume
      integer(4)                :: iq                    ! current edge
      integer(4)                :: ito, ifrom            ! from and to volume indices
      real   (4)                :: q1, q2                ! flows

      integer(4) ithandl /0/
      if ( timon ) call timstrt ( "dlwqm2", ithandl )

! set the diagonal

      do iseg = 1 , noseg
         diag(iseg) = volnew(iseg)/real(idt)
      enddo
      do iseg = noseg+1, noseg+nobnd
         diag(iseg) = 1.0
      enddo

! reset the offdiagonal entries

      do iq = 1, nomat
         mat(iq) = 0.0
      enddo

      do iq = 1 , noq
         ifrom = ipoint(1,iq)
         ito   = ipoint(2,iq)
         if ( ifrom .eq. 0 .or. ito .eq. 0 ) cycle

         if ( flowtot(iq) .gt. 0.0 ) then
           q1 = ( flowtot(iq) + disptot(iq) ) * theta(iq)
           q2 = ( 0.0         - disptot(iq) ) * theta(iq)
         else
           q1 = ( 0.0         + disptot(iq) ) * theta(iq)
           q2 = ( flowtot(iq) - disptot(iq) ) * theta(iq)
         endif

         if ( ifrom .gt. 0  ) then
            diag( ifrom  ) = diag( ifrom  ) + q1
            mat (fmat(iq)) = mat (fmat(iq)) + q2
         endif
         if ( ito   .gt. 0  ) then
            diag(  ito   ) = diag(  ito   ) - q2
            mat (tmat(iq)) = mat (tmat(iq)) - q1
         endif
      enddo

! finally scale the matrix to avoid possible round-off errors in gmres
! this scaling may need some adaption for future domain decomposition b.c.

      if ( iscale .eq. 1 ) then
         do iseg = 1, noseg + nobnd
            ifrom = rowpnt(iseg-1) + 1
            ito = rowpnt(iseg)

! check on zero's required for methods 17 and 18

            if ( abs(diag(iseg)) .lt. 1.0e-35 ) diag(iseg) = 1.0

            do iq = ifrom, ito
               mat(iq) = mat(iq) / diag(iseg)
            enddo

! copy of diag for later scaling purposes in dlwqf4

            diagcc(iseg) = diag(iseg)
            diag  (iseg) = 1.0
         enddo
      else
         do iseg = 1, noseg + nobnd
            diagcc(iseg) = 1.0
         enddo
      endif

      if ( timon ) call timstop ( ithandl )
      return
      end subroutine dlwqm2