/*
** (c) 1996-2000 The Regents of the University of California (through
** E.O. Lawrence Berkeley National Laboratory), subject to approval by
** the U.S. Department of Energy.  Your use of this software is under
** license -- the license agreement is attached and included in the
** directory as license.txt or you may contact Berkeley Lab's Technology
** Transfer Department at TTD@lbl.gov.  NOTICE OF U.S. GOVERNMENT RIGHTS.
** The Software was developed under funding from the U.S. Government
** which consequently retains certain rights as follows: the
** U.S. Government has been granted for itself and others acting on its
** behalf a paid-up, nonexclusive, irrevocable, worldwide license in the
** Software to reproduce, prepare derivative works, and perform publicly
** and display publicly.  Beginning five (5) years after the date
** permission to assert copyright is obtained from the U.S. Department of
** Energy, and subject to any subsequent five (5) year renewals, the
** U.S. Government is granted for itself and others acting on its behalf
** a paid-up, nonexclusive, irrevocable, worldwide license in the
** Software to reproduce, prepare derivative works, distribute copies to
** the public, perform publicly and display publicly, and to permit
** others to do so.
*/

#include "REAL.H"
#include "CONSTANTS.H"
#include "MACPROJ_F.H"
#include "BCTypes.H"

#define DIMS lo_1,lo_2,lo_3,hi_1,hi_2,hi_3
#define CDIMS loc_1,loc_2,loc_3,hic_1,hic_2,hic_3

c *************************************************************************
c ** INITSIGMA **
c ** Define the 1/rho coefficients at the top level of the multigrid
c *************************************************************************

      subroutine FORT_INITSIGMA(sigmax,sigmay,sigmaz,rho,
     $                          areax,areay,areaz,DIMS,bc,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T  sigmax(lo_1 -ng+1:hi_1 +ng  ,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmay(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng  ,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmaz(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng  )
      REAL_T    rho(lo_1-3:hi_1+3,lo_2-3:hi_2+3, lo_3-3:hi_3+3)
      REAL_T  areax(lo_1  :hi_1+1,lo_2  :hi_2  , lo_3  :hi_3  )
      REAL_T  areay(lo_1  :hi_1  ,lo_2  :hi_2+1, lo_3  :hi_3  )
      REAL_T  areaz(lo_1  :hi_1  ,lo_2  :hi_2  , lo_3  :hi_3+1)
      integer bc(2,3)

c     Local variables
      integer i,j,k
      REAL_T minsig,maxsig

      do k = lo_3,hi_3 
      do j = lo_2,hi_2 

        do i = lo_1,hi_1+1 
          sigmax(i,j,k) = two * areax(i,j,k) / (rho(i,j,k) + rho(i-1,j,k))
        enddo

        if (BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) then
          sigmax(lo_1,j,k) = areax(lo_1,j,k) / rho(lo_1-1,j,k)
        else if (BCX_LO .eq. OUTLET) then
          sigmax(lo_1,j,k) = areax(lo_1,j,k) / rho(lo_1,j,k)
        endif

        if (BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) then
          sigmax(hi_1+1,j,k) = areax(hi_1+1,j,k) / rho(hi_1+1,j,k)
        else if (BCX_HI .eq. OUTLET) then
          sigmax(hi_1+1,j,k) = areax(hi_1+1,j,k) / rho(hi_1,j,k)
        endif

      enddo
      enddo

      do k = lo_3,hi_3 
      do i = lo_1,hi_1 

        do j = lo_2,hi_2+1 
          sigmay(i,j,k) = two * areay(i,j,k) / (rho(i,j,k) + rho(i,j-1,k))
        enddo

        if (BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) then
          sigmay(i,lo_2,k) = areay(i,lo_2,k) / rho(i,lo_2-1,k)
        else if (BCY_LO .eq. OUTLET) then
          sigmay(i,lo_2,k) = areay(i,lo_2,k) / rho(i,lo_2  ,k)
        endif

        if (BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) then
          sigmay(i,hi_2+1,k) = areay(i,hi_2,k) / rho(i,hi_2+1,k)
        else if (BCY_HI .eq. OUTLET) then
          sigmay(i,hi_2+1,k) = areay(i,hi_2,k) / rho(i,hi_2  ,k)
        endif

      enddo
      enddo

      do j = lo_2,hi_2 
      do i = lo_1,hi_1 

        do k = lo_3,hi_3+1 
          sigmaz(i,j,k) = two * areaz(i,j,k) / (rho(i,j,k) + rho(i,j,k-1))
        enddo

        if (BCZ_LO .eq. WALL .or. BCZ_LO .eq. INLET) then
          sigmaz(i,j,lo_3) = areaz(i,j,lo_3) / rho(i,j,lo_3-1)
        else if (BCZ_LO .eq. OUTLET) then
          sigmaz(i,j,lo_3) = areaz(i,j,lo_3) / rho(i,j,lo_3)
        endif

        if (BCZ_HI .eq. WALL .or. BCZ_HI .eq. INLET) then
          sigmaz(i,j,hi_3+1) = areaz(i,j,hi_3) / rho(i,j,hi_3+1)
        else if (BCZ_HI .eq. OUTLET) then
          sigmaz(i,j,hi_3+1) = areaz(i,j,hi_3) / rho(i,j,hi_3)
        endif
      enddo
      enddo

      return
      end


c *************************************************************************
c ** PROJUMAC **
c ** Update the edge-based velocities
c *************************************************************************

      subroutine FORT_PROJUMAC(uadv,vadv,wadv,sigmax,sigmay,sigmaz,phi,
     $                         areax,areay,areaz,dx,DIMS,bc,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T   uadv(lo_1  :hi_1+1,lo_2:hi_2  ,lo_3:hi_3  )
      REAL_T   vadv(lo_1  :hi_1  ,lo_2:hi_2+1,lo_3:hi_3  )
      REAL_T   wadv(lo_1  :hi_1  ,lo_2:hi_2  ,lo_3:hi_3+1)
      REAL_T  sigmax(lo_1 -ng+1:hi_1 +ng  ,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmay(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng  ,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmaz(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng  )
      REAL_T    phi(lo_1-ng:hi_1+ng,lo_2-ng:hi_2+ng,lo_3-ng:hi_3+ng)
      REAL_T  areax(lo_1  :hi_1+1,lo_2  :hi_2  , lo_3  :hi_3  )
      REAL_T  areay(lo_1  :hi_1  ,lo_2  :hi_2+1, lo_3  :hi_3  )
      REAL_T  areaz(lo_1  :hi_1  ,lo_2  :hi_2  , lo_3  :hi_3+1)
      REAL_T     dx(3)
      integer bc(2,3)

c     Local variables
      integer i,j,k

      call setbc(phi,DIMS,bc,ng)

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1+1
          uadv(i,j,k) = uadv(i,j,k) - sigmax(i,j,k) / areax(i,j,k) * 
     $                  (phi(i,j,k)-phi(i-1,j,k))/dx(1)
      enddo
      enddo
      enddo

       do k = lo_3,hi_3 
       do j = lo_2,hi_2+1 
       do i = lo_1,hi_1 
          vadv(i,j,k) = vadv(i,j,k) - sigmay(i,j,k) / areay(i,j,k) * 
     $                  (phi(i,j,k)-phi(i,j-1,k))/dx(2)
       enddo
       enddo
       enddo

       do k = lo_3,hi_3+1 
       do j = lo_2,hi_2 
       do i = lo_1,hi_1 
          wadv(i,j,k) = wadv(i,j,k) - sigmaz(i,j,k) / areaz(i,j,k) * 
     $                 (phi(i,j,k)-phi(i,j,k-1))/dx(3)
       enddo
       enddo
       enddo

      return
      end

c *************************************************************************
c ** RESIDUAL **
c ** Compute the residual R = f - D( sig G(phi) )
c *************************************************************************

      subroutine FORT_RESIDUAL(resid,phi,f,sigmax,sigmay,sigmaz,DIMS,
     $                         dx,resnorm,bc,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T  resid(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3+ng-1)
      REAL_T    phi(lo_1-ng  :hi_1+ng  ,lo_2-ng  :hi_2+ng  ,lo_3-ng  :hi_3+ng  )
      REAL_T      f(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3+ng-1)
      REAL_T  sigmax(lo_1 -ng+1:hi_1 +ng  ,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmay(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng  ,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmaz(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng  )
      REAL_T dx(3)
      REAL_T resnorm
      integer bc(2,3)

c     Local variables
      REAL_T hxinv, hyinv, hzinv
      REAL_T rfac, corr
      integer i,j,k

      hxinv = one/dx(1)
      hyinv = one/dx(2)
      hzinv = one/dx(3)

      resnorm = zero

      call setbc(phi,DIMS,bc,ng)

      do k = lo_3,hi_3 
      do j = lo_2,hi_2 
        do i = lo_1,hi_1 

          rfac = ( sigmax(i+1,j,k) + sigmax(i,j,k))*hxinv + 
     $           ( sigmay(i,j+1,k) + sigmay(i,j,k))*hyinv +
     $           ( sigmaz(i,j,k+1) + sigmaz(i,j,k))*hzinv

          corr = 
     $      ( sigmax(i+1,j,k)*phi(i+1,j,k) + sigmax(i,j,k)*phi(i-1,j,k))*hxinv
     $     +( sigmay(i,j+1,k)*phi(i,j+1,k) + sigmay(i,j,k)*phi(i,j-1,k))*hyinv
     $     +( sigmaz(i,j,k+1)*phi(i,j,k+1) + sigmaz(i,j,k)*phi(i,j,k-1))*hzinv

          resid(i,j,k) = f(i,j,k) - (corr - rfac*phi(i,j,k))

          resnorm = max(abs(resid(i,j,k)), resnorm)

        enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** RELAX **
c ** Jacobi relaxation
c *************************************************************************

      subroutine FORT_RELAX(phi,f,sigmax,sigmay,sigmaz,DIMS,dx,bc,nnrelax,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T    phi(lo_1-ng  :hi_1+ng  ,lo_2-ng  :hi_2+ng  ,lo_3-ng  :hi_3+ng  )
      REAL_T      f(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3+ng-1)
      REAL_T sigmax(lo_1 -ng+1:hi_1 +ng  ,lo_2 -ng+1:hi_2 +ng-1,
     $              lo_3 -ng+1:hi_3 +ng-1)
      REAL_T sigmay(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng  ,
     $              lo_3 -ng+1:hi_3 +ng-1)
      REAL_T sigmaz(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng-1,
     $              lo_3 -ng+1:hi_3 +ng  )
      REAL_T dx(3)
      integer bc(2,3)
      integer nnrelax

c     Local variables
      REAL_T rfac, corr
      integer i, j, k
      integer iter, iinc
      integer extra_xlo,extra_xhi
      integer extra_ylo,extra_yhi
      integer extra_zlo,extra_zhi

      if (ng .lt. 2*nnrelax) then
        print *,'NOT ENOUGH GHOST CELLS FOR RELAX '
        print *,'NG ',ng
        print *,'NNRELAX ',nnrelax
        stop
      endif

      call setbc(phi,DIMS,bc,ng)

      do iter = 1, 2*nnrelax

          extra_xlo = cvmgt(ng-iter,0,BCX_LO .eq. INTERIOR .or. BCX_LO .eq. PERIODIC)
          extra_xhi = cvmgt(ng-iter,0,BCX_HI .eq. INTERIOR .or. BCX_HI .eq. PERIODIC)
          extra_ylo = cvmgt(ng-iter,0,BCY_LO .eq. INTERIOR .or. BCY_LO .eq. PERIODIC)
          extra_yhi = cvmgt(ng-iter,0,BCY_HI .eq. INTERIOR .or. BCY_HI .eq. PERIODIC)
          extra_zlo = cvmgt(ng-iter,0,BCZ_LO .eq. INTERIOR .or. BCZ_LO .eq. PERIODIC)
          extra_zhi = cvmgt(ng-iter,0,BCZ_HI .eq. INTERIOR .or. BCZ_HI .eq. PERIODIC)

          do k = lo_3-extra_zlo,hi_3+extra_zhi
          do j = lo_2-extra_ylo,hi_2+extra_yhi
           iinc = mod(j+k+iter+1+extra_xlo+2*ng,2)
           do i = lo_1-extra_xlo+iinc,hi_1+extra_xhi,2

              rfac = (sigmax(i+1,j,k) + sigmax(i,j,k))/dx(1) 
     $              +(sigmay(i,j+1,k) + sigmay(i,j,k))/dx(2)
     $              +(sigmaz(i,j,k+1) + sigmaz(i,j,k))/dx(3)

              corr = 
     $        ( sigmax(i+1,j,k)*phi(i+1,j,k) + sigmax(i,j,k)*phi(i-1,j,k))/dx(1)
     $       +( sigmay(i,j+1,k)*phi(i,j+1,k) + sigmay(i,j,k)*phi(i,j-1,k))/dx(2)
     $       +( sigmaz(i,j,k+1)*phi(i,j,k+1) + sigmaz(i,j,k)*phi(i,j,k-1))/dx(3)

              phi(i,j,k) = (corr - f(i,j,k))/rfac

            enddo
          enddo
          enddo

          call setbc(phi,DIMS,bc,ng)

      enddo

      return
      end


c *************************************************************************
c ** BC **
c ** Impose boundary conditions
c *************************************************************************

      subroutine setbc(phi,DIMS,bc,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T  phi(lo_1-ng:hi_1+ng,lo_2-ng:hi_2+ng,lo_3-ng:hi_3+ng)
      integer bc(2,3)

c     Local variables
      integer i, j, k, is, ie, js, je, ks, ke
      integer ilo,ihi,jlo,jhi,klo,khi

      is = lo_1
      ie = hi_1
      js = lo_2
      je = hi_2
      ks = lo_3
      ke = hi_3

      ilo = cvmgt(lo_1-ng,lo_1,BCX_LO .eq. INTERIOR .or. BCX_LO .eq. PERIODIC)
      ihi = cvmgt(hi_1+ng,hi_1,BCX_HI .eq. INTERIOR .or. BCX_HI .eq. PERIODIC)
      jlo = cvmgt(lo_2-ng,lo_2,BCY_LO .eq. INTERIOR .or. BCY_LO .eq. PERIODIC)
      jhi = cvmgt(hi_2+ng,hi_2,BCY_HI .eq. INTERIOR .or. BCY_HI .eq. PERIODIC)
      klo = cvmgt(lo_3-ng,lo_3,BCZ_LO .eq. INTERIOR .or. BCZ_LO .eq. PERIODIC)
      khi = cvmgt(hi_3+ng,hi_3,BCZ_HI .eq. INTERIOR .or. BCZ_HI .eq. PERIODIC)

      if (BCZ_LO .eq. OUTLET) then
        do j = jlo,jhi
        do i = ilo,ihi
          phi(i,j,ks-1) = -phi(i,j,ks)
        enddo
        enddo
      elseif (BCZ_LO .eq. WALL .or. BCZ_LO .eq. INLET) then
        do j = jlo,jhi
        do i = ilo,ihi
          phi(i,j,ks-1) =  phi(i,j,ks)
        enddo
        enddo
      endif

      if (BCZ_HI .eq. OUTLET) then
        do j = jlo,jhi
        do i = ilo,ihi
          phi(i,j,ke+1) = -phi(i,j,ke)
        enddo
        enddo
      elseif (BCZ_HI .eq. WALL .or.  BCZ_HI .eq. INLET) then
        do j = jlo,jhi
        do i = ilo,ihi
          phi(i,j,ke+1) =  phi(i,j,ke)
        enddo
        enddo
      endif

      if (BCY_LO .eq. OUTLET) then
        do k = klo,khi
        do i = ilo,ihi
          phi(i,js-1,k) = -phi(i,js,k)
        enddo
        enddo
      elseif (BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) then
        do k = klo,khi
        do i = ilo,ihi
          phi(i,js-1,k) =  phi(i,js,k)
        enddo
        enddo
      endif

      if (BCY_HI .eq. OUTLET) then
        do k = klo,khi
        do i = ilo,ihi
          phi(i,je+1,k) = -phi(i,je,k)
        enddo
        enddo
      elseif (BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) then
        do k = klo,khi
        do i = ilo,ihi
          phi(i,je+1,k) = -phi(i,je,k)
          phi(i,je+1,k) =  phi(i,je,k)
        enddo
        enddo
      endif

      if (BCX_LO .eq. OUTLET) then
        do k = klo,khi
        do j = jlo,jhi
          phi(is-1,j,k) = -phi(is,j,k)
        enddo
        enddo
      elseif (BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) then
        do k = klo,khi
        do j = jlo,jhi
          phi(is-1,j,k) =  phi(is,j,k)
        enddo
        enddo
      endif

      if (BCX_HI .eq. OUTLET) then
        do k = klo,khi
        do j = jlo,jhi
          phi(ie+1,j,k) = -phi(ie,j,k)
        enddo
        enddo
      elseif (BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) then
        do k = klo,khi
        do j = jlo,jhi
          phi(ie+1,j,k) =  phi(ie,j,k)
        enddo
        enddo
      endif

      return
      end

c *************************************************************************
c ** RHSMAC **
c ** Compute the right-hand-side D(U) for the MAC projection
c *************************************************************************

      subroutine FORT_RHSMAC(uadv,vadv,wadv,divu_src,rhs,
     $                       areax,areay,areaz,vol,DIMS,rhsnorm,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T     uadv(lo_1   :hi_1+1   ,lo_2  :hi_2      ,lo_3   :hi_3  )
      REAL_T     vadv(lo_1   :hi_1     ,lo_2  :hi_2+1    ,lo_3   :hi_3  )
      REAL_T     wadv(lo_1   :hi_1     ,lo_2  :hi_2      ,lo_3   :hi_3+1)
      REAL_T divu_src(lo_1   :hi_1     ,lo_2  :hi_2      ,lo_3   :hi_3  )
      REAL_T      rhs(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3+ng-1)
      REAL_T    areax(lo_1   :hi_1+1   ,lo_2  :hi_2      ,lo_3   :hi_3  )
      REAL_T    areay(lo_1   :hi_1     ,lo_2  :hi_2+1    ,lo_3   :hi_3  )
      REAL_T    areaz(lo_1   :hi_1     ,lo_2  :hi_2      ,lo_3   :hi_3+1)
      REAL_T      vol(lo_1-1 :hi_1+1   ,lo_2-1:hi_2+1    ,lo_3-1 :hi_3+1)
      REAL_T  rhsnorm

c     Local variables
      integer i,j,k

      rhsnorm = zero

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          rhs(i,j,k) = areax(i+1,j,k)*uadv(i+1,j,k) - areax(i,j,k)*uadv(i,j,k)
     $                +areay(i,j+1,k)*vadv(i,j+1,k) - areay(i,j,k)*vadv(i,j,k)
     $                +areaz(i,j,k+1)*wadv(i,j,k+1) - areaz(i,j,k)*wadv(i,j,k)
 
          rhs(i,j,k) = rhs(i,j,k) - vol(i,j,k)*divu_src(i,j,k)

          rhsnorm = max(rhsnorm,abs(rhs(i,j,k)))

        enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** COARSIGMA **
c ** Coarsen the edge-based sigma coefficients
c *************************************************************************

      subroutine FORT_COARSIGMA(sigmax,sigmay,sigmaz,
     $                          sigmaxc,sigmayc,sigmazc,DIMS,CDIMS,ng)

      implicit none

      integer DIMS
      integer CDIMS
      integer ng
      REAL_T  sigmax(lo_1 -ng+1:hi_1 +ng  ,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmay(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng  ,
     $               lo_3 -ng+1:hi_3 +ng-1)
      REAL_T  sigmaz(lo_1 -ng+1:hi_1 +ng-1,lo_2 -ng+1:hi_2 +ng-1,
     $               lo_3 -ng+1:hi_3 +ng  )
      REAL_T sigmaxc(loc_1-ng+1:hic_1+ng  ,loc_2-ng+1:hic_2+ng-1,
     $               loc_3-ng+1:hic_3+ng-1)
      REAL_T sigmayc(loc_1-ng+1:hic_1+ng-1,loc_2-ng+1:hic_2+ng  ,
     $               loc_3-ng+1:hic_3+ng-1)
      REAL_T sigmazc(loc_1-ng+1:hic_1+ng-1,loc_2-ng+1:hic_2+ng-1,
     $               loc_3-ng+1:hic_3+ng  )

c     Local variables
      integer i,j,k

      do k = lo_3,hi_3  ,2
      do j = lo_2,hi_2  ,2
      do i = lo_1,hi_1+1,2
          sigmaxc(i/2,j/2,k/2) = (sigmax(i,j,k  ) + sigmax(i,j+1,k  ) +
     $                            sigmax(i,j,k+1) + sigmax(i,j+1,k+1) ) * eighth
      enddo
      enddo
      enddo

      do k = lo_3,hi_3  ,2
      do j = lo_2,hi_2+1,2
      do i = lo_1,hi_1  ,2
          sigmayc(i/2,j/2,k/2) = (sigmay(i,j,k  ) + sigmay(i+1,j,k  ) +
     $                            sigmay(i,j,k+1) + sigmay(i+1,j,k+1) ) * eighth
      enddo
      enddo
      enddo

      do k = lo_3,hi_3+1,2
      do j = lo_2,hi_2  ,2
      do i = lo_1,hi_1  ,2
          sigmazc(i/2,j/2,k/2) = (sigmaz(i,j  ,k) + sigmaz(i+1,j  ,k) +
     $                            sigmaz(i,j+1,k) + sigmaz(i+1,j+1,k) ) * eighth
      enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** RESTRICT **
c ** Conservatively average the residual
c *************************************************************************

      subroutine FORT_RESTRICT(res,resc,DIMS,CDIMS,nex)

      implicit none

      integer DIMS
      integer CDIMS
      integer nex
      REAL_T  res(lo_1 -nex:hi_1 +nex,lo_2 -nex:hi_2 +nex,lo_3 -nex:hi_3 +nex)
      REAL_T resc(loc_1-nex:hic_1+nex,loc_2-nex:hic_2+nex,loc_3-nex:hic_3+nex)

c     Local variables
      integer i,j,k

      do k = lo_3,hi_3,2
      do j = lo_2,hi_2,2
        do i = lo_1,hi_1,2
          resc(i/2,j/2,k/2) = (res(i  ,j,k  ) + res(i  ,j+1,k  ) +
     $                         res(i+1,j,k  ) + res(i+1,j+1,k  ) +
     $                         res(i  ,j,k+1) + res(i  ,j+1,k+1) +
     $                         res(i+1,j,k+1) + res(i+1,j+1,k+1) ) * eighth
        enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** INTERPOLATE **
c ** Piecewise constant interpolation
c *************************************************************************

      subroutine FORT_INTERPOLATE(phi,deltac,DIMS,CDIMS,ng)

      implicit none

      integer DIMS
      integer CDIMS
      integer ng
      REAL_T    phi(lo_1 -ng:hi_1 +ng,lo_2 -ng:hi_2 +ng,lo_3 -ng:hi_3 +ng)
      REAL_T deltac(loc_1-ng:hic_1+ng,loc_2-ng:hic_2+ng,loc_3-ng:hic_3+ng)

c     Local variables
      integer i,j,k

      do k = lo_3,hi_3
      do j = lo_2,hi_2
        do i = lo_1,hi_1
          phi(i,j,k) = phi(i,j,k) + deltac(i/2,j/2,k/2)
        enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** SOLVEMAC **
c ** Conjugate gradient bottom-solver
c *************************************************************************

      subroutine FORT_SOLVEMAC(dest, dest0, source, sigmax, sigmay, sigmaz, 
     $                         sum, r, w, z, work, DIMS, dx, bc, norm,
     $                         prob_norm, ng)

      implicit none
      integer DIMS
      integer ng
      REAL_T   dest(lo_1-ng:hi_1+ng,lo_2-ng:hi_2+ng,lo_3-ng:hi_3+ng)
      REAL_T  dest0(lo_1-1 :hi_1+1 ,lo_2-1 :hi_2+1 ,lo_3-1 :hi_3+1)
      REAL_T source(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3+ng-1)
      REAL_T sigmax(lo_1-ng+1:hi_1 +ng  ,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3 +ng-1)
      REAL_T sigmay(lo_1-ng+1:hi_1 +ng-1,lo_2-ng+1:hi_2+ng  ,lo_3-ng+1:hi_3 +ng-1)
      REAL_T sigmaz(lo_1-ng+1:hi_1 +ng-1,lo_2-ng+1:hi_2+ng-1,lo_3-ng+1:hi_3 +ng  )
      REAL_T    sum(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T      r(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T      w(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T      z(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T   work(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T dx(3)
      integer bc(2,3)
      REAL_T norm
      REAL_T prob_norm

c     Local variables
      integer i,j,k
      integer iter
      REAL_T alpha, beta, rho, rhol
      REAL_T local_norm
      REAL_T tol,tolfac

      tolfac = 1.0d-3

      call macperiodic(dest,DIMS,bc,ng)
      call       setbc(dest,DIMS,bc,ng)

      do k = lo_3-1,hi_3+1
      do j = lo_2-1,hi_2+1
      do i = lo_1-1,hi_1+1
         dest0(i,j,k) = dest(i,j,k)
          dest(i,j,k) = zero
      enddo
      enddo
      enddo

 10   do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          w(i,j,k) = 
     $     ( sigmax(i+1,j,k)*dest0(i+1,j,k) + 
     $       sigmax(i  ,j,k)*dest0(i-1,j,k) )/dx(1)
     $    +( sigmay(i,j+1,k)*dest0(i,j+1,k) + 
     $       sigmay(i,j  ,k)*dest0(i,j-1,k) )/dx(2)
     $    +( sigmaz(i,j,k+1)*dest0(i,j,k+1) + 
     $       sigmaz(i,j,k  )*dest0(i,j,k-1) )/dx(3)
     $   -( (sigmax(i+1,j,k) + sigmax(i,j,k))/dx(1) + 
     $      (sigmay(i,j+1,k) + sigmay(i,j,k))/dx(2) +
     $      (sigmaz(i,j,k+1) + sigmaz(i,j,k))/dx(3) )*dest0(i,j,k)
      enddo
      enddo
      enddo

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
        r(i,j,k) = source(i,j,k) - w(i,j,k)
      enddo
      enddo
      enddo

      rho        = zero
      local_norm = zero

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          z(i,j,k) = r(i,j,k) / sum(i,j,k)
          rho = rho + z(i,j,k) * r(i,j,k)
          local_norm = max(local_norm,abs(r(i,j,k)))
      enddo
      enddo
      enddo

      norm = local_norm

      tol = Max(tolfac*local_norm,1.0d-15*prob_norm)
      if (norm .le. tol) return

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          work(i,j,k) = zero
          dest(i,j,k) = z(i,j,k)
      enddo
      enddo
      enddo

      iter = 0

c     write(6,1000) iter, norm/prob_norm

100   call macperiodic(dest,DIMS,bc,ng)
      call       setbc(dest,DIMS,bc,ng)

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          w(i,j,k) = 
     $     ( sigmax(i+1,j,k)*dest(i+1,j,k) + 
     $       sigmax(i  ,j,k)*dest(i-1,j,k) )/dx(1)
     $    +( sigmay(i,j+1,k)*dest(i,j+1,k) + 
     $       sigmay(i,j  ,k)*dest(i,j-1,k) )/dx(2)
     $    +( sigmaz(i,j,k+1)*dest(i,j,k+1) + 
     $       sigmaz(i,j,k  )*dest(i,j,k-1) )/dx(3) - 
     $    ( (sigmax(i+1,j,k) + sigmax(i,j,k))/dx(1) + 
     $      (sigmay(i,j+1,k) + sigmay(i,j,k))/dx(2) +
     $      (sigmaz(i,j,k+1) + sigmaz(i,j,k))/dx(3) )*dest(i,j,k)
      enddo
      enddo
      enddo

      alpha = zero
      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          alpha = alpha + dest(i,j,k)*w(i,j,k)
      enddo
      enddo
      enddo

      alpha = rho / alpha
      rhol = rho
      rho = zero
      norm = zero

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          work(i,j,k) = work(i,j,k) + alpha * dest(i,j,k)
          r(i,j,k) = r(i,j,k) - alpha * w(i,j,k)
          z(i,j,k) = r(i,j,k) / sum(i,j,k)
          rho = rho + z(i,j,k) * r(i,j,k)
          norm = max(norm,abs(r(i,j,k)))
      enddo
      enddo
      enddo

      iter = iter+1
c     write(6,1000) iter, norm/prob_norm

      if (norm .le. tol) then

        do k = lo_3,hi_3
        do j = lo_2,hi_2
        do i = lo_1,hi_1
            dest(i,j,k) = work(i,j,k) + dest0(i,j,k)
        enddo
        enddo
        enddo

        call setbc(dest,DIMS,bc,ng)

      else if (iter .ge. 100  .or.  norm .ge. 100.d0 * local_norm) then

        tolfac = 10.d0 * tolfac
        iter = 1
        do k = lo_3,hi_3
        do j = lo_2,hi_2
        do i = lo_1,hi_1
            dest(i,j,k) = zero
        enddo
        enddo
        enddo
        goto 10

      else

        beta = rho / rhol
        do k = lo_3,hi_3
        do j = lo_2,hi_2
        do i = lo_1,hi_1
            dest(i,j,k) = z(i,j,k) + beta * dest(i,j,k)
        enddo
        enddo
        enddo
        goto 100

      endif

c     call flush(6)

1000  format('Res/Res0 in solve: ',i4,2x,e12.5)
c     call flush(6)

      return
      end

c *************************************************************************
c ** MKSUMMAC **
c ** Pre-compute the sum of coefficients for the conjugate gradient solver
c *************************************************************************

      subroutine FORT_MKSUMMAC(sum,sigmax,sigmay,sigmaz,DIMS,dx,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T    sum(lo_1-1   :hi_1+1   ,lo_2-1   :hi_2+1  ,
     $              lo_3-1   :hi_3+1   )
      REAL_T sigmax(lo_1-ng+1:hi_1+ng  ,lo_2-ng+1:hi_2+ng-1,
     $              lo_3-ng+1:hi_3+ng-1)
      REAL_T sigmay(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng  ,
     $              lo_3-ng+1:hi_3+ng-1)
      REAL_T sigmaz(lo_1-ng+1:hi_1+ng-1,lo_2-ng+1:hi_2+ng-1,
     $              lo_3-ng+1:hi_3+ng  )
      REAL_T dx(3)

c     Local variables
      integer i,j,k

      do k = lo_3,hi_3
      do j = lo_2,hi_2
      do i = lo_1,hi_1
          sum(i,j,k) = (sigmax(i+1,j,k) + sigmax(i,j,k))/dx(1) +
     $                 (sigmay(i,j+1,k) + sigmay(i,j,k))/dx(2) +
     $                 (sigmaz(i,j,k+1) + sigmaz(i,j,k))/dx(3) 
          sum(i,j,k) = -sixth*sum(i,j,k)
      enddo
      enddo
      enddo

      return
      end

c *************************************************************************
c ** MACPERIODIC **
c  Impose periodic boundary conditions on the single grid data in the
c   conjugate gradient bottom solver.
c *************************************************************************

      subroutine macperiodic(dest,DIMS,bc,ng)

      implicit none

      integer DIMS
      integer ng
      REAL_T   dest(lo_1-ng:hi_1+ng,lo_2-ng:hi_2+ng,lo_3-ng:hi_3+ng)
      integer bc(2,3)

      integer i,j,k

      if (BCX_LO .eq. PERIODIC .and. BCX_HI .eq. PERIODIC) then
        do k = lo_3,hi_3
        do j = lo_2,hi_2
          dest(lo_1-1,j,k) = dest(hi_1,j,k)
          dest(hi_1+1,j,k) = dest(lo_1,j,k)
        enddo
        enddo
      endif
      if (BCY_LO .eq. PERIODIC .and. BCY_HI .eq. PERIODIC) then
        do k = lo_3,hi_3
        do i = lo_1,hi_1
          dest(i,lo_2-1,k) = dest(i,hi_2,k)
          dest(i,hi_2+1,k) = dest(i,lo_2,k)
        enddo
        enddo
      endif
      if (BCZ_LO .eq. PERIODIC .and. BCZ_HI .eq. PERIODIC) then
        do j = lo_2,hi_2
        do i = lo_1,hi_1
          dest(i,j,lo_3-1) = dest(i,j,hi_3)
          dest(i,j,hi_3+1) = dest(i,j,lo_3)
        enddo
        enddo
      endif

      return
      end
