/*
** (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 "HGPROJ_F.H"
#include "BCTypes.H"

#if BL_USE_FLOAT
#define sixteenth  .0625e0
#else
#define sixteenth  .0625d0
#endif

#define DIMS lo_1,lo_2,hi_1,hi_2
#define CDIMS loc_1,loc_2,hic_1,hic_2
#define GDIMS g_lo_1,g_lo_2,g_hi_1,g_hi_2
#define PDIMS p_lo_1,p_lo_2,p_hi_1,p_hi_2

#define NINEPT

c *************************************************************************
c ** GRADHG **
c ** Compute the cell-centered gradient of the nodal pressure field
c *************************************************************************

      subroutine FORT_GRADHG(gphi,GDIMS,phi,PDIMS,DIMS,dx)

      implicit none

      integer DIMS
      integer p_lo_1, p_lo_2
      integer p_hi_1, p_hi_2
      integer g_lo_1, g_lo_2
      integer g_hi_1, g_hi_2
      REAL_T  gphi(g_lo_1:g_hi_1,g_lo_2:g_hi_2,2)
      REAL_T   phi(p_lo_1:p_hi_1,p_lo_2:p_hi_2  )
      REAL_T  dx(2)

c     Local variables
      integer i, j

      do j = lo_2,hi_2
        do i = lo_1,hi_1
          gphi(i,j,1) = half*(phi(i+1,j) + phi(i+1,j+1) - 
     $                        phi(i  ,j) - phi(i  ,j+1) ) /dx(1)
          gphi(i,j,2) = half*(phi(i,j+1) + phi(i+1,j+1) - 
     $                        phi(i,j  ) - phi(i+1,j  ) ) /dx(2)
        enddo
      enddo

      return
      end

c *************************************************************************
c ** RHSHG **
c ** Compute the right-hand-side D(V) for the projection
c *************************************************************************

      subroutine FORT_RHSHG(du,u,divu_src,vol,DIMS,dx,bc,norm,ng,is_singular,
     $                      sum_src,sum_fac)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T        du(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T         u(lo_1-3:hi_1+3,lo_2-3:hi_2+3,2)
      REAL_T  divu_src(lo_1  :hi_1+1,lo_2  :hi_2+1)
      REAL_T       vol(lo_1-1:hi_1+1,lo_2-1:hi_2+1)
      REAL_T  dx(2)
      REAL_T  sum_src,sum_fac
      integer is_singular
      integer bc(2,2)

c     Local variables
      REAL_T fac,norm
      REAL_T factor
      REAL_T vavg
      REAL_T hx,hy
      integer i, j
      integer istart,iend
      integer jstart,jend

      hx = dx(1)
      hy = dx(2)

      norm = zero

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)

      if (BCX_LO .eq. WALL) then
        do j = lo_2-1,hi_2+1
          u(lo_1-1,j,1) = zero
          u(lo_1-1,j,2) = zero
        enddo
      endif
      if (BCX_HI .eq. WALL) then
        do j = lo_2-1,hi_2+1
          u(hi_1+1,j,1) = zero
          u(hi_1+1,j,2) = zero
        enddo
      endif
      if (BCY_LO .eq. WALL) then
        do i = lo_1-1,hi_1+1
          u(i,lo_2-1,1) = zero
          u(i,lo_2-1,2) = zero
        enddo
      endif
      if (BCY_HI .eq. WALL) then
        do i = lo_1-1,hi_1+1
          u(i,hi_2+1,1) = zero
          u(i,hi_2+1,2) = zero
        enddo
      endif

      do j = jstart,jend 
      do i = istart,iend 

         du(i,j) = ( vol(i  ,j)*u(i  ,j,1) + vol(i  ,j-1)*u(i  ,j-1,1) 
     $              -vol(i-1,j)*u(i-1,j,1) - vol(i-1,j-1)*u(i-1,j-1,1)) / (two*hx) +
     $             ( vol(i  ,j)*u(i  ,j,2) - vol(i  ,j-1)*u(i  ,j-1,2) 
     $              +vol(i-1,j)*u(i-1,j,2) - vol(i-1,j-1)*u(i-1,j-1,2)) / (two*hy)

         fac = one
         fac = cvmgt(two*fac,fac,i.eq.lo_1 .and. 
     $               (BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) )
         fac = cvmgt(two*fac,fac,i.eq.hi_1+1 .and. 
     $               (BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) )
         fac = cvmgt(two*fac,fac,j.eq.lo_2 .and. 
     $               (BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) )
         fac = cvmgt(two*fac,fac,j.eq.hi_2+1 .and. 
     $               (BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) )

         du(i,j) = fac * du(i,j)

         vavg = fourth * (vol(i,j) + vol(i-1,j) + vol(i,j-1) + vol(i-1,j-1))

         du(i,j) = du(i,j) - vavg * divu_src(i,j)

      enddo
      enddo

      if (is_singular .eq. 1) then
        sum_src  = zero
        sum_fac  = zero
        do j = jstart, jend 
        do i = istart, iend 
          factor = one
          factor = cvmgt(half*factor,factor,i.eq.lo_1 .or. i.eq.hi_1+1)
          factor = cvmgt(half*factor,factor,j.eq.lo_2 .or. j.eq.hi_2+1)
          sum_src = sum_src + factor * du(i,j)
          sum_fac = sum_fac + factor
        enddo
        enddo
      endif

      do j = jstart,jend 
         do i = istart,iend 
           norm = max(norm, abs(du(i,j)))
         enddo
      enddo

      return
      end

c *************************************************************************
c ** ADJUSTRHS **
c ** Adjust the RHS so it sums to zero if the problem has no outflow boundaries.
c *************************************************************************

      subroutine FORT_ADJUSTRHS(du,DIMS,bc,ng,sum_src)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer bc(2,2)
      integer ng
      REAL_T  du(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  sum_src

c     Local variables
      integer i, j
      integer istart,iend,jstart,jend

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)

      do j = jstart,jend 
         do i = istart,iend 
           du(i,j) = du(i,j) - sum_src
         enddo
      enddo

      return
      end

c *************************************************************************
c ** PROJUHG **
c ** Define the updated pressure and vector field
c *************************************************************************

      subroutine FORT_PROJUHG(u,pressure,phi,gradphi,rhonph,DIMS,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T         u(lo_1-3:hi_1+3,lo_2-3:hi_2+3,2)
      REAL_T  pressure(lo_1  :hi_1+1,lo_2  :hi_2+1)
      REAL_T       phi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T   gradphi(lo_1-1:hi_1+1,lo_2-1:hi_2+1,2)
      REAL_T    rhonph(lo_1-1:hi_1+1,lo_2-1:hi_2+1)

c     Local variables
      integer i, j

      do j = lo_2,hi_2 
        do i = lo_1,hi_1 
          u(i,j,1) = u(i,j,1) - gradphi(i,j,1)/rhonph(i,j)
          u(i,j,2) = u(i,j,2) - gradphi(i,j,2)/rhonph(i,j)
        enddo
      enddo

      do j = lo_2,hi_2+1
        do i = lo_1,hi_1+1
c          pressure(i,j) = pressure(i,j) + phi(i,j)
           pressure(i,j) = phi(i,j)
        enddo
      enddo

      return
      end

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

      subroutine FORT_RESIDUAL(resid,phi,source,sigma,dgphi,
     $                         DIMS,dx,resnorm,bc,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T  resid(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T    phi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T source(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  sigma(lo_1-ng:hi_1  +ng,lo_2-ng:hi_2+  ng)
      REAL_T  dgphi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  dx(2)
      REAL_T  resnorm
      integer bc(2,2)

c     Local variables
      integer i,j
      integer istart,iend
      integer jstart,jend

      resnorm = zero

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)

      call makedgphi(phi,dgphi,sigma,DIMS,dx,bc,ng,istart,jstart,iend,jend)

      do j = jstart,jend
        do i = istart,iend
          resid(i,j) = source(i,j) - dgphi(i,j)
        enddo
      enddo

      do j = jstart,jend 
        do i = istart,iend 
          resnorm = max(resnorm,abs(resid(i,j)))
        enddo
      enddo

      return
      end

#ifdef NINEPT

c *************************************************************************
c ** RELAX **
c ** Gauss-Seidel relaxation
c *************************************************************************

      subroutine FORT_RELAX(phi,source,sigma,dgphi,DIMS,dx,bc,nnrelax,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T     phi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  source(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T   sigma(lo_1-ng:hi_1  +ng,lo_2-ng:hi_2  +ng)
      REAL_T   dgphi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T dx(2)
      integer bc(2,2)
      integer nnrelax

c     Local variables
      REAL_T  hxsqinv,hysqinv
      REAL_T  rfac
      REAL_T  mult
      integer i,j
      integer extra_xlo
      integer extra_xhi
      integer extra_ylo
      integer extra_yhi
      integer iter,iinc
      integer istart,iend
      integer jstart,jend

      hxsqinv = one/(dx(1)*dx(1))
      hysqinv = one/(dx(2)*dx(2))

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)
 
      if (nnrelax .gt. ng) then
        print *,'DONT HAVE ENOUGH BUFFER CELLS IN HG:RELAX '
        print *,'NG = ',ng
        print *,'NRELAX = ',nnrelax
        stop
      endif

c     do iter = 1, 2*nnrelax 
      do iter = 1, 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)

          call makedgphi(phi,dgphi,sigma,DIMS,dx,bc,ng,
     $                   istart-extra_xlo,jstart-extra_ylo,
     $                   iend  +extra_xhi,jend  +extra_yhi)

          do j = jstart-extra_ylo,jend+extra_yhi
          do i = istart-extra_xlo,iend+extra_xhi

c         do j = jstart-extra_ylo,jend+extra_yhi
c           iinc = mod(j+iter+1+extra_xlo+2*ng,2)
c           do i = istart-extra_xlo+iinc,iend+extra_xhi,2

#if 0
              dgphi(i,j) = sixth * (hxsqinv * 
     $         (sigma(i-1,j-1) * (phi(i-1,j-1) - phi(i,j-1) + two * phi(i-1,j)) +
     $          sigma(i-1,j  ) * (phi(i-1,j+1) - phi(i,j+1) + two * phi(i-1,j)) +
     $          sigma(i  ,j-1) * (phi(i+1,j-1) - phi(i,j-1) + two * phi(i+1,j)) +
     $          sigma(i  ,j  ) * (phi(i+1,j+1) - phi(i,j+1) + two * phi(i+1,j)) -
     $             two*(sigma(i-1,j-1) + sigma(i-1,j) + 
     $                  sigma(i  ,j-1) + sigma(i  ,j)) * phi(i,j)) +
     $                            hysqinv * 
     $         (sigma(i-1,j-1) * (phi(i-1,j-1) - phi(i-1,j) + two * phi(i,j-1)) +
     $          sigma(i-1,j  ) * (phi(i-1,j+1) - phi(i-1,j) + two * phi(i,j+1)) +
     $          sigma(i  ,j-1) * (phi(i+1,j-1) - phi(i+1,j) + two * phi(i,j-1)) +
     $          sigma(i  ,j  ) * (phi(i+1,j+1) - phi(i+1,j) + two * phi(i,j+1)) -
     $             two*(sigma(i-1,j-1) + sigma(i-1,j) + 
     $                  sigma(i  ,j-1) + sigma(i  ,j)) *  phi(i,j)))
#endif
  
              mult = one
              mult = cvmgt(two*mult,mult,
     $                     i.eq.lo_1   .and. (BCX_LO.eq.WALL.or.BCX_LO.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     j.eq.lo_2   .and. (BCY_LO.eq.WALL.or.BCY_LO.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     i.eq.hi_1+1 .and. (BCX_HI.eq.WALL.or.BCX_HI.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     j.eq.hi_2+1 .and. (BCY_HI.eq.WALL.or.BCY_HI.eq.INLET))

c             dgphi(i,j) = mult * dgphi(i,j)

              rfac = mult * (hxsqinv + hysqinv) * 
     $               (sigma(i-1,j-1) + sigma(i-1,j) + 
     $                sigma(i  ,j-1) + sigma(i  ,j))

              rfac = three/rfac
              phi(i,j) = phi(i,j) + rfac*(dgphi(i,j) - source(i,j))

          enddo
          enddo
      enddo

      return
      end
#endif


#ifdef FIVEPT

c *************************************************************************
c ** RELAX **
c ** Gauss-Seidel relaxation
c *************************************************************************

      subroutine FORT_RELAX(phi,source,sigma,dgphi,DIMS,dx,bc,nnrelax,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T     phi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  source(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T   sigma(lo_1-ng:hi_1  +ng,lo_2-ng:hi_2  +ng)
      REAL_T   dgphi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T dx(2)
      integer bc(2,2)
      integer nnrelax

c     Local variables
      REAL_T  hxsqinv,hysqinv
      REAL_T  rfac, mult
      integer i,j
      integer extra_xlo
      integer extra_xhi
      integer extra_ylo
      integer extra_yhi
      integer iter
      integer istart,iend
      integer jstart,jend
      integer iinc

      hxsqinv = one/(dx(1)*dx(1))
      hysqinv = one/(dx(2)*dx(2))

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)
 
      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)

          do j = jstart-extra_ylo,jend+extra_yhi
            iinc = mod(j+iter+1+extra_xlo+2*ng,2)
            do i = istart-extra_xlo+iinc,iend+extra_xhi,2

              dgphi(i,j) = hysqinv * (
     $          half*(sigma(i,j  )+sigma(i-1,j  )) * (phi(i,j+1)-phi(i,j)) +
     $          half*(sigma(i,j-1)+sigma(i-1,j-1)) * (phi(i,j-1)-phi(i,j)) )
                         + hxsqinv * (
     $          half*(sigma(i,j  )+sigma(i  ,j-1)) * (phi(i+1,j)-phi(i,j)) +
     $          half*(sigma(i-1,j)+sigma(i-1,j-1)) * (phi(i-1,j)-phi(i,j)) )

              mult = one
              mult = cvmgt(two*mult,mult,
     $                     i.eq.lo_1   .and. (BCX_LO.eq.WALL.or.BCX_LO.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     j.eq.lo_2   .and. (BCY_LO.eq.WALL.or.BCY_LO.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     i.eq.hi_1+1 .and. (BCX_HI.eq.WALL.or.BCX_HI.eq.INLET))
              mult = cvmgt(two*mult,mult,
     $                     j.eq.hi_2+1 .and. (BCY_HI.eq.WALL.or.BCY_HI.eq.INLET))

              dgphi(i,j) = mult*dgphi(i,j)

              rfac =  mult * half * (
     $                     hysqinv * (
     $               (sigma(i,j  )+sigma(i-1,j  ))
     $               (sigma(i,j-1)+sigma(i-1,j-1)) )
                         + hxsqinv * (
     $               (sigma(i,j  )+sigma(i  ,j-1))
     $               (sigma(i-1,j)+sigma(i-1,j-1)) ) )

              rfac = one/rfac

              phi(i,j) = phi(i,j) + rfac*(dgphi(i,j) - source(i,j))

            enddo
            enddo

      enddo

      return
      end

#endif

c *************************************************************************
c ** COARSIG **
c ** Coarsening of the sig coefficients
c *************************************************************************

      subroutine FORT_COARSIG(sigma,sigmac,DIMS,CDIMS,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer loc_1, loc_2
      integer hic_1, hic_2
      integer ng
      REAL_T  sigma(lo_1 -ng:hi_1 +ng,lo_2 -ng:hi_2 +ng)
      REAL_T sigmac(loc_1-ng:hic_1+ng,loc_2-ng:hic_2+ng)

c     Local variables
      integer i ,j

      do j = lo_2,hi_2,2
        do i = lo_1,hi_1,2 
          sigmac(i/2,j/2) = (sigma(i  ,j) + sigma(i  ,j+1)+ 
     $                       sigma(i+1,j) + sigma(i+1,j+1)) * fourth
        enddo
      enddo

      return
      end

c *************************************************************************
c ** RESTRICT **
c ** Conservative restriction of the residual
c *************************************************************************

      subroutine FORT_RESTRICT(res,resc,DIMS,CDIMS,bc,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer loc_1, loc_2
      integer hic_1, hic_2
      integer ng
      REAL_T   res(lo_1 -ng:hi_1 +1+ng,lo_2 -ng:hi_2 +1+ng)
      REAL_T  resc(loc_1-ng:hic_1+1+ng,loc_2-ng:hic_2+1+ng)
      integer bc(2,2)

c     Local variables
      integer i,j,ii,jj
      integer istart,iend
      integer jstart,jend

      istart = cvmgt((lo_1  )/2+1,(lo_1  )/2,BCX_LO .eq. OUTLET)
      iend   = cvmgt((hi_1+1)/2-1,(hi_1+1)/2,BCX_HI .eq. OUTLET)
      jstart = cvmgt((lo_2  )/2+1,(lo_2  )/2,BCY_LO .eq. OUTLET)
      jend   = cvmgt((hi_2+1)/2-1,(hi_2+1)/2,BCY_HI .eq. OUTLET)

      do j = jstart,jend
        do i = istart,iend

          ii = 2*i
          jj = 2*j

          resc(i,j) = fourth*res(ii  ,jj) + 
     $               eighth*(res(ii+1,jj  ) + res(ii-1,jj  ) + 
     $                       res(ii  ,jj+1) + res(ii  ,jj-1) ) +
     $            sixteenth*(res(ii+1,jj+1) + res(ii+1,jj-1) + 
     $                       res(ii-1,jj+1) + res(ii-1,jj-1) )
        enddo
      enddo

      if ((BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) .and. lo_1 .eq. 2*loc_1) then
        i = loc_1
        ii = 2*i

        do j = jstart,jend
          jj = 2*j
          resc(i,j) = fourth*(res(ii,jj  ) + res(ii+1,jj  )) + 
     $                eighth*(res(ii,jj-1) + res(ii+1,jj-1)+
     $                        res(ii,jj+1) + res(ii+1,jj+1) )
        enddo
      endif

      if ((BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) .and. lo_2 .eq. 2*loc_2) then
        j = loc_2
        jj = 2*j

        do i = istart,iend
          ii = 2*i
          resc(i,j) = fourth*(res(ii  ,jj) + res(ii  ,jj+1)) + 
     $                eighth*(res(ii+1,jj) + res(ii+1,jj+1)+
     $                        res(ii-1,jj) + res(ii-1,jj+1) )
        enddo
      endif

      if ((BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) .and. hi_1 .eq. 2*hic_1+1) then
        i = hic_1+1
        ii = 2*i

        do j = jstart,jend
          jj = 2*j
          resc(i,j) = fourth*(res(ii,jj  ) + res(ii-1,jj  )) + 
     $                eighth*(res(ii,jj-1) + res(ii-1,jj-1)+
     $                        res(ii,jj+1) + res(ii-1,jj+1) )
        enddo
      endif

      if ((BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) .and. hi_2 .eq. 2*hic_2+1) then
        j = hic_2+1
        jj = 2*j

        do i = istart,iend
          ii = 2*i
          resc(i,j) = fourth*(res(ii  ,jj) + res(ii  ,jj-1)) + 
     $                eighth*(res(ii+1,jj) + res(ii+1,jj-1)+
     $                        res(ii-1,jj) + res(ii-1,jj-1) )
        enddo
      endif

      if ( (BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) .and.
     $     (BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) .and.
     $     (lo_1 .eq. 2*loc_1 .and. lo_2 .eq. 2*loc_2) ) then
        i = loc_1
        j = loc_2
        ii = 2*i
        jj = 2*j
        resc(i,j) = fourth*(res(ii,jj  ) + res(ii+1,jj  ) +
     $                      res(ii,jj+1) + res(ii+1,jj+1) )
      endif
 
      if ( (BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) .and.
     $     (BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) .and.
     $     (hi_1 .eq. 2*hic_1+1 .and. hi_2 .eq. 2*hic_2+1) ) then
        i = hic_1+1
        j = hic_2+1
        ii = 2*i
        jj = 2*j
        resc(i,j) = fourth*(res(ii,jj  ) + res(ii-1,jj  ) +
     $                      res(ii,jj-1) + res(ii-1,jj-1) )
      endif

      if ( (BCX_LO .eq. WALL .or. BCX_LO .eq. INLET) .and.
     $     (BCY_HI .eq. WALL .or. BCY_HI .eq. INLET) .and.
     $     (lo_1 .eq. 2*loc_1 .and. hi_2 .eq. 2*hic_2+1) ) then
        i = loc_1
        j = hic_2+1
        ii = 2*i
        jj = 2*j
        resc(i,j) = fourth*(res(ii,jj  ) + res(ii+1,jj  ) +
     $                      res(ii,jj-1) + res(ii+1,jj-1) )
      endif

      if ( (BCX_HI .eq. WALL .or. BCX_HI .eq. INLET) .and.
     $     (BCY_LO .eq. WALL .or. BCY_LO .eq. INLET) .and.
     $     (hi_1 .eq. 2*hic_1+1 .and. lo_2 .eq. 2*loc_2) ) then
        i = hic_1+1
        j = loc_2
        ii = 2*i
        jj = 2*j
        resc(i,j) = fourth*(res(ii,jj  ) + res(ii-1,jj  ) +
     $                      res(ii,jj+1) + res(ii-1,jj+1) )
      endif


      return
      end

c *************************************************************************
c ** INTERP **
c ** Simple bilinear interpolation
c *************************************************************************

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

      implicit none

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

c     Local variables
      integer i,j

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

      do j = lo_2+1,hi_2  ,2
      do i = lo_1  ,hi_1+1,2
        phi(i,j) = phi(i,j) + half*(deltac(i/2,j/2)+deltac(i/2,j/2+1))
      enddo
      enddo

      do j = lo_2  ,hi_2+1,2
      do i = lo_1+1,hi_1  ,2
        phi(i,j) = phi(i,j) + half*(deltac(i/2,j/2)+deltac(i/2+1,j/2))
      enddo
      enddo

      do j = lo_2+1,hi_2,2
      do i = lo_1+1,hi_1,2
        phi(i,j) = phi(i,j) + fourth*(deltac(i/2,j/2  )+deltac(i/2+1,j/2  )
     $                               +deltac(i/2,j/2+1)+deltac(i/2+1,j/2+1))
      enddo
      enddo

      return
      end

c *************************************************************************
c ** MAKEDGPHI **
c ** Compute D(sig G(phi))
c *************************************************************************

      subroutine makedgphi(phi,dgphi,sigma,DIMS,dx,bc,ng,rlo_1,rlo_2,rhi_1,rhi_2)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer rlo_1,rlo_2
      integer rhi_1, rhi_2
      integer ng
      REAL_T    phi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  dgphi(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  sigma(lo_1-ng:hi_1  +ng,lo_2-ng:hi_2  +ng)
      REAL_T  dx(2)
      integer bc(2,2)

c     Local variables
      REAL_T hxsqinv
      REAL_T hysqinv
      REAL_T mult
      integer istart,iend
      integer jstart,jend
      integer i,j
      integer ii,jj

      hxsqinv = one/(dx(1)*dx(1))
      hysqinv = one/(dx(2)*dx(2))

      istart = cvmgt(lo_1+1,rlo_1,BCX_LO .eq. OUTLET)
      jstart = cvmgt(lo_2+1,rlo_2,BCY_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,rhi_1,BCX_HI .eq. OUTLET)
      jend   = cvmgt(hi_2  ,rhi_2,BCY_HI .eq. OUTLET)

      do j = jstart,jend
      do i = istart,iend

#ifdef NINEPT
            dgphi(i,j) = sixth * (hxsqinv * 
     $       (sigma(i-1,j-1) * (phi(i-1,j-1) - phi(i,j-1) + two * phi(i-1,j)) +
     $        sigma(i-1,j  ) * (phi(i-1,j+1) - phi(i,j+1) + two * phi(i-1,j)) +
     $        sigma(i  ,j-1) * (phi(i+1,j-1) - phi(i,j-1) + two * phi(i+1,j)) +
     $        sigma(i  ,j  ) * (phi(i+1,j+1) - phi(i,j+1) + two * phi(i+1,j)) -
     $           two*(sigma(i-1,j-1) + sigma(i-1,j) + 
     $                sigma(i  ,j-1) + sigma(i  ,j)) * phi(i,j)) +
     $                          hysqinv * 
     $       (sigma(i-1,j-1) * (phi(i-1,j-1) - phi(i-1,j) + two * phi(i,j-1)) +
     $        sigma(i-1,j  ) * (phi(i-1,j+1) - phi(i-1,j) + two * phi(i,j+1)) +
     $        sigma(i  ,j-1) * (phi(i+1,j-1) - phi(i+1,j) + two * phi(i,j-1)) +
     $        sigma(i  ,j  ) * (phi(i+1,j+1) - phi(i+1,j) + two * phi(i,j+1)) -
     $           two*(sigma(i-1,j-1) + sigma(i-1,j) + 
     $                sigma(i  ,j-1) + sigma(i  ,j)) *  phi(i,j)))
#endif
#ifdef FIVEPT
            dgphi(i,j) = hysqinv * (
     $        half*(sigma(i,j  )+sigma(i-1,j  )) * (phi(i,j+1)-phi(i,j)) +
     $        half*(sigma(i,j-1)+sigma(i-1,j-1)) * (phi(i,j-1)-phi(i,j)) )
                       + hxsqinv * (
     $        half*(sigma(i,j  )+sigma(i  ,j-1)) * (phi(i+1,j)-phi(i,j)) +
     $        half*(sigma(i-1,j)+sigma(i-1,j-1)) * (phi(i-1,j)-phi(i,j)) )
#endif

            mult = one
            mult = cvmgt(two*mult,mult,
     $                   i.eq.lo_1   .and. (BCX_LO.eq.WALL.or.BCX_LO.eq.INLET))
            mult = cvmgt(two*mult,mult,
     $                   j.eq.lo_2   .and. (BCY_LO.eq.WALL.or.BCY_LO.eq.INLET))
            mult = cvmgt(two*mult,mult,
     $                   i.eq.hi_1+1 .and. (BCX_HI.eq.WALL.or.BCX_HI.eq.INLET))
            mult = cvmgt(two*mult,mult,
     $                   j.eq.hi_2+1 .and. (BCY_HI.eq.WALL.or.BCY_HI.eq.INLET))
 
            dgphi(i,j) = mult*dgphi(i,j)
 
        enddo
      enddo

      return
      end

c *************************************************************************
c ** SOLVEHG **
c *************************************************************************

      subroutine FORT_SOLVEHG(dest,dest0,source,sigma,sum,r,w,z,work,
     $                        DIMS,dx,bc,maxiter,norm,prob_norm,ng)

      implicit none

      integer lo_1, lo_2
      integer hi_1, hi_2
      integer ng
      REAL_T   dest(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  dest0(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T source(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T  sigma(lo_1-ng:hi_1  +ng,lo_2-ng:hi_2  +ng)
      REAL_T    sum(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T      r(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T      w(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T      z(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T   work(lo_1-ng:hi_1+1+ng,lo_2-ng:hi_2+1+ng)
      REAL_T dx(2)
      integer bc(2,2)
      integer maxiter
      REAL_T norm
      REAL_T prob_norm

c     Local variables
      REAL_T  factor
      REAL_T  alpha
      REAL_T  beta
      REAL_T  rho
      REAL_T  rhol
      REAL_T  tol, tolfac
      logical testx
      logical testy
      integer i,j,iter
      integer istart,iend
      integer jstart,jend
      REAL_T local_norm

      tolfac = 1.0d-3 

      istart = cvmgt(lo_1+1,lo_1  ,BCX_LO .eq. OUTLET)
      iend   = cvmgt(hi_1  ,hi_1+1,BCX_HI .eq. OUTLET)
      jstart = cvmgt(lo_2+1,lo_2  ,BCY_LO .eq. OUTLET)
      jend   = cvmgt(hi_2  ,hi_2+1,BCY_HI .eq. OUTLET)

      call setperiodic(dest,DIMS,bc,ng)

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

  10  call makedgphi(dest0,w,sigma,DIMS,dx,bc,ng,istart,jstart,iend,jend)

      rho = zero
      norm = zero

      do j = jstart, jend 
        do i = istart, iend 
          r(i,j) = source(i,j) - w(i,j)
        enddo
      enddo

      local_norm = zero
      do j = jstart, jend 
        do i = istart, iend 
          factor = one
          testx  = (i .eq. lo_1 .or. i .eq. hi_1+1)
          testy  = (j .eq. lo_2 .or. j .eq. hi_2+1)
          factor = cvmgt(factor*half,factor,testx)
          factor = cvmgt(factor*half,factor,testy)
          local_norm  = max(local_norm, abs(r(i,j)))
          z(i,j) = r(i,j) 
          rho    = rho + z(i,j) * r(i,j) * factor
          norm   = max(norm,abs(r(i,j)))
        enddo
      enddo

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

      do j = jstart, jend 
        do i = istart, iend 
          work(i,j) = zero
          dest(i,j) = z(i,j)
        enddo
      enddo

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

 100  call setperiodic(dest,DIMS,bc,ng)

      call makedgphi(dest,w,sigma,DIMS,dx,bc,ng,istart,jstart,iend,jend)

      alpha = zero
      do j = jstart, jend 
        do i = istart, iend 
          factor = one
          testx  = (i .eq. lo_1 .or. i .eq. hi_1+1)
          testy  = (j .eq. lo_2 .or. j .eq. hi_2+1)
          factor = cvmgt(factor*half,factor,testx)
          factor = cvmgt(factor*half,factor,testy)
          alpha  = alpha + dest(i,j)*w(i,j) * factor
        enddo
      enddo

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

      do j = jstart, jend 
        do i = istart, iend 
          factor = one
          testx  = (i .eq. lo_1 .or. i .eq. hi_1+1)
          testy  = (j .eq. lo_2 .or. j .eq. hi_2+1)
          factor = cvmgt(factor*half,factor,testx)
          factor = cvmgt(factor*half,factor,testy)
          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) * factor
          norm   = max(norm,abs(r(i,j)))
        enddo
      enddo

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

      if (norm .le. tol) then
         
         do j = jstart, jend 
            do i = istart, iend 
               dest(i,j) = work(i,j) + dest0(i,j)
            enddo
         enddo
         
      else if (iter .ge. maxiter .or. norm .ge. 100.d0*local_norm) then

        tolfac = 10.d0 * tolfac
        iter = 1
        do j = jstart, jend 
          do i = istart, iend 
            dest(i,j) = zero
          enddo
        enddo
        goto 10

      else

        beta = rho / rhol
        do j = jstart, jend 
          do i = istart, iend 
            dest(i,j) = z(i,j) + beta * dest(i,j)
          enddo
        enddo
        goto 100

      endif

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

      return
      end

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

      subroutine setperiodic(dest,DIMS,bc,ng)

      implicit none

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

      integer i,j

      if (BCX_LO .eq. PERIODIC .and. BCX_HI .eq. PERIODIC) then
        do j = lo_2,hi_2+1
          dest(hi_1+2,j) = dest(lo_1+1,j)
          dest(lo_1-1,j) = dest(hi_1  ,j)
        enddo
      endif

      if (BCY_LO .eq. PERIODIC .and. BCY_HI .eq. PERIODIC) then
        do i = lo_1,hi_1+1
          dest(i,hi_2+2) = dest(i,lo_2+1)
          dest(i,lo_2-1) = dest(i,hi_2  )
        enddo
      endif

      if (BCX_LO .eq. PERIODIC .and. BCY_LO .eq. PERIODIC) then
        dest(lo_1-1,lo_2-1) = dest(hi_1  ,hi_2  )
        dest(lo_1-1,hi_2+2) = dest(hi_1  ,lo_2+1)
        dest(hi_1+2,lo_2-1) = dest(lo_1+1,hi_2  )
        dest(hi_1+2,hi_2+2) = dest(lo_1+1,lo_2+1)
      endif

      return
      end
