      program mur_fit
C
C     Fit to the Murnaghan equation of state.
C     Modified (input/output streamlined, comments added)
C     from  WIEN97, SRC_eosfit/
C     by A. Postnikov,  postnikov@univ-metz.fr
C
C     implicit real*8 (a-h,o-z)
      implicit none
      integer length,mdata
      parameter (length=256, mdata=401)
      character*80   finame,foname
      character*256  string

      real*8 AA,BB,CC,bfac, bmod,ene,third,
     .       x(4),x12,x23,x31,y(mdata),dx,diag(mdata),
     .       epsfcn,factor,fjac(mdata,4),ftol,
     .       qtf(4),vol,wa1(4),wa2(4),wa3(4),wa4(mdata),xmax,xmin,
     .       xx,ee,press,entha
      integer i,iflag,info,imid,ipvt(4),j,ldfjac,m,maxfev,mode,
     .        n,nfev,nprint
      common /vol/ vol(mdata)
      common /ene/ ene(mdata)
C     dimension ipvt(4),qtf(4),wa1(4),wa2(4),wa3(4),wa4(mdata)
      external eos2,murna,murna1
      data third/0.3333333333333333d0/
C     conversion factor for bulk modulus, e.g.
C     1 Ry/(a.u.**3) = 13.61 * 1.60219 * 100  / (0.529177**3) GPa
C     1 Ry/(a.u.**3) = 13.61 * 1.60219 * 1000 / (0.529177**3) Kbar
C     1 eV/(AA**3)   = 1.60219 Mbar
C     etc.
      data bfac/147153.05/  !  volume in a.u.**3, energy in Ry, B in Kbar
c
      print *,' Give name of an input file containing data columnwise:'
      print *,' [ Unit cell volume in (a.u.)^3 ] ;  [ Energy in Ry ]'
      print *,' ( lines with # in the 1st position will be ignored )'
      read *, finame
      open (55,file=finame,status='OLD',form='FORMATTED',err=11)
      goto 12
   11 continue
      print *,' Error opening file ',finame
      stop
   12 continue
C --- read in data till end of file:
      i=1
 1    read(55,101,end=2) string
      if (string(1:1).eq.'#') then
        goto 1     !  ignore lines beginning with  #
      else
        read(string(1:length),*,err=3) vol(i),ene(i)
      endif
C
C     vol(i)=vol(i)**3/2.d0       ! if input is bcc lattice constant (in a.u.)
C     vol(i)=vol(i)/(0.529177**3) ! if input volume is in Ang**3
C     ene(i)=ene(i)/13.61         ! if input energy is in eV

      i=i+1
      goto 1
    3 continue
      print *,' Error reading data in data line ',i
      stop
    2 continue
      m=i-1
      if (m.gt.mdata) then
        print *,' Number of data points =',m,
     .          ' exceeds ',mdata
        stop
      endif
      if (m.lt.5) then
        print *,' Number of data points =',m,
     .          ' is too small for a reasonable fit'
        stop
      endif
C
      print *,'  Output file for results and fit: '
      read *, foname
      open (66,file=foname,form='FORMATTED',err=21)
      goto 22
   21 continue
      print *,' Error opening file ',foname
      stop
   22 continue
C
C --- initialize parameters to call subr. lmdif:
C     three-point parabolic fit: E = AA*vol**2 + BB*vol + CC
      imid = m/2
      x12 = vol(1) - vol(imid)
      x23 = vol(imid) - vol(m)
      x31 = vol(m) - vol(1)
      AA = - ene(1)   /x12/x31 -
     -       ene(imid)/x12/x23 -
     -       ene(m)   /x31/x23
      BB =   ene(1)   *(vol(imid)+vol(m)   )/x12/x31 +
     +       ene(imid)*(vol(1)   +vol(m)   )/x12/x23 +
     +       ene(m)   *(vol(1)   +vol(imid))/x31/x23
      CC = - ene(1)   *vol(imid)*vol(m)   /x12/x31 -
     -       ene(imid)*vol(1)   *vol(m)   /x12/x23 -
     -       ene(m)   *vol(1)   *vol(imid)/x31/x23
C
C     initial guess of Murnaghan parameters from 2d ortder E(V) fit:
      x(1) = CC - 0.25d0*BB*BB/AA  !  min. energy
      x(2) = -0.5d0*BB/AA          !  equilibrium volume
      x(3) = - BB                  !  bulk modulus
C     x(4) = 1.d0                  !  dB/dP
      x(4) = -10.0                 !  dB/dP
      bmod = x(3) * bfac
      write(66,200)
  200 format('#'/,
     .       '#  Rough estimate !!! from the 2d order polynomial fit:')
      write(66,203) x(2),x(1),bmod,x(4)

      ftol=1.d-9
      maxfev=10000
C     epsfcn=0.01d0
      epsfcn=0.001d0
      mode=1
      nprint=1
      factor=100.d0
      ldfjac=100
      n=4
      call lmdif(murna,m,n,x,y,ftol,ftol,ftol,maxfev,epsfcn,diag,mode,
     *           factor,nprint,
     *           info,nfev,fjac,ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
      write(66,201) info
  201 format('#  Fit to Murnaghan equation of state, info code=',i4)
      write(66,202)
  202 format('#'/,'#   Input V,(a.u.)^3',
     .       5x,'Input E (Ry)',4x,'Absolute fit error')
      do j=1,m
        write(66,206)vol(j),ene(j),y(j)
      enddo
C     After fitting, x(1) is min. energy in initial units,
C                    x(2) is equilibrium volume in initial units,
C                    x(4) is d(Bulk modulus)/d(Pressure) - dimensionless.
C                    x(3) is bulk modulus and needs conversion:
      bmod = x(3) * bfac
      write(66,203) x(2),x(1),bmod,x(4)
  203 format(/,'#  Equilibrium volume, a.u.^3 :',f12.6,/,
     .         '#  Minimum energy, Ry         :',f12.6,/,
     .         '#  Bulk modulus, Kbar         :',f12.6,/,
     .         '#  BP                         :',f12.6)
C
C.....create plotfile for Murnaghan fit:
      xmin=vol(1)
      xmax=vol(m)
      dx=(xmax-xmin)/(mdata-1)
      write(66,205)
  205 format('#'/'#  Total energy fitted to Murnahgan EOS:',/
     .       '#    Volume(a.u.^3)',7x,'Energy(Ry)',5x,'Pressure (GPa)',
     .       6x,'Enthalpy(Ry)')
      do j=1,mdata
        xx=xmin+(j-1)*dx
        call murna1(x,xx,ee,press,entha)
        write(66,206) xx,ee,press,entha
      enddo
  206 format(4f18.8)
  101 format(a256)
C
      stop
      end
C
C------------------------------------------------------------------
C
      subroutine murna(m,n,x,fvec,iflag)
C     fit to the Murnaghan equation of state.
C
C     On fitting, the parameters x(1)...x(4) are:
C        x(1) : minimum energy E
C        x(2) : volume V at equilibrium
C        x(3) : bulk modulus B = V*d^2(E)/d(V)^2 in corresponding units
C        x(4) : d(B)/d(P), dimensionless
C
      implicit none
      integer mdata,m,n,i,iflag
      parameter (mdata=401)
      real*8 x(*),fvec(*),vol,ene,xh
      common /vol/ vol(mdata)
      common /ene/ ene(mdata)
      if (n.eq.3) then
       xh=x(4)
       x(4)=5.0d0
      endif
      do i=1,m
        fvec(i)=x(1)+
     +          x(3)*vol(i)/(x(4)*(x(4)-1.d0))*(x(4)*
     *          (1.d0-x(2)/vol(i))+(x(2)/vol(i))**x(4)-1.d0) - ene(i)
      enddo
      if (n.eq.3) then
       x(4)=xh
      endif
      return
      end
C
C------------------------------------------------------------------
C
      subroutine murna1(x,vol,ene,press,entha)
C     fit to the Murnaghan equation of state.
C
C     On fitting, the parameters x(1)...x(4) are:
C        x(1) : minimum energy E in Ry
C        x(2) : volume V at equilibrium in a.u.^3
C        x(3) : bulk modulus B = V*d^2(E)/d(V)^2 in corresponding units
C        x(4) : d(B)/d(P), dimensionless
C
      implicit none
      real*8 x(4),vol,ene,press,entha
C --- fit to energy:
        ene = x(1)+
     +        x(3)*vol/(x(4)*(x(4)-1.d0))*
     *       (x(4)*(1.d0-x(2)/vol)+(x(2)/vol)**x(4) - 1.d0)
C --- fit to pressure:
        press = x(3)/x(4)*((x(2)/vol)**x(4) - 1.d0)
C --- enthalpy:
	entha = ene + press*vol
C     The pressure was so far in Ry/a.u.^3; convert into GPa:
	press = press * 14714.0d0

      return
      end
C
* ======================================================================
* NIST Guide to Available Math Software.
* Fullsource for module LMDIF from package MINPACK.
* Retrieved from NETLIB on Tue Dec  1 11:06:31 1998.
* ======================================================================
      subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
     *                 diag,mode,factor,nprint,info,nfev,fjac,ldfjac,
     *                 ipvt,qtf,wa1,wa2,wa3,wa4)
      integer m,n,maxfev,mode,nprint,info,nfev,ldfjac
      integer ipvt(n)
      double precision ftol,xtol,gtol,epsfcn,factor
      double precision x(n),fvec(m),diag(n),fjac(ldfjac,n),qtf(n),
     *                 wa1(n),wa2(n),wa3(n),wa4(m)
      external fcn
c     **********
c
c     subroutine lmdif
c
c     the purpose of lmdif is to minimize the sum of the squares of
c     m nonlinear functions in n variables by a modification of
c     the levenberg-marquardt algorithm. the user must provide a
c     subroutine which calculates the functions. the jacobian is
c     then calculated by a forward-difference approximation.
c
c     the subroutine statement is
c
c       subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
c                        diag,mode,factor,nprint,info,nfev,fjac,
c                        ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
c
c     where
c
c       fcn is the name of the user-supplied subroutine which
c         calculates the functions. fcn must be declared
c         in an external statement in the user calling
c         program, and should be written as follows.
c
c         subroutine fcn(m,n,x,fvec,iflag)
c         integer m,n,iflag
c         double precision x(n),fvec(m)
c         ----------
c         calculate the functions at x and
c         return this vector in fvec.
c         ----------
c         return
c         end
c
c         the value of iflag should not be changed by fcn unless
c         the user wants to terminate execution of lmdif.
c         in this case set iflag to a negative integer.
c
c       m is a positive integer input variable set to the number
c         of functions.
c
c       n is a positive integer input variable set to the number
c         of variables. n must not exceed m.
c
c       x is an array of length n. on input x must contain
c         an initial estimate of the solution vector. on output x
c         contains the final estimate of the solution vector.
c
c       fvec is an output array of length m which contains
c         the functions evaluated at the output x.
c
c       ftol is a nonnegative input variable. termination
c         occurs when both the actual and predicted relative
c         reductions in the sum of squares are at most ftol.
c         therefore, ftol measures the relative error desired
c         in the sum of squares.
c
c       xtol is a nonnegative input variable. termination
c         occurs when the relative error between two consecutive
c         iterates is at most xtol. therefore, xtol measures the
c         relative error desired in the approximate solution.
c
c       gtol is a nonnegative input variable. termination
c         occurs when the cosine of the angle between fvec and
c         any column of the jacobian is at most gtol in absolute
c         value. therefore, gtol measures the orthogonality
c         desired between the function vector and the columns
c         of the jacobian.
c
c       maxfev is a positive integer input variable. termination
c         occurs when the number of calls to fcn is at least
c         maxfev by the end of an iteration.
c
c       epsfcn is an input variable used in determining a suitable
c         step length for the forward-difference approximation. this
c         approximation assumes that the relative errors in the
c         functions are of the order of epsfcn. if epsfcn is less
c         than the machine precision, it is assumed that the relative
c         errors in the functions are of the order of the machine
c         precision.
c
c       diag is an array of length n. if mode = 1 (see
c         below), diag is internally set. if mode = 2, diag
c         must contain positive entries that serve as
c         multiplicative scale factors for the variables.
c
c       mode is an integer input variable. if mode = 1, the
c         variables will be scaled internally. if mode = 2,
c         the scaling is specified by the input diag. other
c         values of mode are equivalent to mode = 1.
c
c       factor is a positive input variable used in determining the
c         initial step bound. this bound is set to the product of
c         factor and the euclidean norm of diag*x if nonzero, or else
c         to factor itself. in most cases factor should lie in the
c         interval (.1,100.). 100. is a generally recommended value.
c
c       nprint is an integer input variable that enables controlled
c         printing of iterates if it is positive. in this case,
c         fcn is called with iflag = 0 at the beginning of the first
c         iteration and every nprint iterations thereafter and
c         immediately prior to return, with x and fvec available
c         for printing. if nprint is not positive, no special calls
c         of fcn with iflag = 0 are made.
c
c       info is an integer output variable. if the user has
c         terminated execution, info is set to the (negative)
c         value of iflag. see description of fcn. otherwise,
c         info is set as follows.
c
c         info = 0  improper input parameters.
c
c         info = 1  both actual and predicted relative reductions
c                   in the sum of squares are at most ftol.
c
c         info = 2  relative error between two consecutive iterates
c                   is at most xtol.
c
c         info = 3  conditions for info = 1 and info = 2 both hold.
c
c         info = 4  the cosine of the angle between fvec and any
c                   column of the jacobian is at most gtol in
c                   absolute value.
c
c         info = 5  number of calls to fcn has reached or
c                   exceeded maxfev.
c
c         info = 6  ftol is too small. no further reduction in
c                   the sum of squares is possible.
c
c         info = 7  xtol is too small. no further improvement in
c                   the approximate solution x is possible.
c
c         info = 8  gtol is too small. fvec is orthogonal to the
c                   columns of the jacobian to machine precision.
c
c       nfev is an integer output variable set to the number of
c         calls to fcn.
c
c       fjac is an output m by n array. the upper n by n submatrix
c         of fjac contains an upper triangular matrix r with
c         diagonal elements of nonincreasing magnitude such that
c
c                t     t           t
c               p *(jac *jac)*p = r *r,
c
c         where p is a permutation matrix and jac is the final
c         calculated jacobian. column j of p is column ipvt(j)
c         (see below) of the identity matrix. the lower trapezoidal
c         part of fjac contains information generated during
c         the computation of r.
c
c       ldfjac is a positive integer input variable not less than m
c         which specifies the leading dimension of the array fjac.
c
c       ipvt is an integer output array of length n. ipvt
c         defines a permutation matrix p such that jac*p = q*r,
c         where jac is the final calculated jacobian, q is
c         orthogonal (not stored), and r is upper triangular
c         with diagonal elements of nonincreasing magnitude.
c         column j of p is column ipvt(j) of the identity matrix.
c
c       qtf is an output array of length n which contains
c         the first n elements of the vector (q transpose)*fvec.
c
c       wa1, wa2, and wa3 are work arrays of length n.
c
c       wa4 is a work array of length m.
c
c     subprograms called
c
c       user-supplied ...... fcn
c
c       minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
c
c       fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,iflag,iter,j,l
      double precision actred,delta,dirder,epsmch,fnorm,fnorm1,gnorm,
     *                 one,par,pnorm,prered,p1,p5,p25,p75,p0001,ratio,
     *                 sum,temp,temp1,temp2,xnorm,zero
      double precision dpmpar,enorm
      data one,p1,p5,p25,p75,p0001,zero
     *     /1.0d0,1.0d-1,5.0d-1,2.5d-1,7.5d-1,1.0d-4,0.0d0/
c
c     epsmch is the machine precision.
c
      epsmch = dpmpar(1)
c
      info = 0
      iflag = 0
      nfev = 0
c
c     check the input parameters for errors.
c
      if (n .le. 0 .or. m .lt. n .or. ldfjac .lt. m
     *    .or. ftol .lt. zero .or. xtol .lt. zero .or. gtol .lt. zero
     *    .or. maxfev .le. 0 .or. factor .le. zero) go to 300
      if (mode .ne. 2) go to 20
      do 10 j = 1, n
         if (diag(j) .le. zero) go to 300
   10    continue
   20 continue
c
c     evaluate the function at the starting point
c     and calculate its norm.
c
      iflag = 1
      call fcn(m,n,x,fvec,iflag)
      nfev = 1
      if (iflag .lt. 0) go to 300
      fnorm = enorm(m,fvec)
c
c     initialize levenberg-marquardt parameter and iteration counter.
c
      par = zero
      iter = 1
c
c     beginning of the outer loop.
c
   30 continue
c
c        calculate the jacobian matrix.
c
         iflag = 2
         call fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa4)
         nfev = nfev + n
         if (iflag .lt. 0) go to 300
c
c        if requested, call fcn to enable printing of iterates.
c
         if (nprint .le. 0) go to 40
         iflag = 0
         if (mod(iter-1,nprint) .eq. 0) call fcn(m,n,x,fvec,iflag)
         if (iflag .lt. 0) go to 300
   40    continue
c
c        compute the qr factorization of the jacobian.
c
         call qrfac(m,n,fjac,ldfjac,.true.,ipvt,n,wa1,wa2,wa3)
c
c        on the first iteration and if mode is 1, scale according
c        to the norms of the columns of the initial jacobian.
c
         if (iter .ne. 1) go to 80
         if (mode .eq. 2) go to 60
         do 50 j = 1, n
            diag(j) = wa2(j)
            if (wa2(j) .eq. zero) diag(j) = one
   50       continue
   60    continue
c
c        on the first iteration, calculate the norm of the scaled x
c        and initialize the step bound delta.
c
         do 70 j = 1, n
            wa3(j) = diag(j)*x(j)
   70       continue
         xnorm = enorm(n,wa3)
         delta = factor*xnorm
         if (delta .eq. zero) delta = factor
   80    continue
c
c        form (q transpose)*fvec and store the first n components in
c        qtf.
c
         do 90 i = 1, m
            wa4(i) = fvec(i)
   90       continue
         do 130 j = 1, n
            if (fjac(j,j) .eq. zero) go to 120
            sum = zero
            do 100 i = j, m
               sum = sum + fjac(i,j)*wa4(i)
  100          continue
            temp = -sum/fjac(j,j)
            do 110 i = j, m
               wa4(i) = wa4(i) + fjac(i,j)*temp
  110          continue
  120       continue
            fjac(j,j) = wa1(j)
            qtf(j) = wa4(j)
  130       continue
c
c        compute the norm of the scaled gradient.
c
         gnorm = zero
         if (fnorm .eq. zero) go to 170
         do 160 j = 1, n
            l = ipvt(j)
            if (wa2(l) .eq. zero) go to 150
            sum = zero
            do 140 i = 1, j
               sum = sum + fjac(i,j)*(qtf(i)/fnorm)
  140          continue
            gnorm = dmax1(gnorm,dabs(sum/wa2(l)))
  150       continue
  160       continue
  170    continue
c
c        test for convergence of the gradient norm.
c
         if (gnorm .le. gtol) info = 4
         if (info .ne. 0) go to 300
c
c        rescale if necessary.
c
         if (mode .eq. 2) go to 190
         do 180 j = 1, n
            diag(j) = dmax1(diag(j),wa2(j))
  180       continue
  190    continue
c
c        beginning of the inner loop.
c
  200    continue
c
c           determine the levenberg-marquardt parameter.
c
            call lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,par,wa1,wa2,
     *                 wa3,wa4)
c
c           store the direction p and x + p. calculate the norm of p.
c
            do 210 j = 1, n
               wa1(j) = -wa1(j)
               wa2(j) = x(j) + wa1(j)
               wa3(j) = diag(j)*wa1(j)
  210          continue
            pnorm = enorm(n,wa3)
c
c           on the first iteration, adjust the initial step bound.
c
            if (iter .eq. 1) delta = dmin1(delta,pnorm)
c
c           evaluate the function at x + p and calculate its norm.
c
            iflag = 1
            call fcn(m,n,wa2,wa4,iflag)
            nfev = nfev + 1
            if (iflag .lt. 0) go to 300
            fnorm1 = enorm(m,wa4)
c
c           compute the scaled actual reduction.
c
            actred = -one
            if (p1*fnorm1 .lt. fnorm) actred = one - (fnorm1/fnorm)**2
c
c           compute the scaled predicted reduction and
c           the scaled directional derivative.
c
            do 230 j = 1, n
               wa3(j) = zero
               l = ipvt(j)
               temp = wa1(l)
               do 220 i = 1, j
                  wa3(i) = wa3(i) + fjac(i,j)*temp
  220             continue
  230          continue
            temp1 = enorm(n,wa3)/fnorm
            temp2 = (dsqrt(par)*pnorm)/fnorm
            prered = temp1**2 + temp2**2/p5
            dirder = -(temp1**2 + temp2**2)
c
c           compute the ratio of the actual to the predicted
c           reduction.
c
            ratio = zero
            if (prered .ne. zero) ratio = actred/prered
c
c           update the step bound.
c
            if (ratio .gt. p25) go to 240
               if (actred .ge. zero) temp = p5
               if (actred .lt. zero)
     *            temp = p5*dirder/(dirder + p5*actred)
               if (p1*fnorm1 .ge. fnorm .or. temp .lt. p1) temp = p1
               delta = temp*dmin1(delta,pnorm/p1)
               par = par/temp
               go to 260
  240       continue
               if (par .ne. zero .and. ratio .lt. p75) go to 250
               delta = pnorm/p5
               par = p5*par
  250          continue
  260       continue
c
c           test for successful iteration.
c
            if (ratio .lt. p0001) go to 290
c
c           successful iteration. update x, fvec, and their norms.
c
            do 270 j = 1, n
               x(j) = wa2(j)
               wa2(j) = diag(j)*x(j)
  270          continue
            do 280 i = 1, m
               fvec(i) = wa4(i)
  280          continue
            xnorm = enorm(n,wa2)
            fnorm = fnorm1
            iter = iter + 1
  290       continue
c
c           tests for convergence.
c
            if (dabs(actred) .le. ftol .and. prered .le. ftol
     *          .and. p5*ratio .le. one) info = 1
            if (delta .le. xtol*xnorm) info = 2
            if (dabs(actred) .le. ftol .and. prered .le. ftol
     *          .and. p5*ratio .le. one .and. info .eq. 2) info = 3
            if (info .ne. 0) go to 300
c
c           tests for termination and stringent tolerances.
c
            if (nfev .ge. maxfev) info = 5
            if (dabs(actred) .le. epsmch .and. prered .le. epsmch
     *          .and. p5*ratio .le. one) info = 6
            if (delta .le. epsmch*xnorm) info = 7
            if (gnorm .le. epsmch) info = 8
            if (info .ne. 0) go to 300
c
c           end of the inner loop. repeat if iteration unsuccessful.
c
            if (ratio .lt. p0001) go to 200
c
c        end of the outer loop.
c
         go to 30
  300 continue
c
c     termination, either normal or user imposed.
c
      if (iflag .lt. 0) info = iflag
      iflag = 0
      if (nprint .gt. 0) call fcn(m,n,x,fvec,iflag)
      return
c
c     last card of subroutine lmdif.
c
      end
      double precision function dpmpar(i)
      integer i
c     **********
c
c     Function dpmpar
c
c     This function provides double precision machine parameters
c     when the appropriate set of data statements is activated (by
c     removing the c from column 1) and all other data statements are
c     rendered inactive. Most of the parameter values were obtained
c     from the corresponding Bell Laboratories Port Library function.
c
c     The function statement is
c
c       double precision function dpmpar(i)
c
c     where
c
c       i is an integer input variable set to 1, 2, or 3 which
c         selects the desired machine parameter. If the machine has
c         t base b digits and its smallest and largest exponents are
c         emin and emax, respectively, then these parameters are
c
c         dpmpar(1) = b**(1 - t), the machine precision,
c
c         dpmpar(2) = b**(emin - 1), the smallest magnitude,
c
c         dpmpar(3) = b**emax*(1 - b**(-t)), the largest magnitude.
c
c     Argonne National Laboratory. MINPACK Project. November 1996.
c     Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More'
c
c     **********
      integer mcheps(4)
      integer minmag(4)
      integer maxmag(4)
      double precision dmach(3)
      equivalence (dmach(1),mcheps(1))
      equivalence (dmach(2),minmag(1))
      equivalence (dmach(3),maxmag(1))
c
c     Machine constants for the IBM 360/370 series,
c     the Amdahl 470/V6, the ICL 2900, the Itel AS/6,
c     the Xerox Sigma 5/7/9 and the Sel systems 85/86.
c
c     data mcheps(1),mcheps(2) / z34100000, z00000000 /
c     data minmag(1),minmag(2) / z00100000, z00000000 /
c     data maxmag(1),maxmag(2) / z7fffffff, zffffffff /
c
c     Machine constants for the Honeywell 600/6000 series.
c
c     data mcheps(1),mcheps(2) / o606400000000, o000000000000 /
c     data minmag(1),minmag(2) / o402400000000, o000000000000 /
c     data maxmag(1),maxmag(2) / o376777777777, o777777777777 /
c
c     Machine constants for the CDC 6000/7000 series.
c
c     data mcheps(1) / 15614000000000000000b /
c     data mcheps(2) / 15010000000000000000b /
c
c     data minmag(1) / 00604000000000000000b /
c     data minmag(2) / 00000000000000000000b /
c
c     data maxmag(1) / 37767777777777777777b /
c     data maxmag(2) / 37167777777777777777b /
c
c     Machine constants for the PDP-10 (KA processor).
c
c     data mcheps(1),mcheps(2) / "114400000000, "000000000000 /
c     data minmag(1),minmag(2) / "033400000000, "000000000000 /
c     data maxmag(1),maxmag(2) / "377777777777, "344777777777 /
c
c     Machine constants for the PDP-10 (KI processor).
c
c     data mcheps(1),mcheps(2) / "104400000000, "000000000000 /
c     data minmag(1),minmag(2) / "000400000000, "000000000000 /
c     data maxmag(1),maxmag(2) / "377777777777, "377777777777 /
c
c     Machine constants for the PDP-11.
c
c     data mcheps(1),mcheps(2) /   9472,      0 /
c     data mcheps(3),mcheps(4) /      0,      0 /
c
c     data minmag(1),minmag(2) /    128,      0 /
c     data minmag(3),minmag(4) /      0,      0 /
c
c     data maxmag(1),maxmag(2) /  32767,     -1 /
c     data maxmag(3),maxmag(4) /     -1,     -1 /
c
c     Machine constants for the Burroughs 6700/7700 systems.
c
c     data mcheps(1) / o1451000000000000 /
c     data mcheps(2) / o0000000000000000 /
c
c     data minmag(1) / o1771000000000000 /
c     data minmag(2) / o7770000000000000 /
c
c     data maxmag(1) / o0777777777777777 /
c     data maxmag(2) / o7777777777777777 /
c
c     Machine constants for the Burroughs 5700 system.
c
c     data mcheps(1) / o1451000000000000 /
c     data mcheps(2) / o0000000000000000 /
c
c     data minmag(1) / o1771000000000000 /
c     data minmag(2) / o0000000000000000 /
c
c     data maxmag(1) / o0777777777777777 /
c     data maxmag(2) / o0007777777777777 /
c
c     Machine constants for the Burroughs 1700 system.
c
c     data mcheps(1) / zcc6800000 /
c     data mcheps(2) / z000000000 /
c
c     data minmag(1) / zc00800000 /
c     data minmag(2) / z000000000 /
c
c     data maxmag(1) / zdffffffff /
c     data maxmag(2) / zfffffffff /
c
c     Machine constants for the Univac 1100 series.
c
c     data mcheps(1),mcheps(2) / o170640000000, o000000000000 /
c     data minmag(1),minmag(2) / o000040000000, o000000000000 /
c     data maxmag(1),maxmag(2) / o377777777777, o777777777777 /
c
c     Machine constants for the Data General Eclipse S/200.
c
c     Note - it may be appropriate to include the following card -
c     static dmach(3)
c
c     data minmag/20k,3*0/,maxmag/77777k,3*177777k/
c     data mcheps/32020k,3*0/
c
c     Machine constants for the Harris 220.
c
c     data mcheps(1),mcheps(2) / '20000000, '00000334 /
c     data minmag(1),minmag(2) / '20000000, '00000201 /
c     data maxmag(1),maxmag(2) / '37777777, '37777577 /
c
c     Machine constants for the Cray-1.
c
c     data mcheps(1) / 0376424000000000000000b /
c     data mcheps(2) / 0000000000000000000000b /
c
c     data minmag(1) / 0200034000000000000000b /
c     data minmag(2) / 0000000000000000000000b /
c
c     data maxmag(1) / 0577777777777777777777b /
c     data maxmag(2) / 0000007777777777777776b /
c
c     Machine constants for the Prime 400.
c
c     data mcheps(1),mcheps(2) / :10000000000, :00000000123 /
c     data minmag(1),minmag(2) / :10000000000, :00000100000 /
c     data maxmag(1),maxmag(2) / :17777777777, :37777677776 /
c
c     Machine constants for the VAX-11.
c
c     data mcheps(1),mcheps(2) /   9472,  0 /
c     data minmag(1),minmag(2) /    128,  0 /
c     data maxmag(1),maxmag(2) / -32769, -1 /
c
c     Machine constants for IEEE machines.
c
      data dmach(1) /2.22044604926d-16/
      data dmach(2) /2.22507385852d-308/
      data dmach(3) /1.79769313485d+308/
c
      dpmpar = dmach(i)
      return
c
c     Last card of function dpmpar.
c
      end
      double precision function enorm(n,x)
      integer n
      double precision x(n)
c     **********
c
c     function enorm
c
c     given an n-vector x, this function calculates the
c     euclidean norm of x.
c
c     the euclidean norm is computed by accumulating the sum of
c     squares in three different sums. the sums of squares for the
c     small and large components are scaled so that no overflows
c     occur. non-destructive underflows are permitted. underflows
c     and overflows do not occur in the computation of the unscaled
c     sum of squares for the intermediate components.
c     the definitions of small, intermediate and large components
c     depend on two constants, rdwarf and rgiant. the main
c     restrictions on these constants are that rdwarf**2 not
c     underflow and rgiant**2 not overflow. the constants
c     given here are suitable for every known computer.
c
c     the function statement is
c
c       double precision function enorm(n,x)
c
c     where
c
c       n is a positive integer input variable.
c
c       x is an input array of length n.
c
c     subprograms called
c
c       fortran-supplied ... dabs,dsqrt
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i
      double precision agiant,floatn,one,rdwarf,rgiant,s1,s2,s3,xabs,
     *                 x1max,x3max,zero
      data one,zero,rdwarf,rgiant /1.0d0,0.0d0,3.834d-20,1.304d19/
      s1 = zero
      s2 = zero
      s3 = zero
      x1max = zero
      x3max = zero
      floatn = n
      agiant = rgiant/floatn
      do 90 i = 1, n
         xabs = dabs(x(i))
         if (xabs .gt. rdwarf .and. xabs .lt. agiant) go to 70
            if (xabs .le. rdwarf) go to 30
c
c              sum for large components.
c
               if (xabs .le. x1max) go to 10
                  s1 = one + s1*(x1max/xabs)**2
                  x1max = xabs
                  go to 20
   10          continue
                  s1 = s1 + (xabs/x1max)**2
   20          continue
               go to 60
   30       continue
c
c              sum for small components.
c
               if (xabs .le. x3max) go to 40
                  s3 = one + s3*(x3max/xabs)**2
                  x3max = xabs
                  go to 50
   40          continue
                  if (xabs .ne. zero) s3 = s3 + (xabs/x3max)**2
   50          continue
   60       continue
            go to 80
   70    continue
c
c           sum for intermediate components.
c
            s2 = s2 + xabs**2
   80    continue
   90    continue
c
c     calculation of norm.
c
      if (s1 .eq. zero) go to 100
         enorm = x1max*dsqrt(s1+(s2/x1max)/x1max)
         go to 130
  100 continue
         if (s2 .eq. zero) go to 110
            if (s2 .ge. x3max)
     *         enorm = dsqrt(s2*(one+(x3max/s2)*(x3max*s3)))
            if (s2 .lt. x3max)
     *         enorm = dsqrt(x3max*((s2/x3max)+(x3max*s3)))
            go to 120
  110    continue
            enorm = x3max*dsqrt(s3)
  120    continue
  130 continue
      return
c
c     last card of function enorm.
c
      end
      subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
      integer m,n,ldfjac,iflag
      double precision epsfcn
      double precision x(n),fvec(m),fjac(ldfjac,n),wa(m)
c     **********
c
c     subroutine fdjac2
c
c     this subroutine computes a forward-difference approximation
c     to the m by n jacobian matrix associated with a specified
c     problem of m functions in n variables.
c
c     the subroutine statement is
c
c       subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
c
c     where
c
c       fcn is the name of the user-supplied subroutine which
c         calculates the functions. fcn must be declared
c         in an external statement in the user calling
c         program, and should be written as follows.
c
c         subroutine fcn(m,n,x,fvec,iflag)
c         integer m,n,iflag
c         double precision x(n),fvec(m)
c         ----------
c         calculate the functions at x and
c         return this vector in fvec.
c         ----------
c         return
c         end
c
c         the value of iflag should not be changed by fcn unless
c         the user wants to terminate execution of fdjac2.
c         in this case set iflag to a negative integer.
c
c       m is a positive integer input variable set to the number
c         of functions.
c
c       n is a positive integer input variable set to the number
c         of variables. n must not exceed m.
c
c       x is an input array of length n.
c
c       fvec is an input array of length m which must contain the
c         functions evaluated at x.
c
c       fjac is an output m by n array which contains the
c         approximation to the jacobian matrix evaluated at x.
c
c       ldfjac is a positive integer input variable not less than m
c         which specifies the leading dimension of the array fjac.
c
c       iflag is an integer variable which can be used to terminate
c         the execution of fdjac2. see description of fcn.
c
c       epsfcn is an input variable used in determining a suitable
c         step length for the forward-difference approximation. this
c         approximation assumes that the relative errors in the
c         functions are of the order of epsfcn. if epsfcn is less
c         than the machine precision, it is assumed that the relative
c         errors in the functions are of the order of the machine
c         precision.
c
c       wa is a work array of length m.
c
c     subprograms called
c
c       user-supplied ...... fcn
c
c       minpack-supplied ... dpmpar
c
c       fortran-supplied ... dabs,dmax1,dsqrt
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,j
      double precision eps,epsmch,h,temp,zero
      double precision dpmpar
      data zero /0.0d0/
c
c     epsmch is the machine precision.
c
      epsmch = dpmpar(1)
c
      eps = dsqrt(dmax1(epsfcn,epsmch))
      do 20 j = 1, n
         temp = x(j)
         h = eps*dabs(temp)
         if (h .eq. zero) h = eps
         x(j) = temp + h
         call fcn(m,n,x,wa,iflag)
         if (iflag .lt. 0) go to 30
         x(j) = temp
         do 10 i = 1, m
            fjac(i,j) = (wa(i) - fvec(i))/h
   10       continue
   20    continue
   30 continue
      return
c
c     last card of subroutine fdjac2.
c
      end
      subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1,
     *                 wa2)
      integer n,ldr
      integer ipvt(n)
      double precision delta,par
      double precision r(ldr,n),diag(n),qtb(n),x(n),sdiag(n),wa1(n),
     *                 wa2(n)
c     **********
c
c     subroutine lmpar
c
c     given an m by n matrix a, an n by n nonsingular diagonal
c     matrix d, an m-vector b, and a positive number delta,
c     the problem is to determine a value for the parameter
c     par such that if x solves the system
c
c           a*x = b ,     sqrt(par)*d*x = 0 ,
c
c     in the least squares sense, and dxnorm is the euclidean
c     norm of d*x, then either par is zero and
c
c           (dxnorm-delta) .le. 0.1*delta ,
c
c     or par is positive and
c
c           abs(dxnorm-delta) .le. 0.1*delta .
c
c     this subroutine completes the solution of the problem
c     if it is provided with the necessary information from the
c     qr factorization, with column pivoting, of a. that is, if
c     a*p = q*r, where p is a permutation matrix, q has orthogonal
c     columns, and r is an upper triangular matrix with diagonal
c     elements of nonincreasing magnitude, then lmpar expects
c     the full upper triangle of r, the permutation matrix p,
c     and the first n components of (q transpose)*b. on output
c     lmpar also provides an upper triangular matrix s such that
c
c            t   t                   t
c           p *(a *a + par*d*d)*p = s *s .
c
c     s is employed within lmpar and may be of separate interest.
c
c     only a few iterations are generally needed for convergence
c     of the algorithm. if, however, the limit of 10 iterations
c     is reached, then the output par will contain the best
c     value obtained so far.
c
c     the subroutine statement is
c
c       subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
c                        wa1,wa2)
c
c     where
c
c       n is a positive integer input variable set to the order of r.
c
c       r is an n by n array. on input the full upper triangle
c         must contain the full upper triangle of the matrix r.
c         on output the full upper triangle is unaltered, and the
c         strict lower triangle contains the strict upper triangle
c         (transposed) of the upper triangular matrix s.
c
c       ldr is a positive integer input variable not less than n
c         which specifies the leading dimension of the array r.
c
c       ipvt is an integer input array of length n which defines the
c         permutation matrix p such that a*p = q*r. column j of p
c         is column ipvt(j) of the identity matrix.
c
c       diag is an input array of length n which must contain the
c         diagonal elements of the matrix d.
c
c       qtb is an input array of length n which must contain the first
c         n elements of the vector (q transpose)*b.
c
c       delta is a positive input variable which specifies an upper
c         bound on the euclidean norm of d*x.
c
c       par is a nonnegative variable. on input par contains an
c         initial estimate of the levenberg-marquardt parameter.
c         on output par contains the final estimate.
c
c       x is an output array of length n which contains the least
c         squares solution of the system a*x = b, sqrt(par)*d*x = 0,
c         for the output par.
c
c       sdiag is an output array of length n which contains the
c         diagonal elements of the upper triangular matrix s.
c
c       wa1 and wa2 are work arrays of length n.
c
c     subprograms called
c
c       minpack-supplied ... dpmpar,enorm,qrsolv
c
c       fortran-supplied ... dabs,dmax1,dmin1,dsqrt
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,iter,j,jm1,jp1,k,l,nsing
      double precision dxnorm,dwarf,fp,gnorm,parc,parl,paru,p1,p001,
     *                 sum,temp,zero
      double precision dpmpar,enorm
      data p1,p001,zero /1.0d-1,1.0d-3,0.0d0/
c
c     dwarf is the smallest positive magnitude.
c
      dwarf = dpmpar(2)
c
c     compute and store in x the gauss-newton direction. if the
c     jacobian is rank-deficient, obtain a least squares solution.
c
      nsing = n
      do 10 j = 1, n
         wa1(j) = qtb(j)
         if (r(j,j) .eq. zero .and. nsing .eq. n) nsing = j - 1
         if (nsing .lt. n) wa1(j) = zero
   10    continue
      if (nsing .lt. 1) go to 50
      do 40 k = 1, nsing
         j = nsing - k + 1
         wa1(j) = wa1(j)/r(j,j)
         temp = wa1(j)
         jm1 = j - 1
         if (jm1 .lt. 1) go to 30
         do 20 i = 1, jm1
            wa1(i) = wa1(i) - r(i,j)*temp
   20       continue
   30    continue
   40    continue
   50 continue
      do 60 j = 1, n
         l = ipvt(j)
         x(l) = wa1(j)
   60    continue
c
c     initialize the iteration counter.
c     evaluate the function at the origin, and test
c     for acceptance of the gauss-newton direction.
c
      iter = 0
      do 70 j = 1, n
         wa2(j) = diag(j)*x(j)
   70    continue
      dxnorm = enorm(n,wa2)
      fp = dxnorm - delta
      if (fp .le. p1*delta) go to 220
c
c     if the jacobian is not rank deficient, the newton
c     step provides a lower bound, parl, for the zero of
c     the function. otherwise set this bound to zero.
c
      parl = zero
      if (nsing .lt. n) go to 120
      do 80 j = 1, n
         l = ipvt(j)
         wa1(j) = diag(l)*(wa2(l)/dxnorm)
   80    continue
      do 110 j = 1, n
         sum = zero
         jm1 = j - 1
         if (jm1 .lt. 1) go to 100
         do 90 i = 1, jm1
            sum = sum + r(i,j)*wa1(i)
   90       continue
  100    continue
         wa1(j) = (wa1(j) - sum)/r(j,j)
  110    continue
      temp = enorm(n,wa1)
      parl = ((fp/delta)/temp)/temp
  120 continue
c
c     calculate an upper bound, paru, for the zero of the function.
c
      do 140 j = 1, n
         sum = zero
         do 130 i = 1, j
            sum = sum + r(i,j)*qtb(i)
  130       continue
         l = ipvt(j)
         wa1(j) = sum/diag(l)
  140    continue
      gnorm = enorm(n,wa1)
      paru = gnorm/delta
      if (paru .eq. zero) paru = dwarf/dmin1(delta,p1)
c
c     if the input par lies outside of the interval (parl,paru),
c     set par to the closer endpoint.
c
      par = dmax1(par,parl)
      par = dmin1(par,paru)
      if (par .eq. zero) par = gnorm/dxnorm
c
c     beginning of an iteration.
c
  150 continue
         iter = iter + 1
c
c        evaluate the function at the current value of par.
c
         if (par .eq. zero) par = dmax1(dwarf,p001*paru)
         temp = dsqrt(par)
         do 160 j = 1, n
            wa1(j) = temp*diag(j)
  160       continue
         call qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2)
         do 170 j = 1, n
            wa2(j) = diag(j)*x(j)
  170       continue
         dxnorm = enorm(n,wa2)
         temp = fp
         fp = dxnorm - delta
c
c        if the function is small enough, accept the current value
c        of par. also test for the exceptional cases where parl
c        is zero or the number of iterations has reached 10.
c
         if (dabs(fp) .le. p1*delta
     *       .or. parl .eq. zero .and. fp .le. temp
     *            .and. temp .lt. zero .or. iter .eq. 10) go to 220
c
c        compute the newton correction.
c
         do 180 j = 1, n
            l = ipvt(j)
            wa1(j) = diag(l)*(wa2(l)/dxnorm)
  180       continue
         do 210 j = 1, n
            wa1(j) = wa1(j)/sdiag(j)
            temp = wa1(j)
            jp1 = j + 1
            if (n .lt. jp1) go to 200
            do 190 i = jp1, n
               wa1(i) = wa1(i) - r(i,j)*temp
  190          continue
  200       continue
  210       continue
         temp = enorm(n,wa1)
         parc = ((fp/delta)/temp)/temp
c
c        depending on the sign of the function, update parl or paru.
c
         if (fp .gt. zero) parl = dmax1(parl,par)
         if (fp .lt. zero) paru = dmin1(paru,par)
c
c        compute an improved estimate for par.
c
         par = dmax1(parl,par+parc)
c
c        end of an iteration.
c
         go to 150
  220 continue
c
c     termination.
c
      if (iter .eq. 0) par = zero
      return
c
c     last card of subroutine lmpar.
c
      end
      subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
      integer m,n,lda,lipvt
      integer ipvt(lipvt)
      logical pivot
      double precision a(lda,n),rdiag(n),acnorm(n),wa(n)
c     **********
c
c     subroutine qrfac
c
c     this subroutine uses householder transformations with column
c     pivoting (optional) to compute a qr factorization of the
c     m by n matrix a. that is, qrfac determines an orthogonal
c     matrix q, a permutation matrix p, and an upper trapezoidal
c     matrix r with diagonal elements of nonincreasing magnitude,
c     such that a*p = q*r. the householder transformation for
c     column k, k = 1,2,...,min(m,n), is of the form
c
c                           t
c           i - (1/u(k))*u*u
c
c     where u has zeros in the first k-1 positions. the form of
c     this transformation and the method of pivoting first
c     appeared in the corresponding linpack subroutine.
c
c     the subroutine statement is
c
c       subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
c
c     where
c
c       m is a positive integer input variable set to the number
c         of rows of a.
c
c       n is a positive integer input variable set to the number
c         of columns of a.
c
c       a is an m by n array. on input a contains the matrix for
c         which the qr factorization is to be computed. on output
c         the strict upper trapezoidal part of a contains the strict
c         upper trapezoidal part of r, and the lower trapezoidal
c         part of a contains a factored form of q (the non-trivial
c         elements of the u vectors described above).
c
c       lda is a positive integer input variable not less than m
c         which specifies the leading dimension of the array a.
c
c       pivot is a logical input variable. if pivot is set true,
c         then column pivoting is enforced. if pivot is set false,
c         then no column pivoting is done.
c
c       ipvt is an integer output array of length lipvt. ipvt
c         defines the permutation matrix p such that a*p = q*r.
c         column j of p is column ipvt(j) of the identity matrix.
c         if pivot is false, ipvt is not referenced.
c
c       lipvt is a positive integer input variable. if pivot is false,
c         then lipvt may be as small as 1. if pivot is true, then
c         lipvt must be at least n.
c
c       rdiag is an output array of length n which contains the
c         diagonal elements of r.
c
c       acnorm is an output array of length n which contains the
c         norms of the corresponding columns of the input matrix a.
c         if this information is not needed, then acnorm can coincide
c         with rdiag.
c
c       wa is a work array of length n. if pivot is false, then wa
c         can coincide with rdiag.
c
c     subprograms called
c
c       minpack-supplied ... dpmpar,enorm
c
c       fortran-supplied ... dmax1,dsqrt,min0
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,j,jp1,k,kmax,minmn
      double precision ajnorm,epsmch,one,p05,sum,temp,zero
      double precision dpmpar,enorm
      data one,p05,zero /1.0d0,5.0d-2,0.0d0/
c
c     epsmch is the machine precision.
c
      epsmch = dpmpar(1)
c
c     compute the initial column norms and initialize several arrays.
c
      do 10 j = 1, n
         acnorm(j) = enorm(m,a(1,j))
         rdiag(j) = acnorm(j)
         wa(j) = rdiag(j)
         if (pivot) ipvt(j) = j
   10    continue
c
c     reduce a to r with householder transformations.
c
      minmn = min0(m,n)
      do 110 j = 1, minmn
         if (.not.pivot) go to 40
c
c        bring the column of largest norm into the pivot position.
c
         kmax = j
         do 20 k = j, n
            if (rdiag(k) .gt. rdiag(kmax)) kmax = k
   20       continue
         if (kmax .eq. j) go to 40
         do 30 i = 1, m
            temp = a(i,j)
            a(i,j) = a(i,kmax)
            a(i,kmax) = temp
   30       continue
         rdiag(kmax) = rdiag(j)
         wa(kmax) = wa(j)
         k = ipvt(j)
         ipvt(j) = ipvt(kmax)
         ipvt(kmax) = k
   40    continue
c
c        compute the householder transformation to reduce the
c        j-th column of a to a multiple of the j-th unit vector.
c
         ajnorm = enorm(m-j+1,a(j,j))
         if (ajnorm .eq. zero) go to 100
         if (a(j,j) .lt. zero) ajnorm = -ajnorm
         do 50 i = j, m
            a(i,j) = a(i,j)/ajnorm
   50       continue
         a(j,j) = a(j,j) + one
c
c        apply the transformation to the remaining columns
c        and update the norms.
c
         jp1 = j + 1
         if (n .lt. jp1) go to 100
         do 90 k = jp1, n
            sum = zero
            do 60 i = j, m
               sum = sum + a(i,j)*a(i,k)
   60          continue
            temp = sum/a(j,j)
            do 70 i = j, m
               a(i,k) = a(i,k) - temp*a(i,j)
   70          continue
            if (.not.pivot .or. rdiag(k) .eq. zero) go to 80
            temp = a(j,k)/rdiag(k)
            rdiag(k) = rdiag(k)*dsqrt(dmax1(zero,one-temp**2))
            if (p05*(rdiag(k)/wa(k))**2 .gt. epsmch) go to 80
            rdiag(k) = enorm(m-j,a(jp1,k))
            wa(k) = rdiag(k)
   80       continue
   90       continue
  100    continue
         rdiag(j) = -ajnorm
  110    continue
      return
c
c     last card of subroutine qrfac.
c
      end
      subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
      integer n,ldr
      integer ipvt(n)
      double precision r(ldr,n),diag(n),qtb(n),x(n),sdiag(n),wa(n)
c     **********
c
c     subroutine qrsolv
c
c     given an m by n matrix a, an n by n diagonal matrix d,
c     and an m-vector b, the problem is to determine an x which
c     solves the system
c
c           a*x = b ,     d*x = 0 ,
c
c     in the least squares sense.
c
c     this subroutine completes the solution of the problem
c     if it is provided with the necessary information from the
c     qr factorization, with column pivoting, of a. that is, if
c     a*p = q*r, where p is a permutation matrix, q has orthogonal
c     columns, and r is an upper triangular matrix with diagonal
c     elements of nonincreasing magnitude, then qrsolv expects
c     the full upper triangle of r, the permutation matrix p,
c     and the first n components of (q transpose)*b. the system
c     a*x = b, d*x = 0, is then equivalent to
c
c                  t       t
c           r*z = q *b ,  p *d*p*z = 0 ,
c
c     where x = p*z. if this system does not have full rank,
c     then a least squares solution is obtained. on output qrsolv
c     also provides an upper triangular matrix s such that
c
c            t   t               t
c           p *(a *a + d*d)*p = s *s .
c
c     s is computed within qrsolv and may be of separate interest.
c
c     the subroutine statement is
c
c       subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
c
c     where
c
c       n is a positive integer input variable set to the order of r.
c
c       r is an n by n array. on input the full upper triangle
c         must contain the full upper triangle of the matrix r.
c         on output the full upper triangle is unaltered, and the
c         strict lower triangle contains the strict upper triangle
c         (transposed) of the upper triangular matrix s.
c
c       ldr is a positive integer input variable not less than n
c         which specifies the leading dimension of the array r.
c
c       ipvt is an integer input array of length n which defines the
c         permutation matrix p such that a*p = q*r. column j of p
c         is column ipvt(j) of the identity matrix.
c
c       diag is an input array of length n which must contain the
c         diagonal elements of the matrix d.
c
c       qtb is an input array of length n which must contain the first
c         n elements of the vector (q transpose)*b.
c
c       x is an output array of length n which contains the least
c         squares solution of the system a*x = b, d*x = 0.
c
c       sdiag is an output array of length n which contains the
c         diagonal elements of the upper triangular matrix s.
c
c       wa is a work array of length n.
c
c     subprograms called
c
c       fortran-supplied ... dabs,dsqrt
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,j,jp1,k,kp1,l,nsing
      double precision cos,cotan,p5,p25,qtbpj,sin,sum,tan,temp,zero
      data p5,p25,zero /5.0d-1,2.5d-1,0.0d0/
c
c     copy r and (q transpose)*b to preserve input and initialize s.
c     in particular, save the diagonal elements of r in x.
c
      do 20 j = 1, n
         do 10 i = j, n
            r(i,j) = r(j,i)
   10       continue
         x(j) = r(j,j)
         wa(j) = qtb(j)
   20    continue
c
c     eliminate the diagonal matrix d using a givens rotation.
c
      do 100 j = 1, n
c
c        prepare the row of d to be eliminated, locating the
c        diagonal element using p from the qr factorization.
c
         l = ipvt(j)
         if (diag(l) .eq. zero) go to 90
         do 30 k = j, n
            sdiag(k) = zero
   30       continue
         sdiag(j) = diag(l)
c
c        the transformations to eliminate the row of d
c        modify only a single element of (q transpose)*b
c        beyond the first n, which is initially zero.
c
         qtbpj = zero
         do 80 k = j, n
c
c           determine a givens rotation which eliminates the
c           appropriate element in the current row of d.
c
            if (sdiag(k) .eq. zero) go to 70
            if (dabs(r(k,k)) .ge. dabs(sdiag(k))) go to 40
               cotan = r(k,k)/sdiag(k)
               sin = p5/dsqrt(p25+p25*cotan**2)
               cos = sin*cotan
               go to 50
   40       continue
               tan = sdiag(k)/r(k,k)
               cos = p5/dsqrt(p25+p25*tan**2)
               sin = cos*tan
   50       continue
c
c           compute the modified diagonal element of r and
c           the modified element of ((q transpose)*b,0).
c
            r(k,k) = cos*r(k,k) + sin*sdiag(k)
            temp = cos*wa(k) + sin*qtbpj
            qtbpj = -sin*wa(k) + cos*qtbpj
            wa(k) = temp
c
c           accumulate the tranformation in the row of s.
c
            kp1 = k + 1
            if (n .lt. kp1) go to 70
            do 60 i = kp1, n
               temp = cos*r(i,k) + sin*sdiag(i)
               sdiag(i) = -sin*r(i,k) + cos*sdiag(i)
               r(i,k) = temp
   60          continue
   70       continue
   80       continue
   90    continue
c
c        store the diagonal element of s and restore
c        the corresponding diagonal element of r.
c
         sdiag(j) = r(j,j)
         r(j,j) = x(j)
  100    continue
c
c     solve the triangular system for z. if the system is
c     singular, then obtain a least squares solution.
c
      nsing = n
      do 110 j = 1, n
         if (sdiag(j) .eq. zero .and. nsing .eq. n) nsing = j - 1
         if (nsing .lt. n) wa(j) = zero
  110    continue
      if (nsing .lt. 1) go to 150
      do 140 k = 1, nsing
         j = nsing - k + 1
         sum = zero
         jp1 = j + 1
         if (nsing .lt. jp1) go to 130
         do 120 i = jp1, nsing
            sum = sum + r(i,j)*wa(i)
  120       continue
  130    continue
         wa(j) = (wa(j) - sum)/sdiag(j)
  140    continue
  150 continue
c
c     permute the components of z back to components of x.
c
      do 160 j = 1, n
         l = ipvt(j)
         x(l) = wa(j)
  160    continue
      return
c
c     last card of subroutine qrsolv.
c
      end


!*****************************************************************************************
!>
!  Analytical ephemeris routines for solar system bodies.

    module analytical_ephemeris_module

    use numbers_module
    use kind_module
    use conversion_module

    implicit none

    private

    public :: simpson_lunar_ephemeris

    contains
!*****************************************************************************************

!*****************************************************************************************
!>
!  A simple analytical lunar ephemeris model.
!  Returns Lunar cartesian coordinates
!  (mean equator and equinox of epoch J2000).
!
!### Reference
!  * "An alternative lunar ephemeris model for on-board flight software use",
!    D. G. Simpson, Proceedings of the 1999 NASA/GSFC Flight Mechanics Symposium,
!    p. 175-184).
!  * [David G. Simpson Personal Web Site](http://www.davidgsimpson.com/software.html)
!
!### Reference code
!
!```fortran
!  T = (JD - 2451545.0D0)/36525.0D0
!  X =  383.0D3 * SIN( 8399.685D0 * T + 5.381D0)   &
!      + 31.5D3 * SIN(   70.990D0 * T + 6.169D0)   &
!      + 10.6D3 * SIN(16728.377D0 * T + 1.453D0)   &
!      +  6.2D3 * SIN( 1185.622D0 * T + 0.481D0)   &
!      +  3.2D3 * SIN( 7143.070D0 * T + 5.017D0)   &
!      +  2.3D3 * SIN(15613.745D0 * T + 0.857D0)   &
!      +  0.8D3 * SIN( 8467.263D0 * T + 1.010D0)
!  Y =  351.0D3 * SIN( 8399.687D0 * T + 3.811D0)   &
!      + 28.9D3 * SIN(   70.997D0 * T + 4.596D0)   &
!      + 13.7D3 * SIN( 8433.466D0 * T + 4.766D0)   &
!      +  9.7D3 * SIN(16728.380D0 * T + 6.165D0)   &
!      +  5.7D3 * SIN( 1185.667D0 * T + 5.164D0)   &
!      +  2.9D3 * SIN( 7143.058D0 * T + 0.300D0)   &
!      +  2.1D3 * SIN(15613.755D0 * T + 5.565D0)
!  Z =  153.2D3 * SIN( 8399.672D0 * T + 3.807D0)   &
!      + 31.5D3 * SIN( 8433.464D0 * T + 1.629D0)   &
!      + 12.5D3 * SIN(   70.996D0 * T + 4.595D0)   &
!      +  4.2D3 * SIN(16728.364D0 * T + 6.162D0)   &
!      +  2.5D3 * SIN( 1185.645D0 * T + 5.167D0)   &
!      +  3.0D3 * SIN(  104.881D0 * T + 2.555D0)   &
!      +  1.8D3 * SIN( 8399.116D0 * T + 6.248D0)
!```
!
!@note Also added velocity output, which is not present in reference code.

    subroutine simpson_lunar_ephemeris(jd,r_moon,v_moon)

    implicit none

    real(wp),intent(in)                        :: jd     !! Julian date
    real(wp),dimension(3),intent(out)          :: r_moon !! Moon position (km)
    real(wp),dimension(3),intent(out),optional :: v_moon !! Moon velocity (km/s)

    real(wp) :: t  !! time in Julian centuries from J2000
    real(wp),dimension(7) :: xterms,yterms,zterms

    !coefficients:
    real(wp),dimension(7),parameter :: xcoeffs = [  383.0e3_wp, 31.5e3_wp, &
                                                     10.6e3_wp, 6.2e3_wp, &
                                                      3.2e3_wp, 2.3e3_wp, &
                                                      0.8e3_wp]
    real(wp),dimension(7),parameter :: ycoeffs = [  351.0e3_wp, 28.9e3_wp, &
                                                     13.7e3_wp, 9.7e3_wp, &
                                                      5.7e3_wp, 2.9e3_wp, &
                                                      2.1e3_wp]
    real(wp),dimension(7),parameter :: zcoeffs = [  153.2e3_wp, 31.5e3_wp, &
                                                     12.5e3_wp, 4.2e3_wp, &
                                                      2.5e3_wp, 3.0e3_wp, &
                                                      1.8e3_wp]
    real(wp),dimension(7),parameter :: xa = [ 8399.685_wp, 70.990_wp, &
                                             16728.377_wp, 1185.622_wp, &
                                              7143.070_wp, 15613.745_wp, &
                                              8467.263_wp]
    real(wp),dimension(7),parameter :: xp = [ 5.381_wp, 6.169_wp, &
                                              1.453_wp, 0.481_wp, &
                                              5.017_wp, 0.857_wp, &
                                              1.010_wp]
    real(wp),dimension(7),parameter :: ya = [ 8399.687_wp, 70.997_wp, &
                                              8433.466_wp, 16728.380_wp, &
                                              1185.667_wp, 7143.058_wp, &
                                             15613.755_wp]
    real(wp),dimension(7),parameter :: yp = [3.811_wp, 4.596_wp, &
                                             4.766_wp, 6.165_wp, &
                                             5.164_wp, 0.300_wp, &
                                             5.565_wp]
    real(wp),dimension(7),parameter :: za = [ 8399.672_wp, 8433.464_wp, &
                                                70.996_wp, 16728.364_wp, &
                                              1185.645_wp, 104.881_wp, &
                                              8399.116_wp]
    real(wp),dimension(7),parameter :: zp = [3.807_wp, 1.629_wp, &
                                             4.595_wp, 6.162_wp, &
                                             5.167_wp, 2.555_wp, &
                                             6.248_wp]
    real(wp),dimension(7),parameter :: vxcoeffs = xcoeffs*xa
    real(wp),dimension(7),parameter :: vycoeffs = ycoeffs*ya
    real(wp),dimension(7),parameter :: vzcoeffs = zcoeffs*za

    t = (jd - 2451545.0_wp)*day2century

    xterms = xa * t + xp
    yterms = ya * t + yp
    zterms = za * t + zp

    r_moon(1) = dot_product(xcoeffs, sin(xterms))
    r_moon(2) = dot_product(ycoeffs, sin(yterms))
    r_moon(3) = dot_product(zcoeffs, sin(zterms))

    !v_moon is just d(r_moon)/dt:
    ! [convert units to km/s]
    if (present(v_moon)) then
        v_moon(1) =  dot_product(vxcoeffs, cos(xterms)) / (century2day * day2sec)
        v_moon(2) =  dot_product(vycoeffs, cos(yterms)) / (century2day * day2sec)
        v_moon(3) =  dot_product(vzcoeffs, cos(zterms)) / (century2day * day2sec)
    end if

    end subroutine simpson_lunar_ephemeris
!*****************************************************************************************

!*****************************************************************************************
    end module analytical_ephemeris_module
!*****************************************************************************************


#include "scalar.fpp"
!=====================================================================!
! A Diagonally Implicit Runge Kutta integrator module for first and
! second order systems.
!
! Author: Komahan Boopathy (komahan@gatech.edu)
!=====================================================================!

module runge_kutta_integrator

  use integrator_class, only : integrator
  use physics_class,    only : physics
  use linear_algebra,   only : solve
  implicit none

  private
  public :: DIRK

  ! Useful constants
  type(scalar), parameter :: ONE  = 1.0d0
  type(scalar), parameter :: ZERO = 0.0d0

  !===================================================================!
  ! Abstract Runge-Kutta type
  !===================================================================!

  type, abstract, extends(integrator) :: RK

     integer :: num_stages = 1 ! default number of stages
     integer :: order = 2      ! order of accuracy, only for informatory purposes
     integer :: current_stage = 0

     !----------------------------------------------------------------!
     ! The Butcher Tableau
     !----------------------------------------------------------------!

     type(scalar), dimension(:,:), allocatable :: A ! forms the coeff matrix
     type(scalar), dimension(:), allocatable   :: B ! multiplies the state derivatives
     type(scalar), dimension(:), allocatable   :: C ! multiplies the time

     !----------------------------------------------------------------!
     ! The stage time and its corresponding derivatives
     !----------------------------------------------------------------!

     real(dp), dimension(:), allocatable :: T
     type(scalar), dimension(:,:,:), allocatable :: Q
     type(scalar), dimension(:,:,:), allocatable :: QDOT
     type(scalar), dimension(:,:,:), allocatable :: QDDOT

     !----------------------------------------------------------------!
     ! The lagrange multipliers
     !----------------------------------------------------------------!

     type(scalar), dimension(:,:,:), allocatable :: LAM

   contains

     ! Routines for integration

     procedure          :: integrate => integrate
     procedure, private :: approximateStates
     procedure, private :: getLinearCoeff

     procedure, private :: TimeMarch
     procedure, private :: CheckButcherTableau

     procedure(buthcher_interface), private, deferred :: SetupButcherTableau

     procedure :: testAdjoint6

  end type RK

  !-------------------------------------------------------------------!
  ! Diagonally implicit Runge-Kutta
  !-------------------------------------------------------------------!

  type, extends(RK) :: DIRK

     type(scalar), dimension(:,:,:), allocatable :: rhsbin
     type(scalar), dimension(:,:), allocatable :: psibin
     type(scalar), dimension(:,:), allocatable :: phibin

   contains

     ! Destructor
     procedure :: finalize

     procedure :: setupButcherTableau => ButcherDIRK

     !----------------------------------------------------------------!
     ! Adjoint procedures
     !----------------------------------------------------------------!
     procedure, public  :: marchBackwards
     procedure, private :: assembleRHS
     procedure, public :: computeTotalDerivative
     procedure, public :: evalFunc => evalFuncDIRK

     procedure, private :: evaluate_adjoint
     procedure, private :: distribute_contributions

  end type DIRK

  !===================================================================!
  ! Interfaces for deferred specialized procedures
  !===================================================================!

  interface

     !================================================================!
     ! Interface for setting the Butcher tableau for each type of RK
     ! scheme
     !================================================================!

     subroutine buthcher_interface(this)
       import RK
       class(RK) :: this
     end subroutine buthcher_interface

  end interface

  interface DIRK
     module procedure initialize
  end interface DIRK

contains

  !===================================================================!
  ! Initialize the dirk datatype and construct the tableau
  !===================================================================!

  type(DIRK) function initialize( system, tinit, tfinal, h, second_order, num_stages ) result(this)

    class(physics), target :: system
    integer  , OPTIONAL, intent(in) :: num_stages
    real(dp) , OPTIONAL, intent(in) :: tinit, tfinal
    real(dp), OPTIONAL, intent(in) :: h
    logical  , OPTIONAL, intent(in) :: second_order

    print *, "======================================"
    print *, ">> Diagonally-Implicit-Runge-Kutta  <<"
    print *, "======================================"

    call this % construct(system, tinit, tfinal, h, second_order)

    !-----------------------------------------------------------------!
    ! Set the order/stages of integration
    !-----------------------------------------------------------------!

    if (present(num_stages)) then
       this % num_stages = num_stages
    end if
    print '("  >> Number of stages       : ",i4)', this % num_stages

    !-----------------------------------------------------------------!
    ! Allocate space for the stage states and time
    !-----------------------------------------------------------------!

    allocate(this % T(this % num_stages))
    this % T = 0.0d0

    allocate(this % Q(this % num_steps, this % num_stages, this % nsvars))
    this % Q = 0.0d0

    allocate(this % QDOT(this % num_steps, this % num_stages, this % nsvars))
    this % QDOT = 0.0d0

    allocate(this % QDDOT(this % num_steps, this % num_stages, this % nsvars))
    this % QDDOT = 0.0d0

    !-----------------------------------------------------------------!
    ! Allocate space for the lagrange multipliers
    !-----------------------------------------------------------------!

    allocate(this % lam(this % num_steps, this % num_stages, this % nsvars))
    this % lam = 0.0d0

    !-----------------------------------------------------------------!
    ! Allocate space for the tableau
    !-----------------------------------------------------------------!

    allocate(this % A(this % num_stages, this % num_stages))
    this % A = 0.0d0

    allocate(this % B(this % num_stages))
    this % B = 0.0d0

    allocate(this % C(this % num_stages))
    this % C = 0.0d0

    call this % setupButcherTableau()
    call this % checkButcherTableau()

    !-----------------------------------------------------------------!
    ! Setup adjoint RHS
    !-----------------------------------------------------------------!

    this % num_rhs_bins = this % num_stages

    allocate(this % rhs(this % nsvars))
    this % rhs = 0.0d0

    !-----------------------------------------------------------------!
    ! Setup adjoint RHS
    !-----------------------------------------------------------------!

    this % num_rhs_bins = this % num_stages

    allocate(this % rhsbin(this % num_steps, this % num_stages, this % nsvars))
    this % rhsbin = 0.0d0

    allocate(this % phibin(this % num_steps, this % nsvars))
    this % phibin = 0.0d0

    allocate(this % psibin(this % num_steps, this % nsvars))
    this % psibin = 0.0d0

  end function initialize

  !===================================================================!
  ! Routine that checks if the Butcher Tableau entries are valid for
  ! the chosen number of stages/order
  !===================================================================!

  subroutine checkButcherTableau(this)

    class(RK) :: this
    integer :: i

    do i = 1, this  % num_stages
       if (abs(this % C(i) - sum(this % A(i,:))) .gt. 5.0d-16) then
          print *, "WARNING: sum(A(i,j)) != C(i)", i, this % num_stages
       end if
    end do

    if (abs(sum(this % B) - 1.0d0) .gt. 5.0d-16) then
       print *, "WARNING: sum(B) != 1", this % num_stages
    end if

  end subroutine checkButcherTableau

  !===================================================================!
  ! Deallocate the allocated variables
  !===================================================================!

  subroutine finalize(this)

    class(DIRK) :: this

    call this % destruct()

    ! Clear butcher's tableau
    if(allocated(this % A)) deallocate(this % A)
    if(allocated(this % B)) deallocate(this % B)
    if(allocated(this % C)) deallocate(this % C)

    ! Clear stage state values
    if(allocated(this % QDDOT)) deallocate(this % QDDOT)
    if(allocated(this % QDOT)) deallocate(this % QDOT)
    if(allocated(this % Q)) deallocate(this % Q)
    if(allocated(this % T)) deallocate(this % T)

    ! Clear stage lagrange multipliers
    if(allocated(this % lam)) deallocate(this % lam)

    ! Adjoint variables and RHS
    if(allocated(this % rhs)) deallocate(this % rhs)
    if(allocated(this % rhsbin)) deallocate(this % rhsbin)
    if(allocated(this % psibin)) deallocate(this % psibin)
    if(allocated(this % phibin)) deallocate(this % phibin)

  end subroutine finalize

  !===================================================================!
  ! Overridden Time integration logic
  !===================================================================!

  subroutine Integrate(this)

    use nonlinear_algebra, only: nonlinear_solve

    class(RK) :: this
    type(scalar)  :: alpha, beta, gamma
    integer :: k, i

    ! Set states to zero

    this % U     = 0.0d0
    this % UDOT  = 0.0d0
    this % UDDOT = 0.0d0
    this % time  = 0.0d0

    this % Q     = 0.0d0
    this % QDOT  = 0.0d0
    this % QDDOT = 0.0d0
    this % T     = 0.0d0

    call this % system % getInitialStates(this % time(1), this % u(1,:), this % udot(1,:))

    !-----------------------------------------------------------------!
    ! March in time
    !-----------------------------------------------------------------!

    this % current_step = 1

    time: do k = 2, this % num_steps

       this % current_step = k

       !-----------------------------------------------------------------!
       ! Loop over stages
       !-----------------------------------------------------------------!

       do i = 1, this % num_stages

          this % current_stage = i

          ! Find the stage times
          this % T(i) = this % time(k-1) + this % C(i)*this % h

          ! Guess the solution for stage states
          call this % approximateStates()

          call this % getLinearCoeff(alpha, beta, gamma)

          ! solve the non linear stage equations using Newton's method
          call nonlinear_solve(this % system, &
               & alpha, beta, gamma, &
               & this % time(k), this % q(k,i,:), this % qdot(k,i,:), this % qddot(k,i,:))

       end do

       ! Advance the state to the current step
       call this % timeMarch(this % u, this % udot, this % uddot)

    end do time

  end subroutine Integrate

  !===================================================================!
  ! Time backwards in stage and backwards in time to solve for the
  ! adjoint variables
  !===================================================================!

  subroutine marchBackwards( this )

    class(DIRK)    :: this
    type(integer) :: k,i

    time: do k = this % num_steps, 2, -1

       this % current_step = k

       stage: do i = this % num_stages, 1, -1

          this % current_stage = i

          !--------------------------------------------------------------!
          ! Solve the adjoint equation at each step
          !--------------------------------------------------------------!

          ! print *, k, i, this % lam(k,i,:), this % psi(k,:), this % phi(k,:)
          call this % evaluate_adjoint(this % lam(k,i,:), this % psi(k,:), this % phi(k,:))
          !print *,  k, i, this % lam(k,i,:), this % psi(k,:), this % phi(k,:)

          !--------------------------------------------------------------!
          ! Drop the contributions from this step to corresponding bins
          !--------------------------------------------------------------!

          call this % distribute_contributions(this % rhsbin, this % psibin, this % phibin)

       end do stage

    end do time

  end subroutine marchBackwards

  !===================================================================!
  ! Evaluates the adjoint variable values at the current step
  !===================================================================!

  subroutine evaluate_adjoint(this, mu, psi, phi)

    class(DIRK)                  :: this
    type(integer)                :: k, i
    type(scalar)                 :: alpha, beta, gamma
    type(scalar), dimension(:) :: mu, psi, phi

    ! Retrive the current step number
    k = this % current_step
    i = this % current_stage

    ! Evaluate PHI (no linear solution necessary)
    phi = this % phibin(k,:)

    ! Evaluate PSI (no linear solution necessary)
    psi = this % psibin(k,:)

    ! Evaluate MU
    call this % getLinearCoeff(alpha, beta, gamma)

    call this % adjointSolve(mu, &
         & alpha, beta, gamma, &
         & this % T(i), &
         & this % q(k,i,:), this % qdot(k,i,:), this % qddot(k,i,:))

  end subroutine evaluate_adjoint

  !===================================================================!
  ! Add contributions from kth step to k- steps
  !===================================================================!

  subroutine distribute_contributions(this, rhsbin, psibin, phibin)

    class(DIRK)                  :: this
    type(integer)                :: k, s, i, j, p
    type(scalar)                 :: alpha, beta, gamma
    type(scalar), dimension(:,:,:) :: rhsbin
    type(scalar), dimension(:,:) :: phibin, psibin

    ! Retrive the current step number
    k = this % current_step
    i = this % current_stage
    s = this % num_stages

    !-----------------------------------------------------------------!
    ! Add contributions from k to k- PHIBIN
    !-----------------------------------------------------------------!

    phibin(k-1,:) = phibin(k-1,:) + this % phi(k,:)

    gamma = 0.0d0
    beta  = 0.0d0
    alpha = this%h*this%B(i)

    call this % addFuncResAdjPdt(phibin(k-1,:), &
         & alpha, beta, gamma, this % T(i), &
         & this % Q(k,i,:), this % Qdot(k,i,:), this % Qddot(k,i,:), &
         & this % lam(k,i,:))

    !-----------------------------------------------------------------!
    ! Add contributions from k to k- PSIBIN
    !-----------------------------------------------------------------!

    psibin(k-1,:) = psibin(k-1,:) + this % psi(k,:)

    gamma = 0.0d0
    beta  = this % h * this % B(i)
    alpha = this % h * this % B(i) * this % h * sum(this % A(i,1:i))

    call this % addFuncResAdjPdt(psibin(k-1,:), &
         & alpha, beta, gamma, this % T(i), &
         & this % Q(k,i,:), this % Qdot(k,i,:), this % Qddot(k,i,:), &
         & this % lam(k,i,:))

    psibin(k-1,:) = psibin(k-1,:) + beta*this%phi(k,:)

  end subroutine distribute_contributions

  !===================================================================!
  ! Update the states based on RK Formulae
  !===================================================================!

  subroutine timeMarch(this, q, qdot, qddot)

    implicit none

    class(RK) :: this
    type(scalar),  dimension(:,:) :: q, qdot, qddot ! current state
    integer :: m, k

    ! Store the current time step
    k = this % current_step

    ! Increment the time
    this % time(k) = this % time(k-1) + this % h

    ! March q to next time step
    forall(m=1:this%nsvars)
       q(k,m) = q(k-1,m) + this % h*sum(this % B(1:this%num_stages) &
            &* this % QDOT(k, 1:this%num_stages, m))
    end forall

    if (this % second_order) then

       ! March qdot
       forall(m=1:this%nsvars)
          qdot(k,m) = qdot(k-1,m) + this % h*sum(this % B(1:this%num_stages) &
               &* this % QDDOT(k, 1:this%num_stages, m))
       end forall

       ! March qddot
       forall(m=1:this%nsvars)
          qddot(k,m) = sum(this % B(1:this%num_stages) &
               &* this % QDDOT(k,1:this%num_stages,m))
       end forall

    else

       ! March qdot
       forall(m=1:this%nsvars)
          qdot(k,m) = sum(this % B(1:this%num_stages) &
               & * this % QDOT(k,1:this%num_stages,m))
       end forall

    end if

  end subroutine timeMarch

  !===================================================================!
  ! Butcher's tableau for DIRK
  !===================================================================!

  subroutine ButcherDIRK(this)

    class(DIRK) :: this
    type(scalar), parameter :: PI = 22.0d0/7.0d0
    type(scalar), parameter :: tmp  = 1.0d0/(2.0d0*dsqrt(3.0d0))
    type(scalar), parameter :: half = 1.0d0/2.0d0
    type(scalar), parameter :: one  = 1.0d0
    type(scalar), parameter :: alpha = 2.0d0*cos(PI/18.0d0)/dsqrt(3.0d0)

    ! Put the entries into the tableau (ROGER ALEXANDER 1977)
    if (this % num_stages .eq. 1) then

       ! Implicit mid-point rule (A-stable)

       this % A(1,1)    = half
       this % B(1)      = one
       this % C(1)      = half

       this % order     = 2

!!$       ! Implicit Euler (Backward Euler) but first order accurate
!!$       this % A(1,1) = one
!!$       this % B(1)   = one
!!$       this % C(1)   = one
!!$       this % order  = 1

    else if (this % num_stages .eq. 2) then

       ! Crouzeix formula (A-stable)

       this % A(1,1)    = half + tmp
       this % A(2,1)    = -one/dsqrt(3.0d0)
       this % A(2,2)    = this % A(1,1)

       this % B(1)      = half
       this % B(2)      = half

       this % C(1)      = half + tmp
       this % C(2)      = half - tmp

       this % order     = 3

    else if (this % num_stages .eq. 3) then

       ! Crouzeix formula (A-stable)

       this % A(1,1)    = (one+alpha)*half
       this % A(2,1)    = -half*alpha
       this % A(3,1)    =  one + alpha

       this % A(2,2)    = this % A(1,1)
       this % A(3,2)    = -(one + 2.0d0*alpha)
       this % A(3,3)    = this % A(1,1)

       this % B(1)      = one/(6.0d0*alpha*alpha)
       this % B(2)      = one - one/(3.0d0*alpha*alpha)
       this % B(3)      = this % B(1)

       this % C(1)      = (one + alpha)*half
       this % C(2)      = half
       this % C(3)      = (one - alpha)*half

       this % order     = 4

    else if (this % num_stages .eq. 4) then

       stop "Four stage DIRK formula does not exist"

    else

       print *, this % num_stages
       stop "DIRK Butcher tableau is not implemented for the requested&
            & order/stages"

    end if

  end subroutine ButcherDIRK

  !===================================================================!
  ! Returns the linearization scalar coefficients: alpha, beta, gamma
  !===================================================================!

  subroutine getLinearCoeff( this, alpha, beta, gamma )

    class(RK) :: this
    type(scalar), intent(out)   :: alpha, beta, gamma
    integer :: k, i

    k = this % current_step
    i = this % current_stage

    if (this % second_order) then
       gamma = 1.0d0
       beta  = this % h * this%A(i,i)
       alpha = this % h * this%A(i,i)* this % h * this%A(i,i)
    else
       gamma = 0.0d0
       beta  = 1.0d0
       alpha = this % h * this % A(i,i)
    end if

  end subroutine getLinearCoeff

  !===================================================================!
  ! Approximate the state variables at each step using BDF formulae
  !===================================================================!

  subroutine approximateStates( this )

    class(RK) :: this
    integer    :: k, m, i

    k = this % current_step
    i = this % current_stage

    if (this % second_order) then

       ! guess qddot
       if (i .eq. 1) then ! copy previous global state
          this % QDDOT(k,i,:) = this % UDDOT(k-1,:)
       else ! copy previous local state
          this % QDDOT(k,i,:) = this % QDDOT(k,i-1,:)
       end if

       ! compute the stage velocity states for the guessed QDDOT
       forall(m = 1 : this % nsvars)
          this % QDOT(k,i,m) = this % udot(k-1,m) &
               & + this % h*sum(this % A(i,:)&
               & * this % QDDOT(k,:, m))
       end forall

       ! compute the stage states for the guessed QDDOT
       forall(m = 1 : this % nsvars)
          this % Q(k,i,m) = this % u(k-1,m) &
               & + this % h*sum(this % A(i,:)*this % QDOT(k,:, m))
       end forall

    else

       ! guess qdot
       if (i .eq. 1) then ! copy previous global state
          this % QDOT(k,i,:) = this % UDOT(k-1,:)
       else ! copy previous local state
          this % QDOT(k,i,:) = this % QDOT(k,i-1,:)
       end if

       ! compute the stage states for the guessed
       forall(m = 1 : this % nsvars)
          this % Q(k,i,m) = this % u(k-1,m) &
               & + this % h*sum(this % A(i,:)*this % QDOT(k,:, m))
       end forall

    end if

  end subroutine approximateStates

  !===================================================================!
  ! Function that puts together the right hand side of the adjoint
  ! equation into the supplied rhs vector.
  !===================================================================!

  subroutine assembleRHS(this, rhs)

    class(DIRK)                                :: this
    type(scalar), dimension(:), intent(inout) :: rhs
    type(scalar)                              :: scale
    integer                                   :: k, j, i, m, s, p
    type(scalar)                              :: alpha, beta, gamma

    k = this % current_step
    i = this % current_stage
    s = this % num_stages

    ! Replace from rhsbin for this step (contains k+ terms)
    ! rhs = this % rhsbin(k,i,:)
    !print *, k , i, rhs

    ! Calculate and add terms from k-th step
    call this % getLinearCoeff(alpha, beta, gamma)

    alpha = alpha* this % B(i)
    beta  = beta* this % B(i)
    gamma = gamma* this % B(i)

    ! Add the state variable sensitivity from the previous step
    call this % system % func % addFuncSVSens(rhs, &
         & alpha, beta, gamma, &
         & this % T(i), &
         & this % system % X, &
         & this % Q(k,i,:), &
         & this % Qdot(k,i,:), &
         & this % Qddot(k,i,:))

    ! Add contributions from psi
    rhs = rhs + this% B(i)*this % psi(k,:)

    ! Add contributions from PHI
    scale = 0.0d0
    do j = i, s
       scale = scale + this % h * this % B(j) * this % A(j,i)
    end do
    rhs = rhs + scale * this % phi(k,:)

    !-----------------------------------------------------------------!
    ! Add contributions from k to k- RHSBIN
    !-----------------------------------------------------------------!

    do j = i+1, s
!!$       gamma = 0.0d0
!!$       beta  = this % B(j) * this % h * this % A(j,i)
!!$       alpha = 0.0d0
!!$       do p = i, j
!!$          alpha = alpha + this % A(j,p) * this % A(p,i)
!!$       end do
!!$       alpha = alpha * this % h**2 * this % (j)

       gamma = 0.0d0
       beta  = this % B(j) * this % h * this % A(j,i)
       do p = i, j
          alpha = this % B(j) * this % h * this % h * this % A(j,p) * this % A(p,i)
       end do

       !! Need to fix this
       call this % addFuncResAdjPdt(rhs, &
            & alpha, beta, gamma, this % T(j), &
            & this % Q(k,j,:), this % Qdot(k,j,:), this % qddot(k,j,:), &
            & this % lam(k,j,:))

    end do

    ! Negate the RHS
    rhs = -rhs

  end subroutine assembleRHS

  !===================================================================!
  ! Compute the total derivative of the function with respect to the
  ! design variables and return the gradient
  !===================================================================!

  subroutine computeTotalDerivative( this, dfdx )

    class(DIRK)                                  :: this
    type(scalar) , dimension(:), intent(inout)   :: dfdx
    type(scalar) , dimension(:,:), allocatable   :: dRdX
    integer                                      :: k, j

    if (.not.allocated(dRdX)) allocate(dRdX(this % nSVars, this % nDVars))

    dRdX = 0.0d0
    dfdx = 0.0d0

    !-----------------------------------------------------------------!
    ! Compute dfdx
    !-----------------------------------------------------------------!

    ! always use scale
    do k = 2, this % num_steps
       do j = 1, this % num_stages
          call this % system % func % addFuncDVSens(dfdx, this % h * this % B(j),&
               & this % T(j),  &
               & this % system % x, this % Q(k,j,:), this % QDOT(k,j,:), &
               & this % QDDOT(k,j,:) )
       end do
    end do

    !-----------------------------------------------------------------!
    ! Compute the total derivative
    !-----------------------------------------------------------------!

    do k = 2, this % num_steps
       do j = 1, this % num_stages

          call this % system % getResidualDVSens(dRdX, this % h * this % B(j), this % T(j), &
               & this % system % x, this % Q(k,j,:), this % QDOT(k,j,:), this % QDDOT(k,j,:))

          dfdx = dfdx + matmul(this % lam(k,j,:), drdx) ! check order

       end do
    end do

    !-----------------------------------------------------------------!
    ! Special logic for initial condition (use the adjoint variable
    ! for the second time step)
    !-----------------------------------------------------------------!

!!$    call this % system % func % addFuncDVSens(dfdx, 1.0d0, this % time(1), &
!!$         & this % system % x, this % u(1,:), this % udot(1,:), this % uddot(2,:) )
!!$
!!$    call this % system % getResidualDVSens(dRdX, 1.0d0, this % time(1), &
!!$         & this % system % x, this % u(1,:), this % udot(1,:), this % uddot(2,:))
!!$    dfdx = dfdx + matmul(this % lam(2,:), dRdX)

    deallocate(dRdX)

  end subroutine computeTotalDerivative

  ! loop second order
  subroutine testAdjoint6(this, num_func, func, num_dv, x, dfdx, dfdxTmp)

    use function_class , only : abstract_function

    class(RK)                                  :: this
    class(abstract_function)                   :: func
    type(scalar), dimension(:), intent(inout)  :: x
    integer, intent(in)                        :: num_func, num_dv
    type(scalar), allocatable                  :: mat(:,:), tmpmat(:,:)
    type(scalar), allocatable                  :: rhs(:)
    type(scalar)                               :: alpha, beta, gamma
    type(scalar), allocatable                  :: dfdx(:), dfdxTmp(:)
    integer                                    :: size, info, ipiv
    type(scalar) , dimension(:,:), allocatable :: dRdX
    type(scalar)                               :: fvals, fvalstmp
    integer                                    :: k, ii, j, p

    allocate(rhs(this%nsvars))
    allocate(mat(this%nsvars, this%nsvars))
    allocate(tmpmat(this%nsvars, this%nsvars))

    !-----------------------------------------------------------------!
    ! Set the objective function into the system
    !-----------------------------------------------------------------!

    call this % system % setFunction(func)

    !-----------------------------------------------------------------!
    ! Set the number of variables, design variables into the system
    !-----------------------------------------------------------------!

    if (num_dv .ne. this % system % num_design_vars) stop "NDV mismatch"

    call this % system % setDesignVars(num_dv, x)

    this % nDVars = num_dv

    !-----------------------------------------------------------------!
    ! Integrate forward in time to solve for the state variables
    !-----------------------------------------------------------------!

    call this % integrate()

    do k = this % num_steps, 2, -1

       if (k .lt. this % num_steps) then

          rhs  = this % psi(k+1,:)

          do ii = 1, this % num_stages

             ! Add the psi terms
             alpha    = this % h * this % B(ii)
             beta     = 0.0d0
             gamma    = 0.0d0

             call this % system % assembleJacobian(mat, alpha, beta, gamma, &
                  & this % T(ii), this % Q(k+1,ii,:), this % qdot(k+1,ii,:), this % qddot(k+1,ii,:))

             rhs = rhs + matmul(transpose(mat),this % lam(k+1,ii,:))

             ! Add function contributions too
             call this % system % func % addFuncSVSens(rhs(1:1), alpha, beta, gamma, &
                  & this % T(ii), this % system % x, &
                  & this % Q(k+1,ii,:), this % qdot(k+1,ii,:), this % qddot(k+1,ii,:))

          end do

          this % psi(k,:) = rhs/1.0d0 ! note the positive sign

          !-----------------------------------------------------------!
          ! Add the phi terms
          !-----------------------------------------------------------!

          rhs  = this % phi(k+1,:)

          rhs = rhs + this % h * this % psi(k+1,:)

          do ii = 1, this % num_stages

             alpha = this % h * this % B(ii) * this % h * this % C(ii)
             beta  = this % h * this % B(ii)
             gamma = 0.0d0

             call this % system % assembleJacobian(mat, alpha, beta, gamma, &
                  & this % T(ii), this % Q(k+1,ii,:), this % qdot(k+1,ii,:), this % qddot(k+1,ii,:))

             rhs = rhs + matmul(transpose(mat), this % lam(k+1,ii,:))

             ! Add function contributions too
             call this % system % func % addFuncSVSens(rhs(1:1), alpha, beta, gamma, &
                  & this % T(ii), this % system % x, &
                  & this % Q(k+1,ii,:), this % qdot(k+1,ii,:), this % qddot(k+1,ii,:))

          end do

          this % phi(k,:) = rhs/1.0d0 ! note the positive sign

       else

          this % psi (k,:) = 0.0d0
          this % phi (k,:) = 0.0d0

       end if

!       print *, "PSI: ", k, this % psi(k,:)
!       print *, "PHI: ", k, this % phi(k,:)

       !-----------------------------------------------------------!
       ! Compute the stage adjoint variables
       !-----------------------------------------------------------!

       do ii = this % num_stages, 1, -1

          !-----------------------------------------------------------------!
          ! LAMBDA 22
          !-----------------------------------------------------------------!

          ! Assemble Jacobian
          mat      = 0.0d0

          alpha    = this % B(ii) * this % h * this % A(ii,ii) * this % h * this % A(ii,ii)
          beta     = this % B(ii) * this % h * this % A(ii,ii)
          gamma    = this % B(ii) * 1.0d0

          call this % system % assembleJacobian(mat, alpha, beta, gamma, &
               & this % T(ii), this % Q(k,ii,:), this % QDOT(k,ii,:), this % QDDOT(k,ii,:))

          ! Assemble RHS
          rhs = 0.0d0

          call this % system % func % addFuncSVSens(rhs, alpha, beta, gamma,  &
               & this % T(ii), this % system % X, &
               & this % Q(k,ii,:), this % QDOT(k,ii,:), this % QDDOT(k,ii,:))

          do j = ii+1, this % num_stages

             gamma = 0.0d0
             beta  = this % B(j) * this % h * this % A(j,ii)
             alpha = 0.0d0
             do p = ii, j
                alpha = alpha + this % A(j,p) * this % A(p,ii)
             end do
             alpha = alpha * this % h**2 * this % b(j)

             call this % system % assembleJacobian(tmpmat, alpha, beta, gamma, &
                  & this % T(j), this % Q(k,j,:), this % qdot(k,j,:), this % qddot(k,j,:))

             rhs(:) = rhs(:) + matmul(transpose(tmpmat(:,:)),this % lam(k,j,:))

             ! Add function contribution from next stage
             call this % system % func % addFuncSVSens(rhs, alpha, beta, gamma,  &
                  & this % T(j), this % system % x, &
                  & this % Q(k,j,:), this % qdot(k,j,:), this % qddot(k,j,:))

          end do

          rhs = rhs + this % B(ii) * this % phi(k,:)

          do j = ii, this % num_stages
             rhs = rhs + this % h * this % B(j) * this % A(j,ii) * this % psi(k,:)
          end do

          ! Solve for mu22
          this % lam(k,ii,:) = solve(mat, -rhs)

       end do

    end do

    ! Compute the adjoint total derivative
    call this % computeTotalDerivative(dfdx)

    deallocate(mat, tmpmat, rhs)

  end subroutine testAdjoint6

  subroutine evalFuncDIRK(this, x, fval)

    class(DIRK)                               :: this
    type(scalar), dimension(:), intent(in)    :: x
    type(scalar), intent(inout)               :: fval
    type(scalar)                              :: ftmp
    integer                                   :: k, j

    fval = 0.0d0
    ftmp = 0.0d0

!!$    do concurrent(k = 2 : this % num_steps)
!!$       call this % system % func % getFunctionValue(ftmp(k), this % time(k), &
!!$            & x, this % U(k,:), this % UDOT(k,:), this % UDDOT(k,:))
!!$    end do
!!$
!!$    ! fval = sum(ftmp)/dble(this % num_steps)
!!$    fval = this % h*sum(ftmp)
!!$
    do concurrent(k = 2 : this % num_steps)
       do concurrent(j = 1 : this % num_stages)
          call this % system % func % getFunctionValue(ftmp, this % T(j), x, &
               & this % Q(k,j,:), this % QDOT(k,j,:), this % QDDOT(k,j,:))
          fval = fval +  this % h * this % B(j) * ftmp
       end do
    end do

  end subroutine evalFuncDIRK

end module runge_kutta_integrator


program main
  use iso_fortran_env
  use modfnn
  implicit none
  !
  integer :: batch_size = 1
  integer :: batch_size_for_accuracy = 100
  integer :: mnist_data_count = 1000
  real(8) :: learning_rate = 0.1d0
  !
  type(network), allocatable :: net
  real(8), allocatable:: x(:, :)
  integer, allocatable :: batch_index(:)
  real(8), allocatable :: batch_label(:, :)
  real(8), allocatable :: batch_input(:, :)
  real(8), allocatable :: grad(:, :)
  type(matptr), allocatable :: labels(:)
  type(matptr), allocatable :: imgs(:)
  integer :: i

  allocate(batch_index(batch_size))

  call allocRandomMatrix(x, 2, 784)

  call read_MNIST(mnist_data_count, imgs, labels)
  call initWithTwoLayer(net, 784, 100, 10)
  !call disp(predictWithTwoLayer(net, x))

  do i = 1, 100
    ! 訓練用バッチを準備
    batch_index = random_integer_array(1, mnist_data_count, batch_size)
    call getBatchFromIndexList(labels, batch_label, batch_index)
    call getBatchFromIndexList(imgs, batch_input, batch_index)
    !call disp(batch_label)
    !call disp(maxloc(batch_label, 2))
    !call disp(net%W(2)%p)
    ! パラメータを勾配に基いて更新
    call num_grad_of_network(net, lossWithTwoLayer, batch_input, &
      net%W(2)%p, batch_label, grad)
    net%W(2)%p = net%W(2)%p - grad * learning_rate
    !
    call num_grad_of_network(net, lossWithTwoLayer, batch_input, &
      net%b(2)%p, batch_label, grad)
    net%b(2)%p = net%b(2)%p - grad * learning_rate
    ! 現時点の精度を表示
    batch_index = random_integer_array(1, mnist_data_count, batch_size_for_accuracy)
    call getBatchFromIndexList(labels, batch_label, batch_index)
    call getBatchFromIndexList(imgs, batch_input, batch_index)
    call disp(accuracyForBatch(net, batch_input, batch_label))

  end do

  !call getBatch(labels, )
contains
end program