! fortran_free_source

! This module holds external procedures (subroutine and functions)
!  for the pfc2D model (PERFCODE).
! Using module creates an explicit interface for the procedures

module pfc2Dfuns

implicit none

contains

!   1. F_LinearIndex
!   2. F_por
!   3. F_RHS_n
!   4. F_RHS_n1

!=============================================================
Function F_LinearIndex( i, j, jmax )
!   Converts grid index to one-dimensional storage location
implicit none
integer, intent( in ) :: i, j, jmax
integer               :: F_LinearIndex
F_LinearIndex = ( i - 1) * jmax + j
end Function F_LinearIndex
!==============================================================

!===================================================================================
!Function to switch the porosity on/off if the water is in/out of the pavement
FUNCTION F_por(h, b_pfc, por)
IMPLICIT NONE
REAL h, F_por, b_pfc, por
if     ( h >= b_pfc ) then
            F_por = 1.
ELSEIF ( h < b_pfc  )  then
            F_por = 1./por
end if
END Function F_por
!====================================================================================

Function F_RHS_n( i, j, Cw, Ce, Cs, Cn, rr, pf, dt ) Result( Fn )
!   Computes the RHS of the linear system for time level n
use shared, only: h_old, Z, imax, jmax
implicit none
! Arguments
integer, intent( in ) :: i, j
real   , intent( in ) :: Cw, Ce, Cs, Cn, rr, pf, dt
! Internal variables
!   added a bunch of dummy variables with if statements to have this function
!   also work at the boundaries.
real :: Fn
real :: hw, he, hn, hs, Zw, Ze, Zs, Zn

! Thicknesses
if( i == 1 )   then; hw = 0.0; else; hw = h_old(i-1,j); endif
if( j == 1 )   then; hs = 0.0; else; hs = h_old(i,j-1); endif
if( j == jmax) then; hn = 0.0; else; hn = h_old(i,j+1); endif
if( i == imax) then; he = 0.0; else; he = h_old(i+1,j); endif
! Elevations
if( i == 1 )   then; Zw = 0.0; else; Zw = Z(i-1,j); endif
if( j == 1 )   then; Zs = 0.0; else; Zs = Z(i,j-1); endif
if( j == jmax) then; Zn = 0.0; else; Zn = Z(i,j+1); endif
if( i == imax) then; Ze = 0.0; else; Ze = Z(i+1,j); endif

!Compute the RHS from time level n
    Fn    = h_old(i,j)   +                        &
            pf * dt / 2. * (  Cw * hw  +  Cs * hs &
                            + Cn * hn  +  Ce * he &
                            + Cw * Zw  +  Cs * Zs &
                            + Cn * Zn  +  Ce * Ze &
                            -  (Cw + Cs + Cn + Ce) * h_old(i,j)       &
                            -  (Cw + Cs + Cn + Ce) *     Z(i,j)       &
                            + rr                                        )
end function F_RHS_n
!====================================================================================

Function F_RHS_n1( i, j, Cw1, Ce1, Cs1, Cn1, rr, pf, dt ) Result (F1)
!   Computes the part of the RHS due to time level n+1
use shared, only: Z, imax, jmax
implicit none
!   Arguments
integer, intent( in ) :: i,j
real   , intent( in ) :: Cw1, Ce1, Cs1, Cn1, rr, pf, dt
!   Internal Variables
real :: F1
real :: Zw, Ze, Zs, Zn

! Elevations
if( i == 1 )   then; Zw = 0.0; else; Zw = Z(i-1,j); endif
if( j == 1 )   then; Zs = 0.0; else; Zs = Z(i,j-1); endif
if( j == jmax) then; Zn = 0.0; else; Zn = Z(i,j+1); endif
if( i == imax) then; Ze = 0.0; else; Ze = Z(i+1,j); endif

F1        = pf * dt / 2. * (  Cw1 * Zw  +  Cs1 * Zs &
                            + Cn1 * Zn  +  Ce1 * Ze &
                            - (Cw1 + Cs1 + Cn1 + Ce1)* Z(i,j)   &
                            + rr                                   )
end function F_RHS_n1
!=================================================================================

end module pfc2Dfuns


c general purpose Bayesian calculation program
c
c input: poster = fortran function (at the end of this program) to
c                 compute posterior distribution at a given set of parameter
c                 values.  First call is to read data and create crude
c                 parameter estimates
c        data file : first line contains: nseq=# of parallel metrop sequences
c                                         ndraws=length of sequences
c                                         seed = random seed (0 to 2147483647)
c                    subsequent lines contain data to be read by poster
c
c output: crude parameter estimates
c         posterior mode (and approx s.e. from 2nd derivative matrix)
c         metropolis acceptance rate
c         metropolis/Gelman-Rubin style output
c
c parameter statement sets maximum allowed:
c    number of parameters
c    number of Metrop sequences
c    number of draws per sequence (must be even #)
c    number of values for final inferences = mxseq*mxdraw/2
      implicit integer (a-z)
      parameter(mxparm=50,mxseq=10,mxdraw=2000,mxtot=20000)
      double precision parm(mxparm),secder(mxparm,mxparm)
      double precision invsecdr(mxparm,mxparm),covar(mxparm,mxparm)
      double precision poster,lh,draws(mxseq,mxdraw,mxparm+1)
      double precision summ(mxparm+1,10),crudsumm(mxparm+1,3)
      double precision start(mxseq,mxparm)
c read main control parameters and write first line of output
      read (5,*) nseq,ndraws,seed
      if (nseq.gt.mxseq.or.ndraws.gt.mxdraw) then
        write (6,189)
189     format('too many sequences (>10) or draws (>2000) specified')
        go to 201
        end if
      nd = ndraws - (ndraws/2)
      write (6,191) nseq,ndraws,seed,nd
191   format('METROPOLIS PROGRAM',5x,i2,' sequences of length ',i6,
     .       ' with random seed ',i10,/,'metropolis results use last ',
     .       i6,' values in each sequence')
      write (6,192)
192   format(i10)
c generate crude estimates and evaluate likelihood at crude estimates
      nparm = -1
      lh = poster(nparm,parm)
      if (nparm.gt.mxparm) then
        write (6,188)
188     format('posterior subroutine used too many parameters (>200)')
        go to 201
        end if
c save crude estimates for output
      do 10 i = 1,nparm
        crudsumm(i,1) = parm(i)
 10     continue
      crudsumm(nparm+1,1) = lh
c Newton-Raphson w/ steepest ascent to reach mode
      call getmode(nparm,parm,secder,lh,err)
      if (err.eq.1) then
        write (6,101)
101     format ('program stopping -- failed to find mode')
        go to 201
        end if
c at mode: find second deriv matrix, then cholesky decomp of secder matrix,
c          (we find upper triangle R with R^T R = second derivative matrix)
c          then invert cholesky triangle, and finally compute covar matrix
c          (we find upper triangle R with R^T R = second derivative matrix)
c          mode (inverse chol x inverse chol^T)
      call seconder(nparm,parm,secder)
      call cholesky(secder,nparm,err)
      if (err.eq.1) then
        write (6,102)
102     format ('program stopping -- 2nd deriv matrix is not < 0')
        go to 201
        end if
      call invuptri(secder,nparm,invsecdr)
      call getcovar(invsecdr,nparm,covar)
c collect post mode and approx s.e. for output
      do 110 i = 1,nparm
        crudsumm(i,2) = parm(i)
        crudsumm(i,3) = dsqrt(covar(i,i))
110     continue
      crudsumm(nparm+1,2) = lh
c
c METROPOLIS SECTION OF PROGRAM
c includes routines for getting starting values and convergence check
c
c get starting values for the metropolis sequences
      call cholesky(covar,nparm,err)
      call getstart(start,seed,nparm,parm,secder,covar,nseq)
c carry out metropolis -- check that acceptance rate is in correct range
      call metrop(nparm,parm,covar,nseq,start,ndraws,draws,err,seed)
      if (err.eq.2) write (6,104)
104   format ('never got accept rate in good range in 4 tries')
c carry out convergence check on the parallel metropolis sequences
      call convchk(draws,nseq,ndraws,nparm,summ,err)
      if (err.eq.3) write (6,103)
103   format ('metrop did not converge (R>1.1 for a param) -- rerun',
     .        ' with more draws')
c
c write output
      write (6,197)
197   format(9x,' crud estm',' post mode',' asympt se',10x,
     .          ' metrop t-approxim: mean+95%ci')
      do 190 i = 1,nparm
        write (6,198) i,(crudsumm(i,j),j=1,3),(summ(i,j),j=3,5)
198     format ('parm',i3,2x,3(1x,f9.5),10x,3(1x,f9.5))
190     continue
      write (6,195) (crudsumm(nparm+1,j),j=1,2),(summ(nparm+1,j),j=3,5)
195   format ('likhood',2x,2(1x,f9.5),20x,3(1x,f9.5))
      write (6,192)
      write (6,196)
196   format(9x,' estim. R ','97.5%ile-R','  2.5%ile ','   25%ile ',
     .          '   50%ile ','   75%ile ',' 97.5%ile ')
      do 200 i = 1,nparm
        write (6,199) i,(summ(i,j),j=1,2),(summ(i,j),j=6,10)
199     format ('parm',i3,2x,7(1x,f9.5))
200     continue
      write (6,194) (summ(nparm+1,j),j=1,2),(summ(nparm+1,j),j=6,10)
194   format ('likhood',2x,7(1x,f9.5))
      write (6,192)
c     do 300 i = 1,nseq
c     do 300 j = 1,ndraws
cwrite (6,292) (draws(i,j,k),k=1,nparm+1)
c92     format(3f14.8,1x,e14.8)
c00     continue
201   stop
      end

      subroutine metrop(nparm,parm,covar,nseq,start,ndraws,draws,
     .                  err,seed)
c generate nseq metropolis chains each of length ndraws
      implicit integer(a-z)
      parameter(mxparm=50,mxseq=10,mxdraw=2000)
      double precision parm(mxparm),covar(mxparm,mxparm)
      double precision poster,draws(mxseq,mxdraw,mxparm+1)
      double precision start(mxseq,mxparm),restart(mxseq,mxparm)
      double precision curr(mxparm),cand(mxparm)
      double precision curpst,newpst,ratio,chkpost(mxseq)
      double precision count(mxseq),scale,totchk,unif,rangen
      err = 0
c Gelman,Gilks,James result suggests this scale for normal
      scale = 2.4d0/dsqrt(dble(nparm))
c monitor metropolis acceptance rate after ncheck draws (unless ndraws is small)
      ncheck = 100
      if (ndraws.le.2*ncheck) ncheck = ndraws
c
c metropolis loop -- 1 until time to check on accept rate
      fail = 0
11    totchk = 0.d0
      do 200 i = 1,nseq
        count(i) = 0
        do 1 ii = 1,nparm
          curr(ii) = start(i,ii)
 1        continue
        curpst = poster(nparm,curr)
        do 100 j = 1,ncheck
c generate candidate from mvn with chosen scale
          call getcand(nparm,curr,covar,cand,scale,seed)
          newpst = poster(nparm,cand)
          ratio = dexp(newpst-curpst)
c accept/reject candidate
          unif = 0.d0
          if (ratio.lt.1.d0) unif=rangen(seed)
          if (unif.lt.ratio) then
            curpst = newpst
            count(i) = count(i) + 1
            do 10 jj = 1,nparm
              curr(jj) = cand(jj)
 10           continue
            end if
c save draw
            do 20 jj = 1,nparm
              draws(i,j,jj) = curr(jj)
 20           continue
            draws(i,j,nparm+1) = curpst
100       continue
c if accept rate check is being performed save last draw and posterior
        if (ncheck.lt.ndraws) then
          chkpost(i) = curpst
          do 30 jj = 1,nparm
            restart(i,jj) = curr(jj)
 30         continue
          totchk = totchk + count(i)
          end if
200     continue
c compute, evaluate, and write metropolis acceptance rate
      if (ncheck.lt.ndraws) then
        totchk = totchk / (nseq*ncheck)
        write (6,250) totchk,ncheck
250     format('metrop accept rate is ',f6.3,' after ',i4,' draws/seq')
        if (totchk.lt.0.1d0.or.totchk.gt.0.5d0) then
          fail = fail + 1
          if (fail.eq.4)  then
            err = 2
            end if
          if (totchk.gt.0.5d0) scale = scale * 1.25d0
          if (totchk.lt.0.1d0) scale = scale / 1.25d0
          go to 11
          end if
c resume metropolis after check (whether accept rate is OK or not)
251     do 290 i = 1,nseq
          curpst = chkpost(i)
          do 261 ii = 1,nparm
            curr(ii) = restart(i,ii)
261         continue
          do 280 j = ncheck+1,ndraws
            call getcand(nparm,curr,covar,cand,scale,seed)
            newpst = poster(nparm,cand)
            ratio = dexp(newpst-curpst)
            unif = 0.d0
            if (ratio.lt.1.d0) unif=rangen(seed)
            if (unif.lt.ratio) then
              curpst = newpst
              count(i) = count(i) + 1
              do 275 jj = 1,nparm
                curr(jj) = cand(jj)
275             continue
              end if
              do 279 jj = 1,nparm
                draws(i,j,jj) = curr(jj)
279             continue
              draws(i,j,nparm+1) = curpst
280         continue
290       continue
        end if
291   totchk = 0.d0
      do 292 i = 1,nseq
        totchk = totchk + count(i)
292     continue
      totchk = totchk / (nseq*ndraws)
      write (6,250) totchk,ndraws
      return
      end

      subroutine convchk(draws,nseq,ndraws,nparm,summ,err)
c assess convergence of metropolis draws
      implicit integer(a-z)
      parameter(mxparm=50,mxseq=10,mxdraw=2000,mxtot=20000)
      double precision draws(mxseq,mxdraw,mxparm+1)
      double precision summ(mxparm+1,10),sum,sum2,x,mn,s2,vec(mxtot)
      double precision summn,sums2,summnsq,sums2sq,desc(5)
      double precision summns2,summn2s2,temp,fq,qf,tq,qt,df1
      double precision W,muhat,B,varW,varB,covWB,postvar
      double precision sig2hat,varpostvar,postdf,varlodf
      ni = ndraws/2
      nd = ndraws - ni
c for each scaler parameter
      do 1000 ip = 1,nparm+1
        count = 0
        summn = 0.d0
        sums2 = 0.d0
        summnsq = 0.d0
        sums2sq = 0.d0
        summns2 = 0.d0
        summn2s2 = 0.d0
c accumulate mean and s.d within each sequence
      do 100 i = 1,nseq
        sum = 0.d0
        sum2 = 0.d0
        do 90 j = 1,nd
          count = count + 1
          x = draws(i,ni+j,ip)
          vec(count) = x
          sum = sum + x
          sum2 = sum2 + x*x
 90       continue
        mn = sum /nd
        s2 = (sum2 - sum*mn) / (nd-1)
c accumulate sums, cross products needed for Gelman-Rubin scale reduction factor
        summn = summn + mn
        summnsq = summnsq + mn*mn
        sums2 = sums2 + s2
        sums2sq = sums2sq + s2*s2
        summns2 = summns2 + mn*s2
        summn2s2 = summn2s2 + mn*mn*s2
100     continue
c G-R calculations
      W = sums2 / nseq
      muhat = summn / nseq
      B = nd*(summnsq - summn*muhat)/(nseq-1)
      varW = (sums2sq - sums2*W)/((nseq-1)*nseq)
      varB = 2.d0*B*B/(nseq-1)
      covWB = nd*( ((summn2s2 - summnsq*W)/(nseq-1)) -
     .       ((summns2 - summn*W)*2.d0*muhat/(nseq-1)) )/nseq
      sig2hat = ((nd-1.d0)*W + B)/nd
      postvar = sig2hat + B/(nd*nseq)
      varpostvar = (((nd-1)**2)*varW + ((nseq + 1)**2)*varB/(nseq*nseq)
     .               + 2*(nd-1)*(nseq+1)*covWB/nseq)/(nd*nd)
      postdf = 2*postvar*postvar / varpostvar
      tq = qt(postdf)
c compute conservative t approximation
      summ(ip,3) = muhat - tq*sqrt(postvar)
      summ(ip,4) = muhat
      summ(ip,5) = muhat + tq*sqrt(postvar)
c compute quantiles
      call analy(vec,count,desc)
      do 987 jj = 1,5
        summ(ip,jj+5) = desc(jj)
987     continue
c compute scale reduction factor and .975 point
c currently uses a fairly lame F-distn quantile approx
      varlodf = 2*W*W /varW
      df1 = nseq - 1.d0
      fq = qf(df1,varlodf)
      summ(ip,1) = dsqrt(postvar*postdf/(W*(postdf-2.d0)))
      temp = (nseq + 1.d0)*(B/W)/(nseq*nd)
      temp = ((nd-1.d0)/nd) + temp*fq
      summ(ip,2) = dsqrt( temp*postdf/(postdf-2.d0) )
      if (summ(ip,1).gt.1.1d0) err = 3
1000  continue
      return
      end

      subroutine getmode(nparm,parm,secder,lh,merr)
c find mode of posterior distribution
      parameter(mxparm=50)
      implicit integer(a-z)
      double precision parm(mxparm),secder(mxparm,mxparm)
      double precision tmpparm(mxparm),parminc(mxparm)
      double precision der1(mxparm),poster
      double precision toler,ztol,lh,oldlh
      double precision sum,t
      data toler/1.d-12/,ztol/1.d-20/
      data mxloop/100/
      merr = 0
      end = 0
      lh = -999999.d99
      do 10 loop=1,mxloop
        oldlh = lh
c  compute vector of first derivatives
        call firstder(nparm,parm,der1)
c  compute negative of second derivative matrix numerically
        call seconder(nparm,parm,secder)
c  Newton-Raphson step
c  --get cholesky decomposition of secder (writing over secder)
        call cholesky(secder,nparm,err)
        if (err.eq.0) then
c  --solve for Newton step unless Cholesky decomp fails
          do 50 i=1,nparm
            sum = 0.d0
            do 49 kk = 1,i-1
              sum = sum + secder(kk,i)*parminc(kk)
 49           continue
            parminc(i) = (der1(i) - sum) / secder(i,i)
 50         continue
          do 60 i=1,nparm
            ii = nparm + 1 - i
            sum = 0.d0
            do 59 kk = ii+1,nparm
              sum = sum + secder(ii,kk)*parminc(kk)
 59           continue
            parminc(ii) = (parminc(ii) - sum) / secder(ii,ii)
 60         continue
          t = 1.d0
        else
c can not do Newton-Raphson step -- use steepest ascent step
          do 70 i=1,nparm
            parminc(i) = der1(i)
  70        continue
          t = 0.1d0
        end if
c check that step increases posterior density or reduce step (mainly for
c                                                             steep ascent)
2003    do 2004 i = 1,nparm
2004      tmpparm(i) = parm(i) + parminc(i)*t
          lh = poster(nparm,tmpparm)
          if (lh.le.oldlh) then
          t = t / 2.d0
          if (t.lt.ztol) then
            t = 0.d0
            go to 2009
            end if
          go to 2003
          end if
c found a good step
2009    do 2010 i = 1,nparm
2010      parm(i) = parm(i) + t*parminc(i)
c test for convergence
        if (lh.lt.(oldlh+toler)) then
          end = 1
          go to 101
          end if
 10     continue
      merr = 1
      write (6,102) mxloop,oldlh,lh
 102  format(' did not find max in ',i3,' steps',/,
     .       'poster dens for last two steps:',2e15.8)
 101  return
      end

      subroutine getstart(start,seed,nparm,parm,secder,covar,nseq)
c get starting values for metropolis by impt wtd sampling from t_4 approx
      parameter(mxparm=50,mxseq=10,mximpt=2000)
      implicit integer(a-z)
      double precision parm(mxparm),secder(mxparm,mxparm)
      double precision covar(mxparm,mxparm)
      double precision gaudev,gamdev,sum,bigset(mximpt,mxparm)
      double precision norms(mxparm),poster,new(mxparm),tdens
      double precision rangen,uni,start(mxseq,mxparm)
      double precision fac,sumwt,thewt(mximpt),bigwt(mximpt)
      data four/4/,two/2/
c generate 1000 draws from t_4 approx at post mode
      sumwt = 0.d0
      do 1000 i = 1,mximpt
        do 10 ii = 1,nparm
          norms(ii) = gaudev(seed)
 10       continue
        fac = dsqrt(2.d0/gamdev(two,seed))
        do 100 ii = 1,nparm
          sum = 0.d0
          do 99 j = 1,ii
            sum = sum + covar(j,ii)*norms(j)
 99         continue
          new(ii) = parm(ii) + sum*fac
          bigset(i,ii) = new(ii)
100       continue
c compute importance ratio
        thewt(i) = dexp(poster(nparm,new)-
     .                  tdens(new,parm,secder,nparm,four))
        sumwt = sumwt + thewt(i)
        bigwt(i) = sumwt
1000    continue
c sample nseq starting values using importance ratios as weights (w/out replac)
      do 2000 i = 1,nseq
        uni = rangen(seed)*sumwt
c binary search to locate sampled value
        call locate(bigwt,mximpt,uni,ix)
        do 1100 j = 1,nparm
          start(i,j) = bigset(ix,j)
1100      continue
        sumwt = sumwt - thewt(ix)
        do 1200 k = ix,mximpt
          bigwt(k) = bigwt(k) - thewt(ix)
1200      continue
2000    continue
      return
      end

      subroutine getcand(nparm,curr,vcov,cand,scale,seed)
c generate candidate value for metropolis -- using normal jumping distn with
c                                            s.d. multiplied by scale
      parameter(mxparm=50)
      implicit integer(a-z)
      double precision curr(mxparm),cand(mxparm),vcov(mxparm,mxparm)
      double precision scale,gaudev,norms(mxparm),sum
      do 10 i = 1,nparm
        norms(i) = gaudev(seed)
 10     continue
      do 100 i = 1,nparm
        sum = 0.d0
        do 99 j = 1,i
          sum = sum + vcov(j,i)*norms(j)
 99       continue
        cand(i) = curr(i) + scale*sum
100     continue
      return
      end

      double precision function tdens(x,mu,sigma,dim,df)
c evaluate the t-density used to generate Metropolis starts
      implicit integer (a-z)
      parameter(mxparm=50)
      double precision x(mxparm),mu(mxparm),sigma(mxparm,mxparm)
      double precision sum,sumvsq
      sumvsq = 0.d0
      do 60 i = 1,dim
        sum = 0.d0
        do 59 kk = i,dim
          sum = sum + sigma(i,kk)*(x(kk)-mu(kk))
 59       continue
        sumvsq = sumvsq + sum*sum
 60     continue
      tdens = -0.5d0*(dim+df)*dlog(df + sumvsq)
      return
      end

      subroutine firstder(nparm,parm,der1)
c numerically evaluate vector of first derivatives of posterior distn
      implicit integer (a-z)
      parameter(mxparm=50)
      double precision step,delta,parm(mxparm),der1(mxparm)
      double precision l1,l2,poster
      data step/1.d-04/
      do 10 i=1,nparm
        delta = step*parm(i)
        parm(i) = parm(i) + delta
        l1 = poster(nparm,parm)
        parm(i) = parm(i) - 2.d0*delta
        l2 = poster(nparm,parm)
        parm(i) = parm(i) + delta
        der1(i) = (l1 - l2)/(2.d0*delta)
 10     continue
      return
      end

      subroutine seconder(nparm,parm,secder)
c numerically evaluate matrix of second derivatives of posterior distn
c (actually finds negative of second derivs --> inverse of asymp covar matrix)
      implicit integer (a-z)
      parameter(mxparm=50)
      double precision parm(mxparm),secder(mxparm,mxparm)
      double precision step,delta1,delta2,poster
      double precision l1,l2,l3,l4
      data step/1.d-04/
      do 10 i = 1,nparm
        delta1 = step*parm(i)
        do 11 j = i,nparm
          delta2 = step*parm(j)
          parm(i) = parm(i) + delta1
          parm(j) = parm(j) + delta2
          l1 = poster(nparm,parm)
          parm(j) = parm(j) - 2.d0*delta2
          l2 = poster(nparm,parm)
          parm(i) = parm(i) - 2.d0*delta1
          l3 = poster(nparm,parm)
          parm(j) = parm(j) + 2.d0*delta2
          l4 = poster(nparm,parm)
          secder(i,j) = -1.d0*(l1 + l3 - l2 - l4)/(4.d0*delta1*delta2)
          parm(i) = parm(i) + delta1
          parm(j) = parm(j) - delta2
 11       continue
        secder(j,i) = secder(i,j)
 10     continue
      return
      end

      subroutine getcovar(invchlpr,n,covar)
c compute asymptotic covar matrix of normal/t approx
c --- if R is cholesky decomp of second deriv matrix and T = R^(-1)
c --- then this finds T T^t
      implicit integer (a-z)
      parameter(mxparm=50)
      double precision invchlpr(mxparm,mxparm),covar(mxparm,mxparm)
      double precision sum
      do 200 i = 1,n
        do 199 j = i,n
          sum = 0.d0
          do 198 k = j,n
            sum = sum + invchlpr(i,k)*invchlpr(j,k)
 198        continue
          covar(i,j) = sum
          if (j.ne.i) covar(j,i) = sum
 199      continue
 200    continue
      return
      end

      subroutine invuptri(x,n,y)
c compute inverse of upper triangular matrix (used here to invert cholesky
c                                             triangle of second deriv matrix)
      implicit integer (a-z)
      parameter(mxparm=50)
      double precision x(mxparm,mxparm),y(mxparm,mxparm),sum
      do 200 i = 1,n
        j = n + 1 - i
        y(j,j) = 1.d0 / x(j,j)
        do 200 ii=1,j-1
          k = j - ii
          sum = 0.d0
          do 199 iii = k+1,j
            sum = sum + x(k,iii)*y(iii,j)
 199        continue
          y(k,j) = -1.d0*sum/x(k,k)
 200      continue
      return
      end

      subroutine cholesky(x,n,err)
c compute cholesky decomposition of the matrix x
      implicit integer(a-z)
      parameter(mxparm=50)
      double precision x(mxparm,mxparm),sum
      err = 0
      do 32 i = 1,n
        sum = 0.d0
        do 29 kk = 1,i-1
          sum = sum + x(kk,i)*x(kk,i)
 29       continue
        if (x(i,i).le.sum) then
          err = 1
          return
          end if
        x(i,i) =dsqrt(x(i,i)-sum)
        do 31 j = i+1,n
          sum = 0.d0
          do 30 kk = 1,i-1
            sum = sum + x(kk,j)*x(kk,i)
 30         continue
          x(i,j) = (x(i,j) - sum)/x(i,i)
 31       continue
 32     continue
      do 40 i = 2,n
        do 40 j = 1,i-1
          x(i,j) = 0.d0
40        continue
      return
      end

      subroutine locate(xx,n,x,j)
c binary search routine (used to carry out importance weighted samples)
      parameter(mximpt=2000)
      implicit integer (a-z)
      double precision xx(mximpt),x
      jl = 0
      ju = n
  10  if ((ju-jl).gt.1.) then
        jm = (ju + jl)/2
        if (x.gt.xx(jm)) then
          jl = jm
        else
          ju = jm
        end if
      go to 10
      end if
      j = ju
      return
      end

      subroutine analy(x,n,desc)
c sorts vector and obtains quantiles (2.5, 25, 50, 75, 97.5)
      parameter(mxtot=20000)
      implicit integer(a-z)
      double precision x(mxtot)
      double precision desc(5),ixr,r
      call hpsort(n,x)
      ix = (n+1)/40
      ixr = dble(n+1)/40
      r = ixr - ix
      desc(1) = (1.0-r)*x(ix) + r*x(ix+1)
      ix = (n+1)/4
      ixr = dble(n+1)/4
      r = ixr - ix
      desc(2) = (1.0-r)*x(ix) + r*x(ix+1)
      ix = (n+1)/2
      ixr = dble(n+1)/2
      r = ixr - ix
      desc(3) = (1.0-r)*x(ix) + r*x(ix+1)
      ix = 3*(n+1)/4
      ixr = 3*dble(n+1)/4
      r = ixr - ix
      desc(4) = (1.0-r)*x(ix) + r*x(ix+1)
      ix = 39*(n+1)/40
      ixr = 39*dble(n+1)/40
      r = ixr - ix
      desc(5) = (1.0-r)*x(ix) + r*x(ix+1)
      return
      end

      subroutine hpsort(n,ra)
c heap sort from Numerical recipes
      implicit integer (a-z)
      parameter(mxtot=20000)
      double precision rra,ra(mxtot)
      l = (n/2) + 1
      ir = n
  10  continue
        if (l.gt.1) then
          l = l - 1
          rra=ra(l)
        else
          rra=ra(ir)
          ra(ir)=ra(1)
          ir = ir - 1
          if (ir.eq.1) then
            ra(1) = rra
            return
          end if
        end if
        i = l
        j = l + l
 20     if (j.le.ir) then
          if (j.lt.ir) then
            if (ra(j).lt.ra(j+1)) j = j + 1
          end if
          if (rra.lt.ra(j)) then
            ra(i)=ra(j)
            i = j
            j = j + j
          else
            j = ir + 1
          end if
          go to 20
        end if
        ra(i) = rra
      go to 10
      end

      double precision function rangen(ix)
c portable uniform random number generator -- mult=16807, base=2**31
      double precision m
      integer a,p,ix,b15,b16,xhi,xalo,leftlo,fhi,k
      data a/16807/,b15/32768/,b16/65536/,p/2147483647/
      data m/4.6566128752458e-10/
      xhi = ix/b16
      xalo = (ix-xhi*b16)*a
      leftlo = xalo/b16
      fhi = xhi*a + leftlo
      k = fhi / b15
      oldix = ix
      ix = (((xalo - leftlo*b16) - p) + (fhi-k*b15)*b16) + k
      if (ix.lt.0) ix = ix + p
      rangen = dble(ix)*m
      if (rangen.ge.1.d0.or.rangen.le.0.d0) write (6,1)
1       format('apparent problem in rangen')
      return
      end

      double precision function gamdev(alph,iseed)
c generate gamma random variables (shape parameter = alph is an integer)
c from Numerical Recipes
      implicit integer (a-z)
      double precision x,rangen,y,s,e,pi,alphm1
      data pi/3.141592654d0/
      if (alph.le.0) then
        write (6,1)
  1     format ('problem in gamdev')
        x = 0.d0
      else if (alph.lt.6) then
        x = 1.d0
        do 11 j = 1,alph
          x = x*rangen(iseed)
 11       continue
        x = -dlog(x)
      else
 21     y = tan(pi*rangen(iseed))
        alphm1 = alph - 1.d0
        s = dsqrt(2.d0*alphm1+1.d0)
        x = s*y + alphm1
        if (x.le.0.d0) go to 21
        e = (1.d0+y*y)*dexp(alphm1*dlog(x/alphm1)-s*y)
        if (rangen(iseed).gt.e) go to 21
      end if
      gamdev = x
      return
      end

      double precision function gaudev(ix)
c generate normal random variable
      implicit integer (a-z)
      double precision v1,v2,r,gset,fac,rangen
      data iset/0/
      if (iset.eq.0) then
 1      v1 = 2.d0*rangen(ix) - 1.d0
        v2 = 2.d0*rangen(ix) - 1.d0
        r = v1**2 + v2**2
        if (r.ge.1.d0) go to 1
        fac = dsqrt(-2.d0*dlog(r)/r)
        gset = v1*fac
        gaudev= v2*fac
        iset = 1
      else
        gaudev = gset
        iset = 0
      endif
      return
      end

      double precision function qt(df)
c from Abramowitz and Stegun -- inverse function for t-distn
c currently hardwired for .975 pct point -- to change just replace xp
      implicit integer (a-z)
      double precision df,xp,xp3,xp5,xp7,xp9
      data xp/1.959964d0/
      xp3 = xp*xp*xp
      xp5 = xp3*xp*xp
      xp7 = xp5*xp*xp
      xp9 = xp7*xp*xp
      qt = xp + (xp3+xp)/(4.d0*df) +
     .          (5*xp5 + 16*xp3 + 3*xp)/(96.d0*df*df) +
     .          (3*xp7 + 19*xp5 + 17*xp3  - 15*xp)/(384.d0*df*df*df) +
     .          (79*xp9 + 776*xp7 + 1482*xp5 - 1920*xp3 - 945*xp)/
     .                                         (92160.d0*df*df*df*df)
      return
      end

      double precision function qf(df1,df2)
c from Abramowitz and Stegun -- approx to inverse function for F-distn
c approximation doesn't look too good
c currently hardwired for .975 pct point -- to change just replace xp
      double precision df1,df2,a,b,lam,h,w,xp
      data xp/1.959964d0/
      a = df2 / 2.d0
      b = df1 / 2.d0
      lam = (xp*xp - 3.d0) / 6.d0
      h = 2.d0 / ( (1.d0/(df1-1.d0)) + (1.d0/(df2-1.d0)) )
      w = xp*sqrt(h + lam)/h - (lam+(5.d0/6.d0)-(2.d0/(3.d0*h)))*
     .                         ((1.d0/(df1-1.d0))-(1.d0/(df2-1.d0)))
      qf = dexp(2.d0*w)
      return
      end


submodule(Mod_CauchyElement) UPCauchyElement_NH

   implicit none

contains

   !-------------LHS------------

   module subroutine up_elmVU_NH(e,nd,tn,dvol,J,mu,gdev,Fmat,Finv,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !  -tau_s*(symgrad(v_h)_ij,
      !                          1/2(F_jK*du_i/dX_K + F_iK*du_j/dX_K)
      !                          - 1/nd*F_lK*du_l/dX_K*ii_ij)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: nd,tn
      real(rp),             intent(in)    :: dvol,J,mu,gdev(tn)
      real(rp),             intent(in)    :: Fmat(nd,nd),Finv(nd,nd)
      real(rp),             intent(inout) :: elmat(nd,e%pnode,nd,e%pnode)
      integer(ip)           :: inode,jnode,idime
      real(rp)              :: B(nd,nd),B1(nd,nd)
      real(rp)              :: BB(nd,nd),Bt(nd,nd),Bts(nd,nd)
      real(rp)              :: aux,aux2,aux3

      aux  = (1.0_rp/(J*4.0_rp))
      aux2 = (1.0_rp/(J*nd*2.0_rp))
      aux3 = (1.0_rp/(4.0_rp*mu))

      do inode=1,e%pnode
          do jnode=1,e%pnode

              B   = 0.0_rp
              B1  = 0.0_rp
              Bt  = 0.0_rp
              Bts = 0.0_rp

              !1/4(dv_j/dx_i+dv_i/dx_j,F_iK*du_j/dX_K + F_jK*du_i/dX_K)
              call sld_calculate_2Fdu_tensor(e,nd,inode,jnode,Fmat,B1)

              !1/2(dv_j/dx_i+dv_i/dx_j,1/nd*F_lK*du_l/dX_K*ii_ij)
              call sld_calculate_traceFdu_tensor(e,nd,inode,jnode,Fmat,Bt)

              B   = aux*B1 - aux2*Bt

              !1/2(dv_j/dx_i+dv_i/dx_j,(J/2)F^-1_Kl*du_l/dX_K*S_ij)
              call sld_calculate_traceFduS_tensor(e,nd,tn,inode,jnode,Finv,gdev,Bts)
              B = B - aux3*Bts

              elmat(1:nd,inode,1:nd,jnode) = elmat(1:nd,inode,1:nd,jnode)+B(:,:)*dvol

          end do
      end do

   end subroutine up_elmVU_NH

   module subroutine up_elmVP_NH(e,nd,dvol,mu,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !                (d(v_i)/dx_i),p)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: nd
      real(rp),    intent(in)          :: dvol,mu
      real(rp),    intent(inout)       :: elmat(nd,e%pnode,1,e%pnode)
      integer(ip)                      :: inode,jnode
      real(rp)                         :: G(nd),GN(nd),aux

      aux = 1.0_rp/(2.0_rp*mu)
      do inode=1,e%pnode

          call sld_calculateB_inditial(e,nd,inode,G)

          do jnode=1,e%pnode

              GN = 0.0_rp
              GN = aux*G*e%shape(jnode,e%igaus)*dvol

              elmat(1:nd,inode,1,jnode) = elmat(1:nd,inode,1,jnode) + GN(:)

          end do
      end do

   end subroutine up_elmVP_NH

end submodule UPCauchyElement_NH


!***********************************************************************************************************************************
! UNIT:
!
!  (MODULE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  MODULE MR_MOD_FUNC_TBUV

    USE MR_KINDS

    USE MR_DEF_CONSTS_N_REF_PARS

    IMPLICIT NONE

    PRIVATE

    PUBLIC :: MR_FUNC_TBUV_COMP

!***********************************************************************************************************************************

  CONTAINS

!***********************************************************************************************************************************
! UNIT:
!
!  (SUBROUTINE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  FUNCTION MR_FUNC_TBUV_COMP( SIGMA , H , HKS , HKS2 , TBUV_MOD2 , UV_MOD , UV_COMP ) RESULT( TBUV_COMP )

   !DIR$ ATTRIBUTES VECTOR : UNIFORM( SIGMA ) :: MR_FUNC_TBUV_COMP

    IMPLICIT NONE

    REAL(PARD_KIND) , INTENT(IN ) :: SIGMA

    REAL(FDRD_KIND) , INTENT(IN ) :: H , HKS
    REAL(FDRD_KIND) , INTENT(IN ) :: HKS2 , TBUV_MOD2

    REAL(FDRD_KIND) , INTENT(IN ) :: UV_MOD , UV_COMP

    REAL(FDRD_KIND) :: TBUV_COMP

    TBUV_COMP = RBT * UV_COMP * UV_MOD /   &
    & MR_FUNC_CHEZY( SIGMA , H , HKS , HKS2 , TBUV_MOD2 )**2

  END FUNCTION MR_FUNC_TBUV_COMP

!***********************************************************************************************************************************
! UNIT:
!
!  (SUBROUTINE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  FUNCTION MR_FUNC_CHEZY( SIGMA , H , HKS , HKS2 , TBUV_MOD2 ) RESULT( CHEZY )

   !DIR$ ATTRIBUTES VECTOR : UNIFORM( SIGMA ) :: MR_FUNC_CHEZY

    IMPLICIT NONE

    REAL(PARD_KIND) , INTENT(IN ) :: SIGMA

    REAL(FDRD_KIND) , INTENT(IN ) :: H , HKS
    REAL(FDRD_KIND) , INTENT(IN ) :: HKS2 , TBUV_MOD2

    REAL(FDRD_KIND) :: CHEZY

   !DIR$ FORCEINLINE
    CHEZY = LOG( (1.0+SIGMA) * H / HKS ) / KAR + MR_FUNC_ROUGHNESS_STATE( HKS2 , TBUV_MOD2 )
   !END DIR$ FORCEINLINE

!***********************************************************************************************************************************

  CONTAINS

!***********************************************************************************************************************************
! UNIT:
!
!  (SUBROUTINE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  FUNCTION MR_FUNC_ROUGHNESS_STATE( HKS2 , TBUV_MOD2 ) RESULT( ROUGHNESS_STATE )

   !DIR$ ATTRIBUTES VECTOR :: MR_FUNC_ROUGHNESS_STATE

    IMPLICIT NONE

    REAL(FDRD_KIND) , INTENT(IN ) :: HKS2 , TBUV_MOD2

    REAL(PARD_KIND) :: RESHEARS
    REAL(PARD_KIND) :: CTMP1 , CTMP2

    REAL(FDRD_KIND) :: ROUGHNESS_STATE

    RESHEARS = MAX( RET * HKS2 * SQRT( TBUV_MOD2 ) , 1.0 )

    CTMP1 = LOG( RESHEARS )
    CTMP2 = CTMP1**2.55

    ROUGHNESS_STATE =   &
    &           8.5         * ( 1.0 - EXP( -0.0594 * CTMP2 ) ) +   &
    & ( CTMP1 / KAR + 5.5 ) *         EXP( -0.0705 * CTMP2 )

  END FUNCTION MR_FUNC_ROUGHNESS_STATE

  END FUNCTION MR_FUNC_CHEZY

  END MODULE MR_MOD_FUNC_TBUV


************************************************************************
* Classical VANCA smoother, SCGS method
*
* Extended calling convention
*
* This routine performs one smoothing step
*
*    x_new = x  +  RLXSM ( Vanca(x) - x )
*
* with Vanca(x) being a better solution than X to the system Ax=b
* calculated by a local Gauss-Seidel approach.
*
* In:
*   U1,
*   U2     - array [1..NU] of double precision
*   P      - array [1..NP] of double precision
*            Velocity and Pressure components of the current
*            solution vector
*   U1OLD,
*   U2OLD  - array [1..NU] of double precision
*   POLD   - array [1..NP] of double precision
*            Auxiliary vectors
*   F1,
*   F2     - array [1..NU] of double precision
*   FP     - array [1..NP] of double precision
*            Right hand side of the system for velocity and pressure
*   A,
*   KCOLA,
*   KLDA   - Current system matrix for velocity components
*
*   B1,
*   B2,
*   KCOLB,
*   KLDB   - The two pressure matrices
*
*   NU     - Length of velocity vectors and dimension of A
*   NP     - Length of pressure vectors and dimension of B1, B2
*
*   KMBD,
*   KVERT,
*   KMID,
*   KXNPR,
*   NVT,
*   NEL    - Usual geometry information;
*            KXNPR(.) decides on Dirichlet/Neumann nodes.
*
*   RLXSM  - Relaxation parameter; 0 <= RLXSM < 1
*
* Out:
*   U1,U2,P - Smoothed solution for velocity and pressure
************************************************************************

      SUBROUTINE  VANCS2 (U1,U2,P,U1OLD,U2OLD,POLD,F1,F2,FP,
     *                    A,KCOLA,KLDA,B1,B2,KCOLB,KLDB,NU,NP,
     *                    KMID,KXNPR,NEL,NVT,RLXSM)

      IMPLICIT NONE

C common blocks

      INCLUDE 'cout.inc'

      INCLUDE 'cbasictria.inc'

C parameters

      DOUBLE PRECISION U1(*),U2(*),P(*),U1OLD(*),U2OLD(*),POLD(*)
      DOUBLE PRECISION F1(*),F2(*),FP(*)
      DOUBLE PRECISION A(*),B1(*),B2(*)
      INTEGER KCOLA(*),KLDA(*),KCOLB(*),KLDB(*)
      INTEGER KMID(NNVE,*),KXNPR(2,*)
      INTEGER NU,NP
      INTEGER NEL,NVT
      DOUBLE PRECISION RLXSM

C     Local arrays for informations about one element

      DOUBLE PRECISION AA(4),BB1(4),BB2(4),FF1(4),FF2(4)
      INTEGER IU(4)
      LOGICAL BDBC(4)

C     local variables

      INTEGER IEL,II,IMID,IA1,IA2,IA,IB,I,J,IB1,IB2,JP1,JP2
      DOUBLE PRECISION FFP,PJP,AOFF,A1,A2

C     Copy UOLD:=U. We use that for updating U later:

      CALL LCP1 (U1,U1OLD,NU)
      CALL LCP1 (U2,U2OLD,NU)
      CALL LCP1 (P ,POLD ,NP)

C=======================================================================
C     Block Gauss-Seidel on Schur Complement
C=======================================================================

C     Basic algorithm:
C
C     What are we doing here? Well, we want to perform *smoothing*.
C     This is an iteration to the solution vector in the form of a
C     defect correction:
C
C       x_n+1  =  x_n + Omega C^-1 (b - A x_n)
C
C     With C=A, this simplifies to the simple update formula:
C
C       x_n+1  =  x_n + Omega (x* - x_n)
C
C     where x* is the solution of the problem. In our case, we replace
C     x* by an approximated "better" solution x calculated from x_n.
C     Furthermore by replacing Omega=1-RLXSM, we obtain the smoothing
C     formula
C
C       x_n+1  =  RLXSM x_n + (1-RLXSM) x
C
C     Now the big question: How to calculate x from x_n???
C     This is done in a "local Block-Gauss-Seidel" manner. This means,
C     we subsequently update some entries in x_n and use these in
C     later computations to update some other entries. Note that this is
C     not a Gauss-Seidel algorithm in closer sense, but more a
C     Gauss-Seidel type method: We calculate new entries from old ones
C     and from previously calculated new ones!
C
C     This is done in the following way: Imagine our global system:
C
C         [ A   B ] (u) = (f)
C         [ B^t 0 ] (p)   (g)
C
C     In the Navier-Stokes equations with (u,p) being the solution
C     vector, there should be g=0 - but this cannot be assumed
C     as it does not happen in general.
C     Now the algorithm for generating a new (u,p) vector from the old
C     one reads roughly as follows:
C
C     a) Restrict to a small part of the domain, in our case to one cell.
C     b) Fetch all the data (velocity, pressure) on that cell. On the
C        first cell, we have only "old" velocity entries. These values
C        are updated and then the calculation proceeds with the 2nd cell.
C
C               old                      new
C            |---X---|                |---X---|
C            |       |                |       |
C        old X   1   X old   -->  new X   1   X new
C            |       |                |       |
C            |---X---|                |---X---|
C               old                      new
C
C        From the second cell on, there might be "old" data and "new"
C        data on that cell - the old data that has not been updated and
C        perhaps some already updated velocity data from a neighbor cell.
C
C               new     old                   new     new
C            |---X---|---X---|             |---X---|---X---|
C            |       |       |             |       |       |
C        new X   1   X   2   X old --> new X   1   X   2   X new
C            |       |new    |             |       |newer  |
C            |---X---|---X---|             |---X---|---X---|
C               new     old                   new     new
C
C        These values are updated and then the calculation proceeds
C        with the next cell.
C        As can be seen in the above picture, the "new" node in the
C        middle is even going to be a "newer" node when handled again
C        for the 2nd cell. This is meant by "Gauss-Seldel" character:
C        Information is updated subsequently by using "old" data and
C        "new" data from a previous calculation.
C
C     So we start with a loop over all elements

      DO IEL=1,NEL

C       We now have the element
C                                            U3/V3
C       |---------|                       |----X----|
C       |         |                       |         |
C       |   IEL   |   with DOF's    U4/V4 X    P    X U2/V2
C       |         |                       |         |
C       |---------|                       |----X----|
C                                            U1/V1
C
C       Fetch the pressure P on the current element into FFP

        FFP=FP(IEL)

C       Loop over all 4 U-nodes of that element

        DO II=1,4

C         Set I to the DOF that belongs to our edge II:

          IMID = KMID(II,IEL)
          I    = IMID-NVT

C         Write the number of the edge/node to IU:

          IU(II) = I

C         Write into BDBC whether the edge is a Dirichlet-
C         Boundary edge:

          BDBC(II) = IAND(KXNPR(1,IMID),2**12).NE.0

C         Put on AA(.) the diagonal entry of matrix A

          IA1    = KLDA(I)
          AA(II) = A(IA1)

C         Set FF1/FF2 initially to the value F1/F2 of the right hand
C         side vector that belongs to our current DOF corresponding
C         to II

          FF1(II) = F1(I)
          FF2(II) = F2(I)

C         What do we have at this point?
C         FF1/FF2: "local" RHS vector belonging to the DOF's on the
C                  current element
C         AA     : Diagonal entries of A belonging to these DOF's
C         BDBC   : Whether the DOF's belong to Dirichlet nodes
C
C         And at the moment:
C         I      : number of current DOF on element IEL
C         II     : "local" number of DOF on element IEL, i.e.
C                  number of the edge
C
C         Now comes the crucial point with the "update": How to
C         subsequently update the vertex values, such that the whole
C         thing still converges to the solution, even if a node
C         is updated more than once? Here, we use a typical
C         matrix-decomposition approach:
C
C         Again consider the problem:
C
C            [ A   B ] (u) = (f)
C            [ B^t 0 ] (p)   (g)
C
C         We assume, that all components in the vector (u,p) are
C         given - except for the four velocity unknowns and the
C         pressure unknown on the current element; these five unknowns
C         are located anywhere in the (u,p) vector. The idea is to
C         shift "everything known" to the right hand side to obtain
C         a system for only these unknowns!
C
C         Extracting all the lines of the system that correspond to
C         DOF's on our single element IEL results in a rectangular
C         system of the form
C
C            [ === A^ === B~ ] (|) = (f)
C            [ B~^t       0  ] (u)   (g)
C                              (|)
C                              (p)
C
C         <=>  [ MAT1 MAT2 B~ ] (u) = (fu)
C              [    ****   0  ] (p)   (fp)
C
C         with A^ being an 4 x (2*NVT) matrix for the two velocity
C         components and B being an (2*4) x 1 matrix that couples the
C         velocities to the pressure on our current element.
C         B~ is a 8 x 2 matrix: As every velocity couples with at most
C         two pressure elements on the neighbour cell, so we have
C         2 columns in the B-matrix.
C
C                IEL                              IEL
C             |--------|             |--------|--------|
C             |        |             |        |        |
C             |   P    |      or     |   Q    X   P    |
C             |        |             |        |        |
C           --|---X----|--           |--------|--------|
C
C         Now, throw all summands to the RHS vector that do
C         not correspond to the velocity/pressure DOF's on our single
C         element IEL!
C
C         That way, A^ is reduced to a square matrix with two square
C         submatrices A~ of size 4 x 4. The 8 x 2-matrix B~ reduces to
C         two 4 x 1 submatrices (originally, every velocity couples with
C         two pressure elements on the neighbour cell, so we have
C         2 columns in the B-matrix).
C
C         The system roughly looks like:
C
C         [ ....        B1 ] (u1)   (f1)           (| )           (| )           (| )
C         [ .A~.        |  ] (u2)   (| )           (| )           (| )           (| )
C         [ ....        |  ] (u3)   (| )           (| )           (| )           (| )
C         [ ....        |  ] (u4)   (| )           (| )           (| )           (| )
C         [       ....  B2 ] (v1) = (f2) - sum u_i (ci) - sum v_i (di) - sum   Q (qi)
C         [       .A~.  |  ] (v2)   (| )   i>4     (| )   i>4     (| )  Q in N   (| )
C         [       ....  |  ] (v3)   (| )           (| )           (| )           (| )
C         [       ....  |  ] (v4)   (| )           (| )           (| )           (| )
C         [ B1^T  B2^T  0  ] (p )   (fp)           (0 )           (0 )           (0 )
C
C         assuming in the example that element IEL contains u1,u2,u3,u4,
C         v1,v2,v3,v4 and p as DOF's, and using the notation that ci
C         and di are the i th column of matrix MAT1 and MAT2 above,
C         respectively.
C         Q is the pressure on the neighbour elements in the
C         neighbourhood N of element IEL and
C         qi the corresponding column from the B~-matrix.
C
C         But we do even more! Consider the 4x4-matrix A~. This can be
C         decomposed into A~ = L+S+R with L being the lower left, R
C         being the upper right and S being the diagonal part of A~.
C         Again, throw Lu, Ru and Lv, Rv to the right hand side vector,
C         leaving alone the diagonal 4x4 matrix S on the left:
C
C         [ *           B1 ] (u1)   (d11)
C         [  *          |  ] (u2)   (d12)
C         [   *         |  ] (u3)   (d13)
C         [    *        |  ] (u4)   (d14)       [ S         B1 ] (ul)   (du)
C         [       *     B2 ] (v1) = (d21)  <=>  [      S    B2 ] (vl) = (dv)
C         [        *    |  ] (v2)   (d22)       [ B1^t B2^t  0 ] (p )   (dp)
C         [         *   |  ] (v3)   (d23)
C         [          *  |  ] (v4)   (d24)
C         [ B1^T  B2^T  0  ] (p )   (d33)
C
C         with d1k = f1k - sum  A~(k,i) u_i.
C                          i<>k
C         d2k is calculated similar. (du, dv, dp) is called
C         "local residuum".
C
C         Note: This only holds for inner edges!
C         Do we have a boundary edge?

          IF (.NOT.BDBC(II)) THEN

C           No, we have an inner edge.
C
C           Create a "local" residuum!
C           Take the original RHS and subtract all entries of "A*u" to
C           this RHS as being given. What we throw here to the RHS is
C           therefore on one hand a set of entries of the old velocities
C           as well as a set of entries which might have been calculated
C           by a previous update of another cell in our neighbourhood.

            IA2=KLDA(I+1)-1
            DO IA=IA1+1,IA2
              J=KCOLA(IA)
              AOFF=A(IA)
              FF1(II) = FF1(II)-AOFF*U1(J)
              FF2(II) = FF2(II)-AOFF*U2(J)
            END DO

C           Get BB1,BB2 and modify FF1,FF2.
C
C           In every line of each B-matrix, there is either one entry
C           (in case of boundary nodes) or there are at most two entries
C           (for inner nodes, which where each velocity component couples
C           with two elements:
C
C                IEL                              IEL
C             |--------|             |--------|--------|
C             |        |             |        |        |
C             |   P1   |      or     |   P2   X   P1   |
C             |        |             |        |        |
C           --|---X----|--           |--------|--------|
C
C           Our current edge is either an inner edge with two elements
C           attached, or a boundary edge with Neumann boundary
C           conditions.
C
C           In the case that two elements meet each other, we subtract
C           the B*pressure of the neighbour cell from the RHS.
C
C           In the case that we have only one element, there's
C           only one entry in the row of the B-matrix. We fetch the
C           first and last entry of the line, which will coincide
C           then (IB1=IB2). As there is no neighbour cell, we must
C           not subtract anything from the RHS! Nevertheless we have to
C           note the column numbers in the B-matrices to our BB1/BB2
C           vectors.
C
C           Get the first and last entry from row I of the B-matrices:

            IB1=KLDB(I)
            IB2=KLDB(I+1)-1
            JP1=KCOLB(IB1)
            JP2=KCOLB(IB2)

C           Check which of these two belong to the current element.
C           The corresponding other one is to be subtracted from the RHS
C           vector.

            IF (JP1.EQ.IEL)  THEN

              PJP = P(JP2)
              BB1(II) = B1(IB1)
              BB2(II) = B2(IB1)

C             Don't subtract anything from the RHS if there's
C             no neighbour element!

              IF (IB1.NE.IB2) THEN
                FF1(II) = FF1(II)-B1(IB2)*PJP
                FF2(II) = FF2(II)-B2(IB2)*PJP
              END IF

            ELSE IF (JP2.EQ.IEL)  THEN

              PJP = P(JP1)
              BB1(II) = B1(IB2)
              BB2(II) = B2(IB2)

C             Don't subtract anything from the RHS if there's
C             no neighbour element!

              IF (IB1.NE.IB2) THEN
                FF1(II) = FF1(II)-B1(IB1)*PJP
                FF2(II) = FF2(II)-B2(IB1)*PJP
              END IF

            ELSE
              WRITE (MTERM,*)
     *              'ERROR in SMOOTH: IEL entry in B not found'
              STOP
            ENDIF

C           That's it, FF1/FF2 form the "local defect".

          ELSE

C           Hmmm, the edge II is a Dirichlet boundary edge.
C           That means there is no adjacent element with a pressure that
C           we have to subtract from the RHS vector.
C           Simply write the one and only element in the current line
C           of the B-matrices to BB1/BB2.

            IB = KLDB(I)
            BB1(II) = B1(IB)
            BB2(II) = B2(IB)

          END IF ! (not BDBC(II))

C         Go to the next DOF on our current element IEL...

        END DO ! II

C       Call the subroutine ELUPD2 for element update.
C
C       This routine will now "solve" the above system
C
C          [ S         B1 ] (ul)   (du)
C          [      S    B2 ] (vl) = (dv)
C          [ B1^t B2^t  0 ] (p )   (dp)
C
C       and write the new velocity/pressure values (ul,vl,p) to the
C       solution vector (U1,U2,P).
C       Note that although we called (du,dv,dp) "local defect", it's
C       not a real defect! (du,dv,dp) is the original RHS vector
C       where a lot of parts of the original system have been
C       subtracted in equivalent formulations! Therefore, the solution
C       of this local system really gives new values for velocity/
C       pressure and not "update" values like in a defect correction
C       approach!

        CALL  ELUPD2 (U1,U2,P,IEL,IU,BDBC,AA,BB1,BB2,FF1,FF2,FFP)

C       Go to the next element...

      END DO ! IEL

C=======================================================================
C
C     At this point we are finished calculating a (hopefully) better
C     solution (U1,U2,P) than (U1OLD,U2OLD,POLD).
C
C     Relaxation step.
C
C     We don't use directly (U1,U2,P) (although we could do),
C     but we use a relaxation to "blend" the old solution
C     into the new one. This allowes the user to influence the
C     amount of smoothing. RLXSM=1.0 means: Take the full newly
C     calculated vector as solution.
C
C          U := RLXSM*U + (1-RLXSM)*UOLD
C             = U + (1-RLXSM) (UOLD-U)

      IF (RLXSM.NE.1D0) THEN
        A1= 1D0-RLXSM
        A2=     RLXSM
        CALL  LLC1 (U1OLD,U1,NU,A1,A2)
        CALL  LLC1 (U2OLD,U2,NU,A1,A2)
        CALL  LLC1 (POLD ,P ,NP,A1,A2)
      ENDIF

C     That's it, smoothing finished.

      END

************************************************************************
* Classical VANCA solver, SCGS method
*
* Extended calling convention
*
* This routine performs one fix point iteration step
*
*    x_new = x  +  RLXSM ( Vanca(x) - x )
*
* with Vanca(x) being a better solution than X to the system Ax=b
* calculated by a local Gauss-Seidel approach.
* This is basically the same as the Vanca-smoother in VANCAS above,
* but in difference the routine returns information about
* the maximum velocity/divergence defect to the caller.
*
* In:
*   U1,
*   U2     - array [1..NU] of double precision
*   P      - array [1..NP] of double precision
*            Velocity and Pressure components of the current
*            solution vector
*   U1OLD,
*   U2OLD  - array [1..NU] of double precision
*   POLD   - array [1..NP] of double precision
*            Auxiliary vectors
*   F1,
*   F2     - array [1..NU] of double precision
*   FP     - array [1..NP] of double precision
*            Right hand side of the system for velocity and pressure
*   A,
*   KCOLA,
*   KLDA   - Current system matrix for velocity components
*
*   B1,
*   B2,
*   KCOLB,
*   KLDB   - The two pressure matrices
*
*   NU     - Length of velocity vectors and dimension of A
*   NP     - Length of pressure vectors and dimension of B1, B2
*
*   KMBD,
*   KVERT,
*   KMID,
*   KNPR,
*   NVT,
*   NEL    - Usual geometry information;
*            KNPR(.)<>0 identifies boundary edges
*
*   RLXSM  - Relaxation parameter; 0 <= RLXSM < 1
*
* Out:
*   U1,U2,P - Improved solution for velocity and pressure
*   DMAXU   - Maximum velocity defect
*   DMAXP   - Maximum defect in the divergence
************************************************************************

      SUBROUTINE  VANCE2 (U1,U2,P,U1OLD,U2OLD,POLD,F1,F2,FP,
     *                    A,KCOLA,KLDA,B1,B2,KCOLB,KLDB,NU,NP,
     *                    KMID,KXNPR,NEL,NVT,RLXPAR,DMAXU,DMAXP)

      IMPLICIT NONE

C common blocks

      INCLUDE 'cout.inc'

      INCLUDE 'cbasictria.inc'

C parameters

      DOUBLE PRECISION U1(*),U2(*),P(*),U1OLD(*),U2OLD(*),POLD(*)
      DOUBLE PRECISION F1(*),F2(*),FP(*)
      DOUBLE PRECISION RLXPAR,DMAXP,FFPH
      DOUBLE PRECISION A(*),B1(*),B2(*)
      INTEGER KCOLA(*),KLDA(*),KCOLB(*),KLDB(*)
      INTEGER KMID(NNVE,*),KXNPR(2,*)
      INTEGER NU,NP,NEL,NVT

C     Local arrays for informations about one element

      DOUBLE PRECISION AA(4),BB1(4),BB2(4),FF1(4),FF2(4)
      INTEGER IU(4)
      LOGICAL BDBC(4)

C     local variables

      INTEGER IEL,II,IMID,IA1,IA2,IA,IB,I,J,IB1,IB2,JP,JP1,JP2
      INTEGER IB3,JP3,ICOLA1
      DOUBLE PRECISION FFP,PJP,AOFF,A1,A2,DMAXU,AH

C     DMAXU/DMAXP will be build by taking a maximum; initialize
C     with 0.

      DMAXU=0D0
      DMAXP=0D0

C     Copy U->UOLD; we need it later for the relaxation step
C     Unew = Uold + RLXSM (U-Uold).

      CALL  LCP1 (U1,U1OLD,NU)
      CALL  LCP1 (U2,U2OLD,NU)
      CALL  LCP1 (P ,POLD ,NP)
C
C=======================================================================
C     Block Gauss-Seidel on Schur Complement
C=======================================================================

C     Loop over all elements to (re-)calculate the DOF's in velocity and
C     pressure on each element.

      DO IEL=1,NEL

C       Fetch the right hand side of the pressure in the equation
C       to FFP.

        FFP =FP(IEL)
        FFPH=FFP

C       Loop over all 4 U-nodes of that element

        DO II=1,4

          IMID=KMID(II,IEL)
          I=IMID-NVT

C         Write the global number of DOF into IU:

          IU(II)=I

C         Is this DOF of Dirichlet type?

          BDBC(II) = IAND(KXNPR(1,IMID),2**12).NE.0

C         We treat the current solution vector (u,p) as
C         given except for the five unknowns on the current element.
C         Therefore we split the system matrix for the velocities
C         A into A=L+S+U with S being diagonal.
C         In the velocity system Au=f <=> (L+S+U)u=f we shift everything
C         given to the right hand side, resulting in Su=f-(L+U)u
C         with S being the 4x4 diagonal matrix for the four velocity
C         unknowns on the current element, with a_ii on the main
C         diagonal.
C
C         Put on AA(.) the diagonal entry of matrix A, which forms the
C         matrix S:

          IA1 = KLDA(I)
          AA(II) = A(IA1)

C         Form the RHS f-(L+U)u of the system.
C         Initial setting of FF1,FF2 is the right hand side f:

          FF1(II) = F1(I)
          FF2(II) = F2(I)

C         Check if the current node is a boundary node

          IF (.NOT.BDBC(II)) THEN

C           No, the edge is an inner edge or a Neumann boundary edge:
C
C                          IEL               IEL
C             |--------|--------|         |--------|
C             |        |        |         |        |
C             |   Q    X   P    |         |   P1   |
C             |        |        |         |        |
C             |--------|--------|       --|---X----|--Neumann
C
C           Put all off-diagonal entries of A to the right hand side,
C           i.e. subtract (L+U)u (with L,U not being only the lower
C           left/upper right part of a triangular matrix, but the lower
C           left/upper right part of the rectangular matrix that arises
C           when extracting only the lines from the matrix that
C           correspond to our four DOF's!

            IA2=KLDA(I+1)-1
            DO IA=IA1+1,IA2
              J=KCOLA(IA)
              AOFF=A(IA)
              FF1(II)=FF1(II)-AOFF*U1(J)
              FF2(II)=FF2(II)-AOFF*U2(J)
            END DO

C           Get BB1,BB2 and modify FF1,FF2
C
C           The velocity part is subtracted from f, but that's not
C           enough. As the main matrix has the form
C             [ A         B1 ]
C             [      A    B2 ]
C             [ B1^t B2^t 0  ],
C           shifting everything given to the RHS vector after extracting
C           only the crucial lines also makes it necessary to subtract
C           something regarding the pressure.
C           Our current element is connected to a neighbour element, as
C           we are looking to an inner edge. Get the pressure Q on the
C           neighbour elements and subtract Bx^t*Q from the RHS.
C
C           In case our edge is Neumann-boundary type, there's only one
C           entry in the B-Matrix, and thus the first and last entry of
C           the row in the B-matrix are the same. We fetch the
C           first and last entry of the line, which will coincide
C           then (IB1=IB2). As there is no neighbour cell, we must
C           not subtract anything from the RHS! Nevertheless we have to
C           note the column numbers in the B-matrices to our BB1/BB2
C           vectors.
C
C           Write the two 4x1-matrices of the pressure P on the current
C           element to BB1 and BB2.
C
C           IB1 = Index of 1st B-entry
C           IB2 = Index of 2nd B-entry (=IB1 if Neumann bdry. edge)
C           JP1 = Column of 1st B-entry
C           JP2 = Column of 2nd B-entry (=JP1 if Neumann bdry. edge)

            IB1=KLDB(I)
            IB2=KLDB(I+1)-1
            JP1=KCOLB(IB1)
            JP2=KCOLB(IB2)

            IF (JP1.EQ.IEL)  THEN
              PJP=P(JP2)
              BB1(II)=B1(IB1)
              BB2(II)=B2(IB1)

C             Don't subtract anything from the RHS if there's
C             no neighbour element!

              IF (IB1.NE.IB2) THEN
                FF1(II)=FF1(II)-B1(IB2)*PJP
                FF2(II)=FF2(II)-B2(IB2)*PJP
              END IF

              IB3    =IB1
              JP3    =JP1

            ELSE IF (JP2.EQ.IEL)  THEN

              PJP=P(JP1)
              BB1(II)=B1(IB2)
              BB2(II)=B2(IB2)

C             Don't subtract anything from the RHS if there's
C             no neighbour element!

              IF (IB1.NE.IB2) THEN
                FF1(II)=FF1(II)-B1(IB1)*PJP
                FF2(II)=FF2(II)-B2(IB1)*PJP
              END IF

              IB3    =IB2
              JP3    =JP2

            ELSE
              WRITE(MTERM,*)
     *             'ERROR in VANCAE: IEL entry in B not found'
              STOP
            END IF

C           Additionally to IBx/JPx we now have:
C
C           PJP = pressure on the neighbour cell
C           IB3 = the index of the B-entry of the neighbour cell
C           JP3 = the column number of the B-entry of the neighbour cell
C
C           FF1/FF2 currently shows the value of
C                f-(L+D)u-(neighbouring pressure)*B
C           By subtracting also (S*u) and the pressure contribution
C           on the current cell, we form the residuum of the
C           velocity. The maximum of the residuum is stored to DMAXU.

            AH=A(IA1)
            ICOLA1=KCOLA(IA1)

C           The column number of the B-matrices corresponds to the
C           DOF in the pressure P. JP3 is the column number of
C           the entry in the B-matrix corresponding to the current
C           cell. By accessing P(JP3), fetch the pressure on the
C           current cell to PJP:

            PJP=P(JP3)

            DMAXU = MAX(DMAXU,ABS(FF1(II)-AH*U1(ICOLA1)-B1(IB3)*PJP))
            DMAXU = MAX(DMAXU,ABS(FF2(II)-AH*U2(ICOLA1)-B2(IB3)*PJP))

C           FFPH on the other hand is the original RHS of the divergence
C           equation B^t*u=0 (was set that way above). Subtract B^t*u
C           from that to form the "divergence defect" g-B^t*u on the
C           current cell.

            FFPH = FFPH - BB1(II)*U1(IU(II))-BB2(II)*U2(IU(II))

          ELSE

C           II is a Dirichlet boundary node. There's for sure no
C           neighbour cell attached, so don't have to subtract something
C           like a pressure part from the RHS.
C           Simply get the entries of the two 4x1 B-matrices for the
C           current element.

            IB = KLDB(I)
            JP = KCOLB(IB)
            BB1(II) = B1(IB)
            BB2(II) = B2(IB)

C           FFPH is the original RHS of the divergence equation
C           B^t*u=0 (was set that way above). Subtract B^t*u
C           from that to form the "divergence defect" g-B^t*u on the
C           current cell.

            FFPH = FFPH - BB1(II)*U1(IU(II))-BB2(II)*U2(IU(II))

          END IF ! (not BDBC(II))

C         Fetch the next DOF on the current element...

        END DO ! II

C       Build the maximum "divergence defect" g-B^t*u in DMAXP.
C       REMARK: This is a difference to the original implementation
C       of VANCA, which does not use an ABS() here!

        DMAXP=MAX(DMAXP,ABS(FFPH))

C       Call the subroutine ELUPD2 for element update.
C
C       This routine will now "solve" the above system
C
C          [ S         B1 ] (ul)   (du)
C          [      S    B2 ] (vl) = (dv)
C          [ B1^t B2^t  0 ] (p )   (dp)
C
C       and write the new velocity/pressure values (ul,vl,p) to the
C       solution vector (U1,U2,P).
C       Note that although we called (du,dv,dp) "local defect", it's
C       not a real defect! (du,dv,dp) is the original RHS vector
C       where a lot of parts of the original system have been
C       subtracted in equivalent formulations! Therefore, the solution
C       of this local system really gives new values for velocity/
C       pressure and not "update" values like in a defect correction
C       approach!

        CALL  ELUPD2 (U1,U2,P,IEL,IU,BDBC,AA,BB1,BB2,FF1,FF2,FFP)

C       Improve the DOF's on the next element...

      END DO ! IEL

C=======================================================================
C
C     At this point we are finished calculating a (hopefully) better
C     solution (U1,U2,P) than (U1OLD,U2OLD,POLD).
C
C     Relaxation step.
C
C     We don't use directly (U1,U2,P) (although we could do),
C     but we use a relaxation to "blend" the old solution
C     into the new one. This allowes the user to influence the
C     amount of smoothing. RLXSM=1.0 means: Take the full newly
C     calculated vector as solution.
C
C          U := RLXSM*U + (1-RLXSM)*UOLD
C             = U + (1-RLXSM) (UOLD-U)

      IF (RLXPAR.NE.1D0) THEN
        A1 = 1D0-RLXPAR
        A2 =     RLXPAR
        CALL  LLC1 (U1OLD,U1,NU,A1,A2)
        CALL  LLC1 (U2OLD,U2,NU,A1,A2)
        CALL  LLC1 (POLD ,P ,NP,A1,A2)
      ENDIF

      END

************************************************************************
* Element update for vector in Vanca smoother
*
* This routine performs a block Gauss-Seidel algorithm to update
* the solution vector (U1,U2,P) for all unknowns of element IEL.
*
* In:
*   U1,
*   U2,
*   P      - array [1..*] of double precision
*            Velocity and pressure components of solution vector
*   IEL    - The element where the algorithm should work on
*   IU     - array [1..4] of integer
*            Numbers of the global velocity DOF's on element IEL
*   BDBC   - array [1..4] of boolean
*            =true , if the DOF is a boundary node
*            =false, otherwise
*   AA     - array [1..4] of double
*            Diagonal elements of the system matrix corresponding
*            to the DOF's in IU(.)
*   BB1,
*   BB2    - array [1..4] of double
*            Local pressure matrices, corresponding to the DOF's in
*            IU(.); both are 4x1 matrices!
*   FF1,
*   FF2    - array [1..4] of double
*            Local RHS vector for velocity, corresponding to the
*            DOF's in IU(.)
*   FFP    - Pressure RHS on element IEL
*
* Out:
*   The components of (U1,U2,P) belonging to element IEL are updated.
************************************************************************

      SUBROUTINE ELUPD2 (U1,U2,P,IEL,IU,BDBC,AA,BB1,BB2,FF1,FF2,FFP)

      IMPLICIT NONE

      INCLUDE 'cout.inc'

      DOUBLE PRECISION U1(*),U2(*),P(*)

C     parameters

      DOUBLE PRECISION AA(4),BB1(4),BB2(4),FF1(4),FF2(4)
      DOUBLE PRECISION AI(4),DD1(4),DD2(4),UU1(4),UU2(4)
      LOGICAL BDBC(4)
      INTEGER IU(4)

C local variables

      INTEGER II, IEL, I
      DOUBLE PRECISION DP, FFP, PP

C     This routine uses a "Block-Gauss-Seidel" strategy to update the
C     solution vector (U1,U2,UP) locally to a new iterate.
C     "Block-Gauss-Seidel" means here, that the new solution vector
C     is calculated by parts of the old one and by parts of the new
C     one, which were calculated by a previous call to this routine.
C
C     What we want to calculate are two things: 1.) a new pressure and
C     2.) a new velocity. Both can be calculated from the
C     RHS using the Schur-Complement approach.
C
C     Assume we have a system:
C
C      [ S   B ] (u) = (f)
C      [ B^t 0 ] (p)   (g)
C
C     with >>S, B, u, p, f, g given locally on one patch of the mesh.<<
C     Here S is given as a diagonal matrix with diagonal entries of
C     the original system matrix. We can write:
C
C                u = S^-1 (f-Bp)
C            B^t u = g
C
C     Inserting the first equation into the second one gives:
C
C           B^t S^-1 (f-Bp) = g
C
C      <=>   -B^t S^-1 B p  =  g - B^t S^-1 f
C            ***********       **************
C               =: DP              =: FFP
C
C     Remark that DP is a 1x1-system, i.e. a scalar! Therefore
C     calculating DP^-1 to get p=DP^-1*FFP on the current cell
C     is trivial! So FFP/DP will be the pressure on the element IEL.
C
C     Calculating an update for the velocity
C
C          u = S^-1 (f-Bp)
C
C     is then also trivial as S (and thus S^-1) is a diagonal matrix.

      DO II=1,4

C       DD1/DD2 saves the entries of the B1/B2-matrix - except for
C       if the vertex is a boundary vertex!

        IF (BDBC(II))  THEN
          DD1(II)=0D0
          DD2(II)=0D0
        ELSE
          DD1(II)=BB1(II)
          DD2(II)=BB2(II)
        ENDIF

        IF (DABS(AA(II)).LT.1.D-10)  THEN
C          WRITE(MTERM,*)'ERROR in ELUPD2: diagonal entry is nearly zero'
C          WRITE(MTERM,*)'II, I, BNDR(II) : ',II,IU(II),BDBC(II)
          RETURN
        ENDIF

C       AI(.) saves the diagonal matrix S^-1:

        AI(II)=1D0/AA(II)

      END DO

C     Factorization loop
C
C     What we at first want to calculate is p with
C
C             - B^t S^-1 B p  =  g - B^t S^-1 f
C
C     To calculate that for local B, S, f and p, consider at first
C     the dimensions in this system:
C
C     a) B is a 4x1 matrix
C     b) S^-1 is a diagonal matrix, given by the 4 diagonal entries of A
C     c) B^t S^-1 B is therefore a 1x1 matrix, thus a scalar
C
C     So in the factorization loop we can calculate:
C
C       DP  = - (B^T S^-1 B)
C       FFP = g - (B^T S^-1 f)
C
C     As S and S^-1 are a diagonal matrices, we can exploit
C     B^T S^-1  =  S^-1 B^T  which saves some multiplications...

      DP=0D0

      DO II = 1,4
        DP  = DP  - AI(II)*(BB1(II)*DD1(II)+BB2(II)*DD2(II))
        FFP = FFP - AI(II)*(BB1(II)*FF1(II)+BB2(II)*FF2(II))
      END DO

C     Solution "loop":

      IF (DABS(DP).LT.1.D-10)  THEN
c        WRITE(MTERM,*)'ERROR in ELUPD2: DP is nearly zero'
        RETURN
      ENDIF

C     At first we calculate the pressure on element IEL,
C     which is simply given by multiplying FFP with the
C     inverte "matrix" DP, i.e.:

      PP = FFP/DP

C     With the help of the pressure, calculate the velocity.
C     This can be done again by the Schur-Complement approach using
C
C           u = S^-1 (f-Bp)
C
C     locally on the current cell:

      DO II=1,4
        UU1(II) = AI(II)*(FF1(II)-DD1(II)*PP)
        UU2(II) = AI(II)*(FF2(II)-DD2(II)*PP)
      END DO

C     Update the global solution vector (U1,U2,P).
C     Overwrite the pressure on element IEL and the
C     velocity in the four DOF's on element IEL by the
C     new pressure/velocity values in these nodes:

      P(IEL)=PP

      DO II = 1,4

C       Don't overwrite velocities on Dirichlet-boundary vertices!

        IF (.NOT.BDBC(II)) THEN
          I=IU(II)
          U1(I)=UU1(II)
          U2(I)=UU2(II)
        END IF

      END DO

      END


C$Procedure DHFA ( Time derivative of half angle )

      DOUBLE PRECISION FUNCTION DHFA( STATE, BODYR)

C$ Abstract
C
C     Calculate the value of the time derivative of the
C     half angle of a spherical body given a state vector
C     STATE and body radius BODYR.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     None.
C
C$ Keywords
C
C     None.
C
C$ Declarations

      IMPLICIT NONE

      DOUBLE PRECISION      STATE (6)
      DOUBLE PRECISION      BODYR

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  --------------------------------------------------
C     STATE      I   SPICE state vector
C     BODYR      I   Radius of body
C
C$ Detailed_Input
C
C     STATE      the state vector of a target body as seen from an
C                observer.
C
C     BODYR      the radius of the target body observed from the
C                position in STATE; the target body assumed as a sphere.
C
C$ Detailed_Output
C
C     The function returns the double precision value of the time
C     derivative of the half angle of a spherical body in radians
C     per second.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1) A negative value for BODYR causes SPICE(BADRADIUS) to signal.
C
C     2) A position component of STATE equaling the zero vector
C        causes SPICE(DEGENERATECASE) to signal.
C
C     3) A condition where the body radius exceeds the distance from
C        the body to the observer causes SPICE(BADGEOMETRY) to signal.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     In this discussion, the notation
C
C        < V1, V2 >
C
C     indicates the dot product of vectors V1 and V2.
C
C     The expression
C
C                     body_radius
C        sin(ALPHA) = -----------                                  (1)
C                       range
C
C     describes the half angle (ALPHA) of a spherical body, i.e. the
C     angular radius of the spherical body as viewed by an observer at
C     distance 'range'.
C
C     Solve for ALPHA
C
C                   -1  body_radius
C        ALPHA = sin  ( ----------- )                              (2)
C                         range
C
C     Take the derivative of ALPHA with respect to time
C
C      d                   1                   d    body_radius
C      --(ALPHA) =  --------------------- *    __ (----------- )   (3)
C      dt           1 -   body_radius  2   1/2 dt    range
C                (      [ ----------- ]   )
C                          range
C
C      d              - body_radius             1      d
C      --(ALPHA) =  --------------------- *   ------ * __(range)   (4)
C      dt           1 -   body_radius  2  1/2      2   dt
C                (      [ ----------- ]  )    range
C                          range
C
C      With
C                        _  _
C      d               < R, V >              -
C      -- ( range )  = -------- ,  range = ||R||                   (5)
C      dt                 -
C                       ||R||
C
C     Apply (5) to equation (4)
C                                                        _  _
C      d              - body_radius             1      < R, V >
C      --(ALPHA) =  --------------------- *   ------ *  --------   (6)
C      dt           1 -   body_radius  2  1/2     2     range
C                (      [ ----------- ]  )    range
C                            range
C
C     Carry range through the denominator gives
C
C                                                _  _
C      d              - body_radius            < R, V >
C      --(ALPHA) =  --------------------- *    --------            (7)
C      dt                 2            2  1/2        2
C                   (range - body_radius )      range
C
C      So since
C                       -    -         _  _
C         ^  -       <  R,   V >     < R, V >
C       < R, V >   =   ---        =  --------
C                       -              range
C                     ||R||
C
C                                                ^  _
C      d              - body_radius            < R, V >
C      --(ALPHA) =  --------------------- *    --------            (8)
C      dt                 2            2  1/2
C                   (range - body_radius )      range
C
C
C$ Examples
C
C           PROGRAM DHFA_EX
C           IMPLICIT              NONE
C
C           INTEGER               DIM
C
C           DOUBLE PRECISION      ET
C           DOUBLE PRECISION      LT
C           DOUBLE PRECISION      DHADT
C           DOUBLE PRECISION      RAD   (3)
C           DOUBLE PRECISION      STATE (6)
C
C           INTEGER               STRLEN
C           PARAMETER           ( STRLEN = 64 )
C
C           CHARACTER*(STRLEN)    BEGSTR
C
C
C           DOUBLE PRECISION      SPD
C           DOUBLE PRECISION      DHFA
C     C
C     C     Load kernels.
C     C
C           CALL FURNSH ('standard.tm')
C
C     C
C     C     An approximate time corresponding to a maximal angular
C     C     separation between the earth and Moon as seen from the sun.
C     C
C           BEGSTR = '2007-DEC-17 04:04:46.935443 (TDB)'
C           CALL STR2ET( BEGSTR, ET )
C
C           CALL BODVRD ('SUN', 'RADII', 3, DIM, RAD )
C
C           CALL SPKEZR ('MOON', ET, 'J2000', 'NONE', 'SUN', STATE, LT )
C
C     C
C     C     The derivative of the half angle at ET should have a near-to
C     C     maximal value as the Moon velocity vector points either
C     C     towards the sun or away.
C     C
C           DHADT = DHFA( STATE, RAD(1) )
C           WRITE(*,*) 'Half angle derivative at begin time  : ', DHADT
C
C     C
C     C     Two weeks later the derivate should have a similar
C     C     magnitude but the opposite sign.
C     C
C           ET = SPD() * 14.D0 + ET
C
C           CALL SPKEZR ('MOON', ET, 'J2000', 'NONE', 'SUN', STATE, LT )
C
C           DHADT = DHFA( STATE, RAD(1) )
C           WRITE(*,*) 'Half angle derivative two weeks later: ', DHADT
C
C           END
C
C   The program compiled on OS X with g77 outputs (radians/sec):
C
C     Half angle derivative at begin time  :  -2.53879935E-11
C     Half angle derivative two weeks later:   2.94362059E-11
C
C   As expected, the derivate values have similar magnitudes but
C   opposite signs.
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     E.D. Wright    (JPL)
C
C$ Version
C
C-    SPICELIB Version 1.0.1, 06-JUL-2009 (EDW)
C
C        Rename of the ZZDHA call to DHFA.
C
C-    SPICELIB Version 1.0.0, 10-FEB-2009 (EDW) (NJB)
C
C-&

C$ Index_Entries
C
C   time derivative of half angle
C
C-&

C
C     SPICELIB functions
C
      DOUBLE PRECISION      VDOT
      LOGICAL               RETURN
      LOGICAL               VZERO

      DOUBLE PRECISION      P     (3)
      DOUBLE PRECISION      R
      DOUBLE PRECISION      RNGRAT
      DOUBLE PRECISION      BASE

      IF ( RETURN () ) THEN
         DHFA = 0.D0
         RETURN
      ELSE
         CALL CHKIN( 'DHFA' )
      END IF

C
C     A zero body radius (point object) returns a zero for the
C     derivative. A negative value indicates an error
C     the caller should diagnose.
C
      IF ( BODYR .EQ. 0 ) THEN

         DHFA = 0.D0
         CALL CHKOUT( 'DHFA' )
         RETURN

      ELSE IF ( BODYR .LT. 0 ) THEN

         DHFA = 0.D0
         CALL SETMSG( 'Non physical case. The input body radius '
     .      //        'has a negative value.'                      )
         CALL SIGERR( 'SPICE(BADRADIUS)'                           )
         CALL CHKOUT( 'DHFA'                                       )
         RETURN

      END IF

C
C     Normalize the position component of STATE. Store the unit vector
C     in P.
C
      CALL UNORM( STATE(1), P, R)

      IF ( VZERO(P) ) THEN

         DHFA = 0.D0
         CALL SETMSG( 'The position component of the input state '
     .      //        'vector equals the zero vector.'             )
         CALL SIGERR( 'SPICE(DEGENERATECASE)'                      )
         CALL CHKOUT( 'DHFA'                                       )
         RETURN

      END IF

C
C     Calculate the range rate.
C
      RNGRAT = VDOT( P, STATE(4) )

C
C     Confirm R > BODYR.
C
      BASE = R**2 - BODYR**2

      IF ( BASE .LE. 0 ) THEN

         DHFA = 0.D0
         CALL SETMSG( 'Invalid case. The body radius, #1, equals '
     .      //        'or exceeds the range to the target, #2.'    )
         CALL ERRDP ( '#1', BODYR                                  )
         CALL ERRDP ( '#2', R                                      )
         CALL SIGERR( 'SPICE(BADGEOMETRY)'                         )
         CALL CHKOUT( 'DHFA'                                       )
         RETURN

      END IF

C
C     Now we safely take the square root of BASE.
C
      BASE = DSQRT( BASE )
      DHFA = -(RNGRAT * BODYR)/(BASE * R)

      CALL CHKOUT( 'DHFA' )
      RETURN

      END


PROGRAM main

  ! --- AMReX Modules ---
  USE amrex_fort_module,                ONLY: &
    amrex_real
  USE amrex_parallel_module,            ONLY: &
    amrex_parallel_ioprocessor, &
    amrex_parallel_communicator

  ! --- Local Modules ---
  USE MF_TwoMoment_UtilitiesModule,     ONLY: &
    MF_ComputeTimeStep,                &
    MF_ComputeTimeStep_Fancy,          &
    MF_ComputeFromConserved,           &
    MF_ComputeFromConserved_Euler
  USE MF_UtilitiesModule,     ONLY: &
    WriteNodalDataToFile, &
    WriteEulerToFile
  USE MyAmrDataModule,                  ONLY: &
    MF_uCR, &
    MF_uPR, &
    MF_uCF, &
    MF_uPF, &
    MF_uAF, &
    MF_uGF
  USE MF_TwoMoment_TallyModule,         ONLY: &
    MF_ComputeTally_TwoMoment
  USE MF_Euler_TallyModule,         ONLY: &
    MF_ComputeTally_Euler
  USE InitializationModule,             ONLY: &
    wrt,              &
    InitializeProgram
  USE FinalizationModule,               ONLY: &
    FinalizeProgram
  USE MyAmrModule,                      ONLY: &
    nLevels,   &
    StepNo,    &
    t,         &
    dt,        &
    nX,        &
    xR,        &
    xL,        &
    nNodes,    &
    t_end,     &
    CFL,       &
    t_wrt,     &
    dt_wrt,    &
    dt_rel,    &
    BA,        &
    GEOM
  USE ProgramHeaderModule,  ONLY: &
    nDOFZ
  USE MF_TwoMoment_TimeSteppingModule_Relativistic,      ONLY: &
    MF_Update_IMEX_RK

  ! --- thornado Modules ---
  USE InputOutput,           ONLY: &
    WriteFieldsAMReX_PlotFile, &
    WriteFieldsAMReX_Checkpoint, &
    ReadCheckpointFile
  USE InputOutputEuler,           ONLY: &
     WriteFieldsAMReX_PlotFile_Euler
  USE GeometryFieldsModuleE, ONLY: &
    uGE
  USE UnitsModule,            ONLY: &
    ActivateUnitsDisplay, &
    DescribeUnitsDisplay, &
    UnitsDisplay, &
    Millisecond, &
    Kilometer

  IMPLICIT NONE

  INTEGER :: num
  REAL(amrex_real) :: n

  n = 1.0_amrex_real
  CALL InitializeProgram

num = 1
  DO WHILE( ALL( t .LT. t_end ) )

    StepNo = StepNo + 1
    IF ( dt_rel .NE. 0.0_amrex_real ) THEN

      dt = dt_rel

    ELSE

      CALL MF_ComputeTimeStep_Fancy( MF_uGF, nX, nNodes, xR, xL, CFL, dt )

    END IF

    IF( ALL( t + dt .LE. t_end ) )THEN
      t = t + dt
    ELSE
      dt = t_end - [t]
      t  = [t_end]
    END IF
    IF( amrex_parallel_ioprocessor() )THEN
      !WRITE(*,'(8x,A8,I8.8,A5,ES13.6E3,1x,A,A6,ES13.6E3,1x,A)') &
       print*,  'StepNo: ', StepNo(0), ' t = ', t / UnitsDisplay % TimeUnit , &
       TRIM( UnitsDisplay % TimeLabel ), ' dt = ', dt(0) / UnitsDisplay % TimeUnit, &
       TRIM( UnitsDisplay % TimeLabel )
    END IF
    CALL MF_Update_IMEX_RK &
           ( t, dt, uGE, MF_uGF, MF_uCF, MF_uCR, GEOM, &
            Verbose_Option = amrex_parallel_ioprocessor()  )

      CALL MF_ComputeTally_Euler( GEOM, MF_uGF, MF_uCF, t(0), &
                                Verbose_Option = .FALSE. )

      CALL MF_ComputeTally_TwoMoment( GEOM, MF_uGF, MF_uCF, MF_uCR, &
                                    t(0), Verbose_Option = .FALSE. )
    IF( ALL( t + dt .GT. t_wrt ) )THEN
      t_wrt = t_wrt + dt_wrt
      wrt   = .TRUE.

    END IF

    IF( wrt )THEN

      CALL MF_ComputeFromConserved( MF_uGF, MF_uCF, MF_uCR, MF_uPR )

      CALL WriteFieldsAMReX_PlotFile &
               ( t(0), StepNo, &
                 MF_uCR_Option = MF_uCR, &
                 MF_uPR_Option = MF_uPR, &
                 num_Option = num )

      CALL MF_ComputeFromConserved_Euler( MF_uGF, MF_uCF, MF_uPF, MF_uAF )

      CALL WriteFieldsAMReX_PlotFile_Euler &
             ( t(0), StepNo, &
               MF_uGF_Option = MF_uGF, &
               MF_uCF_Option = MF_uCF, &
               MF_uPF_Option = MF_uPF, &
               MF_uAF_Option = MF_uAF, &
               num_Option = num )

!      CALL MF_ComputeTally_Euler( GEOM, MF_uGF, MF_uCF, t(0), &
!                                Verbose_Option = .FALSE. )

!      CALL MF_ComputeTally_TwoMoment( GEOM, MF_uGF, MF_uCF, MF_uCR, &
!                                    t(0), Verbose_Option = .FALSE. )

      num = num + 1
      wrt = .FALSE.
    END IF

  END DO

  IF (nDOFZ .GT. 1) THEN

!    CALL WriteNodalDataToFile( GEOM, MF_uGF, MF_uCF, MF_uCR, 'thornado_')

  END IF

  CALL MF_ComputeFromConserved( MF_uGF, MF_uCF, MF_uCR, MF_uPR )

  CALL WriteFieldsAMReX_Checkpoint &
      ( StepNo, nLevels, dt, t, t_wrt, BA % P, &
        MF_uCR % P,  &
        MF_uPR % P  )

  CALL WriteFieldsAMReX_PlotFile &
           ( t(0), StepNo, &
             MF_uCR_Option = MF_uCR, &
             MF_uPR_Option = MF_uPR, &
             num_Option = num )

  CALL MF_ComputeFromConserved_Euler( MF_uGF, MF_uCF, MF_uPF, MF_uAF )

  CALL WriteFieldsAMReX_PlotFile_Euler &
             ( t(0), StepNo, &
               MF_uGF_Option = MF_uGF, &
               MF_uCF_Option = MF_uCF, &
               MF_uPF_Option = MF_uPF, &
               MF_uAF_Option = MF_uAF, &
               num_Option = num )

  CALL MF_ComputeTally_Euler( GEOM, MF_uGF, MF_uCF, t(0), &
                                Verbose_Option = .FALSE. )

  CALL MF_ComputeTally_TwoMoment( GEOM, MF_uGF, MF_uCF, MF_uCR, &
                                    t(0), Verbose_Option = .FALSE. )
  CALL FinalizeProgram( GEOM )

END PROGRAM main


      MODULE m_sphpts
      CONTAINS
      SUBROUTINE sphpts(p,n,r,pos)
c     *******************************************************
c     generates points on sphere at pos with radius r
c     e. wimmer     feb. 1980
c     modified to give a better distribution of points
c     m. weinert    jan. 1982
c     *******************************************************
      USE m_qranf
      USE m_constants, ONLY : tpi_const
      IMPLICIT NONE
C     .. Scalar Arguments ..
      REAL r
      INTEGER n
C     ..
C     .. Array Arguments ..
      REAL p(3,n),pos(3)
C     ..
C     .. Local Scalars ..
      REAL phi,t,tc,x,xr,y,yr,z
      INTEGER i,j
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC cos,sin,sqrt
C     ..
      j = 0
      xr = sqrt(13.e0)
      yr = sqrt(7.e0)
      DO  i = 1,n
         tc = 2.e0*qranf(xr,j) - 1.e0
         phi = tpi_const*qranf(yr,j)
         t = sqrt(1.e0-tc*tc)
         x = t*cos(phi)
         y = t*sin(phi)
         z = tc
         p(1,i) = r*x + pos(1)
         p(2,i) = r*y + pos(2)
         p(3,i) = r*z + pos(3)
      ENDDO
      RETURN
      END SUBROUTINE
      END