! Copyright © 2021, UChicago Argonne, LLC, OPEN SOURCE LICENSE
! See (root dir/)LICENSE.TXT

!  Basic Hartree-Fock Proxy Application
!  Written by Graham D. Fletcher, Computational Science Division,
!  Argonne National Laboratory
!  Computes the 2-electron (repulsion) term of the Fock operator
!  for a cluster of helium-like atoms

 module params
 real(8)      pi
 parameter  ( pi = 3.1415926535897931d0 )
 real(8)      sqrpi2
 parameter  ( sqrpi2 = ( pi**( -0.5d0 ) )*2.0d0 )
 real(8)      dtol,          rcut
!parameter  ( dtol= 1.0d-10, rcut= 1.0d-12 )
 parameter  ( dtol= 1.0d-12, rcut= 1.0d-12 )
 real(8)      tobohrs
 parameter  ( tobohrs = 1.889725987722d0 )
 end module params

 program   basic_hf_proxy_seq
 use       params
 implicit  none
 integer   natom,ngauss, i,j,ij,k,l,kl, lx
 real(8),  allocatable ::  xpnt( : )
 real(8),  allocatable ::  coef( : )
 real(8),  allocatable ::  geom( : , : )
 real(8),  allocatable ::  fock( : , : )
 real(8),  allocatable ::  dens( : , : )
 real(8),  allocatable ::  schwarz( : )
 real(8)   eri, erep

! ngauss is the number of gaussian-type functions (GTF) per atom.
! The GTF are specified by their exponents (xpnt) and contracted
! using the expansion coefficients (coef) to make an orbital which
! is occupied by a pair of opposite-spin electrons.
! Use ngauss (,xpnt,coef) to adjust the per-atom workload.
! natom is the number of helium-like atoms.
 read *,  ngauss, natom
 allocate( xpnt( ngauss ) )
 allocate( coef( ngauss ) )
 allocate( geom( 3, natom ) )

 read *,  ( xpnt( i ), coef( i ), i = 1, ngauss )
 do  i  =  1,  natom
 read *,  ( geom( j, i ), j = 1, 3 )
 end do

 allocate( dens( natom, natom ) )

! fake ('random') density
 do  i  =  1,  natom
 do  j  =  1,  natom
 dens( i, j )  =  0.1d0
 end do
 dens( i, i )  =  1.0d0
 end do

! OR input a density (triangle)
!do  i  =  1,  natom
!read *, ( dens( i, j ), j  =  1,  i  )
!do  j  =  1,  i
!dens( j, i ) = dens( i, j )
!end do
!end do

! normalize the primitive GTO weights
 do i = 1, ngauss
 coef( i ) = coef( i )*( ( 2.0d0*xpnt( i ) )**0.75d0 )
 end do

!  scale the geometry to Bohrs for energy calculations in AU
 do  i  =  1,  natom
 geom( 1, i )  =  geom( 1, i )*tobohrs
 geom( 2, i )  =  geom( 2, i )*tobohrs
 geom( 3, i )  =  geom( 3, i )*tobohrs
 end do

 allocate( fock( natom, natom ) )

 do  i  =  1,  natom
 do  j  =  1,  natom
 fock( i, j )  =  0.0d0
 end do
 end do

! compute Schwarz Inequality factors for integral screening
 allocate( schwarz( ((natom**2)+natom)/2  ) )

 ij = 0
 do  i  =  1,  natom
 do  j  =  1,  i
 ij = ij + 1
 call ssss( i, j, i, j, ngauss, xpnt, coef, geom, eri )
 schwarz( ij )  =  sqrt( abs( eri ) )
 end do
 end do

!  this loop-structure reflects the 8-fold label symmetry of the integrals

 ij = 0
 do  i  =  1,  natom
 do  j  =  1,  i
 ij = ij + 1
 kl = 0
 do  k  =  1,  i
 lx  =  k
 if ( k == i ) lx  =  j
 do  l  =  1,  lx
 kl = kl + 1
 if ( schwarz( ij )*schwarz( kl ) > dtol ) then
 call ssss( i, j, k, l, ngauss, xpnt, coef, geom, eri )   ! main kernel
 if ( i == j ) eri = eri*0.5d0
 if ( k == l ) eri = eri*0.5d0
 if ( i == k .and. j == l ) eri = eri*0.5d0
 fock( i, j )  =  fock( i, j )  +dens( k, l )*eri*4.0d0
 fock( k, l )  =  fock( k, l )  +dens( i, j )*eri*4.0d0
 fock( i, k )  =  fock( i, k )  -dens( j, l )*eri
 fock( i, l )  =  fock( i, l )  -dens( j, k )*eri
 fock( j, k )  =  fock( j, k )  -dens( i, l )*eri
 fock( j, l )  =  fock( j, l )  -dens( i, k )*eri
 end if
 end do  ;  end do
 end do  ;  end do

! it is customary to symmetrize the Fock matrix,
! but this is not necessary just to compute the 2e- energy
!do  i  =  1,  natom
!do  j  =  1,  i - 1
!fock( i, j ) = ( fock( i, j ) + fock( j, i ) )*0.5d0
!fock( j, i ) = fock( i, j )
!end do
!end do
!  print Fock (triangle)
!do  i  =  1,  natom
!print *, ( fock( i, j ), j  =  1,  i  )
!end do

!  trace Fock with the density, print the 2e- energy
 erep  =  0.0d0
 do  i  =  1,  natom
 do  j  =  1,  natom
 erep  =  erep  +  fock( i, j )*dens( i, j )
 end do
 end do
 print *, '2e- energy= ',erep*0.5d0

 deallocate( schwarz )
 deallocate( xpnt )
 deallocate( coef )
 deallocate( dens )
 deallocate( fock )
 deallocate( geom )
 end program basic_hf_proxy_seq

 subroutine ssss( i, j, k, l, ngauss, xpnt, coef, geom, eri )
 use        params
 implicit   none
 integer    i,j,k,l, ngauss
 real(8)    xpnt(*), coef(*), geom(3,*), eri
 integer    ib,jb,kb,lb
 real(8)    aij,dij,xij,yij,zij, akl,dkl, aijkl,tt,f0t

!  loops over contracted GTOs

 eri  =  0.0d0
 do  ib  =  1,  ngauss
 do  jb  =  1,  ngauss
 aij = 1.0d0/( xpnt( ib ) + xpnt( jb ) )
 dij = coef( ib )*coef( jb )*exp( -xpnt( ib )*xpnt( jb )*aij*  &
     ( ( geom( 1, i ) - geom( 1, j ) )**2   &
     + ( geom( 2, i ) - geom( 2, j ) )**2   &
     + ( geom( 3, i ) - geom( 3, j ) )**2  )  )*( aij**1.5d0 )
 if ( abs( dij ) > dtol ) then
 xij = aij*( xpnt( ib )*geom( 1, i ) + xpnt( jb )*geom( 1, j ) )
 yij = aij*( xpnt( ib )*geom( 2, i ) + xpnt( jb )*geom( 2, j ) )
 zij = aij*( xpnt( ib )*geom( 3, i ) + xpnt( jb )*geom( 3, j ) )
 do  kb  =  1,  ngauss
 do  lb  =  1,  ngauss
 akl = 1.0d0/( xpnt( kb ) + xpnt( lb ) )
 dkl = dij*coef( kb )*coef( lb )*exp( -xpnt( kb )*xpnt( lb )*akl*  &
     ( ( geom( 1, k ) - geom( 1, l ) )**2   &
     + ( geom( 2, k ) - geom( 2, l ) )**2   &
     + ( geom( 3, k ) - geom( 3, l ) )**2  )  )*( akl**1.5d0 )
 if ( abs( dkl ) > dtol ) then
 aijkl = ( xpnt( ib ) + xpnt( jb ) )*( xpnt( kb ) + xpnt( lb ) )  &
       / ( xpnt( ib ) + xpnt( jb )  +  xpnt( kb ) + xpnt( lb ) )
 tt = aijkl*( ( xij -akl*( xpnt( kb )*geom( 1, k ) + xpnt( lb )*geom( 1, l ) ) )**2  &
            + ( yij -akl*( xpnt( kb )*geom( 2, k ) + xpnt( lb )*geom( 2, l ) ) )**2  &
            + ( zij -akl*( xpnt( kb )*geom( 3, k ) + xpnt( lb )*geom( 3, l ) ) )**2  )
 f0t  =  sqrpi2
 if ( tt > rcut )  f0t  =  ( tt**( -0.5d0 ) )*erf( sqrt(tt) )
 eri  =  eri  +  dkl*f0t*sqrt(aijkl)
 end if
 end do  ;  end do
 end if
 end do  ;  end do
 end subroutine ssss


      function bulgedens(r,z)
      psi = pot(r,z)
      bulgedens = bulgedenspsi(psi)

      return
      end

      function bulgedenspsi(energy)

      include 'commonblocks'

      if(energy.lt.psic) then
         bulgedenspsi = 0.
         return
      endif
      if(energy.ge.psi0) then
         bulgedenspsi = denspsibulge(1)
         return
      endif

      rj = 1. + float(npsi-1)*
     +     log((psi0-energy)/(psi0-psid))/log((psi0-psic)/(psi0-psid))
      j = int(rj)
      if(j.lt.1) j = 1
      if(j.ge.npsi) j = npsi-1

      frac = rj - float(j)
      bulgedenspsi = denspsibulge(j) +
     +     frac*(denspsibulge(j+1)-denspsibulge(j))
      return

      end


module simann
  use bklib, only : dp,setseed,runif
  use smglob, only : smglobal, save_sa_checkpoint
  implicit none
  private
  private :: exprep,prt1,prt2,prt3,prt4,prt5,prt6,prt7,prt8,prt9,prt10,prtvec
  public :: sa

contains

!  Version: 3.2
!  Date: 1/22/94.
!  Differences compared to Version 2.0:
!     1. If a trial is out of bounds, a point is randomly selected
!        from LB(i) to UB(i). Unlike in version 2.0, this trial is
!        evaluated and is counted in acceptances and rejections.
!        All corresponding documentation was changed as well.
!  Differences compared to Version 3.0:
!     1. If VM(i) > (UB(i) - LB(i)), VM is set to UB(i) - LB(i).
!        The idea is that if T is high relative to LB & UB, most
!        points will be accepted, causing VM to rise. But, in this
!        situation, VM has little meaning; particularly if VM is
!        larger than the acceptable region. Setting VM to this size
!        still allows all parts of the allowable region to be selected.
!  Differences compared to Version 3.1:
!     1. Test made to see if the initial temperature is positive.
!     2. WRITE statements prettied up.
!     3. References to paper updated.
!
!  Synopsis:
!  This routine implements the continuous simulated annealing global
!  optimization algorithm described in Corana et al.'s article
!  "Minimizing Multimodal Functions of Continuous Variables with the
!  "Simulated Annealing" Algorithm" in the September 1987 (vol. 13,
!  no. 3, pp. 262-280) issue of the ACM Transactions on Mathematical
!  Software.
!
!  A very quick (perhaps too quick) overview of SA:
!     SA tries to find the global optimum of an N dimensional function.
!  It moves both up and downhill and as the optimization process
!  proceeds, it focuses on the most promising area.
!     To start, it randomly chooses a trial point within the step length
!  VM (a vector of length N) of the user selected starting point. The
!  function is evaluated at this trial point and its value is compared
!  to its value at the initial point.
!     In a maximization problem, all uphill moves are accepted and the
!  algorithm continues from that trial point. Downhill moves may be
!  accepted; the decision is made by the Metropolis criteria. It uses T
!  (temperature) and the size of the downhill move in a probabilistic
!  manner. The smaller T and the size of the downhill move are, the more
!  likely that move will be accepted. If the trial is accepted, the
!  algorithm moves on from that point. If it is rejected, another point
!  is chosen instead for a trial evaluation.
!     Each element of VM periodically adjusted so that half of all
!  function evaluations in that direction are accepted.
!     A fall in T is imposed upon the system with the RT variable by
!  T(i+1) = RT*T(i) where i is the ith iteration. Thus, as T declines,
!  downhill moves are less likely to be accepted and the percentage of
!  rejections rise. Given the scheme for the selection for VM, VM falls.
!  Thus, as T declines, VM falls and SA focuses upon the most promising
!  area for optimization.
!
!  The importance of the parameter T:
!     The parameter T is crucial in using SA successfully. It influences
!  VM, the step length over which the algorithm searches for optima. For
!  a small intial T, the step length may be too small; thus not enough
!  of the function might be evaluated to find the global optima. The user
!  should carefully examine VM in the intermediate output (set IPRINT =
!  1) to make sure that VM is appropriate. The relationship between the
!  initial temperature and the resulting step length is function
!  dependent.
!     To determine the starting temperature that is consistent with
!  optimizing a function, it is worthwhile to run a trial run first. Set
!  RT = 1.5 and T = 1.0. With RT > 1.0, the temperature increases and VM
!  rises as well. Then select the T that produces a large enough VM.
!
!  For modifications to the algorithm and many details on its use,
!  (particularly for econometric applications) see Goffe, Ferrier
!  and Rogers, "Global Optimization of Statistical Functions with
!  Simulated Annealing," Journal of Econometrics, vol. 60, no. 1/2,
!  Jan./Feb. 1994, pp. 65-100.
!  For more information, contact
!              Bill Goffe
!              Department of Economics and International Business
!              University of Southern Mississippi
!              Hattiesburg, MS  39506-5072
!              (601) 266-4484 (office)
!              (601) 266-4920 (fax)
!              bgoffe@whale.st.usm.edu (Internet)
!
!  As far as possible, the parameters here have the same name as in
!  the description of the algorithm on pp. 266-8 of Corana et al.
!
!  In this description, SP is single precision, DP is double precision,
!  INT is integer, L is logical and (N) denotes an array of length n.
!  Thus, DP(N) denotes a double precision array of length n.
!
!  Input Parameters:
!    Note: The suggested values generally come from Corana et al. To
!          drastically reduce runtime, see Goffe et al., pp. 90-1 for
!          suggestions on choosing the appropriate RT and NT.
!    X - The starting values for the variables of the function to be
!        optimized. (DP(N))
!    MAX - Denotes whether the function should be maximized or
!          minimized. A true value denotes maximization while a false
!          value denotes minimization. Intermediate output (see IPRINT)
!          takes this into account. (L)
!    RT - The temperature reduction factor. The value suggested by
!         Corana et al. is .85. See Goffe et al. for more advice. (DP)
!    EPS - Error tolerance for termination. If the final function
!          values from the last neps temperatures differ from the
!          corresponding value at the current temperature by less than
!          EPS and the final function value at the current temperature
!          differs from the current optimal function value by less than
!          EPS, execution terminates and IER = 0 is returned. (EP)
!    NS - Number of cycles. After NS*N function evaluations, each
!         element of VM is adjusted so that approximately half of
!         all function evaluations are accepted. The suggested value
!         is 20. (INT)
!    NT - Number of iterations before temperature reduction. After
!         NT*NS*N function evaluations, temperature (T) is changed
!         by the factor RT. Value suggested by Corana et al. is
!         MAX(100, 5*N). See Goffe et al. for further advice. (INT)
!    NEPS - Number of final function values used to decide upon termi-
!           nation. See EPS. Suggested value is 4. (INT)
!    MAXEVL - The maximum number of function evaluations. If it is
!             exceeded, IER = 1. (INT)
!    LB - The lower bound for the allowable solution variables. (DP(N))
!    UB - The upper bound for the allowable solution variables. (DP(N))
!         If the algorithm chooses X(I) .LT. LB(I) or X(I) .GT. UB(I),
!         I = 1, N, a point is from inside is randomly selected. This
!         This focuses the algorithm on the region inside UB and LB.
!         Unless the user wishes to concentrate the search to a par-
!         ticular region, UB and LB should be set to very large positive
!         and negative values, respectively. Note that the starting
!         vector X should be inside this region. Also note that LB and
!         UB are fixed in position, while VM is centered on the last
!         accepted trial set of variables that optimizes the function.
!    C - Vector that controls the step length adjustment. The suggested
!        value for all elements is 2.0. (DP(N))
!    IPRINT - controls printing inside SA. (INT)
!             Values: 0 - Nothing printed.
!                     1 - Function value for the starting value and
!                         summary results before each temperature
!                         reduction. This includes the optimal
!                         function value found so far, the total
!                         number of moves (broken up into uphill,
!                         downhill, accepted and rejected), the
!                         number of out of bounds trials, the
!                         number of new optima found at this
!                         temperature, the current optimal X and
!                         the step length VM. Note that there are
!                         N*NS*NT function evalutations before each
!                         temperature reduction. Finally, notice is
!                         is also given upon achieveing the termination
!                         criteria.
!                     2 - Each new step length (VM), the current optimal
!                         X (XOPT) and the current trial X (X). This
!                         gives the user some idea about how far X
!                         strays from XOPT as well as how VM is adapting
!                         to the function.
!                     3 - Each function evaluation, its acceptance or
!                         rejection and new optima. For many problems,
!                         this option will likely require a small tree
!                         if hard copy is used. This option is best
!                         used to learn about the algorithm. A small
!                         value for MAXEVL is thus recommended when
!                         using IPRINT = 3.
!             Suggested value: 1
!             Note: For a given value of IPRINT, the lower valued
!                   options (other than 0) are utilized.
!
!  Input/Output Parameters:
!    T - On input, the initial temperature. See Goffe et al. for advice.
!        On output, the final temperature. (DP)
!
!  Output Parameters:
!    XOPT - The variables that optimize the function. (DP(N))
!    FOPT - The optimal value of the function. (DP)
!    NACC - The number of accepted function evaluations. (INT)
!    NFCNEV - The total number of function evaluations. In a minor
!             point, note that the first evaluation is not used in the
!             core of the algorithm; it simply initializes the
!             algorithm. (INT).
!    NOBDS - The total number of trial function evaluations that
!            would have been out of bounds of LB and UB. Note that
!            a trial point is randomly selected between LB and UB.
!            (INT)
!    IER - The error return number. (INT)
!          Values: 0 - Normal return; termination criteria achieved.
!                  1 - Number of function evaluations (NFCNEV) is
!                      greater than the maximum number (MAXEVL).
!                  2 - The starting value (X) is not inside the
!                      bounds (LB and UB).
!                  3 - The initial temperature is not positive.
!                  99 - Should not be seen; only used internally.
!
!
!  Required Functions (included):
!    EXPREP - Replaces the function EXP to avoid under- and overflows.
!             It may have to be modified for non IBM-type main-
!             frames. (DP)
!
!  Required Subroutines (included):
!    PRTVEC - Prints vectors.
!    PRT1 ... PRT10 - Prints intermediate output.
!    FCN - Function to be optimized. The form is
!            SUBROUTINE FCN(N,X,F)
!            INTEGER N
!            DOUBLE PRECISION  X(N), F
!            ...
!            function code with F = F(X)
!            ...
!            RETURN
!            END
!          Note: This is the same form used in the multivariable
!          minimization algorithms in the IMSL edition 10 library.
!
!  Machine Specific Features:
!    1. EXPREP may have to be modified if used on non-IBM type main-
!       frames. Watch for under- and overflows in EXPREP.
!    2. Some FORMAT statements use G25.18; this may be excessive for
!       some machines.

  subroutine sa(x,max,rt,eps,ns,nt,neps,maxevl,lb,ub,c,iprint, &
       t,xopt,fopt,nacc,nfcnev,nobds,ier,fcn)

!  Type all external variables.
    real(kind=DP), dimension(:), intent(inout) :: x
    real(kind=DP), dimension(size(x)), intent(in) :: lb,ub,c
    real(kind=DP), dimension(size(x)), intent(out) :: xopt
    real(kind=DP), intent(inout) :: t
    real(kind=DP), intent(in) :: eps, rt
    real(kind=DP), intent(out) :: fopt
    integer, intent(in) ::  ns, nt, neps,  maxevl, iprint
    integer, intent(out) :: nacc,nfcnev,nobds,ier
    logical, intent(in) :: max

!  Type all internal variables.
    real(kind=DP), dimension(neps) ::  fstar
    real(kind=DP), dimension(size(x)) ::  xp,vm
    integer, dimension(size(x)) :: nacp
    real(kind=DP) :: f, fp, p, pp, ratio, shift
    integer :: n, nup, ndown, nrej, nnew, lnobds, h, i, j, m, ios
    logical :: quit

!  Type all functions.
!      DOUBLE PRECISION  EXPREP
!      REAL  RANMAR
    interface
!       function fcn(p) result (fout)
       subroutine fcn(p,fout) ! new
         use bklib
         real(kind=dp), dimension(:), intent(in) :: p
!         real(kind=dp) :: fout
         real(kind=dp), intent(inout) :: fout ! new
!       end function fcn
       end subroutine fcn ! new
    end interface

!  Initialize the random number generator RANMAR. (deleted)
!  call rmarin(iseed1,iseed2)
    call setseed()

!  Set initial values.
    n = size(x)
    nacc = 0
    nobds = 0
    nfcnev = 0
    ier = 99
    xopt = x
    nacp = 0
    fstar = 1.0e20_dp
    vm = ub-lb

!  If the initial temperature is not positive, notify the user and
!  return to the calling routine.
    if (t <= 0.0_dp) then
       open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
       if (ios == 0) then
          write(unit=1,iostat=ios,fmt=*) "  THE INITIAL TEMPERATURE IS NOT POSITIVE. "
          write(unit=1,iostat=ios,fmt=*) "  RESET THE VARIABLE T. "
          close(unit=1,iostat=ios)
       else
          continue
       end if
       ier = 3
       return
    end if
!  If the initial value is out of bounds, notify the user and return
!  to the calling routine.
    if (any(x > ub) .or. any(x < lb)) then
       call prt1()
       ier = 2
       return
    end if

!  Evaluate the function with input X and return value as F.
!    f = fcn(x)
    call fcn(x,f) ! new

!  If the function is to be minimized, switch the sign of the function.
!  Note that all intermediate and final output switches the sign back
!  to eliminate any possible confusion for the user.
    if(.not. max) f = -f
    nfcnev = nfcnev + 1
    fopt = f
    fstar(1) = f
    if(iprint >= 1) call prt2(max,n,x,f)

!  Start the main loop. Note that it terminates if (i) the algorithm
!  succesfully optimizes the function or (ii) there are too many
!  function evaluations (more than MAXEVL).
    do
       nup = 0
       nrej = 0
       nnew = 0
       ndown = 0
       lnobds = 0
       do m = 1, nt
          do j = 1, ns
             do h = 1, n

!  Generate XP, the trial value of X. Note use of VM to choose XP.
                do i = 1, n
                   if (i == h) then
                      call runif(shift)
                      xp(i) = x(i) + (shift*2.0_dp- 1.0_dp) * vm(i)
                   else
                      xp(i) = x(i)
                   end if

!  If XP is out of bounds, select a point in bounds for the trial.
                   if((xp(i) < lb(i)) .or. (xp(i) > ub(i))) then
                      call runif(shift)
                      xp(i) = lb(i) + (ub(i) - lb(i))*shift
                      lnobds = lnobds + 1
                      nobds = nobds + 1
                      if(iprint >= 3) then
                         call prt3(max,n,xp,x,fp,f)
                      end if
                   end if
                end do

!  Evaluate the function with the trial point XP and return as FP.
!                fp= fcn(xp)
                call fcn(xp,fp)
                if(.not. max) fp = -fp
                nfcnev = nfcnev + 1
                if(iprint >= 3) call prt4(max,n,xp,x,fp,f)

!  If too many function evaluations occur, terminate the algorithm.
                if(nfcnev >= maxevl) then
                   call prt5()
                   if (.not. max) then
                      fopt = -fopt
                   end if
                   ier = 1
                   return
                end if

!  Accept the new point if the function value increases.
                if(fp >= f) then
                   if(iprint >= 3) then
                      open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
                      if (ios == 0) then
                         write(unit=1,iostat=ios,fmt=*) "  POINT ACCEPTED"
                         close (unit=1,iostat=ios)
                      else
                         continue
                      end if
                   end if
                   x=xp
                   f = fp
                   nacc = nacc + 1
                   nacp(h) = nacp(h) + 1
                   nup = nup + 1

!  If greater than any other point, record as new optimum.
                   if (fp > fopt) then
                      if(iprint >= 3) then
                         open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
                         if (ios == 0) then
                            write(unit=1,iostat=ios,fmt=*) "  NEW OPTIMUM"
                            close (unit=1,iostat=ios)
                         else
                            continue
                         end if
                      end if
                      xopt=xp
                      fopt = fp
                      nnew = nnew + 1
                   end if

!  If the point is lower, use the Metropolis criteria to decide on
!  acceptance or rejection.
                else
                   p = exprep((fp - f)/t)
                   call runif(pp)
                   if (pp < p) then
                      if(iprint >= 3) call prt6(max)
                      x=xp
                      f = fp
                      nacc = nacc + 1
                      nacp(h) = nacp(h) + 1
                      ndown = ndown + 1
                   else
                      nrej = nrej + 1
                      if(iprint >= 3) then
                         call prt7(max)
                      end if
                   end if
                end if
             end do
          end do

!  Adjust VM so that approximately half of all evaluations are accepted.
          do i = 1, n
             ratio = real(nacp(i),kind=DP) /real(ns,kind=DP)
             if (ratio > 0.6_dp) then
                vm(i) = vm(i)*(1.0_dp + c(i)*(ratio - 0.6_dp)/0.4_dp)
             else if (ratio < 0.4_dp) then
                vm(i) = vm(i)/(1.0_dp + c(i)*((0.4_dp - ratio)/0.4_dp))
             end if
             if (vm(i) > (ub(i)-lb(i))) then
                vm(i) = ub(i) - lb(i)
             end if
          end do

          if(iprint >= 2) then
             call prt8(n,vm,xopt,fopt,x)
          end if

          nacp=0

       end do

       if(iprint >= 1) then
          call prt9(max,n,t,xopt,vm,fopt,nup,ndown,nrej,lnobds,nnew)
       end if

!  Check termination criteria.
       quit = .false.
       fstar(1) = f
       if ((fopt - fstar(1)) <= eps) quit = .true.
       do i = 1, neps
          if (abs(f - fstar(i)) > eps) quit = .false.
       end do
       if (fopt > (-SMGLOBAL%DFPSTOP)) then
          open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
          if (ios == 0) then
             write (unit=1,iostat=ios,fmt=*) "fret < dfpstop"
             close (unit=1,iostat=ios)
          else
             continue
          end if
          quit = .true.
       end if

!  Terminate SA if appropriate.
       if (quit) then
          x=xopt
          ier = 0
          if (.not. max) fopt = -fopt
          if(iprint >= 1) call prt10()
          return
       end if

!  If termination criteria is not met, prepare for another loop.
       t = rt*t
       call save_sa_checkpoint(t,xopt)
       do i = neps, 2, -1
          fstar(i) = fstar(i-1)
       end do

       f = fopt
       x=xopt

!  Loop again.
    end do

  end subroutine sa

function  exprep(rdum) result (exprepout)
!  This function replaces exp to avoid under- and overflows and is
!  designed for IBM 370 type machines. It may be necessary to modify
!  it for other machines. Note that the maximum and minimum values of
!  EXPREP are such that they has no effect on the algorithm.

  real(kind=DP), intent(in) :: rdum
  real(kind=DP) :: exprepout

  if (rdum > 174.0_dp) then
     exprepout = 3.69e+75_dp
  else if (rdum < -180.0_dp) then
     exprepout = 0.0_dp
  else
     exprepout = exp(rdum)
  end if

end function exprep

!subroutine rmarin(ij,kl)
!  This subroutine and the next function generate random numbers. See
!  the comments for SA for more information. The only changes from the
!  orginal code is that (1) the test to make sure that RMARIN runs first
!  was taken out since SA assures that this is done (this test didn't
!  compile under IBM's VS Fortran) and (2) typing ivec as integer was
!  taken out since ivec isn't used. With these exceptions, all following
!  lines are original.

! This is the initialization routine for the random number generator
!     RANMAR()
! NOTE: The seed variables can have values between:    0 <= IJ <= 31328
!                                                      0 <= KL <= 30081
!  real :: U(97), C, CD, CM
!  integer :: I97, J97
!  common /raset1/ U, C, CD, CM, I97, J97
!  if( IJ < 0  .or.  IJ .gt. 31328  .or. KL .lt. 0  .or.  KL .gt. 30081 ) then
!     print "(A)", " The first random number seed must have a value between 0 and 31328"
!     print "(A)"," The second seed must have a value between 0 and 30081"
!     stop
!  endif
!  i = mod(IJ/177, 177) + 2
!  j = mod(IJ    , 177) + 2
!  k = mod(KL/169, 178) + 1
!  l = mod(KL,     169)
!  do ii = 1, 97
!     s = 0.0
!     t = 0.5
!     do jj = 1, 24
!        m = mod(mod(i*j, 179)*k, 179)
!        i = j
!        j = k
!        k = m
!        l = mod(53*l+1, 169)
!        if (mod(l*m, 64) >= 32) then
!           s = s + t
!        endif
!        t = 0.5 * t
!     end do
!     U(ii) = s
!  end do
!  C = 362436.0 / 16777216.0
!  CD = 7654321.0 / 16777216.0
!  CM = 16777213.0 /16777216.0
!  I97 = 97
!  J97 = 33
!end subroutine rmarin

!function ranmar()
!  real :: U(97), C, CD, CM
!  integer :: I97, J97
!  common /raset1/ U, C, CD, CM, I97, J97
!  uni = U(I97) - U(J97)
!  if( uni < 0.0 ) uni = uni + 1.0
!  U(I97) = uni
!  I97 = I97 - 1
!  if(I97 == 0) I97 = 97
!  J97 = J97 - 1
!  if(J97 == 0) J97 = 97
!  C = C - CD
!  if( C .lt. 0.0 ) C = C + CM
!  uni = uni - C
!  if( uni .lt. 0.0 ) uni = uni + 1.0
!  ranmar = uni
!end function ranmar

subroutine prt1()
!  This subroutine prints intermediate output, as does PRT2 through
!  PRT10. Note that if SA is minimizing the function, the sign of the
!  function value and the directions (up/down) are reversed in all
!  output to correspond with the actual function optimization. This
!  correction is because SA was written to maximize functions and
!  it minimizes by maximizing the negative a function.
  integer :: ios
  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write (unit=1,iostat=ios,fmt=*) "  THE STARTING VALUE (X) IS OUTSIDE THE BOUNDS "
     write (unit=1,iostat=ios,fmt=*) "  (LB AND UB). EXECUTION TERMINATED WITHOUT ANY"
     write (unit=1,iostat=ios,fmt=*) "  OPTIMIZATION. RESPECIFY X, UB OR LB SO THAT  "
     write (unit=1,iostat=ios,fmt=*) "  LB(I) < X(I) < UB(I), I = 1, N. "
     close (unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt1

subroutine prt2(max,n,x,f)
  real(kind=DP), dimension(:), intent(in)  ::  x
  real(kind=DP), intent(in) ::  f
  integer, intent(in)  :: n
  logical, intent(in) ::  max
  integer :: ios

  call prtvec(x,n,"INITIAL X")
  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     if (max) then
        write(unit=1,iostat=ios,fmt=*) "  INITIAL F: ", f
     else
        write(unit=1,iostat=ios,fmt=*) "  INITIAL F: ", -f
     end if
     close(unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt2

subroutine prt3(max,n,xp,x,fp,f)

  real(kind=DP), dimension(:), intent(in) ::  xp, x
  real(kind=DP), intent(in) ::  fp, f
  integer, intent(in) ::  n
  logical, intent(in) ::  max
  integer :: ios

  call prtvec(x,n,"CURRENT X")
  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     if (max) then
        write(unit=1,iostat=ios,fmt=*) "  CURRENT F: ", f
     else
        write(unit=1,iostat=ios,fmt=*) "  CURRENT F: ", -f
     end if
     close(unit=1,iostat=ios)
  else
     continue
  end if
  call prtvec(xp,n,"TRIAL X")
  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) "  POINT REJECTED SINCE OUT OF BOUNDS"
     close(unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt3

subroutine prt4(max,n,xp,x,fp,f)

  real(kind=DP), dimension(:), intent(in)  ::  xp, x
  real(kind=DP), intent(in)   ::  fp, f
  integer, intent(in)   :: n
  logical, intent(in)   :: max
  integer :: ios

  call prtvec(x,n,"CURRENT X")
  if (max) then
     open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
     if (ios == 0) then
        write(unit=1,iostat=ios,fmt=*) "  CURRENT F: ",f
        close (unit=1,iostat=ios)
     else
        continue
     end if
     call prtvec(xp,n,"TRIAL X")
     open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
     if (ios == 0) then
        write(unit=1,iostat=ios,fmt=*) "  RESULTING F: ",fp
        close (unit=1,iostat=ios)
     else
        continue
     end if
  else
     open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
     if (ios == 0) then
        write(unit=1,iostat=ios,fmt=*) "  CURRENT F: ",-f
        close (unit=1,iostat=ios)
     else
        continue
     end if
     call prtvec(xp,n,"TRIAL X")
     open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
     if (ios == 0) then
        write(unit=1,iostat=ios,fmt=*) "  RESULTING F: ",-fp
        close (unit=1,iostat=ios)
     else
        continue
     end if
  end if

end subroutine prt4

subroutine prt5()

  integer :: ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) "  TOO MANY FUNCTION EVALUATIONS; CONSIDER "
     write(unit=1,iostat=ios,fmt=*) "  INCREASING MAXEVL OR EPS, OR DECREASING "
     write(unit=1,iostat=ios,fmt=*) "  NT OR RT. THESE RESULTS ARE LIKELY TO BE "
     write(unit=1,iostat=ios,fmt=*) "  POOR."
     close(unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt5

subroutine prt6(max)
  logical, intent(in)   ::  max
  integer :: ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     if (max) then
        write(unit=1,iostat=ios,fmt=*) "  THOUGH LOWER, POINT ACCEPTED"
     else
        write(unit=1,iostat=ios,fmt=*) "  THOUGH HIGHER, POINT ACCEPTED"
     end if
     close (unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt6

subroutine prt7(max)

  logical, intent(in)   :: max
  integer :: ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     if (max) then
        write(unit=1,iostat=ios,fmt=*) "  LOWER POINT REJECTED"
     else
        write(unit=1,iostat=ios,fmt=*) "  HIGHER POINT REJECTED"
     end if
     close(unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt7

subroutine prt8(n,vm,xopt,fopt,x)

  real(kind=DP), dimension(:), intent(in) :: vm, xopt, x
  real(kind=DP), intent(in) :: fopt
  integer, intent(in) :: n
  integer :: ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) " INTERMEDIATE RESULTS AFTER STEP LENGTH ADJUSTMENT"
     close(unit=1,iostat=ios)
  else
     continue
  end if
  call prtvec(vm,n,"NEW STEP LENGTH (VM)")
  call prtvec(xopt,n,"CURRENT OPTIMAL X")
  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) " CURRENT OPTIMAL F(X): ",fopt
     close(unit=1,iostat=ios)
  else
     continue
  end if
  call prtvec(x,n,"CURRENT X")

end subroutine prt8

subroutine prt9(max,n,t,xopt,vm,fopt,nup,ndown,nrej,lnobds,nnew)

  real(kind=DP), dimension(:), intent(in) ::  xopt, vm
  real(kind=DP), intent(in) :: t, fopt
  integer, intent(in) :: n, nup, ndown, nrej, lnobds, nnew
  logical, intent(in) :: max
  integer :: totmov,ios

  totmov = nup + ndown + nrej

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) " INTERMEDIATE RESULTS BEFORE NEXT TEMPERATURE REDUCTION"
     write(unit=1,iostat=ios,fmt=*) "  CURRENT TEMPERATURE:  ",t
     if (max) then
        write(unit=1,iostat=ios,fmt=*) "  MAX FUNCTION VALUE SO FAR: ",fopt
        write(unit=1,iostat=ios,fmt=*) "  TOTAL MOVES:               ", totmov
        write(unit=1,iostat=ios,fmt=*) "    UPHILL:                  ", nup
        write(unit=1,iostat=ios,fmt=*) "    ACCEPTED DOWNHILL:       ", ndown
        write(unit=1,iostat=ios,fmt=*) "    REJECTED DOWNHILL:       ", nrej
        write(unit=1,iostat=ios,fmt=*) "    OUT OF BOUNDS TRIALS:    ", lnobds
        write(unit=1,iostat=ios,fmt=*) "  NEW MAXIMA THIS TEMPERATURE:", nnew
     else
        write(unit=1,iostat=ios,fmt=*) "  MIN FUNCTION VALUE SO FAR: ",-fopt
        write(unit=1,iostat=ios,fmt=*) "  TOTAL MOVES:               ", totmov
        write(unit=1,iostat=ios,fmt=*) "    DOWNHILL:                ", nup
        write(unit=1,iostat=ios,fmt=*) "    ACCEPTED UPHILL:       ", ndown
        write(unit=1,iostat=ios,fmt=*) "    REJECTED UPHILL:       ", nrej
        write(unit=1,iostat=ios,fmt=*) "    OUT OF BOUNDS TRIALS:    ", lnobds
        write(unit=1,iostat=ios,fmt=*) "  NEW MINIMA THIS TEMPERATURE:", nnew
     end if
     close(unit=1,iostat=ios)
  else
     continue
  end if
  call prtvec(xopt,n,"CURRENT OPTIMAL X")
  call prtvec(vm,n,"STEP LENGTH (VM)")
end subroutine prt9

subroutine prt10()

  integer :: ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) "  SA ACHIEVED TERMINATION CRITERIA. IER = 0. "
     close(unit=1,iostat=ios)
  else
     continue
  end if

end subroutine prt10

subroutine prtvec(vector,ncols,name)
!  This subroutine prints the double precision vector named VECTOR.
!  Elements 1 thru NCOLS will be printed. NAME is a character variable
!  that describes VECTOR. Note that if NAME is given in the call to
!  PRTVEC, it must be enclosed in quotes. If there are more than 10
!  elements in VECTOR, 10 elements will be printed on each line.

  integer, intent(in) :: ncols
  real(kind=DP), dimension(ncols), intent(in) :: vector
!  character *(*) name
  character(len=*), intent(in) :: name
  integer :: lines,i,ll,j,ios

  open (unit=1,file=SMGLOBAL%LOGFILE,iostat=ios,action="write",position="append",status="old")
  if (ios == 0) then
     write(unit=1,iostat=ios,fmt=*) name
     if (ncols > 10) then
        lines = floor(real(ncols)/10.0) !  formerly   lines = int(ncols/10.)
        do  i = 1, lines
           ll = 10*(i - 1)
           write(unit=1,iostat=ios,fmt=*) (VECTOR(J),J = 1+LL, 10+LL)
        end do
        write(unit=1,iostat=ios,fmt=*) (VECTOR(J),J = 11+LL, NCOLS)
     else
        write(unit=1,iostat=ios,fmt=*) (VECTOR(J),J = 1, NCOLS)
     end if
     close(unit=1,iostat=ios)
  else
     continue
  end if

! 1000 format( 10(g12.5,1x))

end subroutine prtvec

end module simann


SUBROUTINE dclisb0_pretreatment(A, N, NUD, N1, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    implicit none
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS
    INTEGER:: N, NUD, N1 ,IER
    COMPLEX(dp)::XX, S, SUM, AU, T
    double precision:: EPS1
    INTEGER:: I ,M, J, K1, MJ, K
    ! CHECK THE INPUT DATA
    IER = 0
    EPS1 = 1.0D-14
    M = NUD + 1
    IF ( N <= 0 .OR. NUD <= 0 .OR. N1 < M ) THEN
        IER = 2
        WRITE(*,*) '(SUBR. LISB) INVALID ARGUMENT. ', N, NUD, N1
        RETURN
    ENDIF
    IF (EPS <= 0.0) EPS = EPS1
    ! MODIFIED CHOLESKY DECOMPOSITION
    J = 1
    IF (CDABS(A(M,1)) <= EPS) THEN
        IER = 1
        WRITE(*,*) '(SUBR. LISB) SINGULAR AT STEP # ', J
        RETURN
    ENDIF
    DR(1) = DCMPLX(1.0D0) / A(M,1)
    XX = A(M-1,2)
    A(M-1,2) = A(M-1,2) * DR(1)
    S = A(M,2) - XX * A(M-1,2)
    J = 2
    IF (CDABS(S) <= EPS) THEN
        IER = 1
        WRITE(*,*) '(SUBR. LISB) SINGULAR AT STEP # ', J
        RETURN
    ENDIF
    DR(2) = 1 / S
    IF (M < 3) THEN
        DO J=3,N
            XX = A(1,J)
            A(1,J) = XX * DR(J-1)
            S = A(2,J) - XX * A(1,J)
            IF (CDABS(S) <= EPS) THEN
                IER = 1
                WRITE(*,*) ' (SUBR. LISB) SINGULAR AT STEP # ', J
                RETURN
            ENDIF
            DR(J) = 1 / S
        enddo
    ELSE
        DO J=3,N
            K1 = 1
            IF (J >= M) K1 = J - M + 1
            MJ = M - J
            DO I=K1+1,J-1
                SUM = 0
                DO K=K1,I-1
                    SUM = SUM + A(M-I+K,I) * A(MJ+K,J)
                enddo
                A(MJ+I,J) = A(MJ+I,J) - SUM
            enddo
            SUM = 0
            DO I=K1,J-1
                XX = A(MJ+I,J)
                AU = XX * DR(I)
                SUM = SUM + XX *AU
                A(MJ+I,J) = AU
            enddo
            T = A(M,J) - SUM
            IF (CDABS(T) <= EPS) THEN
                IER = 1
                WRITE(*,*) ' (SUBR. LISB) SINGULAR AT STEP # ', J
                RETURN
            ENDIF
            DR(J) = 1 / T
        enddo
    ENDIF
    RETURN
END

SUBROUTINE DCLISB0(A, N, NUD, N1, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    implicit none
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS
    INTEGER:: N, NUD, N1 ,IER

    call dclisb0_pretreatment(A, N, NUD, N1, B, EPS, DR, Z, IER)
    ENTRY DCSBSUB0(A, N, NUD, N1, B, EPS, DR, Z, IER)
    !  FORWARD SUBSTITUTION
    call dclisb0_kenja(A, N, NUD, N1, B, EPS, DR, Z, IER)
    RETURN
END

SUBROUTINE dclisb0_kenja(A, N, NUD, N1, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    implicit none
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS
    INTEGER:: N, NUD, N1 ,IER
    COMPLEX (dp)::SUM
    INTEGER:: M, J, I1, K, J1
    !  FORWARD SUBSTITUTION
    M = NUD + 1
    IF (M < 3) THEN
        Z(1) = B(1)
        DO J=2,N
            Z(J) = B(J) - A(1,J) * Z(J-1)
        enddo
        DO J=1,N
            Z(J) = Z(J) * DR(J)
        enddo
        B(N) = Z(N)
        DO J=1,N-1
            B(N-J) = Z(N-J) - A(1,N-J+1) * B(N-J+1)
        enddo
    ELSE
        Z(1) = B(1)
        Z(2) = B(2) - A(M-1,2) * Z(1)
        DO J=3,N
            IF (J > M) THEN
                I1 = 1
            ELSE
                I1 = M - J + 1
            ENDIF
            SUM = 0
            DO K=I1,M-1
                SUM = SUM + A(K,J) * Z(J-M+K)
            enddo
            Z(J) = B(J) - SUM
        enddo
        DO  J=1,N
            Z(J) = Z(J) * DR(J)
        enddo

        B(N) = Z(N)
        B(N-1) = Z(N-1) - A(M-1,N) * Z(N)
        DO J=3,N
            J1 = N - J + 1
            I1 = 1
            IF (J < M) I1 = M - J + 1
            SUM = 0
            DO K=I1,M-1
                SUM = SUM + A(K,M-K+J1) * B(M-K+J1)
            enddo
            B(J1) = Z(J1) - SUM
        enddo
    ENDIF
    RETURN
END

SUBROUTINE dclisb_pretreatment(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    implicit none
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS, EPS1
    INTEGER:: N, NUD, N1, NP, IER
    DOUBLE COMPLEX:: XX, S, SUM, AU, T
    INTEGER:: I ,M, J, K1, MJ, K
    !  CHECK THE INPUT DATA
    IER = 0
    EPS1 = 1.0D-14
    M = NUD + 1
    IF ( N <= 0 .OR. NUD <= 0 .OR. N1 < M)  THEN
        IER = 2
        WRITE(*,*) '(SUBR. LISB) INVALID ARGUMENT. ', N, NUD, N1
        RETURN
    ENDIF
    IF (EPS <= 0.0) EPS = EPS1
    ! MODIFIED CHOLESKY DECOMPOSITION
    J = 1
    IF (CDABS(A(M,1)) <= EPS) THEN
        IER = 1
        WRITE(*,*) '(SUBR. LISB) SINGULAR AT STEP # ', J
        RETURN
    ENDIF
    DR(1) = 1 / A(M,1)
    XX = A(M-1,2)
    A(M-1,2) = A(M-1,2) * DR(1)
    S = A(M,2) - XX * A(M-1,2)
    J = 2
    IF (CDABS(S) <= EPS) THEN
        IER = 1
        WRITE(*,*) '(SUBR. LISB) SINGULAR AT STEP # ', J
        RETURN
    ENDIF
    DR(2) = 1 / S
    IF (M < 3) THEN
        DO J=3,N
            XX = A(1,J)
            A(1,J) = XX * DR(J-1)
            S = A(2,J) - XX * A(1,J)
            IF (CDABS(S) <= EPS) THEN
                IER = 1
                WRITE(*,*) ' (SUBR. LISB) SINGULAR AT STEP # ', J
                RETURN
            ENDIF
            DR(J) = 1 / S
        enddo
    ELSE
        DO J=3,N
            K1 = 1
            IF (J >= M) K1 = J - M + 1
            MJ = M - J
            DO  I=K1+1,J-1
                SUM = 0
                DO K=K1,I-1
                    SUM = SUM + A(M-I+K,I) * A(MJ+K,J)
                enddo
                A(MJ+I,J) = A(MJ+I,J) - SUM
            enddo
            SUM = 0
            DO  I=K1,J-1
                XX = A(MJ+I,J)
                AU = XX * DR(I)
                SUM = SUM + XX *AU
                A(MJ+I,J) = AU
            enddo
            T = A(M,J) - SUM
            IF (CDABS(T) <= EPS) THEN
                IER = 1
                WRITE(*,*) ' (SUBR. LISB) SINGULAR AT STEP # ', J
                RETURN
            ENDIF
            DR(J) = 1 / T
        enddo
    ENDIF

    RETURN
END

SUBROUTINE DCLISB(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS
    INTEGER:: N, NUD, N1, NP, IER

    call dclisb_pretreatment(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)
    ! SUBTITUTION
    ENTRY DCSBSUB(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)
    call dclisb_kenja(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)

    RETURN
END

SUBROUTINE dclisb_kenja(A, N, NUD, N1, NP, B, EPS, DR, Z, IER)
!************************************************************************
!*  SIMULTANEOUS LINEAR EQUATIONS WITH REAL SYMMETRIC POSITIVE DEFINITE *
!*      BAND MATRIX BY CHOLESKY METHOD.                                 *
!*  PARAMETERS                                                          *
!*    (1) A : 2-DIM. ARRAY CONTAINING THE MATRIX.                       *
!*    (2) N : ORDER OF THE MATRIX.                                      *
!*    (3) NUD : SIZE OF BAND'S HALF WIDTH.                              *
!*    (4) N1 : ROW SIZE OF THE ARRAY A IN THE 'DIMENSION' STATEMENT.    *
!*    (5) B : 1-DIM. ARRAY CONTAINING THE RIGHT HAND SIDE VECTOR.       *
!*    (6) EPS : PARAMETER TO CHECK SINGURARITY OFF THE MATRIX           *
!*              STANDARD VALUE = 1.0D-14                                *
!*    (7) DR : 1-DIM. WORKING ARRAY.                                    *
!*    (8) Z : 1-DIM. WORKING ARRAY.                                     *
!*    (9) IER : ERROR CODE.                                             *
!*  COPY RIGHT   T. OGUNI   JULY 30 1989   VERSION 1.0                  *
!* modified Kensuke Konishi 2018 in Paris
!************************************************************************
    use parameters
    COMPLEX(dp):: A(N1,N), B(N), DR(N), Z(N)
    double precision:: EPS
    INTEGER:: N, NUD, N1, NP, IER
    DOUBLE COMPLEX:: SUM
    INTEGER:: M, J, I1, K
    ! FORWARD SUBSTITUTION
    M = NUD + 1
    IF (M < 3) THEN
        Z(NP) = B(NP)
        DO J=NP+1,N
            Z(J) = B(J) - A(1,J) * Z(J-1)
        enddo
        B(N) = Z(N) * DR(N)
    ELSE
        Z(NP) = B(NP)
        Z(NP+1) = B(NP+1) - A(M-1,NP+1) * Z(NP)
        DO J=NP+2,N
            IF (J > NP-1+M) THEN
                I1 = 1
            ELSE
                I1 = NP-1+M - J + 1
            ENDIF
            SUM = 0
            DO K=I1,M-1
                SUM = SUM + A(K,J) * Z(J-M+K)
            enddo
            Z(J) = B(J) - SUM
        enddo
        DO J=N-1,N
            Z(J) = Z(J) * DR(J)
        enddo

        B(N) = Z(N)
        B(N-1) = Z(N-1) - A(M-1,N) * Z(N)
    ENDIF

    RETURN
END


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C
C   FFTPACK 5.0
C   Copyright (C) 1995-2004, Scientific Computing Division,
C   University Corporation for Atmospheric Research
C   Licensed under the GNU General Public License (GPL)
C
C   Authors:  Paul N. Swarztrauber and Richard A. Valent
C
C   $Id: sint1i.f,v 1.2 2006-11-21 01:10:20 haley Exp $
C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC

      SUBROUTINE DSINT1I(N,WSAVE,LENSAV,IER)
      DOUBLE PRECISION PI
      DOUBLE PRECISION DT
      INTEGER N,LENSAV,IER
      DOUBLE PRECISION WSAVE(LENSAV)
C
      IER = 0
C
      IF (LENSAV.LT.N/2+N+INT(LOG(DBLE(N)))+4) THEN
          IER = 2
          CALL DXERFFT('SINT1I',3)
          GO TO 300
      END IF
C
      PI = 4.D0*ATAN(1.D0)
      IF (N.LE.1) RETURN
      NS2 = N/2
      NP1 = N + 1
      DT = PI/DBLE(NP1)
      DO 101 K = 1,NS2
          WSAVE(K) = 2.D0*SIN(K*DT)
  101 CONTINUE
      LNSV = NP1 + INT(LOG(DBLE(NP1))) + 4
      CALL DRFFT1I(NP1,WSAVE(NS2+1),LNSV,IER1)
      IF (IER1.NE.0) THEN
          IER = 20
          CALL DXERFFT('SINT1I',-5)
      END IF
C
  300 CONTINUE
      RETURN
      END


!
!   -- MAGMA (version 2.1.0) --
!      Univ. of Tennessee, Knoxville
!      Univ. of California, Berkeley
!      Univ. of Colorado, Denver
!      @date August 2016
!
!  @generated from testing/testing_zgetrf_f.f90, normal z -> c, Tue Aug 30 09:39:19 2016
!
      program testing_cgetrf_f

      use magma

      external slamch, clange, cgemm, cgesv, cgetrs

      real clange, slamch

      real              :: rnumber(2), Anorm, Bnorm, Xnorm, Rnorm
      real, allocatable :: work(:)
      complex, allocatable       :: A(:), B(:), X(:)
      complex, allocatable       :: A2(:)
      integer,    allocatable       :: ipiv(:)

      complex                    :: c_one, c_neg_one
      integer                       :: i, n, info, lda
      integer                       :: nrhs
      real(kind=8)                  :: flops, t, tstart, tend

      PARAMETER          ( nrhs = 1, c_one = 1., c_neg_one = -1. )

      call cublas_init()

      n   = 2048
      lda = n

!------ Allocate CPU memory
      allocate(A(lda*n))
      allocate(A2(lda*n))
      allocate(B(lda*nrhs))
      allocate(X(lda*nrhs))
      allocate(ipiv(n))
      allocate(work(n))

!---- Initializa the matrix
      do i=1,n*n
         call random_number(rnumber)
         A(i) = rnumber(1)
      end do
      A2(:) = A(:)

      do i=1,n*nrhs
         call random_number(rnumber)
         B(i) = rnumber(1)
      end do
      X(:) = B(:)

!---- Call magma LU ----------------
      call magmaf_wtime(tstart)
      call magmaf_cgetrf(n, n, A, lda, ipiv, info)
      call magmaf_wtime(tend)

      if ( info .ne. 0 )  then
         write(*,*) "Info : ", info
      end if

!---- Call solve -------------
      call cgetrs('n', n, nrhs, A, lda, ipiv, X, lda, info)

      if ( info .ne. 0 )  then
         write(*,*) "Info : ", info
      end if

!---- Compare the two results ------
      Anorm = clange('I', n, n,    A2, lda, work)
      Bnorm = clange('I', n, nrhs, B,  lda, work)
      Xnorm = clange('I', n, nrhs, X,  lda, work)

      call cgemm('n', 'n', n,  nrhs, n, c_one, A2, lda, X, lda, c_neg_one, B, lda)
      Rnorm = clange('I', n, nrhs, B, lda, work)

      write(*,*)
      write(*,*  ) 'Solving A x = b using LU factorization:'
      write(*,105) '  || A || = ', Anorm
      write(*,105) '  || b || = ', Bnorm
      write(*,105) '  || x || = ', Xnorm
      write(*,105) '  || b - A x || = ', Rnorm

      flops = 2. * n * n * n / 3.
      t = tend - tstart

      write(*,*)   '  Gflops  = ',  flops / t / 1e9
      write(*,*)

      Rnorm = Rnorm / ( (Anorm*Xnorm+Bnorm) * n * slamch('E') )

      if ( Rnorm > 60. ) then
         write(*,105) '  Solution is suspicious, ', Rnorm
      else
         write(*,105) '  Solution is CORRECT'
      end if

!---- Free CPU memory
      deallocate(A, A2, B, X, ipiv, work)

!---- Free GPU memory
      call cublas_shutdown()

 105  format((a35,es10.3))

      end


module mod_fillps
  use mod_types
  implicit none
  private
  public fillps
  contains
  subroutine fillps(n,dli,dzfi,dti,up,vp,wp,p)
    !@cuf use cudafor
    !
    !  fill the right-hand side of the Poisson equation for the correction pressure.
    !
    !  the discrete divergence is:
    !
    !  w(i,j,k)-w(i,j,k-1)   v(i,j,k)-v(i,j-1,k)   u(i,j,k)-u(i-1,j,k)
    !  ------------------- + ------------------- + -------------------  = div
    !          dz                    dy                    dx
    !
    implicit none
    integer , intent(in), dimension(3) :: n
    real(rp), intent(in), dimension(3) :: dli
    real(rp), intent(in), dimension(0:) :: dzfi
    real(rp), intent(in) :: dti
    real(rp), intent(in ), dimension(0:,0:,0:) :: up,vp,wp
    real(rp), intent(out), dimension(0:,0:,0:) :: p
    real(rp) :: dtidxi,dtidyi!,dtidzi
    !real(rp), dimension(0:n(3)+1) :: dtidzfi
#ifdef USE_CUDA
    attributes(managed) :: up,vp,wp,p,dzfi!,dtidzfi
    integer :: istat
#endif
    integer :: i,j,k,im,jm,km
    !
    dtidxi = dti*dli(1)
    dtidyi = dti*dli(2)
#ifdef USE_CUDA
   !$cuf kernel do(3) <<<*,*>>>
#else
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP SHARED(n,p,up,vp,wp,dti,dzfi,dtidyi,dtidxi) &
    !$OMP PRIVATE(i,j,k,im,jm,km)
#endif
    do k=1,n(3)
      km = k-1
      do j=1,n(2)
        jm = j-1
        do i=1,n(1)
          im = i-1
          p(i,j,k) = ( &
                      (wp(i,j,k)-wp(i,j,km))*dti*dzfi(k)+ &
                      (vp(i,j,k)-vp(i,jm,k))*dtidyi    + &
                      (up(i,j,k)-up(im,j,k))*dtidxi    )
        enddo
      enddo
    enddo
#ifndef USE_CUDA
    !$OMP END PARALLEL DO
#endif
    !@cuf istat=cudaDeviceSynchronize()
    !
    return
  end subroutine fillps
end module mod_fillps


      DO WHILE(F*F <= LST)         !But, F*F might overflow the integer limit so instead,
      DO WHILE(F <= LST/F)                      !Except, LST might also overflow the integer limit, so
      DO WHILE(F <= (IST + 2*(SBITS - 1))/F)    !Which becomes...
      DO WHILE(F <= IST/F + (MOD(IST,F) + 2*(SBITS - 1))/F) !Preserving the remainder from IST/F.


module solv_cap
  integer,       private, save :: Ng1=0, Ng2=0
contains
  subroutine FourirG(G)
    real, intent(in out), dimension(0:,0:) :: G
    complex, allocatable, dimension(:,:)   :: t
    allocate( t(0:2*Ng1-1,0:2*Ng2-1) )
    t(0:Ng1,0:Ng2-1)    = G(:,0:Ng2-1)      ! Fill one quadrant (one extra row)
    t(0:Ng1,Ng2:2*Ng2-1) = G(:,Ng2:1:-1)    ! This quadrant using symmetry
  end subroutine FourirG
end module solv_cap


C
        SUBROUTINE FBRUS(NNN,X,Y,F,RPAR,IPAR)
        IMPLICIT REAL*8 (A-H,O-Z)
        DIMENSION Y(NNN),F(NNN)
        COMMON/PARAM/N,N2,GAMMA,GAMMA2
        I=1
        IU=2*I-1
        IV=2*I
        UI=Y(IU)
        VI=Y(IV)
            UIM=1.D0
            VIM=3.D0
            UIP=Y(IU+2)
            VIP=Y(IV+2)
        PROD=UI*UI*VI
        F(IU)=1.D0+PROD-4.D0*UI+GAMMA*(UIM-2.D0*UI+UIP)
        F(IV)=3.D0*UI-PROD+GAMMA*(VIM-2.D0*VI+VIP)
        DO 5 I=2,N-1
        IU=2*I-1
        IV=2*I
        UI=Y(IU)
        VI=Y(IV)
            UIM=Y(IU-2)
            VIM=Y(IV-2)
            UIP=Y(IU+2)
            VIP=Y(IV+2)
        PROD=UI*UI*VI
        F(IU)=1.D0+PROD-4.D0*UI+GAMMA*(UIM-2.D0*UI+UIP)
        F(IV)=3.D0*UI-PROD+GAMMA*(VIM-2.D0*VI+VIP)
    5   CONTINUE
        I=N
        IU=2*I-1
        IV=2*I
        UI=Y(IU)
        VI=Y(IV)
            UIM=Y(IU-2)
            VIM=Y(IV-2)
            UIP=1.D0
            VIP=3.D0
        PROD=UI*UI*VI
        F(IU)=1.D0+PROD-4.D0*UI+GAMMA*(UIM-2.D0*UI+UIP)
        F(IV)=3.D0*UI-PROD+GAMMA*(VIM-2.D0*VI+VIP)
        RETURN
        END
C
        SUBROUTINE JBRUS(NN,X,Y,DFY,LDFY,RPAR,IPAR)
        IMPLICIT REAL*8 (A-H,O-Z)
        DIMENSION Y(NN),DFY(LDFY,NN)
        COMMON/PARAM/N,N2,GAMMA,GAMMA2
        DO 1 I=1,N
        IU=2*I-1
        IV=2*I
        UI=Y(IU)
        VI=Y(IV)
        UIVI=UI*VI
        UI2=UI*UI
        DFY(3,IU)=2.D0*UIVI-4.D0-GAMMA2
        DFY(2,IV)=UI2
        DFY(4,IU)=3.D0-2.D0*UIVI
        DFY(3,IV)=-UI2-GAMMA2
        DFY(2,IU)=0.D0
        DFY(4,IV)=0.D0
    1   CONTINUE
        DO 2 I=1,N2-2
        DFY(1,I+2)=GAMMA
        DFY(5,I)=GAMMA
    2   CONTINUE
        RETURN
        END


!----------------------------------------------------------------------
!----------------------------------------------------------------------
!   r3b :    The Restricted 3-Body Problem
!----------------------------------------------------------------------
!----------------------------------------------------------------------

 SUBROUTINE FUNC(NDIM,U,ICP,PAR,IJAC,F,DFDU,DFDP)
!---------- ----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM, IJAC, ICP(*)
   DOUBLE PRECISION, INTENT(IN) :: U(NDIM), PAR(*)
   DOUBLE PRECISION, INTENT(OUT) :: F(NDIM), DFDU(NDIM,*), DFDP(NDIM,*)

   DOUBLE PRECISION x,y,z,xp,yp,zp,rmu,T,dE,dM,rmc,dE3,dM3

   x  = U(1)
   y  = U(2)
   z  = U(3)
   xp = U(4)
   yp = U(5)
   zp = U(6)

   rmu = PAR(2)
   T   = PAR(11)

   dE  = SQRT((x+rmu)**2 + y**2 + z**2)
   dM  = SQRT( (x-1+rmu)**2 + y**2 + z**2 )
   rmc = 1 - rmu
   dE3 = 1./dE**3
   dM3 = 1./dM**3

   F(1)= xp
   F(2)= yp
   F(3)= zp
   F(4)= 2*yp + x - rmc*dE3*(x+rmu) - rmu*dM3*(x-1+rmu)
   F(5)=-2*xp + y - rmc*dE3*y       - rmu*dM3*y
   F(6)=          - rmc*dE3*z       - rmu*dM3*z

   F = T*F

 END SUBROUTINE FUNC
!----------------------------------------------------------------------
 SUBROUTINE STPNT
 END SUBROUTINE STPNT
!----------------------------------------------------------------------
 SUBROUTINE BCND(NDIM,PAR,ICP,NBC,U0,U1,FB,IJAC,DBC)
!---------- ----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM, ICP(*), NBC, IJAC
   DOUBLE PRECISION, INTENT(IN) :: PAR(*), U0(NDIM), U1(NDIM)
   DOUBLE PRECISION, INTENT(OUT) :: FB(NBC), DBC(NBC,*)

   DOUBLE PRECISION eps,x,y,z,xp,yp,zp,rmu,dE,dM,U,E
   INTEGER m

   eps=PAR(6)
!  PAR(25:30) are USTART and PAR(31:36) are VSTART
   m=MIN(NBC,NDIM)
   FB(1:m) =  U0(1:m) - ( PAR(25:25+m-1) + eps*PAR(31:31+m-1) )

   FB(NDIM+1:NDIM+3) = U1(1:3) - PAR(21:23)

   x  = U1(1)
   y  = U1(2)
   z  = U1(3)
   xp = U1(4)
   yp = U1(5)
   zp = U1(6)

   rmu = PAR(2)

   dE = SQRT((x+rmu)**2 + y**2 + z**2)
   dM = SQRT( (x-1+rmu)**2 + y**2 + z**2 )

   U = (x**2 + y**2)/2 + (1-rmu)/dE + rmu/dM
   E = (xp**2 + yp**2 + zp**2)/2 - U - rmu*(1-rmu)/2
   FB(NDIM+4) = PAR(3) - E

 END SUBROUTINE BCND
!----------------------------------------------------------------------
 SUBROUTINE ICND(NDIM,PAR,ICP,NINT,U,UOLD,UDOT,UPOLD,FI,IJAC,DINT)
!---------- ----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM, ICP(*), NINT, IJAC
   DOUBLE PRECISION, INTENT(IN) :: PAR(*)
   DOUBLE PRECISION, INTENT(IN) :: U(NDIM), UOLD(NDIM), UDOT(NDIM), UPOLD(NDIM)
   DOUBLE PRECISION, INTENT(OUT) :: FI(NINT), DINT(NINT,*)

   DOUBLE PRECISION FF(NDIM),DFDU(1),DFDP(1)

   CALL FUNC(NDIM,U,ICP,PAR,0,FF,DFDU,DFDP)
   FI(1)=SQRT(FF(1)**2 + FF(2)**2 + FF(3)**2 ) - PAR(12)

 END SUBROUTINE ICND
!----------------------------------------------------------------------
 SUBROUTINE FOPT
 END SUBROUTINE FOPT
!----------------------------------------------------------------------
 SUBROUTINE PVLS
 END SUBROUTINE PVLS
!----------------------------------------------------------------------
!----------------------------------------------------------------------


      SUBROUTINE MB01RT( UPLO, TRANS, N, ALPHA, BETA, R, LDR, E, LDE,
     $                   X, LDX, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrix formula
C
C        R := alpha*R + beta*op( E )*X*op( E )',
C
C     where alpha and beta are scalars, R and X are symmetric matrices,
C     E is an upper triangular matrix, and op( E ) is one of
C
C        op( E ) = E   or   op( E ) = E'.
C
C     The result is overwritten on R.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     UPLO    CHARACTER*1
C             Specifies which triangles of the symmetric matrices R
C             and X are given as follows:
C             = 'U':  the upper triangular part is given;
C             = 'L':  the lower triangular part is given.
C
C     TRANS   CHARACTER*1
C             Specifies the form of op( E ) to be used in the matrix
C             multiplication as follows:
C             = 'N':  op( E ) = E;
C             = 'T':  op( E ) = E';
C             = 'C':  op( E ) = E'.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrices R, E, and X.  N >= 0.
C
C     ALPHA   (input) DOUBLE PRECISION
C             The scalar alpha. When alpha is zero then R need not be
C             set before entry, except when R is identified with X in
C             the call.
C
C     BETA    (input) DOUBLE PRECISION
C             The scalar beta. When beta is zero then E and X are not
C             referenced.
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,N)
C             On entry with UPLO = 'U', the leading N-by-N upper
C             triangular part of this array must contain the upper
C             triangular part of the symmetric matrix R.
C             On entry with UPLO = 'L', the leading N-by-N lower
C             triangular part of this array must contain the lower
C             triangular part of the symmetric matrix R.
C             In both cases, the other strictly triangular part is not
C             referenced.
C             On exit, the leading N-by-N upper triangular part (if
C             UPLO = 'U'), or lower triangular part (if UPLO = 'L'), of
C             this array contains the corresponding triangular part of
C             the computed matrix R.
C
C     LDR     INTEGER
C             The leading dimension of array R.  LDR >= MAX(1,N).
C
C     E       (input) DOUBLE PRECISION array, dimension (LDE,N)
C             On entry, the leading N-by-N upper triangular part of this
C             array must contain the upper triangular matrix E.
C             The remaining part of this array is not referenced.
C
C     LDE     INTEGER
C             The leading dimension of array E.  LDE >= MAX(1,N).
C
C     X       (input) DOUBLE PRECISION array, dimension (LDX,N)
C             On entry, if UPLO = 'U', the leading N-by-N upper
C             triangular part of this array must contain the upper
C             triangular part of the symmetric matrix X and the strictly
C             lower triangular part of the array is not referenced.
C             On entry, if UPLO = 'L', the leading N-by-N lower
C             triangular part of this array must contain the lower
C             triangular part of the symmetric matrix X and the strictly
C             upper triangular part of the array is not referenced.
C             The diagonal elements of this array are modified
C             internally, but are restored on exit.
C
C     LDX     INTEGER
C             The leading dimension of array X.  LDX >= MAX(1,N).
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             This array is not referenced when beta = 0, or N = 0.
C
C     LDWORK  The length of the array DWORK.
C             LDWORK >= N*N, if  beta <> 0;
C             LDWORK >= 0,   if  beta =  0.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -k, the k-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The matrix expression is efficiently evaluated taking the symmetry
C     into account. Specifically, let X = U + L, with U and L upper and
C     lower triangular matrices, defined by
C
C        U = triu( X ) - (1/2)*diag( X ),
C        L = tril( X ) - (1/2)*diag( X ),
C
C     where triu, tril, and diag denote the upper triangular part, lower
C     triangular part, and diagonal part of X, respectively. Then,
C     if UPLO = 'U',
C
C        E*X*E' = ( E*U )*E' + E*( E*U )',  for TRANS = 'N',
C        E'*X*E = E'*( U*E ) + ( U*E )'*E,  for TRANS = 'T', or 'C',
C
C     and if UPLO = 'L',
C
C        E*X*E' = ( E*L' )*E' + E*( E*L' )',  for TRANS = 'N',
C        E'*X*E = E'*( L'*E ) + ( L'*E )'*E,  for TRANS = 'T', or 'C',
C
C     which involve operations like in BLAS 2 and 3 (DTRMV and DSYR2K).
C     This approach ensures that the matrices E*U, U*E, E*L', or L'*E
C     are upper triangular.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires approximately N**3/2 operations.
C
C     CONTRIBUTORS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Apr. 2019.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Elementary matrix operations, matrix algebra, matrix operations.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO, HALF
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0,
     $                  HALF = 0.5D0 )
C     .. Scalar Arguments ..
      CHARACTER         TRANS, UPLO
      INTEGER           INFO, LDE, LDR, LDWORK, LDX, N
      DOUBLE PRECISION  ALPHA, BETA
C     .. Array Arguments ..
      DOUBLE PRECISION  DWORK(*), E(LDE,*), R(LDR,*), X(LDX,*)
C     .. Local Scalars ..
      LOGICAL           LTRANS, LUPLO
      INTEGER           I, J
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DLASCL, DLASET, DSCAL, DTRMV,
     $                  MB01OT, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      INFO = 0
      LUPLO  = LSAME( UPLO,  'U' )
      LTRANS = LSAME( TRANS, 'T' ) .OR. LSAME( TRANS, 'C' )
C
      IF(      ( .NOT.LUPLO  ).AND.( .NOT.LSAME( UPLO,  'L' ) ) )THEN
         INFO = -1
      ELSE IF( ( .NOT.LTRANS ).AND.( .NOT.LSAME( TRANS, 'N' ) ) )THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDR.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDE.LT.1 .OR. ( LTRANS .AND. LDE.LT.N ) .OR.
     $                  ( .NOT.LTRANS .AND. LDE.LT.N ) ) THEN
         INFO = -9
      ELSE IF( LDX.LT.MAX( 1, N ) ) THEN
         INFO = -11
      ELSE IF( ( BETA.NE.ZERO .AND. LDWORK.LT.N*N )
     $     .OR.( BETA.EQ.ZERO .AND. LDWORK.LT.0 ) ) THEN
         INFO = -13
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB01RT', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 )
     $   RETURN
C
      IF ( BETA.EQ.ZERO ) THEN
         IF ( ALPHA.EQ.ZERO ) THEN
C
C           Special case alpha = 0.
C
            CALL DLASET( UPLO, N, N, ZERO, ZERO, R, LDR )
         ELSE
C
C           Special case beta = 0.
C
            IF ( ALPHA.NE.ONE )
     $         CALL DLASCL( UPLO, 0, 0, ONE, ALPHA, N, N, R, LDR, INFO )
         END IF
         RETURN
      END IF
C
C     General case: beta <> 0.
C     Compute W = E*T or W = T*E in DWORK, and apply the updating
C     formula (see METHOD section).
C     Workspace: need N*N.
C
      CALL DSCAL( N, HALF, X, LDX+1 )
C
      IF ( .NOT.LTRANS ) THEN
C
         IF ( LUPLO ) THEN
C
            DO 10 J = 1, N
               CALL DCOPY( J, X(1,J), 1, DWORK(1+(J-1)*N), 1 )
               CALL DTRMV( UPLO, 'NoTran', 'NoDiag', J, E, LDE,
     $                     DWORK(1+(J-1)*N), 1 )
   10       CONTINUE
C
         ELSE
C
            DO 20 J = 1, N
               CALL DCOPY( J, X(J,1), LDX, DWORK(1+(J-1)*N), 1 )
               CALL DTRMV( 'Upper', 'NoTran', 'NoDiag', J, E, LDE,
     $                     DWORK(1+(J-1)*N), 1 )
   20       CONTINUE
C
         END IF
C
      ELSE
C
         IF ( LUPLO ) THEN
C
            DO 30 J = 1, N
               CALL DCOPY( J, E(1,J), 1, DWORK(1+(J-1)*N), 1 )
               CALL DTRMV( UPLO, 'NoTran', 'NoDiag', J, X, LDX,
     $                     DWORK(1+(J-1)*N), 1 )
   30       CONTINUE
C
         ELSE
C
            DO 40 J = 1, N
               CALL DCOPY( J, E(1,J), 1, DWORK(1+(J-1)*N), 1 )
               CALL DTRMV( UPLO, 'Tran', 'NoDiag', J, X, LDX,
     $                     DWORK(1+(J-1)*N), 1 )
   40       CONTINUE
C
         END IF
C
      END IF
C
      CALL DSCAL( N, TWO, X, LDX+1 )
C
      CALL MB01OT( UPLO, TRANS, N, ALPHA, BETA, R, LDR, E, LDE, DWORK,
     $             N )
C
      RETURN
C *** Last line of MB01RT ***
      END


C$Procedure      RAV2XF ( Rotation and angular velocity to transform )

      SUBROUTINE RAV2XF ( ROT, AV, XFORM )

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

      IMPLICIT NONE

      DOUBLE PRECISION      ROT    ( 3, 3 )
      DOUBLE PRECISION      AV     ( 3    )
      DOUBLE PRECISION      XFORM  ( 6, 6 )

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  --------------------------------------------------
C     ROT        I   rotation matrix
C     AV         I   angular velocity vector
C     XFORM      O   state transformation associated with ROT and AV
C
C$ Detailed_Input
C
C     ROT         is a rotation that gives the transformation from
C                 some frame FRAME1 to another frame FRAME2.
C
C     AV          is the angular velocity of the transformation.
C                 In other words, if P is the position of a fixed
C                 point in FRAME2, then from the point of view of
C                 FRAME1,  P rotates (in a right handed sense) about
C                 an axis parallel to AV.  Moreover the rate of rotation
C                 in radians per unit time is given by the length of
C                 AV.
C
C                 More formally, the velocity V of P in FRAME1 is
C                 given by
C                                    t
C                     V  = AV x ( ROT * P )
C
C$ Detailed_Output
C
C     XFORM       is a state transformation matrix associated
C                 with ROT and AV.  If S1 is the state of an object
C                 with respect to FRAME1, then the state S2 of the
C                 object with respect to FRAME2 is given by
C
C                     S2  =  XFORM * S1
C
C                 where "*" denotes Matrix-Vector multiplication.
C
C
C$ Parameters
C
C     None.
C
C$ Files
C
C     None.
C
C$ Exceptions
C
C     Error free.
C
C     1) No checks are performed on ROT to ensure that it is indeed
C        a rotation matrix.
C
C$ Particulars
C
C     This routine is essentially a macro routine for converting
C     a rotation and angular velocity of the rotation to the
C     equivalent state transformation matrix.
C
C     This routine is an inverse of XF2RAV
C
C$ Examples
C
C     Suppose that you wanted to determine state transformation
C     matrix from a platform frame to J2000.
C
C     CALL CKGPAV ( CKID, TIME, TOL, 'J2000', ROT, AV, CLKOUT, FND )
C
C     Recall that ROT and AV are the rotation and angular velocity
C     of the transformation from J2000 to the platform frame.
C
C     IF ( FND ) THEN
C
C        First get the state transformation from J2000 to the platform
C        frame.
C
C        CALL RAV2XF ( ROT,  AV, J2PLT )
C
C        Invert the state transformation matrix (using INVSTM) to
C        the desired state transformation matrix.
C
C        CALL INVSTM ( J2PLT, XFORM )
C
C     END IF
C
C$ Restrictions
C
C     None.
C
C$ Author_and_Institution
C
C     W.L. Taber      (JPL)
C
C$ Literature_References
C
C     None.
C
C$ Version
C
C-   SPICELIB Version 1.1.0, 28-JUL-1997 (WLT)
C
C       The example in version 1.0.0 was incorrect.  The example
C       in version 1.1.0 fixes the previous problem.
C
C-   SPICELIB Version 1.0.0, 18-SEP-1995 (WLT)
C
C
C-&

C$ Index_Entries
C
C    State transformation to rotation and angular velocity
C
C-&

      INTEGER               I
      INTEGER               J

      DOUBLE PRECISION      OMEGAT ( 3, 3 )
      DOUBLE PRECISION      DROTDT ( 3, 3 )

C
C     A state transformation matrix XFORM has the following form
C
C
C         [      |     ]
C         |  R   |  0  |
C         |      |     |
C         | -----+-----|
C         |  dR  |     |
C         |  --  |  R  |
C         [  dt  |     ]
C
C
C     where R is a rotation and dR/dt is the time derivative of that
C     rotation.  From this we can immediately fill in most of the
C     state transformation matrix.
C
      DO I = 1, 3
         DO J = 1,3
            XFORM(I,  J  ) = ROT(I,J)
            XFORM(I+3,J+3) = ROT(I,J)
            XFORM(I,  J+3) = 0.0D0
         END DO
      END DO

C
C     Now for the rest.
C
C     Recall that ROT is a transformation that converts positions
C     in some frame FRAME1 to positions in a second frame FRAME2.
C
C     The angular velocity matrix OMEGA (the cross product matrix
C     corresponding to AV) has the following property.
C
C     If P is the position of an object that is stationary with
C     respect to FRAME2 then the velocity V of that object in FRAME1
C     is given by:
C                          t
C         V  =  OMEGA * ROT  *  P
C
C     But V is also given by
C
C                    t
C               d ROT
C         V =   -----  * P
C                 dt
C
C     So that
C                                  t
C                    t        d ROT
C         OMEGA * ROT    =   -------
C                               dt
C
C     Hence
C
C          d ROT                 t
C          -----   =  ROT * OMEGA
C            dt
C
C
C     From this discussion we can see that we need OMEGA transpose.
C     Here it is.
C
      OMEGAT(1,1) =  0.0D0
      OMEGAT(2,1) = -AV(3)
      OMEGAT(3,1) =  AV(2)

      OMEGAT(1,2) =  AV(3)
      OMEGAT(2,2) =  0.0D0
      OMEGAT(3,2) = -AV(1)

      OMEGAT(1,3) = -AV(2)
      OMEGAT(2,3) =  AV(1)
      OMEGAT(3,3) =  0.0D0

      CALL MXM ( ROT, OMEGAT, DROTDT )

      DO I = 1, 3
         DO J = 1,3
            XFORM(I+3,J) = DROTDT(I,J)
         END DO
      END DO

      RETURN
      END