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)
      double precision:: rhsPert(1:nxlocal,1:nylocal,2)
      double precision:: rhsPert2(1:nxlocal,1:nylocal,2)
c
      integer:: maxiter, iter
      double precision:: L2err, LInfErr, tolerance
      double precision:: lamda, epsilon
      integer:: ndfdu
      double precision, allocatable:: dFdU(:,:)
      double precision, allocatable:: Wtmp(:)
      double precision, allocatable:: WtmpI(:)
      double precision, allocatable:: Vtmp(:,:)
      integer:: i,j,k,l,m,n
c
      call SetBoundaryValuesRZ(Rn,Zn)
c      call WriteTextFile(Rn,Zn)
c
      iter=1
c      maxiter=10
      tolerance=1.D-08
      lamda=0.01D0
      L2err=1.D16
      deltaR=0.D0
      deltaZ=0.D0
c
      call WriteTextFile(Rn,Zn,iter)
c
c     Compute the full Jacobian
      call ComputeRHS(rhs, Rn, Zn)
c
      epsilon=1.D-05
      ndfdu=2*nxlocal*nylocal
      allocate(dFdU(ndfdu,ndfdu))
      allocate(Vtmp(ndfdu,ndfdu))
      allocate(Wtmp(ndfdu))
      allocate(WtmpI(ndfdu))
c      allocate(Vtmp(ndfdu,ndfdu))
      dfdu=1234567.D0
c
      Rold=Rn
      Zold=Zn
c
c$$$      l=1
c$$$      do j=1,nylocal,1
c$$$         do i=1,nxlocal,1
c$$$            Rold=Rn
c$$$            Zold=Zn
c$$$            Rold(i+1,j+1)=Rn(i+1,j+1)+epsilon
c$$$            Zold(i+1,j+1)=Zn(i+1,j+1)
c$$$            call ComputeRHS(rhsPert, Rold, Zold)
c$$$            k=1
c$$$            do n=1,nylocal,1
c$$$               do m=1,nxlocal,1
c$$$                  dFdU(k,l)=(rhsPert(m,n,1)-rhs(m,n,1))/epsilon
c$$$                  dFdU(k+1,l)=(rhsPert(m,n,2)-rhs(m,n,2))/epsilon
c$$$                  k=k+2
c$$$               enddo
c$$$            enddo
c$$$            Rold=Rn
c$$$            Zold=Zn
c$$$            Rold(i+1,j+1)=Rn(i+1,j+1)
c$$$            Zold(i+1,j+1)=Zn(i+1,j+1)+epsilon
c$$$            call ComputeRHS(rhsPert, Rold, Zold)
c$$$            k=1
c$$$            do n=1,nylocal,1
c$$$               do m=1,nxlocal,1
c$$$                  dFdU(k,l+1)=(rhsPert(m,n,1)-rhs(m,n,1))/epsilon
c$$$                  dFdU(k+1,l+1)=(rhsPert(m,n,2)-rhs(m,n,2))/epsilon
c$$$                  k=k+2
c$$$               enddo
c$$$            enddo
c$$$            l=l+2
c$$$         enddo
c$$$      enddo
c$$$c
c$$$      do j=1,ndfdu,1
c$$$         do i=1,ndfdu,1
c$$$c            write(6,*) 'DFDU=',i,j,dfdu(i,j)
c$$$         enddo
c$$$      enddo
c      return
c
c      write(6,*) 'Calling SVD'
c      call svdcmp(dFdU,ndfdu,ndfdu,ndfdu,ndfdu,Wtmp,Vtmp)
c      do k=1,ndfdu,1
c         write(6,*) 'SVDCMP=',k,Wtmp(k)
c      enddo
c$$$      write(6,*) 'Calling ELMHES'
c$$$      call ElmHes(dfdu,ndfdu,ndfdu)
c$$$      write(6,*) 'Calling HQR'
c$$$      call hqr(dfdu,ndfdu,ndfdu,Wtmp,WtmpI)
c$$$      do k=1,ndfdu,1
c$$$         write(6,*) 'SVDCMP=',k,Wtmp(k), WtmpI(k)
c$$$      enddo
c      return
c      
c
      do while(iter.le.maxiter.and.L2err.gt.tolerance)
c
      call ComputeRHS(rhs, Rn, Zn)
      write(6,*) 'MaxRHS', maxval(rhs(:,:,1)), maxval(rhs(:,:,2))
      write(6,*) 'MinRHS', minval(rhs(:,:,1)), minval(rhs(:,:,2))
c    
c      call ComputeNorm(rhs,L2err)
c
c     Jacobian compuation
      l=1
      do j=1,nylocal,1
         do i=1,nxlocal,1
            Rold=Rn
            Zold=Zn
c            Rold(i+1,j+1)=Rn(i+1,j+1)+epsilon
c            Zold(i+1,j+1)=Zn(i+1,j+1)
            Rold(i,j)=Rn(i,j)+epsilon
            Zold(i,j)=Zn(i,j)
            call ComputeRHS(rhsPert, Rold, Zold)
            Rold(i,j)=Rn(i,j)-epsilon
            Zold(i,j)=Zn(i,j)
c            Rold(i+1,j+1)=Rn(i+1,j+1)-epsilon
c            Zold(i+1,j+1)=Zn(i+1,j+1)
            call ComputeRHS(rhsPert2, Rold, Zold)
            k=1
            do n=1,nylocal,1
               do m=1,nxlocal,1
                  dFdU(k,l)=0.5D0*
     &                 (rhsPert(m,n,1)-rhsPert2(m,n,1))/epsilon
                  dFdU(k+1,l)=0.5D0*
     &                 (rhsPert(m,n,2)-rhsPert2(m,n,2))/epsilon
                  k=k+2
               enddo
            enddo
            Rold=Rn
            Zold=Zn
c            Rold(i+1,j+1)=Rn(i+1,j+1)
c            Zold(i+1,j+1)=Zn(i+1,j+1)+epsilon
            Rold(i,j)=Rn(i,j)
            Zold(i,j)=Zn(i,j)+epsilon
            call ComputeRHS(rhsPert, Rold, Zold)
c            Rold(i+1,j+1)=Rn(i+1,j+1)
c            Zold(i+1,j+1)=Zn(i+1,j+1)-epsilon
            Rold(i,j)=Rn(i,j)
            Zold(i,j)=Zn(i,j)-epsilon
            call ComputeRHS(rhsPert2, Rold, Zold)
            k=1
            do n=1,nylocal,1
               do m=1,nxlocal,1
                  dFdU(k,l+1)=0.5D0*
     &                 (rhsPert(m,n,1)-rhsPert2(m,n,1))/epsilon
                  dFdU(k+1,l+1)=0.5D0*
     &                 (rhsPert(m,n,2)-rhsPert2(m,n,2))/epsilon
                  k=k+2
               enddo
            enddo
            l=l+2
         enddo
      enddo
c
      call KrylovSolve(rhs,dFdU,ndfdu,Rn, Zn,deltaR, deltaZ)
c      
      Rold=Rn
      Zold=Zn
c
      Rn(1:nxlocal,1:nylocal)=Rold(1:nxlocal,1:nylocal)+
     &     lamda*deltaR

      Zn(1:nxlocal,1:nylocal)=Zold(1:nxlocal,1:nylocal)+
     &     lamda*deltaZ
c
c      Rn(2:nxlocal+1,2:nylocal+1)=Rold(2:nxlocal+1,2:nylocal+1)+
c     &     lamda*deltaR
c      Zn(2:nxlocal+1,2:nylocal+1)=Zold(2:nxlocal+1,2:nylocal+1)+
c     &     lamda*deltaZ
c
      call SetBoundaryValuesRZ(Rn,Zn)
c      write(6,*) 'MaxRHS', maxval(rhs(:,:,1)), maxval(rhs(:,:,2))
c      write(6,*) 'MinRHS', minval(rhs(:,:,1)), minval(rhs(:,:,2))
      iter=iter+1
      call WriteTextFile(Rn,Zn,iter)
      enddo
c
c      call WriteTextFile(Rn,Zn,iter)
c
      deallocate(dFdU)
      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
      rhs=-rhs
c
      call Source(vx,src)
c      do j=1,nylocal,1
c         do i=1,nxlocal,1
c            write(21,*) i,j,rhs(i,j,1),rhs(i,j,2),
c     &           src(i,j,1),rhs(i,j,1)+src(i,j,1)
c         enddo
c      enddo
      rhs=rhs+src
c      stop
c
c      rhsn(:,:,1:2)=rhs(:,:,1:2)
c      rhsn(:,:,1)=Rc(1:nxlocal,1:nylocal)*rhs(:,:,1)
c      rhsn(:,:,2)=Zc(1:nxlocal,1:nylocal)*rhs(:,:,2)
      rhsn(:,:,1)=rhs(:,:,1)
      rhsn(:,:,2)=rhs(:,:,2)
c      write(6,*) 'MaxRHS', maxval(rhsn(:,:,1)), maxval(rhsn(:,:,2))
c      write(6,*) 'MinRHS', minval(rhsn(:,:,1)), minval(rhsn(:,:,2))
c      write(6,*) 'MinmaxRHS3', maxval(rhs(:,:,3)), minval(rhs(:,:,3))
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)
c               bi=v(i,j,1)
c               bj=v(i,j,2)
c               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,3))/Rc(i,j)
c
c               src(i,j,1) = src(i,j,1)*RJC(i,j)
c               src(i,j,3) = src(i,j,3)*RJC(i,j)
            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))
c               rjac=1.D0
c               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)))
c$$$               write(22,*) i,j,(finv(i+1,j,1)-finv(i,j,1)),
c$$$     &              (finv(i+1,j,2)-finv(i,j,2))
c$$$               write(23,*) i,j,(hinv(i,j+1,1)-hinv(i,j,1)),
c$$$     &              (hinv(i,j+1,2)-hinv(i,j,2))
            enddo
         enddo
c
      return
      end
c-----------------------------------------------------------------------
      subroutine KrylovSolve(rhs,dFdU,ndfdu,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      
      integer:: ndfdu
      double precision:: dfdu(ndfdu,ndfdu)
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-05
c
      nsize=nxlocal*nylocal*2
      allocate(w(nsize))
      allocate(Frhs(nsize))
c
c      rsize=2048
      rsize=ndfdu
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, ndfdu
      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
c      icntl(7)=2048
      icntl(7)=rsize
      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)
         call dgemv('N',nsize,nsize,1.D0,dFdU,nsize,work(colx),1,
     &            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+1,j+1)+1.D-05*deltaR(i,j), 
     &           Zn(i+1,j+1)+1.D-05*deltaZ(i,j)
            l=l+2
         enddo
      enddo
      w=matmul(dfdu,w)-work(1+nsize:2*nsize)
c
      do i=1,nsize,1
         write(6,*) 'RES=', i,w(i)
      enddo
      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