/*
** (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,DIMS,
     $                          bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      implicit none

      integer DIMS
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  , lo_3  :hi_3)
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1, lo_3  :hi_3)
      REAL_T sigmaz(lo_1  :hi_1  ,lo_2  :hi_2  , lo_3  :hi_3+1)
      REAL_T    rho(lo_1-1:hi_1+1,lo_2-1:hi_2+1, lo_3-1:hi_3+1)
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi

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+1,hi_1 
          sigmax(i,j,k) = two / (rho(i,j,k) + rho(i-1,j,k))
        enddo

        if (bcx_lo .eq. PERIODIC) then
          sigmax(lo_1,j,k) = two / (rho(lo_1,j,k) + rho(hi_1,j,k))
        else if (bcx_lo .eq. WALL) then
          sigmax(lo_1,j,k) = one / rho(lo_1,j,k)
        else if (bcx_lo .eq. INLET) then
          sigmax(lo_1,j,k) = one / rho(lo_1-1,j,k)
        else if (bcx_lo .eq. OUTLET) then
          sigmax(lo_1,j,k) = one / rho(lo_1,j,k)
        endif

        if (bcx_hi .eq. PERIODIC) then
          sigmax(hi_1+1,j,k) = sigmax(lo_1,j,k)
        else if (bcx_hi .eq. WALL) then
          sigmax(hi_1+1,j,k) = one / rho(hi_1,j,k)
        else if (bcx_hi .eq. INLET) then
          sigmax(hi_1+1,j,k) = one / rho(hi_1+1,j,k)
        else if (bcx_hi .eq. OUTLET) then
          sigmax(hi_1+1,j,k) = one / rho(hi_1,j,k)
        endif

      enddo
      enddo

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

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

        if (bcy_lo .eq. PERIODIC) then
          sigmay(i,lo_2,k) = two / (rho(i,lo_2,k) + rho(i,hi_2,k))
        else if (bcy_lo .eq. WALL) then
          sigmay(i,lo_2,k) = one / rho(i,lo_2  ,k)
        else if (bcy_lo .eq. INLET) then
          sigmay(i,lo_2,k) = one / rho(i,lo_2-1,k)
        else if (bcy_lo .eq. OUTLET) then
          sigmay(i,lo_2,k) = one / rho(i,lo_2  ,k)
        endif

        if (bcy_hi .eq. PERIODIC) then
          sigmay(i,hi_2+1,k) = sigmay(i,lo_2,k)
        else if (bcy_hi .eq. WALL) then
          sigmay(i,hi_2+1,k) = one / rho(i,hi_2  ,k)
        else if (bcy_hi .eq. INLET) then
          sigmay(i,hi_2+1,k) = one / rho(i,hi_2+1,k)
        else if (bcy_hi .eq. OUTLET) then
          sigmay(i,hi_2+1,k) = one / rho(i,hi_2  ,k)
        endif

      enddo
      enddo

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

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

        if (bcz_lo .eq. PERIODIC) then
          sigmaz(i,j,lo_3) = two / (rho(i,j,lo_3) + rho(i,j,hi_3))
        else if (bcz_lo .eq. WALL) then
          sigmaz(i,j,lo_3) = one / rho(i,j,lo_3)
        else if (bcz_lo .eq. INLET) then
          sigmaz(i,j,lo_3) = one / rho(i,j,lo_3-1)
        else if (bcz_lo .eq. OUTLET) then
          sigmaz(i,j,lo_3) = one / rho(i,j,lo_3)
        endif

        if (bcz_hi .eq. PERIODIC) then
          sigmaz(i,j,hi_3+1) = sigmaz(i,j,lo_3)
        else if (bcz_hi .eq. WALL) then
          sigmaz(i,j,hi_3+1) = one / rho(i,j,hi_3)
        else if (bcz_hi .eq. INLET) then
          sigmaz(i,j,hi_3+1) = one / rho(i,j,hi_3+1)
        else if (bcz_hi .eq. OUTLET) then
          sigmaz(i,j,hi_3+1) = one / rho(i,j,hi_3)
        endif
      enddo
      enddo

      return
      end

c *************************************************************************
c ** GRADMAC **
c ** Compute the gradient of phi
c *************************************************************************

      subroutine FORT_GRADMAC(gradpx,gradpy,gradpz,phi,DIMS,hx,hy,hz,
     $                        bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      implicit none

      integer DIMS
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T gradpx(lo_1:hi_1+1,lo_2:hi_2  ,lo_3:hi_3  )
      REAL_T gradpy(lo_1:hi_1  ,lo_2:hi_2+1,lo_3:hi_3  )
      REAL_T gradpz(lo_1:hi_1  ,lo_2:hi_2  ,lo_3:hi_3+1)
      REAL_T hx
      REAL_T hy
      REAL_T hz
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi

c     Local variables
      integer i, is, ie, j, js, je, k, ks, ke

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

      call bc(phi,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      do k = ks,ke 
      do j = js,je 
        do i = is,ie+1
          gradpx(i,j,k) = (phi(i,j,k) - phi(i-1,j,k))/hx
        enddo

        if (bcx_lo .eq. WALL .or. bcx_lo .eq. INLET) then
          gradpx(is  ,j,k) = zero
        endif
        if (bcx_hi .eq. WALL .or. bcx_hi .eq. INLET) then
          gradpx(ie+1,j,k) = zero
        endif

      enddo
      enddo

      do k = ks,ke 
      do i = is,ie 
        do j = js,je+1 
          gradpy(i,j,k) = (phi(i,j,k) - phi(i,j-1,k))/hy
        enddo

        if (bcy_lo .eq. WALL .or. bcy_lo .eq. INLET) then
          gradpy(i,js  ,k)= zero
        endif
        if (bcy_hi .eq. WALL .or. bcy_hi .eq. INLET) then
          gradpy(i,je+1,k) = zero
        endif

      enddo
      enddo

      do j = js,je 
      do i = is,ie 
        do k = ks,ke+1 
          gradpz(i,j,k) = (phi(i,j,k) - phi(i,j,k-1))/hz
        enddo

        if (bcz_lo .eq. WALL .or. bcz_lo .eq. INLET) then
          gradpz(i,j,ks  )= zero
        endif
        if (bcz_hi .eq. WALL .or. bcz_hi .eq. INLET) then
          gradpz(i,j,ke+1) = zero
        endif

      enddo
      enddo

      return
      end

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

      subroutine FORT_PROJUMAC(uadv,vadv,wadv,gradpx,gradpy,gradpz,rho,DIMS)

      implicit none

      integer DIMS
      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 gradpx(lo_1  :hi_1+1,lo_2:hi_2  ,lo_3:hi_3  )
      REAL_T gradpy(lo_1  :hi_1  ,lo_2:hi_2+1,lo_3:hi_3  )
      REAL_T gradpz(lo_1  :hi_1  ,lo_2:hi_2  ,lo_3:hi_3+1)
      REAL_T    rho(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)

c     Local variables
      REAL_T  rhx,rhy,rhz
      integer i,j,k

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

            rhx = two / (rho(i,j,k) + rho(i-1,j,k))
            uadv(i,j,k) = uadv(i,j,k) - gradpx(i,j,k) * rhx

        enddo
        enddo
        enddo

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

            rhy = two / (rho(i,j,k) + rho(i,j-1,k))
            vadv(i,j,k) = vadv(i,j,k) - gradpy(i,j,k) * rhy

        enddo
        enddo
        enddo

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

            rhz = two / (rho(i,j,k) + rho(i,j,k-1))
            wadv(i,j,k) = wadv(i,j,k) - gradpz(i,j,k) * rhz

        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,
     $                         hx,hy,hz,resnorm,
     $                         bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      implicit none

      integer DIMS
      REAL_T  resid(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T      f(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1,lo_3  :hi_3  )
      REAL_T sigmaz(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3+1)
      REAL_T hx
      REAL_T hy
      REAL_T hz
      REAL_T resnorm
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi

c     Local variables
      REAL_T hxsqinv, hysqinv, hzsqinv
      REAL_T rfac, corr
      integer i,j,k

      hxsqinv = one/(hx*hx)
      hysqinv = one/(hy*hy)
      hzsqinv = one/(hz*hz)

      resnorm = zero

      call bc(phi,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      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))*hxsqinv + 
     $           ( sigmay(i,j+1,k) + sigmay(i,j,k))*hysqinv +
     $           ( sigmaz(i,j,k+1) + sigmaz(i,j,k))*hzsqinv

          corr = 
     $      ( sigmax(i+1,j,k)*phi(i+1,j,k) + sigmax(i,j,k)*phi(i-1,j,k))*hxsqinv
     $     +( sigmay(i,j+1,k)*phi(i,j+1,k) + sigmay(i,j,k)*phi(i,j-1,k))*hysqinv
     $     +( sigmaz(i,j,k+1)*phi(i,j,k+1) + sigmaz(i,j,k)*phi(i,j,k-1))*hzsqinv

          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 ** GSRB **
c ** Gauss-Seidel red-black relaxation
c *************************************************************************

      subroutine FORT_GSRB(phi,f,sigmax,sigmay,sigmaz,DIMS,hx,hy,hz,
     $                     bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi,nngsrb)

      implicit none

      integer DIMS
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      REAL_T      f(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1,lo_3  :hi_3  )
      REAL_T sigmaz(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3+1)
      REAL_T hx
      REAL_T hy
      REAL_T hz
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi
      integer nngsrb

c     Local variables
      REAL_T hxsq, hysq, hzsq, rfac, corr
      integer i, j, k
      integer iter, ioff, iinc

      hxsq = hx*hx
      hysq = hy*hy
      hzsq = hz*hz

      call bc(phi,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      do iter = 1, nngsrb 
        do ioff = 0,1 
          do k = lo_3,hi_3 
          do j = lo_2,hi_2 
            iinc = mod(j+k+ioff,2)
            do i = lo_1+iinc,hi_1,2 

              rfac = (sigmax(i+1,j,k) + sigmax(i,j,k))/hxsq + 
     $               (sigmay(i,j+1,k) + sigmay(i,j,k))/hysq +
     $               (sigmaz(i,j,k+1) + sigmaz(i,j,k))/hzsq

              corr = 
     $        ( sigmax(i+1,j,k)*phi(i+1,j,k) + sigmax(i,j,k)*phi(i-1,j,k))/hxsq +
     $        ( sigmay(i,j+1,k)*phi(i,j+1,k) + sigmay(i,j,k)*phi(i,j-1,k))/hysq +
     $        ( sigmaz(i,j,k+1)*phi(i,j,k+1) + sigmaz(i,j,k)*phi(i,j,k-1))/hzsq

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

            enddo
          enddo
          enddo

          call bc(phi,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

        enddo
      enddo

      return
      end


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

      subroutine bc(phi,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      implicit none

      integer DIMS
      REAL_T  phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi

c     Local variables
      integer i, j, k, is, ie, js, je, ks, ke

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

      if (bcz_lo .eq. OUTLET) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ks-1) = -phi(i,j,ks)
        enddo
        enddo
      elseif (bcz_lo .eq. INLET) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ks-1) =  phi(i,j,ks)
        enddo
        enddo
      elseif (bcz_lo .eq. WALL) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ks-1) = phi(i,j,ks)
        enddo
        enddo
      elseif (bcz_lo .eq. PERIODIC) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ks-1) = phi(i,j,ke)
        enddo
        enddo
      else
        print *,'bogus bcz_lo in bc ',bcz_lo
        stop
      endif

      if (bcz_hi .eq. OUTLET) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ke+1) = -phi(i,j,ke)
        enddo
        enddo
      elseif (bcz_hi .eq. INLET) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ke+1) =  phi(i,j,ke)
        enddo
        enddo
      elseif (bcz_hi .eq. WALL) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ke+1) = phi(i,j,ke)
        enddo
        enddo
      elseif (bcz_hi .eq. PERIODIC) then
        do j = js,je 
        do i = is,ie 
          phi(i,j,ke+1) = phi(i,j,ks)
        enddo
        enddo
      else
        print *,'bogus bcz_hi in bc ',bcz_hi
        stop
      endif

      if (bcy_lo .eq. OUTLET) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,js-1,k) = -phi(i,js,k)
        enddo
        enddo
      elseif (bcy_lo .eq. INLET) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,js-1,k) =  phi(i,js,k)
        enddo
        enddo
      elseif (bcy_lo .eq. WALL) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,js-1,k) = phi(i,js,k)
        enddo
        enddo
      elseif (bcy_lo .eq. PERIODIC) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,js-1,k) = phi(i,je,k)
        enddo
        enddo
      else
        print *,'bogus bcy_lo in bc ',bcy_lo
        stop
      endif

      if (bcy_hi .eq. OUTLET) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,je+1,k) = -phi(i,je,k)
        enddo
        enddo
      elseif (bcy_hi .eq. INLET) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,je+1,k) =  phi(i,je,k)
        enddo
        enddo
      elseif (bcy_hi .eq. WALL) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,je+1,k) = phi(i,je,k)
        enddo
        enddo
      elseif (bcy_hi .eq. PERIODIC) then
        do k = ks-1,ke+1
        do i = is,ie 
          phi(i,je+1,k) = phi(i,js,k)
        enddo
        enddo
      else
        print *,'bogus bcy_hi in bc ',bcy_hi
        stop
      endif

      if (bcx_lo .eq. OUTLET) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(is-1,j,k) = -phi(is,j,k)
        enddo
        enddo
      elseif (bcx_lo .eq. INLET) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(is-1,j,k) =  phi(is,j,k)
        enddo
        enddo
      elseif (bcx_lo .eq. WALL) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(is-1,j,k) = phi(is,j,k)
        enddo
        enddo
      elseif (bcx_lo .eq. PERIODIC) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(is-1,j,k) = phi(ie,j,k)
        enddo
        enddo
      else
        print *,'bogus bcx_lo in bc ',bcx_lo
        stop
      endif

      if (bcx_hi .eq. OUTLET) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(ie+1,j,k) = -phi(ie,j,k)
        enddo
        enddo
      elseif (bcx_hi .eq. INLET) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(ie+1,j,k) =  phi(ie,j,k)
        enddo
        enddo
      elseif (bcx_hi .eq. WALL) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(ie+1,j,k) = phi(ie,j,k)
        enddo
        enddo
      elseif (bcx_hi .eq. PERIODIC) then
        do k = ks-1,ke+1 
        do j = js-1,je+1 
          phi(ie+1,j,k) = phi(is,j,k)
        enddo
        enddo
      else
        print *,'bogus bcx_hi in bc ',bcx_hi
        stop
      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,DIMS,hx,hy,hz,rhsnorm)

      implicit none

      integer DIMS
      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  :hi_1  ,lo_2:hi_2  ,lo_3:hi_3  )
      REAL_T  hx
      REAL_T  hy
      REAL_T  hz
      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) = (uadv(i+1,j,k) - uadv(i,j,k)) / hx +
     $                 (vadv(i,j+1,k) - vadv(i,j,k)) / hy +
     $                 (wadv(i,j,k+1) - wadv(i,j,k)) / hz
 
          rhs(i,j,k) = rhs(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)

      implicit none

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

c     Local variables
      integer i,j,k,twoi,twoj,twok

      do k = loc_3,hic_3 
        twok = 2*(k-loc_3)+lo_3

        do j = loc_2,hic_2 
          twoj = 2*(j-loc_2)+lo_2

          do i = loc_1,hic_1+1 
            twoi = 2*(i-loc_1)+lo_1

            sigmaxc(i,j,k) = fourth*(sigmax(twoi,twoj  ,twok  ) + 
     $                               sigmax(twoi,twoj+1,twok  ) +
     $                               sigmax(twoi,twoj  ,twok+1) +
     $                               sigmax(twoi,twoj+1,twok+1) )
        enddo
      enddo
      enddo

      do k = loc_3,hic_3 
        twok = 2*(k-loc_3)+lo_3

        do j = loc_2,hic_2+1 
          twoj = 2*(j-loc_2)+lo_2

          do i = loc_1,hic_1 
            twoi = 2*(i-loc_1)+lo_1

            sigmayc(i,j,k) = fourth*(sigmay(twoi  ,twoj,twok  ) + 
     $                               sigmay(twoi+1,twoj,twok  ) +
     $                               sigmay(twoi  ,twoj,twok+1) +
     $                               sigmay(twoi+1,twoj,twok+1) )
          enddo
        enddo
      enddo

      do k = loc_3,hic_3+1 
        twok = 2*(k-loc_3)+lo_3

        do j = loc_2,hic_2
          twoj = 2*(j-loc_2)+lo_2

          do i = loc_1,hic_1 
            twoi = 2*(i-loc_1)+lo_1

              sigmazc(i,j,k) = fourth*(sigmaz(twoi  ,twoj  ,twok) + 
     $                                 sigmaz(twoi+1,twoj  ,twok) +
     $                                 sigmaz(twoi  ,twoj+1,twok) +
     $                                 sigmaz(twoi+1,twoj+1,twok) )
          enddo
        enddo
      enddo


      return
      end

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

      subroutine FORT_RESTRICT(res,resc,DIMS,CDIMS)

      implicit none

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

c     Local variables
      integer i,j,k,twoi,twoj,twok

      do k = loc_3,hic_3 
        twok = 2*(k-loc_3)+lo_3
        do j = loc_2,hic_2 
          twoj = 2*(j-loc_2)+lo_2
          do i = loc_1,hic_1 
            twoi = 2*(i-loc_1)+lo_1
            resc(i,j,k) = (res(twoi  ,twoj,twok  ) + res(twoi  ,twoj+1,twok  ) + 
     $                     res(twoi+1,twoj,twok  ) + res(twoi+1,twoj+1,twok  ) +
     $                     res(twoi  ,twoj,twok+1) + res(twoi  ,twoj+1,twok+1) +
     $                     res(twoi+1,twoj,twok+1) + res(twoi+1,twoj+1,twok+1) )
            resc(i,j,k) = resc(i,j,k) * eighth
        enddo
      enddo
      enddo

      return
      end

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

      subroutine FORT_INTERPOLATE(phi,deltac,DIMS,CDIMS)

      implicit none

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

c     Local variables
      integer i,j,k,twoi,twoj,twok

      do k = loc_3,hic_3 
        twok = 2*(k-loc_3)+lo_3
        do j = loc_2,hic_2 
          twoj = 2*(j-loc_2)+lo_2
          do i = loc_1,hic_1 
            twoi = 2*(i-loc_1)+lo_1

            phi(twoi  ,twoj  ,twok  ) = phi(twoi  ,twoj  ,twok  ) + deltac(i,j,k)
            phi(twoi+1,twoj  ,twok  ) = phi(twoi+1,twoj  ,twok  ) + deltac(i,j,k)
            phi(twoi  ,twoj+1,twok  ) = phi(twoi  ,twoj+1,twok  ) + deltac(i,j,k)
            phi(twoi+1,twoj+1,twok  ) = phi(twoi+1,twoj+1,twok  ) + deltac(i,j,k)
            phi(twoi  ,twoj  ,twok+1) = phi(twoi  ,twoj  ,twok+1) + deltac(i,j,k)
            phi(twoi+1,twoj  ,twok+1) = phi(twoi+1,twoj  ,twok+1) + deltac(i,j,k)
            phi(twoi  ,twoj+1,twok+1) = phi(twoi  ,twoj+1,twok+1) + deltac(i,j,k)
            phi(twoi+1,twoj+1,twok+1) = phi(twoi+1,twoj+1,twok+1) + deltac(i,j,k)
          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 , hx, hy, hz, 
     $                         bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi,
     $                         norm, prob_norm)

      implicit none

      integer DIMS

      REAL_T   dest(lo_1-1:hi_1+1,lo_2-1:hi_2+1,lo_3-1:hi_3+1)
      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  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1,lo_3  :hi_3  )
      REAL_T sigmaz(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3+1)
      REAL_T    sum(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T      r(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T      w(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T      z(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T   work(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3  :hi_3  )
      REAL_T hx
      REAL_T hy
      REAL_T hz
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi
      REAL_T norm
      REAL_T prob_norm

c     Local variables
      integer i,j,k,is,ie,js,je,ks,ke,iter
      REAL_T alpha, beta, rho, rhol
      REAL_T local_norm
      REAL_T  tol, tolfac
      REAL_T hxsqinv
      REAL_T hysqinv
      REAL_T hzsqinv

      tolfac = 1.0d-3

      hxsqinv = one/(hx*hx)
      hysqinv = one/(hy*hy)
      hzsqinv = one/(hz*hz)

      call bc(dest,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      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) )*hxsqinv + 
     $     ( sigmay(i,j+1,k)*dest0(i,j+1,k) + 
     $       sigmay(i,j  ,k)*dest0(i,j-1,k) )*hysqinv +
     $     ( sigmaz(i,j,k+1)*dest0(i,j,k+1) + 
     $       sigmaz(i,j,k  )*dest0(i,j,k-1) )*hzsqinv - 
     $    ( (sigmax(i+1,j,k) + sigmax(i,j,k))*hxsqinv + 
     $      (sigmay(i,j+1,k) + sigmay(i,j,k))*hysqinv +
     $      (sigmaz(i,j,k+1) + sigmaz(i,j,k))*hzsqinv )*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 bc(dest,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      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) )*hxsqinv + 
     $     ( sigmay(i,j+1,k)*dest(i,j+1,k) + 
     $       sigmay(i,j  ,k)*dest(i,j-1,k) )*hysqinv +
     $     ( sigmaz(i,j,k+1)*dest(i,j,k+1) + 
     $       sigmaz(i,j,k  )*dest(i,j,k-1) )*hzsqinv - 
     $    ( (sigmax(i+1,j,k) + sigmax(i,j,k))*hxsqinv + 
     $      (sigmay(i,j+1,k) + sigmay(i,j,k))*hysqinv +
     $      (sigmaz(i,j,k+1) + sigmaz(i,j,k))*hzsqinv )*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

      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

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

      return
      end

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

      subroutine FORT_MKSUMMAC(sigmax,sigmay,sigmaz,sum,DIMS,hx,hy,hz,
     $                         bcx_lo,bcx_hi,bcy_lo,bcy_hi,bcz_lo,bcz_hi)

      implicit none

      integer DIMS
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  ,lo_3:hi_3  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1,lo_3:hi_3  )
      REAL_T sigmaz(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3:hi_3+1)
      REAL_T    sum(lo_1  :hi_1  ,lo_2  :hi_2  ,lo_3:hi_3)
      REAL_T hx
      REAL_T hy
      REAL_T hz
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi
      integer bcz_lo,bcz_hi

c     Local variables
      REAL_T  hxsqinv
      REAL_T  hysqinv
      REAL_T  hzsqinv
      integer i,j,k

      hxsqinv = one/(hx*hx)
      hysqinv = one/(hy*hy)
      hzsqinv = one/(hz*hz)

      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))*hxsqinv +
     $                 (sigmay(i,j+1,k) + sigmay(i,j,k))*hysqinv +
     $                 (sigmaz(i,j,k+1) + sigmaz(i,j,k))*hzsqinv
          sum(i,j,k) = -sixth*sum(i,j,k)
        enddo
      enddo
      enddo

      return
      end
