/*
** (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,hi_1,hi_2
#define CDIMS loc_1,loc_2,hic_1,hic_2

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

      subroutine FORT_INITSIGMA(sigmax,sigmay,rho,r,rhalf,DIMS,
     $                          bcx_lo,bcx_hi,bcy_lo,bcy_hi)

      implicit none

      integer DIMS
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1)
      REAL_T    rho(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T      r(lo_1-1:hi_1+1)
      REAL_T  rhalf(lo_1:hi_1+2)
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi

c     Local variables
      integer i,j

      do j = lo_2,hi_2 
        do i = lo_1+1,hi_1 
          sigmax(i,j) = two * rhalf(i) /
     $                  (rho(i,j) + rho(i-1,j))
        enddo

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

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

      do i = lo_1,hi_1 
        do j = lo_2+1,hi_2 
          sigmay(i,j) = two * r(i) /
     $                  (rho(i,j) + rho(i,j-1))
        enddo

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

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

      return
      end

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

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

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T gradpx(lo_1:hi_1+1,lo_2:hi_2  )
      REAL_T gradpy(lo_1:hi_1  ,lo_2:hi_2+1)
      REAL_T hx
      REAL_T hy
      integer bcx_lo, bcx_hi
      integer bcy_lo, bcy_hi

c     Local variables
      integer i, is, ie, j, js, je

      is = lo_1
      ie = hi_1
      js = lo_2
      je = hi_2

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

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

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

      enddo

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

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

      enddo

      return
      end

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

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

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T   uadv(lo_1  :hi_1+1,lo_2:hi_2  )
      REAL_T   vadv(lo_1  :hi_1  ,lo_2:hi_2+1)
      REAL_T gradpx(lo_1  :hi_1+1,lo_2:hi_2  )
      REAL_T gradpy(lo_1  :hi_1  ,lo_2:hi_2+1)
      REAL_T    rho(lo_1-1:hi_1+1,lo_2-1:hi_2+1)

c     Local variables
      REAL_T  rhx,rhy
      integer i,j

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

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

        enddo
      enddo

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

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

        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,r,DIMS,
     $                         hx,hy,resnorm,bcx_lo,bcx_hi,bcy_lo,bcy_hi)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T  resid(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T      f(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1)
      REAL_T      r(lo_1-1:hi_1+1)
      REAL_T hx
      REAL_T hy
      REAL_T resnorm
      integer bcx_lo, bcx_hi
      integer bcy_lo, bcy_hi

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

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

      resnorm = zero

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

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

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

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

          resnorm = max(abs(resid(i,j))/r(i), resnorm)

        enddo
      enddo

      return
      end

c *************************************************************************
c ** GSRB **
c ** Gauss-Seidel red-black or line solve relaxation (depending on hy/hx)
c *************************************************************************

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

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T    phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T      f(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1)
      REAL_T hx
      REAL_T hy
      integer bcx_lo, bcx_hi
      integer bcy_lo, bcy_hi
      integer nngsrb

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

c     Additional temporaries for the line solve
      REAL_T a_ls(0:4096)
      REAL_T b_ls(0:4096)
      REAL_T c_ls(0:4096)
      REAL_T r_ls(0:4096)
      REAL_T u_ls(0:4096)
      REAL_T cf0, cf1, cf2, cf3
      REAL_T delta

      integer do_line
      integer ilen,jlen

      if (hy. gt. 1.5*hx) then
        do_line = 1
        ilen = hi_1-lo_1+1
        if (ilen .gt. 4096) then
          print *,'TOO BIG FOR LINE SOLVE IN GSRB: ilen = ',ilen
          stop
        endif
      else if (hx .gt. 1.5*hy) then
        do_line = 2
        jlen = hi_2-lo_2+1
        if (jlen .gt. 4096) then
          print *,'TOO BIG FOR LINE SOLVE IN GSRB: jlen = ',jlen
          stop
        endif
      else
        do_line = 0
      endif

      hxsq = hx*hx
      hysq = hy*hy

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

      do iter = 1, nngsrb 

c   Standard Gauss-Seidel relaxation with red-black ordering
        if (do_line .eq. 0) then
         do ioff = 0,1 
          do j = lo_2,hi_2 
            iinc = mod(j+ioff,2)
            do i = lo_1+iinc,hi_1,2 

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

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

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

            enddo
          enddo

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

         enddo

c   Line solve in the horizontal direction
        elseif (do_line .eq. 1) then

         do ioff = 0,1 
          do j = lo_2+ioff,hi_2,2 

            cf1 = cvmgt(one, zero, (j .eq. lo_2) .and. 
     $                  (bcy_lo .eq. WALL .or. bcy_lo.eq. INLET))
            cf1 = cvmgt(-one, cf1, (j .eq. lo_2) .and. bcy_lo .eq. OUTLET)

            cf3 = cvmgt(one, zero, (j .eq. hi_2) .and. 
     $                  (bcy_hi .eq. WALL .or. bcy_hi.eq. INLET))
            cf3 = cvmgt(-one, cf3, (j .eq. hi_2) .and. bcy_hi .eq. OUTLET)

            do i = lo_1,hi_1
              a_ls(i-lo_1) = sigmax(i,j) / hxsq

              cf0 = cvmgt(one, zero, (i .eq. lo_1) .and. 
     $                    (bcx_lo .eq. WALL .or. bcx_lo .eq. INLET))
              cf0 = cvmgt(-one, cf0, (i .eq. lo_1) .and. bcx_lo .eq. OUTLET)

              cf2 = cvmgt(one, zero, (i .eq. hi_1) .and. 
     $                    (bcx_hi .eq. WALL .or. bcx_hi .eq. INLET))
              cf2 = cvmgt(-one, cf2, (i .eq. hi_1) .and. bcx_hi .eq. OUTLET)

              delta = (sigmax(i,j)*cf0 + sigmax(i+1,j)*cf2) / hxsq
     $              + (sigmay(i,j)*cf1 + sigmay(i,j+1)*cf3) / hysq
              b_ls(i-lo_1) = -(sigmax(i+1,j) + sigmax(i,j)) / hxsq
     $                       -(sigmay(i,j+1) + sigmay(i,j)) / hysq
              c_ls(i-lo_1) = sigmax(i+1,j) / hxsq
              r_ls(i-lo_1) = f(i,j) -
     $           (sigmay(i,j)*phi(i,j-1) + sigmay(i,j+1)*phi(i,j+1)) / hysq
            enddo

            call tridiag(a_ls,b_ls,c_ls,r_ls,u_ls,ilen)

            do i = lo_1,hi_1
              phi(i,j) = u_ls(i-lo_1)
            enddo

          enddo

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

         enddo


c   Line solve in the vertical direction
        elseif (do_line .eq. 2) then

         do ioff = 0,1 
          do i = lo_1+ioff,hi_1,2 

            cf0 = cvmgt(one, zero, (i .eq. lo_1) .and. 
     $                  (bcx_lo .eq. WALL .or. bcx_lo .eq. INLET))
            cf0 = cvmgt(-one, cf0, (i .eq. lo_1) .and. bcx_lo .eq. OUTLET)

            cf2 = cvmgt(one, zero, (i .eq. hi_1) .and. 
     $                  (bcx_hi .eq. WALL .or. bcx_hi .eq. INLET))
            cf2 = cvmgt(-one, cf2, (i .eq. hi_1) .and. bcx_hi .eq. OUTLET)

            do j = lo_2,hi_2
              a_ls(j-lo_2) = sigmay(i,j) / hysq
              cf1 = cvmgt(one, zero, (j .eq. lo_2) .and. 
     $                    (bcy_lo .eq. WALL .or. bcy_lo.eq. INLET))
              cf1 = cvmgt(-one, cf1, (j .eq. lo_2) .and. bcy_lo .eq. OUTLET)

              cf3 = cvmgt(one, zero, (j .eq. hi_2) .and. 
     $                    (bcy_hi .eq. WALL .or. bcy_hi.eq. INLET))
              cf3 = cvmgt(-one, cf3, (j .eq. hi_2) .and. bcy_hi .eq. OUTLET)
              delta = (sigmax(i,j)*cf0 + sigmax(i+1,j)*cf2) / hxsq
     $              + (sigmay(i,j)*cf1 + sigmay(i,j+1)*cf3) / hysq
              b_ls(j-lo_2) = -(sigmax(i+1,j) + sigmax(i,j)) / hxsq
     $                       -(sigmay(i,j+1) + sigmay(i,j)) / hysq + delta
              c_ls(j-lo_2) = sigmay(i,j+1) / hysq
              r_ls(j-lo_2) = f(i,j) -
     $           (sigmax(i,j)*phi(i-1,j) + sigmax(i+1,j)*phi(i+1,j)) / hxsq
            enddo

            call tridiag(a_ls,b_ls,c_ls,r_ls,u_ls,jlen)

            do j = lo_2,hi_2
              phi(i,j) = u_ls(j-lo_2)
            enddo

          enddo

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

         enddo

       endif

      enddo

      return
      end


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

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

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T  phi(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      integer bcx_lo,bcx_hi
      integer bcy_lo,bcy_hi

c     Local variables
      integer i, j, is, ie, js, je

      is = lo_1
      ie = hi_1
      js = lo_2
      je = hi_2

      if (bcy_lo .eq. OUTLET) then
        do i = is,ie 
          phi(i,js-1) = -phi(i,js)
        enddo
      elseif (bcy_lo .eq. INLET .or. bcy_lo .eq. WALL) then
        do i = is-1,ie+1 
          phi(i,js-1) =  phi(i,js)
        enddo
      elseif (bcy_lo .eq. PERIODIC) then
        do i = is,ie 
          phi(i,js-1) = phi(i,je)
        enddo
      endif

      if (bcy_hi .eq. OUTLET) then
        do i = is,ie 
          phi(i,je+1) = -phi(i,je)
        enddo
      elseif (bcy_hi .eq. INLET .or. bcy_hi .eq. WALL) then
        do i = is-1,ie+1 
          phi(i,je+1) = phi(i,je)
        enddo
      elseif (bcy_hi .eq. PERIODIC) then
        do i = is,ie 
          phi(i,je+1) = phi(i,js)
        enddo
      endif

      if (bcx_lo .eq. OUTLET) then
        do j = js,je 
          phi(is-1,j) = -phi(is,j)
        enddo
      elseif (bcx_lo .eq. INLET .or. bcx_lo .eq. WALL) then
        do j = js-1,je+1 
          phi(is-1,j) =  phi(is,j)
        enddo
      elseif (bcx_lo .eq. PERIODIC) then
        do j = js,je 
          phi(is-1,j) = phi(ie,j)
        enddo
      endif

      if (bcx_hi .eq. OUTLET) then
        do j = js,je 
          phi(ie+1,j) = -phi(ie,j)
        enddo
      elseif (bcx_hi .eq. INLET .or. bcx_hi .eq. WALL) then
        do j = js-1,je+1 
          phi(ie+1,j) = phi(ie,j)
        enddo
      elseif (bcx_hi .eq. PERIODIC) then
        do j = js,je 
          phi(ie+1,j) = phi(is,j)
        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,divu_src,rhs,
     $                       DIMS,hx,hy,r,rhalf,rhsnorm)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T     uadv(lo_1  :hi_1+1,lo_2:hi_2  )
      REAL_T     vadv(lo_1  :hi_1  ,lo_2:hi_2+1)
      REAL_T divu_src(lo_1  :hi_1  ,lo_2:hi_2)
      REAL_T      rhs(lo_1  :hi_1  ,lo_2:hi_2)
      REAL_T        r(lo_1-1:hi_1+1)
      REAL_T    rhalf(lo_1:hi_1+2)
      REAL_T  hx
      REAL_T  hy
      REAL_T  rhsnorm

c     Local variables
      integer i,j

      rhsnorm = zero

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

          rhs(i,j) = (rhalf(i+1)*uadv(i+1,j) - 
     $                rhalf(i  )*uadv(i  ,j))/hx + 
     $            r(i  )*(vadv(i,j+1) - vadv(i,j))/hy

          rhs(i,j) = rhs(i,j) - r(i)*divu_src(i,j)

          rhsnorm = max(rhsnorm,abs(rhs(i,j))/r(i))

        enddo
      enddo

      return
      end

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

      subroutine FORT_COARSIGMA(sigmax,sigmay,sigmaxc,sigmayc,DIMS,CDIMS)

      implicit none
      integer lo_1, lo_2
      integer hi_1, hi_2
      integer loc_1, loc_2
      integer hic_1, hic_2
      REAL_T  sigmax(lo_1 :hi_1 +1,lo_2 :hi_2   )
      REAL_T  sigmay(lo_1 :hi_1   ,lo_2 :hi_2 +1)
      REAL_T sigmaxc(loc_1:hic_1+1,loc_2:hic_2  )
      REAL_T sigmayc(loc_1:hic_1  ,loc_2:hic_2+1)

c     Local variables
      integer i,j,twoi,twoj

      do j = loc_2,hic_2 
        do i = loc_1,hic_1+1 
          twoi = 2*(i-loc_1)+lo_1
          twoj = 2*(j-loc_2)+lo_2
          sigmaxc(i,j) = half*(sigmax(twoi,twoj) + sigmax(twoi,twoj+1))
        enddo
      enddo

      do j = loc_2,hic_2+1 
        do i = loc_1,hic_1 
          twoi = 2*(i-loc_1)+lo_1
          twoj = 2*(j-loc_2)+lo_2
          sigmayc(i,j) = half*(sigmay(twoi,twoj) + sigmay(twoi+1,twoj))
        enddo
      enddo


      return
      end

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

      subroutine FORT_RESTRICT(res,resc,DIMS,CDIMS)

      implicit none
      integer lo_1, lo_2
      integer hi_1, hi_2
      integer loc_1, loc_2
      integer hic_1, hic_2
      REAL_T  res(lo_1 :hi_1 ,lo_2 :hi_2)
      REAL_T resc(loc_1:hic_1,loc_2:hic_2)

c     Local variables
      integer i,j,twoi,twoj

c ::: NOTE: dont need factor of r here for volume-weighting because
c ::: what were calling the residual is really already r*residual

      do j = loc_2,hic_2 
        do i = loc_1,hic_1 
          twoi = 2*(i-loc_1)+lo_1
          twoj = 2*(j-loc_2)+lo_2
          resc(i,j) = ((res(twoi  ,twoj) + res(twoi  ,twoj+1)) + 
     $                 (res(twoi+1,twoj) + res(twoi+1,twoj+1)))*fourth
        enddo
      enddo

      return
      end

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

      subroutine FORT_INTERPOLATE(phi,deltac,DIMS,CDIMS)

      implicit none
      integer lo_1, lo_2
      integer hi_1, hi_2
      integer loc_1, loc_2
      integer hic_1, hic_2
      REAL_T    phi(lo_1 -1:hi_1 +1,lo_2 -1:hi_2 +1)
      REAL_T deltac(loc_1-1:hic_1+1,loc_2-1:hic_2+1)

c     Local variables
      integer i,j,twoi,twoj

      do j = loc_2, hic_2 
        do i = loc_1, hic_1 

          twoi = 2*(i-loc_1)+lo_1
          twoj = 2*(j-loc_2)+lo_2
          phi(twoi  ,twoj  ) = phi(twoi  ,twoj  ) + deltac(i,j)
          phi(twoi+1,twoj  ) = phi(twoi+1,twoj  ) + deltac(i,j)
          phi(twoi  ,twoj+1) = phi(twoi  ,twoj+1) + deltac(i,j)
          phi(twoi+1,twoj+1) = phi(twoi+1,twoj+1) + deltac(i,j)

        enddo
      enddo

      return
      end

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

      subroutine FORT_SOLVEMAC(dest, dest0, source, sigmax, sigmay, sum, 
     $                         r, w, z, work, x, DIMS, hx, hy, 
     $                         bcx_lo, bcx_hi, bcy_lo, bcy_hi, norm, prob_norm)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T   dest(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T  dest0(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T source(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1)
      REAL_T    sum(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T      r(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T      w(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T      z(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T   work(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T      x(lo_1-1:hi_1+1)
      REAL_T hx
      REAL_T hy
      integer bcx_lo, bcx_hi
      integer bcy_lo, bcy_hi
      REAL_T norm
      REAL_T prob_norm

c     Local variables
      integer i,j,iter,is,ie,js,je
      REAL_T alpha, beta, rho, rhol
      REAL_T  tol,tolfac
      REAL_T local_norm
      REAL_T hxsqinv
      REAL_T hysqinv
 
      tolfac = 1.0d-3

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

      is = lo_1
      js = lo_2
      ie = hi_1
      je = hi_2

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

      do j = js-1,je+1 
        do i = is-1,ie+1 
          dest0(i,j) = dest(i,j)
           dest(i,j) = zero
        enddo
      enddo

  10  do j = js,je 
        do i = is,ie 
          w(i,j) = 
     $     ( sigmax(i+1,j)*dest0(i+1,j) + 
     $       sigmax(i  ,j)*dest0(i-1,j) )*hxsqinv + 
     $     ( sigmay(i,j+1)*dest0(i,j+1) + 
     $       sigmay(i,j  )*dest0(i,j-1) )*hysqinv - 
     $    ( (sigmax(i+1,j) + sigmax(i,j))*hxsqinv + 
     $      (sigmay(i,j+1) + sigmay(i,j))*hysqinv )*dest0(i,j)
        enddo
      enddo

      rho = zero
      norm = zero

      do j = js, je 
        do i = is, ie 
          r(i,j) = source(i,j) - w(i,j)
        enddo
      enddo

      local_norm = zero
      do j = js, je 
        do i = is, ie 
          z(i,j) = r(i,j) 
          rho = rho + z(i,j) * r(i,j)
          local_norm = max(local_norm,abs(r(i,j)))
          norm       = max(norm      ,abs(r(i,j)/x(i)))
        enddo
      enddo

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

      do j = js, je 
        do i = is, ie 
          work(i,j) = zero
          dest(i,j) = z(i,j)
        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)

      do j = js,je 
        do i = is,ie 
          w(i,j) = 
     $     ( sigmax(i+1,j)*dest(i+1,j) + 
     $       sigmax(i,j)*dest(i-1,j) )*hxsqinv + 
     $     ( sigmay(i,j+1)*dest(i,j+1) + 
     $       sigmay(i,j)*dest(i,j-1) )*hysqinv - 
     $    ( (sigmax(i+1,j) + sigmax(i,j))*hxsqinv + 
     $      (sigmay(i,j+1) + sigmay(i,j))*hysqinv )*dest(i,j)
        enddo
      enddo


      alpha = zero
      do j = js, je 
        do i = is, ie 
          alpha = alpha + dest(i,j)*w(i,j)
        enddo
      enddo

      alpha = rho / alpha
      rhol = rho
      rho = zero
      norm = zero
      do j = js, je 
        do i = is, ie 
          work(i,j) = work(i,j) + alpha * dest(i,j)
          r(i,j) = r(i,j) - alpha * w(i,j)
          z(i,j) = r(i,j) 
          rho = rho + z(i,j) * r(i,j)
          norm = max(norm,abs(r(i,j)/x(i)))
        enddo
      enddo

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

      if (norm .le. tol) then
         
         do j = js, je 
            do i = is, ie 
               dest(i,j) = work(i,j) + dest0(i,j)
            enddo
         enddo

         call bc(dest,DIMS,bcx_lo,bcx_hi,bcy_lo,bcy_hi)
         
      else if (iter .ge. 100  .or.  norm .ge. 100.d0*local_norm) then

         tolfac = 10.d0 * tolfac
         iter = 1
         do j = js, je 
            do i = is, ie 
               dest(i,j) = zero
            enddo
         enddo
         goto 10

      else

        beta = rho / rhol
        do j = js, je 
          do i = is, ie 
            dest(i,j) = z(i,j) + beta * dest(i,j)
          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(sigmax,sigmay,sum,DIMS,hx,hy,
     $                         bcx_lo,bcx_hi,bcy_lo,bcy_hi)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      REAL_T sigmax(lo_1  :hi_1+1,lo_2  :hi_2  )
      REAL_T sigmay(lo_1  :hi_1  ,lo_2  :hi_2+1)
      REAL_T    sum(lo_1  :hi_1  ,lo_2  :hi_2  )
      REAL_T hx
      REAL_T hy
      integer bcx_lo, bcx_hi
      integer bcy_lo, bcy_hi

c     Local variables
      REAL_T  hxsqinv
      REAL_T  hysqinv
      integer i,j

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

      do j = lo_2,hi_2
        do i = lo_1,hi_1
          sum(i,j) = (sigmax(i+1,j) + sigmax(i,j))*hxsqinv +
     $               (sigmay(i,j+1) + sigmay(i,j))*hysqinv
          sum(i,j) = -fourth*sum(i,j)
        enddo
      enddo

      return
      end



c *************************************************************************
c ** TRIDIAG **
c ** Do a tridiagonal solve 
c *************************************************************************

      subroutine tridiag(a,b,c,r,u,n)

      integer n
      integer nmax

      REAL_T a(n)
      REAL_T b(n)
      REAL_T c(n)
      REAL_T r(n)
      REAL_T u(n)

      parameter (nmax = 4098)

      integer j
      REAL_T bet
      REAL_T gam(nmax)
      if (b(1) .eq. 0) print *,'CANT HAVE B(1) = ZERO'

      bet = b(1)
      u(1) = r(1)/bet

      do j = 2,n
        gam(j) = c(j-1)/bet
        bet = b(j) - a(j)*gam(j)
        if (bet .eq. 0) then
          print *,'TRIDIAG FAILED '
          stop
        endif
        u(j) = (r(j)-a(j)*u(j-1))/bet
      enddo

      do j = n-1,1,-1
        u(j) = u(j) - gam(j+1)*u(j+1)
      enddo

      return
      end

