C     -*- Mode: Fortran; -*-
c-----------------------------------------------------------------
c     Ravi Samtaney
c     Copyright 2004
c     Princeton Plasma Physics Laboratory
c     All Rights Reserved
c-----------------------------------------------------------------
c     $Log$
c-----------------------------------------------------------------------
      subroutine NewtonSolve(Rn, Zn,maxiter)
c-----------------------------------------------------------------
      use mesh
      use mesh_common
      use properties
#ifdef PARALLEL
      use mpistuff
#endif
      use OptionFlags
c
      implicit none
c     
c=======Declarations=========
      double precision:: Rn(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Zn(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Rnew(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Znew(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Rold(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Zold(IXLO:IXHI+1,IYLO:IYHI+1)
c     
      double precision:: deltaR(1:nxlocal,1:nylocal)
      double precision:: deltaZ(1:nxlocal,1:nylocal)
      double precision:: rhs(1:nxlocal,1:nylocal,2)
c
      integer:: maxiter, iter
      double precision:: L2err, LInfErr, tolerance
      double precision:: lamda
c
c      call SetBoundaryValuesRZ(Rn,Zn)
c      call WriteTextFile(Rn,Zn)
c
      iter=1
c      maxiter=10
      tolerance=1.D-08
      lamda=0.0001D0
      L2err=1.D16
      deltaR=0.D0
      deltaZ=0.D0
c
      call WriteTextFile(Rn,Zn,iter)
      do while(iter.le.maxiter.and.L2err.gt.tolerance)
      Rold=Rn
      Zold=Zn
c
      call ComputeRHS(rhs, Rn, Zn)
c    
c      rhs=1.D-04
c      call ComputeNorm(rhs,L2err)
c
      call KrylovSolve(rhs,Rn, Zn,deltaR, deltaZ)
c      
c      Rn(1:nxlocal,1:nylocal)=Rold(1:nxlocal,1:nylocal)+
c     &     lamda*deltaR
c
c      Zn(1:nxlocal,1:nylocal)=Zold(1:nxlocal,1:nylocal)+
c     &     lamda*deltaZ
c
      Rn(2:nxlocal+1,2:nylocal+1)=Rold(2:nxlocal+1,2:nylocal+1)+
     &     lamda*deltaR
      Zn(2:nxlocal+1,2:nylocal+1)=Zold(2:nxlocal+1,2:nylocal+1)+
     &     lamda*deltaZ
      call SetBoundaryValuesRZ(Rn,Zn)
      write(6,*) 'MaxRHS', maxval(rhs(:,:,1)), maxval(rhs(:,:,2))
      write(6,*) 'MinRHS', minval(rhs(:,:,1)), minval(rhs(:,:,2))
      call WriteTextFile(Rn,Zn,iter)
      iter=iter+1
      enddo
c
      call WriteTextFile(Rn,Zn,iter)
c
      return
      end
c-----------------------------------------------------------------------
      subroutine ComputeRHS(rhsn, Rn, Zn)
c-----------------------------------------------------------------
      use mesh
      use properties
#ifdef PARALLEL
      use mpistuff
#endif
      use OptionFlags
c
      implicit none
      double precision:: Zn(ixlo:ixhi+1,iylo:iyhi+1)
      double precision:: Rn(ixlo:ixhi+1,iylo:iyhi+1)
      double precision:: rhsn(1:nxlocal,1:nylocal,2)
c     
c     Local vars
      double precision:: 
     &   ux(IXLO:IXHI,IYLO:IYHI,nvar)
      double precision:: rhs(1:nxlocal,1:nylocal,nvar)
c     
c     Local variables
      double precision:: vx(IXLO:IXHI,IYLO:IYHI,nvar)
      double precision:: finv(1:nxlocal+1,1:nylocal,nvar)
      double precision:: hinv(1:nxlocal,1:nylocal+1,nvar)
c
      double precision:: src(1:nxlocal,1:nylocal,nvar)
c     
      integer:: indr,indp,indz
      integer:: i,j,k,l
c     
c      call mesh_update_bdry_async(ux,nvar)
c     
      call SetBoundaryValuesRZ(Rn,Zn)
      call ComputeMetrics(Rn,Zn)
c
      call ComputeConserved(ux)
c
c      call Exchange(ux,nvar)
      call SetBoundaryValues(ux)
c
c     
      rhs=0.D0
      finv=0.D0
      hinv=0.D0
c     
      call ConservedToPrimitive(ux,vx)
c
      call InviscidFluxXi(finv,vx)
      call InviscidFluxEta(hinv,vx)
c     
      call FluxBoundaryConditions(vx,finv,hinv)
c
      call DivergenceFlux(rhs,finv,hinv)
c     Negative of div(hyperbolic fluxes) goes to rhs
c      rhs=-rhs
c
      call Source(vx,src)
c      rhs=rhs+src
      rhs=rhs-src
c
      rhsn(:,:,1:2)=rhs(:,:,1:2)
c     
      return
      end
c     
c-----------------------------------------------------------------------
      subroutine ConservedToPrimitive(u,v)
      use mesh
      use mesh_common
      use properties
      implicit none
      integer:: ilo, ihi
      double precision:: u(IXLO:IXHI,IYLO:IYHI,nvar)
      double precision:: v(IXLO:IXHI,IYLO:IYHI,nvar)
      integer:: i,j,k,l
      double precision::  bi,bj,bk, press
      double precision:: pressFloor
      
      pressFloor=0.05D0*pambient/pRef

         do j=iylo,iyhi,1
            do i=ixlo,ixhi,1
               v(i,j,1:3)=u(i,j,1:3)
               bi=v(i,j,1)
               bj=v(i,j,2)
               bk=v(i,j,3)
               v(i,j,4)=u(i,j,4)
c               press=v(i,j,8)-0.5D0*(bi*bi+bj*bj+bk*bk)
            enddo
         enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine Source(vx,src)
      use mesh
      use mesh_common
      use properties
      use MagAxisToroidalField
      implicit none
      integer:: ilo, ihi
      double precision:: src(1:nxlocal,1:nylocal,nvar)
      double precision:: vx(IXLO:IXHI,IYLO:IYHI,nvar)
      integer:: i,j,l
c
c     
      src=0.D0
c
         do j=1,nylocal,1
            do i=1,nxlocal,1
               src(i,j,1)=(vx(i,j,3)**2
     &              )/Rc(i,j)
     &              -(vx(i,j,4))/Rc(i,j)
     &              +Btaxis/Rc(I,j)*vx(i,j,3)/Rc(i,j)
               src(i,j,3)=(-
     &                       vx(i,j,1)*(vx(i,j,2)))/Rc(i,j)
c
            enddo
         enddo
      src=-src
      return
      end
c-----------------------------------------------------------------------
      subroutine DivergenceFlux(rhs,finv,hinv)
      use mesh
      implicit none

      double precision:: rhs(1:nxlocal,1:nylocal,nvar)
      double precision:: finv(1:nxlocal+1,1:nylocal,nvar)
      double precision:: hinv(1:nxlocal,1:nylocal+1,nvar)
c
      integer:: i,j,l
c
      double precision:: rjac, RCInv
      rhs=0.D0
c
c
         do j=1,nylocal,1
            do i=1,nxlocal,1
               rjac=1.D0/(RJC(i,j))
               RCInv=1.D0/(RC(i,j))
               rhs(i,j,1:4)=rjac*(
     &              (finv(i+1,j,1:4)-finv(i,j,1:4))
     &              +(hinv(i,j+1,1:4)-hinv(i,j,1:4)))
            enddo
         enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine KrylovSolve(rhs,Rn, Zn,deltaR, deltaZ)
      use mesh
      use mesh_common
      use properties
#ifdef PARALLEL
      use mpistuff
#endif
      use OptionFlags
c
      implicit none
c     
c=======Declarations=========
      double precision:: Rn(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Zn(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Rtmp(IXLO:IXHI+1,IYLO:IYHI+1)
      double precision:: Ztmp(IXLO:IXHI+1,IYLO:IYHI+1)
c     
      double precision:: deltaR(1:nxlocal,1:nylocal)
      double precision:: deltaZ(1:nxlocal,1:nylocal)
      double precision:: rhs(1:nxlocal,1:nylocal,2)
      double precision:: rhsTmp(1:nxlocal,1:nylocal,2)
c
c      double precision:: deltaU(1:nxlocal,1:nylocal,2)
      double precision:: JacDeltaU(1:nxlocal,1:nylocal,2)
      double precision:: epsilon, l2norm
c      
c
      integer:: nsize
      double precision,allocatable:: w(:)
      double precision,allocatable:: Frhs(:)
c     
c     GMRES variables
      integer:: rsize
      integer::nwork
      double precision,allocatable::work(:)
      integer::irc(5)
      integer:: icntl(8)
      double precision:: cntl(5)
      integer:: info(3)
      double precision::rinfo(2)
      integer matvec, precondLeft, precondRight, dotProd
      parameter (matvec=1, precondLeft=2, precondRight=3, dotProd=4)
      integer revcom, colx, coly, colz, nbscal
c
      integer:: i,j,k,l
c
c
      epsilon=1.D-03
c
      nsize=nxlocal*nylocal*2
      allocate(w(nsize))
      allocate(Frhs(nsize))
c
      rsize=2048
c     Work space
      nwork=rsize*rsize+rsize*(nsize+5)+6*nsize+1
c      nwork=rsize*rsize+rsize*(nsize+5)+5*nsize+1
      write(6,*) 'Unknown space size',nsize
      write(6,*) 'Work space size',nwork
      allocate(work(nwork))
      work=0.D0
c
c     Initialize control params to default value
      call init_DGMRES(icntl,cntl)
c     control parameters
c      icntl(1)=6
c      icntl(2)=6
      icntl(3)=20
c     Right Preconditioning
c      icntl(4)=3
c     No preconditioning
      icntl(4)=0
c      icntl(5)=0
c      icntl(5)=2
      icntl(5)=3
c      icntl(6)=1
      icntl(7)=2048
      icntl(8)=0
c
      cntl(1)=1.D-07
c      cntl(3)=1.0D0
c
      l=1
      do j=1,nylocal,1
         do i=1,nxlocal,1
            w(l)=deltaR(i,j)
            w(l+1)=deltaZ(i,j)
            Frhs(l)=-rhs(i,j,1)
            Frhs(l+1)=-rhs(i,j,2)
c            write(6,*) 'RHS',i,j,l,Frhs(l), Frhs(l+1)
            l=l+2
         enddo
      enddo
c
      work(1:nsize)=w(1:nsize)
c      work(1:nsize)=0.D0
      work(nsize+1:2*nsize)=Frhs(1:nsize)
      do i=1,nsize,1
c         write(6,*) 'RZ=', work(i)
      enddo
c      write(6,*) 'MaxFRHS', 
c     &         maxval(dabs(Frhs(:)))
c     
 10   call Drive_DGMRES(nsize,nsize,rsize,nwork,work,irc,icntl,cntl,
     &     info,rinfo)
c
       revcom = irc(1)
       colx   = irc(2)
       coly   = irc(3)
       colz   = irc(4)
       nbscal = irc(5)
c
c       write(6,*) 'REVCOM=', revcom,colx,coly,colz,nbscal
*
       if (revcom.eq.matvec) then
* perform the matrix vector product
*        work(colz) <-- A * work(colx)
          l=colx
          do j=1,nylocal,1
             do i=1,nxlocal,1
                deltaR(i,j)=work(l)
                deltaZ(i,j)=work(l+1)
c                write(6,*) 'DelRZ', i,j,l,deltaR(i,j), deltaZ(i,j)
                l=l+2
             enddo
          enddo
c          write(6,*) 'MaxDELTA', maxval(dabs(deltaR(:,:))), 
c     &         maxval(dabs(deltaZ(:,:))),
c     &         maxval(dabs(work(colx:colx+nsize)))
          Rtmp=Rn
          Ztmp=Zn
c          call computeL2Norm(l2norm,work(colx),nsize)
c          epsilon=1.D-06/(nsize*l2norm+1.D0)*
c     &         (sum(dabs(Rn))+sum(dabs(Zn)))+1.D-06
          epsilon=1.D-05
c          write(6,*) 'EPSILON=',epsilon, l2norm, colx, colz
c          Rtmp(1:nxlocal,1:nylocal)=Rtmp(1:nxlocal,1:nylocal)
c     &         +epsilon*DeltaR
c          Ztmp(1:nxlocal,1:nylocal)=Ztmp(1:nxlocal,1:nylocal)
c     &         +epsilon*DeltaZ
          Rtmp(2:nxlocal+1,2:nylocal+1)=Rtmp(2:nxlocal+1,2:nylocal+1)
     &         +epsilon*DeltaR
          Ztmp(2:nxlocal+1,2:nylocal+1)=Ztmp(2:nxlocal+1,2:nylocal+1)
     &         +epsilon*DeltaZ
          call computeRHS(rhsTmp,Rtmp, Ztmp)
c          write(6,*) 'MaxRHSTMP', maxval(rhsTmp(:,:,1)), 
c     &         maxval(rhsTmp(:,:,2))
c          write(6,*) 'MinRHSTMP', minval(rhsTmp(:,:,1)), 
c     &         minval(rhsTmp(:,:,2))
          JacDeltaU=(rhsTmp-rhs)/epsilon
c     Second order in epsilon - uncomment lines below.
c          JacDeltaU=(rhsTmp)/epsilon
c          Rtmp=Rn
c          Ztmp=Zn
c          Rtmp(1:nxlocal,1:nylocal)=Rtmp(1:nxlocal,1:nylocal)
c     &         -epsilon*DeltaR
c          Ztmp(1:nxlocal,1:nylocal)=Ztmp(1:nxlocal,1:nylocal)
c     &         -epsilon*DeltaZ
c          call computeRHS(rhsTmp,Rtmp, Ztmp)
c          JacDeltaU=0.5D0*(JacDeltaU-(rhsTmp)/epsilon)
          l=colz
          do j=1,nylocal,1
             do i=1,nxlocal,1
                work(l)=JacDeltaU(i,j,1)
                work(l+1)=JacDeltaU(i,j,2)
c                write(6,*) 'JACDELU',l,work(l), work(l+1)
c                work(l)=0.0001*deltaR(i,j)+100*deltaZ(i,j)
c                work(l+1)=deltaZ(i,j)-100*deltaR(i,j)
                l=l+2
             enddo
          enddo
c          call computeL2Norm(l2norm,work(colz),nsize)
c          call computeL2Norm2(l2norm,work(colz),work(nsize+1),nsize)
c          write(6,*) 'L2Norm=',l2norm,colz
cc         call dgemv('N',nsize,nsize,1.D0,Aw,nsize,work(colx),1,
c     &            0.D0,work(colz),1)
         goto 10
*
c       else if (revcom.eq.precondLeft) then
* perform the left preconditioning
*         work(colz) <-- M^{-1} * work(colx)
c         call dcopy(nsize,work(colx),1,work(colz),1)
c         goto 10
*
c       else if (revcom.eq.precondRight) then
c          write(6,*) 'Performing Right Preconditioning'
* perform the right preconditioning
c         call dcopy(nsize,work(colx),1,work(colz),1)
c         call dgemv('N',nsize,nsize,1.D0,PconInv,nsize,work(colx),1,
c     &            0.D0,work(colz),1)
*
c         goto 10
       else if (revcom.eq.dotProd) then
*      perform the scalar product
*      work(colz) <-- work(colx) work(coly)
*
c          write(6,*) 'Performing dot product', colx,coly,colz
         call dgemv('C',nsize,nbscal,1.D0,work(colx),nsize,
     &               work(coly),1,0.D0,work(colz),1)
         goto 10
       endif
      w=work(1:nsize)
      do i=1,nsize,1
         write(6,*) 'SOL=', i,w(i), work(i+nsize)
      enddo
c      w=w+work(nsize+1:2*nsize)
      write(6,*) 'info=', info
      write(6,*) 'rinfo=', rinfo
      write(6,*) 'irc=', irc
c
      l=1
      do j=1,nylocal,1
         do i=1,nxlocal,1
            deltaR(i,j)=w(l)
            deltaZ(i,j)=w(l+1)
            write(6,*) 'deltaRZ', l, deltaR(i,j), deltaZ(i,j)
            write(6,*) 'SRZ', l, Rn(i,j)+deltaR(i,j), 
     &           Zn(i,j)+deltaZ(i,j)
            l=l+2
         enddo
      enddo
c
      deallocate(work)
      deallocate(w)
      deallocate(Frhs)
      return
      end
c      
c-----------------------------------------------------------------------
      subroutine ComputeL2Norm(l2norm,vec,nsize)
      integer:: nsize
      double precision:: vec(nsize), l2norm
      l2norm=dsqrt(sum(vec(:)*vec(:))/nsize)
      return
      end
c-----------------------------------------------------------------------
c-----------------------------------------------------------------------
      subroutine ComputeL2Norm2(l2norm,vec,rhs,nsize)
      integer:: nsize
      double precision:: vec(nsize), rhs(nsize),l2norm
      l2norm=dsqrt(sum((vec(:)-rhs(:))**2)/nsize)
      return
      end
c-----------------------------------------------------------------------
c