C I_Sm.f
C
C VIS-5D function to calculate balanced streamfunction (I_Sm) by inversion 
C of 3-D Ertel PV distribution (from MEAN fields). 

C ==============================================================================

      INTEGER FUNCTION USERFUNC( OUTGRID, OUTNL, OUTLOWLEV,
     *                           INGRID, NR, NC, NL, LOWLEV, MAXNL,
     *                           NVARS, NAMES,
     *                           DATE, TIME,
     *                           PROJECTION, PROJ_ARGS,
     *                           VERTICAL, VERT_ARGS )
C ARGUMENTS:
      implicit none
      INTEGER NVARS
      INTEGER NR, NC, NL(NVARS), LOWLEV(NVARS), MAXNL
      REAL OUTGRID(NR,NC,MAXNL)
      INTEGER OUTNL, OUTLOWLEV
      REAL INGRID(NR,NC,MAXNL,NVARS)
      CHARACTER*8 NAMES(NVARS)
      INTEGER DATE, TIME
      INTEGER PROJECTION
      REAL PROJ_ARGS(*)
      INTEGER VERTICAL
      REAL VERT_ARGS(*)

C LOCAL VARS:
      integer var, ir, ic, il
      integer iu, iv, ih, it, icor, imap, ih0, is0, ierpv 
      integer max_cycles,icycle,max_iters,iter,noconverg,
     $        iter_acum_phi,iter_acum_psi
      real acum_o,acum_f       
      real grav, po, cp, rd, kap, ds, ds2, dssqr

      parameter (grav=9.81,po=100000.,cp=1004.5,rd=2.*cp/7.,kap=rd/cp)

      real thrs_phi,thrs_psi,over_phi,over_psi,under,
     $     res,resmod
      real Fndim,PI2ndim,PIndim,Lndim,Sndim
      real dudy,dudl,dvdx,dvdl,dthetadx,dthetady,dthetadl,absvor,factor,
     $     phixx,phiyy,phipp,phixp1,phixp2,phixp,phiyp1,phiyp2,phiyp,
     $     psix,psiy,psixx,psiyy,psixy,psixp,psiyp,relvor,psiref,delta        
      real p(22),ph(0:22),exnf(22),exnfh(0:22),dexnf(22),dexnfh(0:22),
     $     ppl(22),pplm1(22),pplp1(22),
     $     u(200,200,22),v(200,200,22),
     $     h(200,200,22),t(200,200,22),theta(200,200,22),
     $     cor(200,200),corx(200,200),cory(200,200),
     $     map(200,200),map2(200,200),
     $     q(200,200,0:22),
     $     phi0(200,200,0:22),phi(200,200,0:22),for_phi(200,200,22),
     $     coef_phi(200,200,22),
     $     psi0(200,200,0:22),psi(200,200,0:22),for_psi(200,200,22),
     $     thetaB(200,200),thetaT(200,200),
     $     u0,v0,umaxnl,vmaxnl  

C ==============================================================================
      
C Specify number of levels in OUTGRID
      OUTNL = MAXNL
      OUTLOWLEV = 0
      
C Find the Um, Vm, Hm, Tm, CORIOLIS and MAPFACCR variables (MEAN fields)
      iu = -1
      iv = -1
      ih = -1
      it = -1
      icor = -1
      imap = -1
      do var=1,nvars
        if (names(var) .eq. 'Um') iu = var
        if (names(var) .eq. 'Vm') iv = var
        if (names(var) .eq. 'Hm') ih = var         
        if (names(var) .eq. 'Tm') it = var         
        if (names(var) .eq. 'CORIOLIS') icor = var
        if (names(var) .eq. 'MAPFACCR') imap = var                  
      end do

C If Um, Vm, Hm, Tm, CORIOLIS or MAPFACCR not found, return error 1
      if ( iu.eq.-1 .or. iv.eq.-1 .or. ih.eq.-1 .or. it.eq.-1 .or. 
     $     icor.eq.-1 .or. imap.eq.-1 ) then
        print *, 
     $  "Couldn't find Um, Vm, Hm, Tm, CORIOLIS or MAPFACCR vars!"
        userfunc = 1
        return
      endif

C Grid-length ds (m)
      if (projection.Eq.2) then
        ds=proj_args(6)*1000.      
      else if (projection.Eq.3) then  
        ds=proj_args(5)*1000.
      else
        print *, "Uncomprensible projection data!"
        userfunc = 1
        return
      endif

C Definition max_cycles,max_iters,thrs_phi,thrs_psi,over_phi,over_psi,under,
C ds2, dssqr
      max_cycles=100
      max_iters=500      
      thrs_phi=1.
      thrs_psi=1.E4
      over_phi=2.-3.1415927*(2./Real(nc-1)**2.+2./Real(nr-1)**2.+
     $         2./Real(maxnl-1)**2.)**.5
      over_psi=2.-3.1415927*(2./Real(nc-1)**2.+2./Real(nr-1)**2.)**.5
      under=0.7
      ds2=2.*ds
      dssqr=ds*ds
      
C Constants for nondimensionalization of the system
      Fndim=1.E-4
      PI2ndim=500./Float(maxnl)
      PIndim=PI2ndim**0.5
      Lndim=ds
      Sndim=Fndim*Fndim*Lndim*Lndim/PI2ndim  
      
C ==============================================================================

C NOTE: In VIS5D (ir:N-->S, ic:W-->E, il:B-->T)
      
C Fill p/ph(Pa), exnf/exnfh(J kg-1 K-1), u(m/s), v(m/s), h(m), t(K), theta(K), 
C cor(s-1) and map(adim.)
       
      do il=1,maxnl
        p(il)=1012.5*exp(vert_args(il)/-7.2)*100.
        exnf(il)=cp*((p(il)/po)**kap)  
      enddo
      do il=1,maxnl-1
        exnfh(il)=0.5*(exnf(il)+exnf(il+1))
        ph(il)=(exnfh(il)*(po**kap)/cp)**(1./kap)
      enddo 
      exnfh(0)=2.*exnf(1)-exnfh(1)
      ph(0)=(exnfh(0)*(po**kap)/cp)**(1./kap)      
      exnfh(maxnl)=2.*exnf(maxnl)-exnfh(maxnl-1)      
      ph(maxnl)=(exnfh(maxnl)*(po**kap)/cp)**(1./kap)
               
      do 10 ir=1,nr
      do 10 ic=1,nc
        cor(ir,ic)=ingrid(ir,ic,1,icor)
        map(ir,ic)=ingrid(ir,ic,1,imap)   
        map2(ir,ic)=map(ir,ic)*map(ir,ic)      
      do 10 il=1,maxnl
        u(ir,ic,il)=ingrid(ir,ic,il,iu)
        v(ir,ic,il)=ingrid(ir,ic,il,iv) 
        h(ir,ic,il)=ingrid(ir,ic,il,ih)                   
        t(ir,ic,il)=ingrid(ir,ic,il,it)+273.16
        theta(ir,ic,il)=t(ir,ic,il)*cp/exnf(il)        
10    continue   

C Remove any unstable layers
      do 15 ir=1,nr
      do 15 ic=1,nc
      do 15 il=2,maxnl
        If (theta(ir,ic,il).Lt.theta(ir,ic,il-1)+1.E-2) 
     $     theta(ir,ic,il)=theta(ir,ic,il-1)+1.E-2
15    continue

C Definition of Neumann boundary conditions thetaB and thetaT (K)
      do 100 ir=1,nr
      do 100 ic=1,nc
        thetaB(ir,ic)=theta(ir,ic,1)
        thetaT(ir,ic)=theta(ir,ic,maxnl)               
100   continue      

C Definition dexnf, dexnfh, pplp1, pplm1 and ppl
      dexnfh(0)=exnfh(1)-exnfh(0)
      do 120 il=1,maxnl-1
       dexnf(il)=exnf(il+1)-exnf(il)
       dexnfh(il)=exnfh(il+1)-exnfh(il) 
       pplp1(il)=1./(dexnfh(il)*dexnf(il))           
       pplm1(il)=1./(dexnfh(il-1)*dexnf(il))                     
       ppl(il)=pplp1(il)+pplm1(il)    
120   continue      

C ==============================================================================

C If Hm0 available (MEAN fields), retrieve phi0 and skip calculation
      ih0 = -1
      do var=1,nvars
        if (names(var) .eq. 'Hm0') ih0 = var                        
      end do

      If (ih0.Ne.-1) Then       
       Do 11 ir=1,nr
       Do 11 ic=1,nc
         phi0(ir,ic,0)=grav*ingrid(ir,ic,1,ih0)+thetaB(ir,ic)*
     $                 (exnf(1)-exnfh(0))
       Do 11 il=1,maxnl
         phi0(ir,ic,il)=phi0(ir,ic,il-1)+dexnfh(il-1)*
     $                  (grav*ingrid(ir,ic,il,ih0)-
     $                  phi0(ir,ic,il-1))/(exnf(il)-exnfh(il-1))
11     Continue
       print*,'Retrieved PHI0'
       Goto 111 
      Endif 

C CALCULATION OF FIRST-GUESS GEOPOTENTIAL phi0 (m2 s-2)    

      do 30 ir=1,nr
      do 30 ic=1,nc       
        phi0(ir,ic,0)=grav*h(ir,ic,1)+0.5*dexnfh(0)*theta(ir,ic,1)

        do 40 il=1,maxnl
          phi0(ir,ic,il)=phi0(ir,ic,il-1)-dexnfh(il-1)*theta(ir,ic,il)  
40      continue    

30    continue

      print*,'Calculated PHI0'
 
111   CONTINUE       
      
C ==============================================================================

C If Sm0 available (MEAN fields), retrieve psi0 and skip calculation
      is0 = -1
      do var=1,nvars
        if (names(var) .eq. 'Sm0') is0 = var                        
      end do

      If (is0.Ne.-1) Then       
       Do 22 ir=1,nr
       Do 22 ic=1,nc
         psi0(ir,ic,0)=(grav/Fndim)*ingrid(ir,ic,1,is0)+
     $                 (thetaB(ir,ic)/Fndim)*
     $                 (exnf(1)-exnfh(0))
       Do 22 il=1,maxnl
         psi0(ir,ic,il)=psi0(ir,ic,il-1)+dexnfh(il-1)*
     $                  ((grav/Fndim)*ingrid(ir,ic,il,is0)-
     $                  psi0(ir,ic,il-1))/(exnf(il)-exnfh(il-1))
22     Continue
       print*,'Retrieved PSI0'
       Goto 222 
      Endif 
      
C CALCULATION OF FIRST GUESS STREAMFUNCTION psi0 (m2 s-1)

C Relative vorticity (forcing for_psi) and set initial values
      do 60 ir=2,nr-1
      do 60 ic=2,nc-1
      do 60 il=1,maxnl-1
        dudy=0.5*map(ir,ic)*(u(ir-1,ic,il)+u(ir-1,ic,il+1)-
     $       u(ir+1,ic,il)-u(ir+1,ic,il+1))/ds2     
        dvdx=0.5*map(ir,ic)*(v(ir,ic+1,il)+v(ir,ic+1,il+1)-
     $       v(ir,ic-1,il)-v(ir,ic-1,il+1))/ds2
        relvor=dvdx-dudy
        relvor=Max(relvor,-cor(ir,ic)+1.E-8)              
        for_psi(ir,ic,il)=relvor/map2(ir,ic)                     
        psi0(ir,ic,il)=0.
60    continue      

      DO 65 il=1,maxnl-1

C Set boundary values for each level
       delta=0.
       psiref=phi0(1,1,il)/Fndim
70     continue
       psi0(1,1,il)=psiref 

       do ic=2,nc
         psi0(1,ic,il)=psi0(1,ic-1,il)+
     $                 0.5*ds*(v(1,ic,il)+v(1,ic,il+1)+
     $                         v(1,ic-1,il)+v(1,ic-1,il+1))/
     $                 (map(1,ic)+map(1,ic-1))-
     $                 delta        
       enddo       
       do ir=2,nr
         psi0(ir,nc,il)=psi0(ir-1,nc,il)+
     $                 0.5*ds*(u(ir,nc,il)+u(ir,nc,il+1)+
     $                         u(ir-1,nc,il)+u(ir-1,nc,il+1))/
     $                 (map(ir,nc)+map(ir-1,nc))-
     $                 delta        
       enddo       
       do ic=nc-1,1,-1
         psi0(nr,ic,il)=psi0(nr,ic+1,il)-
     $                 0.5*ds*(v(nr,ic,il)+v(nr,ic,il+1)+
     $                         v(nr,ic+1,il)+v(nr,ic+1,il+1))/
     $                 (map(nr,ic)+map(nr,ic+1))-
     $                 delta        
       enddo       
       do ir=nr-1,1,-1
         psi0(ir,1,il)=psi0(ir+1,1,il)-
     $                 0.5*ds*(u(ir,1,il)+u(ir,1,il+1)+
     $                         u(ir+1,1,il)+u(ir+1,1,il+1))/
     $                 (map(ir,1)+map(ir+1,1))-
     $                 delta        
       enddo   

       If (delta.Eq.0.) Then
        delta=(psi0(1,1,il)-psiref)/Real(2*(nr+nc)-4)
        If (delta.Eq.0.) Goto 75   
        Goto 70
       Endif
       
75     Continue     

C SOR solution for interior values for each level
       Do 80 iter=1,max_iters
       noconverg=0

         Do 85 ir=2,nr-1
         Do 85 ic=2,nc-1
           res=psi0(ir,ic+1,il)+psi0(ir,ic-1,il)+
     $         psi0(ir-1,ic,il)+psi0(ir+1,ic,il)-
     $         4.*psi0(ir,ic,il)-
     $         dssqr*for_psi(ir,ic,il)
           resmod=.25*over_psi*res  
           psi0(ir,ic,il)=psi0(ir,ic,il)+resmod
           If (Abs(resmod).Gt.thrs_psi) noconverg=noconverg+1            
85       Continue 

         If (noconverg.Eq.0) Goto 90
      
80     Continue       

       print*,'Convergence for PSI0(',il,') not reached'
       Goto 65
 
90     Continue 
       print*,'PSI0(',il,') converged at iter=',iter
 
65    CONTINUE

C Finally, top and bottom B.C
      do 95 ir=1,nr
      do 95 ic=1,nc
        psi0(ir,ic,0)=psi0(ir,ic,1)+dexnfh(0)*
     $                thetaB(ir,ic)/Fndim
        psi0(ir,ic,maxnl)=psi0(ir,ic,maxnl-1)-dexnfh(maxnl-1)*
     $                    thetaT(ir,ic)/Fndim
95    continue

222   CONTINUE 

C ==============================================================================

C If ErPVm available (MEAN fields), retrieve q and skip calculation
      ierpv = -1
      do var=1,nvars
        if (names(var) .eq. 'ErPVm') ierpv = var                        
      end do

      If (ierpv.Ne.-1) Then       
       Do 33 ir=1,nr
       Do 33 ic=1,nc
         q(ir,ic,0)=1E-6*ingrid(ir,ic,1,ierpv)
       Do 33 il=1,maxnl
         q(ir,ic,il)=q(ir,ic,il-1)+dexnfh(il-1)*
     $                  (1E-6*ingrid(ir,ic,il,ierpv)-
     $                  q(ir,ic,il-1))/(exnf(il)-exnfh(il-1))
33     Continue
       print*,'Retrieved q'
       Goto 333 
      Endif 
      
C CALCULATION OF ERTEL POTENTIAL VORTICITY q (m2 K s-1 kg-1)   (PV min=1E-2 PVU)  

      DO 20 ir=2,nr-1
      DO 20 ic=2,nc-1

        u0=-map(ir,ic)*(psi0(ir-1,ic,0)-
     $     psi0(ir+1,ic,0))/ds2  
        umaxnl=-map(ir,ic)*(psi0(ir-1,ic,maxnl)-
     $         psi0(ir+1,ic,maxnl))/ds2              
        v0=map(ir,ic)*(psi0(ir,ic+1,0)-
     $     psi0(ir,ic-1,0))/ds2  
        vmaxnl=map(ir,ic)*(psi0(ir,ic+1,maxnl)-
     $         psi0(ir,ic-1,maxnl))/ds2       

      DO 20 il=1,maxnl-1

        dudy=0.5*map(ir,ic)*(u(ir-1,ic,il)+u(ir-1,ic,il+1)-
     $       u(ir+1,ic,il)-u(ir+1,ic,il+1))/ds2
        dvdx=0.5*map(ir,ic)*(v(ir,ic+1,il)+v(ir,ic+1,il+1)-
     $       v(ir,ic-1,il)-v(ir,ic-1,il+1))/ds2

        absvor=dvdx-dudy+cor(ir,ic)
        absvor=Max(absvor,1.E-8)   
     
        If (il.Eq.1) Then
          dudl=(0.5*(u(ir,ic,il+1)+u(ir,ic,il+2))-u0)/
     $         (exnfh(il+1)-exnfh(il-1)) 
          dvdl=(0.5*(v(ir,ic,il+1)+v(ir,ic,il+2))-v0)/
     $         (exnfh(il+1)-exnfh(il-1))                 
        Else If (il.Eq.maxnl-1) Then
          dudl=(umaxnl-0.5*(u(ir,ic,il)+u(ir,ic,il-1)))/
     $         (exnfh(il+1)-exnfh(il-1))
          dvdl=(vmaxnl-0.5*(v(ir,ic,il)+v(ir,ic,il-1)))/
     $         (exnfh(il+1)-exnfh(il-1))                     
        Else
          dudl=0.5*(u(ir,ic,il+1)+u(ir,ic,il+2)-
     $         u(ir,ic,il)-u(ir,ic,il-1))/
     $         (exnfh(il+1)-exnfh(il-1))        
          dvdl=0.5*(v(ir,ic,il+1)+v(ir,ic,il+2)-
     $         v(ir,ic,il)-v(ir,ic,il-1))/
     $         (exnfh(il+1)-exnfh(il-1))   
        Endif
             
        dthetadx=0.5*map(ir,ic)*(theta(ir,ic+1,il)+theta(ir,ic+1,il+1)-
     $           theta(ir,ic-1,il)-theta(ir,ic-1,il+1))/ds2       
        dthetady=0.5*map(ir,ic)*(theta(ir-1,ic,il)+theta(ir-1,ic,il+1)-
     $           theta(ir+1,ic,il)-theta(ir+1,ic,il+1))/ds2
        dthetadl=(theta(ir,ic,il+1)-theta(ir,ic,il))/dexnf(il)
           
        factor=grav*kap*exnfh(il)/ph(il)
        
        q(ir,ic,il)=-factor*( absvor*dthetadl -
     $                          dvdl*dthetadx +
     $                          dudl*dthetady ) 

        q(ir,ic,il)=Max(q(ir,ic,il),1.E-8)
        
        If (il.Eq.1) q(ir,ic,0)=q(ir,ic,il)
        If (il.Eq.maxnl-1) q(ir,ic,maxnl)=q(ir,ic,il)     
  
20    CONTINUE

C Extrapolate values at the lateral boundaries
      Do 25 il=0,maxnl
        Do ir=1,nr
          q(ir,1,il)=2.*q(ir,2,il)-q(ir,3,il)
          q(ir,nc,il)=2.*q(ir,nc-1,il)-q(ir,nc-2,il)          
        Enddo
        Do ic=1,nc
          q(1,ic,il)=2.*q(2,ic,il)-q(3,ic,il)
          q(nr,ic,il)=2.*q(nr-1,ic,il)-q(nr-2,ic,il)          
        Enddo
        q(1,1,il)=-q(2,2,il)+q(2,1,il)+q(1,2,il)
        q(nr,1,il)=-q(nr-1,2,il)+q(nr-1,1,il)+q(nr,2,il)
        q(1,nc,il)=-q(2,nc-1,il)+q(1,nc-1,il)+q(2,nc,il)
        q(nr,nc,il)=-q(nr-1,nc-1,il)+q(nr-1,nc,il)+q(nr,nc-1,il)                         
25    Continue

      print*,'Calculated q'
 
333   CONTINUE  

C ==============================================================================

C NON-DIMENSIONALIZATION OF SYSTEM and psi=psi0, phi=phi0, corx,cory
      ds=ds/Lndim
      ds2=2.*ds
      dssqr=ds*ds
      thrs_phi=thrs_phi/(Sndim*PI2ndim)
      thrs_psi=Fndim*thrs_psi/(Sndim*PI2ndim)
      
      do 910 ir=1,nr
      do 910 ic=1,nc      
        cor(ir,ic)=cor(ir,ic)/Fndim
        thetaB(ir,ic)=thetaB(ir,ic)/(Sndim*PIndim)
        thetaT(ir,ic)=thetaT(ir,ic)/(Sndim*PIndim)
      do 910 il=0,maxnl
        psi0(ir,ic,il)=Fndim*psi0(ir,ic,il)/(Sndim*PI2ndim)
        psi(ir,ic,il)=psi0(ir,ic,il)
        phi0(ir,ic,il)=phi0(ir,ic,il)/(Sndim*PI2ndim)
        phi(ir,ic,il)=phi0(ir,ic,il)
        factor=grav*kap*exnfh(il)/ph(il)
        q(ir,ic,il)=q(ir,ic,il)/(factor*Sndim*Fndim)                
910   continue     

      do 920 ir=2,nr-1
      do 920 ic=2,nc-1 
        corx(ir,ic)=map(ir,ic)*(cor(ir,ic+1)-cor(ir,ic-1))/ds2
        cory(ir,ic)=map(ir,ic)*(cor(ir-1,ic)-cor(ir+1,ic))/ds2        
920   continue

      exnfh(0)=exnfh(0)/PIndim
      dexnfh(0)=dexnfh(0)/PIndim
      do 930 il=1,maxnl-1
        exnf(il)=exnf(il)/PIndim
        dexnf(il)=dexnf(il)/PIndim     
        exnfh(il)=exnfh(il)/PIndim
        dexnfh(il)=dexnfh(il)/PIndim        
        pplp1(il)=PI2ndim*pplp1(il)
        pplm1(il)=PI2ndim*pplm1(il)
        ppl(il)=PI2ndim*ppl(il)                         
930   continue 
      exnf(maxnl)=exnf(maxnl)/PIndim     
      exnfh(maxnl)=exnfh(maxnl)/PIndim

C ==============================================================================

C START CYCLES FOR PV INVERSION (get phi and psi iteratively)

      acum_f=1000000.
       
      DO 1000 icycle=1,max_cycles

       iter_acum_phi=0
       iter_acum_psi=0
       acum_o=acum_f 
       
       print*
       print*,'CYCLE=',icycle

C ==============================================================================

C SOLVE GEOPOTENTIAL phi (m2 s-2)      

C ------------------------------------------------------------------------------
      
C Forcing function for_phi (and coef_phi)   
       Do 2100 ir=2,nr-1
       Do 2100 ic=2,nc-1
       Do 2100 il=1,maxnl-1

         psix=map(ir,ic)*(psi(ir,ic+1,il)-psi(ir,ic-1,il))/                   
     $        ds2 
         psiy=map(ir,ic)*(psi(ir-1,ic,il)-psi(ir+1,ic,il))/                   
     $        ds2 
         psixx=map2(ir,ic)*(psi(ir,ic+1,il)+psi(ir,ic-1,il)-
     $         2.*psi(ir,ic,il))/dssqr   
         psiyy=map2(ir,ic)*(psi(ir-1,ic,il)+psi(ir+1,ic,il)-
     $         2.*psi(ir,ic,il))/dssqr  
     
         relvor=psixx+psiyy        
         relvor=Max(relvor,-cor(ir,ic)+1.E-4)   
       
         psixy=map2(ir,ic)*(psi(ir-1,ic+1,il)-psi(ir-1,ic-1,il)-
     $         psi(ir+1,ic+1,il)+psi(ir+1,ic-1,il))/
     $         (ds2*ds2)       
         psixp=map(ir,ic)*(psi(ir,ic+1,il+1)-psi(ir,ic-1,il+1)-
     $         psi(ir,ic+1,il-1)+psi(ir,ic-1,il-1))/
     $         (ds2*(exnfh(il+1)-exnfh(il-1)))   
         psiyp=map(ir,ic)*(psi(ir-1,ic,il+1)-psi(ir+1,ic,il+1)-
     $         psi(ir-1,ic,il-1)+psi(ir+1,ic,il-1))/
     $         (ds2*(exnfh(il+1)-exnfh(il-1)))         

         phixp1=0.5*(phi0(ir,ic+1,il+1)/dexnfh(il)-
     $          phi0(ir,ic+1,il-1)/dexnfh(il-1)+phi0(ir,ic+1,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))    
         phixp2=0.5*(phi0(ir,ic-1,il+1)/dexnfh(il)-
     $          phi0(ir,ic-1,il-1)/dexnfh(il-1)+phi0(ir,ic-1,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))        
         phixp=map(ir,ic)*(phixp1-phixp2)/ds2 
        
         phiyp1=0.5*(phi0(ir-1,ic,il+1)/dexnfh(il)-
     $          phi0(ir-1,ic,il-1)/dexnfh(il-1)+phi0(ir-1,ic,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))    
         phiyp2=0.5*(phi0(ir+1,ic,il+1)/dexnfh(il)-
     $          phi0(ir+1,ic,il-1)/dexnfh(il-1)+phi0(ir+1,ic,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))               
         phiyp=map(ir,ic)*(phiyp1-phiyp2)/ds2

         for_phi(ir,ic,il)=(q(ir,ic,il)+
     $                     cor(ir,ic)*relvor+
     $                     corx(ir,ic)*psix+cory(ir,ic)*psiy+
     $                     2.*psixx*psiyy-
     $                     2.*psixy*psixy+
     $                     psixp*phixp+psiyp*phiyp)/map2(ir,ic)

         coef_phi(ir,ic,il)=(cor(ir,ic)+relvor)/map2(ir,ic)
                   
2100   Continue

C ------------------------------------------------------------------------------

C SOR solution for phi (3-D)
       Do 2250 iter=1,max_iters
         noconverg=0

         Do 2320 il=1,maxnl-1    
         Do 2310 ir=2,nr-1
         Do 2300 ic=2,nc-1 

           If (il.Eq.1) Then
             res=phi(ir,ic+1,il)+phi(ir,ic-1,il)+
     $           phi(ir-1,ic,il)+phi(ir+1,ic,il)-
     $           4.*phi(ir,ic,il)+
     $           coef_phi(ir,ic,il)*dssqr*     
     $           (pplp1(il)*phi(ir,ic,il+1)+
     $           thetaB(ir,ic)/dexnf(il)-
     $           pplp1(il)*phi(ir,ic,il))-          
     $           dssqr*for_phi(ir,ic,il)                   
             resmod=over_phi*res/
     $              (4.+coef_phi(ir,ic,il)*dssqr*pplp1(il))  
             phi(ir,ic,il)=phi(ir,ic,il)+resmod
           Else If (il.Eq.maxnl-1) Then
             res=phi(ir,ic+1,il)+phi(ir,ic-1,il)+
     $           phi(ir-1,ic,il)+phi(ir+1,ic,il)-
     $           4.*phi(ir,ic,il)+
     $           coef_phi(ir,ic,il)*dssqr*     
     $           (pplm1(il)*phi(ir,ic,il-1)-
     $           thetaT(ir,ic)/dexnf(il)-
     $           pplm1(il)*phi(ir,ic,il))-          
     $           dssqr*for_phi(ir,ic,il)
             resmod=over_phi*res/
     $              (4.+coef_phi(ir,ic,il)*dssqr*pplm1(il))  
             phi(ir,ic,il)=phi(ir,ic,il)+resmod       
           Else
             res=phi(ir,ic+1,il)+phi(ir,ic-1,il)+
     $           phi(ir-1,ic,il)+phi(ir+1,ic,il)-
     $           4.*phi(ir,ic,il)+
     $           coef_phi(ir,ic,il)*dssqr*
     $           (pplp1(il)*phi(ir,ic,il+1)+
     $           pplm1(il)*phi(ir,ic,il-1)-
     $           ppl(il)*phi(ir,ic,il))-
     $           dssqr*for_phi(ir,ic,il)
             resmod=over_phi*res/
     $              (4.+coef_phi(ir,ic,il)*dssqr*ppl(il)) 
             phi(ir,ic,il)=phi(ir,ic,il)+resmod               
           Endif
         
           If (Abs(resmod).Gt.thrs_phi) noconverg=noconverg+1              
                              
2300     Continue
2310     Continue
2320     Continue 

         If (noconverg.Eq.0) Goto 2350
      
2250   Continue       

       print*,'Convergence for PHI not reached'
       iter_acum_phi=iter_acum_phi+iter
       Goto 2375
 
2350   Continue 
       print*,'PHI converged at iter=',iter
       iter_acum_phi=iter_acum_phi+iter       

2375   Continue

C ------------------------------------------------------------------------------              
      
C Top and bottom BCs and underrelaxation
       Do 2400 ir=1,nr
       Do 2400 ic=1,nc
         phi(ir,ic,0)=phi(ir,ic,1)+dexnfh(0)*
     $                thetaB(ir,ic)
         phi(ir,ic,maxnl)=phi(ir,ic,maxnl-1)-dexnfh(maxnl-1)*
     $                    thetaT(ir,ic)
       Do 2410 il=0,maxnl
         phi(ir,ic,il)=under*phi(ir,ic,il)+(1.-under)*phi0(ir,ic,il)
2410   Continue       
2400   Continue              

C ==============================================================================

C SOLVE STREAMFUNCTION psi (m2 s-1)

C ------------------------------------------------------------------------------
     
C Forcing function for_psi     
       Do 1100 ir=2,nr-1
       Do 1100 ic=2,nc-1
       Do 1100 il=1,maxnl-1

         phixx=map2(ir,ic)*(phi(ir,ic+1,il)+phi(ir,ic-1,il)-
     $         2.*phi(ir,ic,il))/dssqr
         phiyy=map2(ir,ic)*(phi(ir-1,ic,il)+phi(ir+1,ic,il)-
     $         2.*phi(ir,ic,il))/dssqr
     
         phipp=pplp1(il)*phi(ir,ic,il+1)+
     $         pplm1(il)*phi(ir,ic,il-1)-
     $         ppl(il)*phi(ir,ic,il)
         phipp=Max(1.E-4,phipp)

         phixp1=0.5*(phi(ir,ic+1,il+1)/dexnfh(il)-
     $          phi(ir,ic+1,il-1)/dexnfh(il-1)+phi(ir,ic+1,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))    
         phixp2=0.5*(phi(ir,ic-1,il+1)/dexnfh(il)-
     $          phi(ir,ic-1,il-1)/dexnfh(il-1)+phi(ir,ic-1,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))        
         phixp=map(ir,ic)*(phixp1-phixp2)/ds2 
        
         phiyp1=0.5*(phi(ir-1,ic,il+1)/dexnfh(il)-
     $          phi(ir-1,ic,il-1)/dexnfh(il-1)+phi(ir-1,ic,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))    
         phiyp2=0.5*(phi(ir+1,ic,il+1)/dexnfh(il)-
     $          phi(ir+1,ic,il-1)/dexnfh(il-1)+phi(ir+1,ic,il)*
     $          (-1./dexnfh(il)+1./dexnfh(il-1)))               
         phiyp=map(ir,ic)*(phiyp1-phiyp2)/ds2     
          
         psix=map(ir,ic)*(psi0(ir,ic+1,il)-psi0(ir,ic-1,il))/                   
     $        ds2 
         psiy=map(ir,ic)*(psi0(ir-1,ic,il)-psi0(ir+1,ic,il))/                   
     $        ds2 
         psixx=map2(ir,ic)*(psi0(ir,ic+1,il)+psi0(ir,ic-1,il)-
     $         2.*psi0(ir,ic,il))/dssqr   
         psiyy=map2(ir,ic)*(psi0(ir-1,ic,il)+psi0(ir+1,ic,il)-
     $         2.*psi0(ir,ic,il))/dssqr
         psixy=map2(ir,ic)*(psi0(ir-1,ic+1,il)-psi0(ir-1,ic-1,il)-
     $         psi0(ir+1,ic+1,il)+psi0(ir+1,ic-1,il))/
     $         (ds2*ds2)       
         psixp=map(ir,ic)*(psi0(ir,ic+1,il+1)-psi0(ir,ic-1,il+1)-
     $         psi0(ir,ic+1,il-1)+psi0(ir,ic-1,il-1))/
     $         (ds2*(exnfh(il+1)-exnfh(il-1)))   
         psiyp=map(ir,ic)*(psi0(ir-1,ic,il+1)-psi0(ir+1,ic,il+1)-
     $         psi0(ir-1,ic,il-1)+psi0(ir+1,ic,il-1))/
     $         (ds2*(exnfh(il+1)-exnfh(il-1)))          

         for_psi(ir,ic,il)=(q(ir,ic,il)+
     $                     phixx+phiyy-
     $                     cor(ir,ic)*phipp-
     $                     corx(ir,ic)*psix-cory(ir,ic)*psiy-
     $                     2.*psixx*psiyy+
     $                     2.*psixy*psixy+
     $                     psixp*phixp+psiyp*phiyp)/
     $                     (map2(ir,ic)*(cor(ir,ic)+phipp)) 
 
1100   Continue

C ------------------------------------------------------------------------------

C SOR solution for psi (for each level)
       Do 1200 il=1,maxnl-1

         Do 1250 iter=1,max_iters
           noconverg=0

           Do 1300 ir=2,nr-1
           Do 1300 ic=2,nc-1
             res=psi(ir,ic+1,il)+psi(ir,ic-1,il)+
     $           psi(ir-1,ic,il)+psi(ir+1,ic,il)-
     $           4.*psi(ir,ic,il)-
     $           dssqr*for_psi(ir,ic,il)
             resmod=.25*over_psi*res  
             psi(ir,ic,il)=psi(ir,ic,il)+resmod

             If (Abs(resmod).Gt.thrs_psi) noconverg=noconverg+1            

1300       Continue 

           If (noconverg.Eq.0) Goto 1350
      
1250     Continue       

         print*,'Convergence for PSI(',il,') not reached'
         iter_acum_psi=iter_acum_psi+iter       
         Goto 1200
 
1350     Continue 
         print*,'PSI(',il,') converged at iter=',iter
         iter_acum_psi=iter_acum_psi+iter
      
1200   Continue      

C ------------------------------------------------------------------------------

C When system convergence reached (or minimum), top and bottom BCs for psi and exit
       acum_f=(iter_acum_phi+iter_acum_psi/Real(maxnl-1))/2.
       If (acum_f.Ge.acum_o) Then 
         Do 1375 ir=1,nr
         Do 1375 ic=1,nc
           psi(ir,ic,0)=psi(ir,ic,1)+dexnfh(0)*
     $                  thetaB(ir,ic)
           psi(ir,ic,maxnl)=psi(ir,ic,maxnl-1)-dexnfh(maxnl-1)*
     $                      thetaT(ir,ic)
1375     Continue        
         print* 
         print*,'SYSTEM CONVERGED'
         print*
         print*
         Goto 1023
       Endif

C ------------------------------------------------------------------------------

C Top and bottom BCs, underrelaxation and new guess psi0=psi, phi0=phi
       Do 1400 ir=1,nr
       Do 1400 ic=1,nc
         psi(ir,ic,0)=psi(ir,ic,1)+dexnfh(0)*
     $                thetaB(ir,ic)
         psi(ir,ic,maxnl)=psi(ir,ic,maxnl-1)-dexnfh(maxnl-1)*
     $                    thetaT(ir,ic)
       Do 1410 il=0,maxnl
         psi(ir,ic,il)=under*psi(ir,ic,il)+(1.-under)*psi0(ir,ic,il)
         psi0(ir,ic,il)=psi(ir,ic,il)
         phi0(ir,ic,il)=phi(ir,ic,il)
1410   Continue
1400   Continue              

C ==============================================================================     

C END CYCLE FOR PV INVERSION
          
1000  CONTINUE      

      print*
      print*,'SYSTEM CONVERGENCE NOT REACHED'
      print*
      print*

1023  Continue

C ==============================================================================

C OUTPUT OF BALANCED STREAMFUNCTION I_Sm (=[Fndim/grav]*psi, 
C in [grav/Fndim] m2 s-1)

      print*,
     $'Output of balanced streamfunction I_Sm ([grav/Fndim] m2 s-1)'
      DO 9999 ir=1,nr
      DO 9999 ic=1,nc
      DO 9999 il=1,maxnl
        outgrid(ir,ic,il)=Sndim*PI2ndim*
     $                    ( psi0(ir,ic,il)-(exnfh(il)-exnf(il))*
     $                    (psi0(ir,ic,il)-psi0(ir,ic,il-1))/
     $                    dexnfh(il-1) )/grav
9999  CONTINUE     

C ==============================================================================
               
      USERFUNC = 0
      RETURN
      END

C ==============================================================================
