*DECK D9GMIC
      DOUBLE PRECISION FUNCTION D9GMIC (A, X, ALX)
C***BEGIN PROLOGUE  D9GMIC
C***SUBSIDIARY
C***PURPOSE  Compute the complementary incomplete Gamma function for A
C            near a negative integer and X small.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C7E
C***TYPE      DOUBLE PRECISION (R9GMIC-S, D9GMIC-D)
C***KEYWORDS  COMPLEMENTARY INCOMPLETE GAMMA FUNCTION, FNLIB, SMALL X,
C             SPECIAL FUNCTIONS
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C Compute the complementary incomplete gamma function for A near
C a negative integer and for small X.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  D1MACH, DLNGAM, XERMSG
C***REVISION HISTORY  (YYMMDD)
C   770701  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890911  Removed unnecessary intrinsics.  (WRB)
C   890911  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C   900720  Routine changed from user-callable to subsidiary.  (WRB)
C***END PROLOGUE  D9GMIC
      DOUBLE PRECISION A, X, ALX, ALNG, BOT, EPS, EULER, FK, FKP1, FM,
     1  S, SGNG, T, TE, D1MACH, DLNGAM
      LOGICAL FIRST
      SAVE EULER, EPS, BOT, FIRST
      DATA EULER / 0.5772156649 0153286060 6512090082 40 D0 /
      DATA FIRST /.TRUE./
C***FIRST EXECUTABLE STATEMENT  D9GMIC
      IF (FIRST) THEN
         EPS = 0.5D0*D1MACH(3)
         BOT = LOG (D1MACH(1))
      ENDIF
      FIRST = .FALSE.
C
      IF (A .GT. 0.D0) CALL XERMSG ('SLATEC', 'D9GMIC',
     +   'A MUST BE NEAR A NEGATIVE INTEGER', 2, 2)
      IF (X .LE. 0.D0) CALL XERMSG ('SLATEC', 'D9GMIC',
     +   'X MUST BE GT ZERO', 3, 2)
C
      M = -(A - 0.5D0)
      FM = M
C
      TE = 1.0D0
      T = 1.0D0
      S = T
      DO 20 K=1,200
        FKP1 = K + 1
        TE = -X*TE/(FM+FKP1)
        T = TE/FKP1
        S = S + T
        IF (ABS(T).LT.EPS*S) GO TO 30
 20   CONTINUE
      CALL XERMSG ('SLATEC', 'D9GMIC',
     +   'NO CONVERGENCE IN 200 TERMS OF CONTINUED FRACTION', 4, 2)
C
 30   D9GMIC = -ALX - EULER + X*S/(FM+1.0D0)
      IF (M.EQ.0) RETURN
C
      IF (M.EQ.1) D9GMIC = -D9GMIC - 1.D0 + 1.D0/X
      IF (M.EQ.1) RETURN
C
      TE = FM
      T = 1.D0
      S = T
      MM1 = M - 1
      DO 40 K=1,MM1
        FK = K
        TE = -X*TE/FK
        T = TE/(FM-FK)
        S = S + T
        IF (ABS(T).LT.EPS*ABS(S)) GO TO 50
 40   CONTINUE
C
 50   DO 60 K=1,M
        D9GMIC = D9GMIC + 1.0D0/K
 60   CONTINUE
C
      SGNG = 1.0D0
      IF (MOD(M,2).EQ.1) SGNG = -1.0D0
      ALNG = LOG(D9GMIC) - DLNGAM(FM+1.D0)
C
      D9GMIC = 0.D0
      IF (ALNG.GT.BOT) D9GMIC = SGNG * EXP(ALNG)
      IF (S.NE.0.D0) D9GMIC = D9GMIC +
     1  SIGN (EXP(-FM*ALX+LOG(ABS(S)/FM)), S)
C
      IF (D9GMIC .EQ. 0.D0 .AND. S .EQ. 0.D0) CALL XERMSG ('SLATEC',
     +   'D9GMIC', 'RESULT UNDERFLOWS', 1, 1)
      RETURN
C
      END


      subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1,
     *                 wa2)
      integer n,ldr
      integer ipvt(n)
      real delta,par
      real 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 ... spmpar,enorm,qrsolv
c
c       fortran-supplied ... abs,amax1,amin1,sqrt
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
      real dxnorm,dwarf,fp,gnorm,parc,parl,paru,p1,p001,sum,temp,zero
      real spmpar,enorm
      data p1,p001,zero /1.0e-1,1.0e-3,0.0e0/
c
c     dwarf is the smallest positive magnitude.
c
      dwarf = spmpar(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/amin1(delta,p1)
c
c     if the input par lies outside of the interval (parl,paru),
c     set par to the closer endpoint.
c
      par = amax1(par,parl)
      par = amin1(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 = amax1(dwarf,p001*paru)
         temp = sqrt(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 (abs(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 = amax1(parl,par)
         if (fp .lt. zero) paru = amin1(paru,par)
c
c        compute an improved estimate for par.
c
         par = amax1(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


! Created by ${USER_NAME} on 11/27/18.

PROGRAM solve_toggle
  !     Solve CME of the toggle switch
  USE StateSpace
  USE KrylovSolver
  IMPLICIT NONE

  INTERFACE
     DOUBLE PRECISION FUNCTION clock()
     END FUNCTION clock
  END INTERFACE

  DOUBLE PRECISION :: t = 100.0d0, exp_tol = 1.0d-8, fsp_tol = 1.0d-4
  TYPE (cme_model) :: model
  TYPE (finite_state_projection) :: fsp_in, fsp
  DOUBLE PRECISION, DIMENSION(nmax) :: p0, p
  DOUBLE PRECISION :: tic
  INTEGER :: i

  CALL RANDOM_SEED()

  CALL model%CREATE(2, 4, 6)
  model%stoichiometry = RESHAPE((/1,0,-1,0,0,1,0,-1/), (/2,4/))
  model%customprop => toggle_propensity
  CALL model%reset_parameters([1.0d0, 100.0d0, 1.0d0, 1.0d0, 100.0d0, 1.0d0])
  model%loaded = .TRUE.

  WRITE(*,*) model%stoichiometry
  write(*,*) model%parameter_val
  write(*,*) toggle_propensity([0,0], 1, [1.0d0, 100.0d0, 1.0d0, 1.0d0, 100.0d0, 1.0d0])
  WRITE(*,*) model%propensity([0,0], 1)

  ! initialize the attributes of the fsp based on the underlying model
  tic = clock()
  write(*,*) 'Allocating memory for the FSP...'
  CALL fsp_in%CREATE(model)
  CALL fsp%CREATE(model)
  WRITE(*, fmt = 200) clock() - tic

  fsp_in%size = 1
  fsp_in%state(:, 1) = [0, 0]
  p0(1) = 1.0
  fsp_in%vector = p0
  fsp = fsp_in
  tic = clock()
  write(*,*) 'Calling the solver...'
  CALL cme_solve(model, t, fsp_in, fsp, fsp_tol, exp_tol, verbosity = 1)
  WRITE(*, fmt = 200) clock() - tic
200 FORMAT ('Elapsed time :', F10.2)

  CALL fsp%clear()
  CALL fsp_in%clear()
CONTAINS
  DOUBLE PRECISION FUNCTION toggle_propensity(state, reaction, parameters)
    INTEGER, INTENT(in) :: state(:), reaction
    DOUBLE PRECISION, INTENT(in), OPTIONAL :: parameters(:)

    SELECT CASE (reaction)
    CASE(1)
       toggle_propensity = parameters(1) + parameters(2)/(1d0 + state(2)**1.5d0)
    CASE(2)
       toggle_propensity = parameters(3)*state(1)
    CASE(3)
       toggle_propensity = parameters(4) + parameters(5)/(1d0 + state(1)**3.5d0)
    CASE(4)
       toggle_propensity = parameters(6)*state(2)
    END SELECT
  END FUNCTION toggle_propensity
END PROGRAM solve_toggle


INCLUDE 'pib_mod.f03'
      program pib_01
      USE pib_mod
!
!     USAGE:
!       ./pib_02.exe <mass> <boxLength> <slope> <qnM> <qnN>
!
!     ABOUT:
!     This program is used as a unit test of the modified Particle-in-a-Box
!     (mPIB) problem. In this problem, the standard PIB potential is modified to
!     include a linear potential within the box (0 < x < L) given by V(x) = bx.
!
!     This specific unit test accepts command line arguments for the mass, box
!     length, slope parameter b, and two PIB quantum numbers (m and n). The
!     program reports kinetic energy, potential energy, and Hamiltonian matrix
!     elements for the system where standard PIB eigenfunctions m and n are
!     taken in the bra and ket of each matrix element, respectively.
!
!     All input and results are taken to be in atomic units.
!
!
!     AUTHOR:
!     H. P. Hratchian, 2020.
!
!
!     Variable Declarations
!
      implicit none
      integer::nArguments,qnM,qnN
      real::mass,boxLength,slope
      character(len=256)::commandLineArg
      logical::fail=.false.
!
!     Format Statements
!
 1000 Format('Modified Particle-in-a-Box Program.'/,'Unit Test 1.',/,/,  &
        'Input Parameters:',/,  &
        2x,'mass      = ',F10.3,/,  &
        2x,'boxLength = ',F10.3,/,  &
        2x,'slope     = ',F10.3,/,  &
        2x,'qnM       = ',I4,/,  &
        2x,'qnN       = ',I4,/)
 2000 Format('< m | T | n > = ',F12.5)
 2010 Format('< m | V | n > = ',F12.5)
 9000 Format('Incorrect number of command line arguments.',/,  &
        'Expected 5 but found ',I2,'.'/,  &
        'The list of arguments is:',/,  &
        3x,'<mass> <boxLength> <slope> <qnM> <qnN>')
 9100 Format('Input parameter ',A,' must be greater than 0.')
 9999 Format(/,'PROGRAM FAILED!')
!
!     Find out how many command line arguments there are, ensure it's the right
!     number, read-in the values, and echo the input as output.
!
      nArguments = COMMAND_ARGUMENT_COUNT()
      if(nArguments.ne.5) then
        write(iOut,9000) nArguments
        fail = .true.
        goto 999
      endIf
      call GET_COMMAND_ARGUMENT(1,commandLineArg)
      read(commandLineArg,*) mass
      call GET_COMMAND_ARGUMENT(2,commandLineArg)
      read(commandLineArg,*) boxLength
      call GET_COMMAND_ARGUMENT(3,commandLineArg)
      read(commandLineArg,*) slope
      call GET_COMMAND_ARGUMENT(4,commandLineArg)
      read(commandLineArg,*) qnM
      call GET_COMMAND_ARGUMENT(5,commandLineArg)
      read(commandLineArg,*) qnN
      write(iOut,1000) mass,boxLength,slope,qnM,qnN
      if(mass.le.0) then
        write(iOut,9100) 'mass'
        fail = .true.
      endIf
      if(boxLength.le.0) then
        write(iOut,9100) 'boxLength'
        fail = .true.
      endIf
      if(slope.le.0) then
        write(iOut,9100) 'slope'
        fail = .true.
      endIf
      if(qnM.le.0) then
        write(iOut,9100) 'qnM'
        fail = .true.
      endIf
      if(qnN.le.0) then
        write(iOut,9100) 'qnN'
        fail = .true.
      endIf
      if(fail) goto 999
!
!     Calculate the requested kinetic and potential energy matrix elements.
!
      write(iOut,2000) kineticEnergy(mass,boxLength,qnM,qnN)
      write(iOut,2010) potentialEnergy(mass,boxLength,slope,qnM,qnN)
!
!     Complete the program...
!
  999 if(fail) write(iOut,9999)
      end program pib_01


subroutine task_rank_to_index (rank,i,j)

!   returns the pair of  beginning indeces for the subdomain on the
!   global grid given the subdomain's rank.

use domain

implicit none

integer rank, i, j

j = rank/nsubdomains_x
i = rank - j*nsubdomains_x

i = i * (nx_gl/nsubdomains_x)
j = j * (ny_gl/nsubdomains_y)

end


!--------------------------------------------------------------------------------------------------!
!--------------------------------------------------------------------------------------------------!
!------------------ EXAMPLES OF EXTERNAL DRIVER ROUTINES FOR SIMULATED ANNEALING ------------------!
!--------------------------------------------------------------------------------------------------!
!--------------------------------------------------------------------------------------------------!

!--------------------------------------------------------------------------------------------------!
! EXAMPLE #1: FIND AN INTEGER POINT IN TWO DIMENSIONS
! - Objective function: obj = -0.5*((x-x0)**2/sx+(y-y0)**2/sy)
! - Initialization: model = 0
! - Proposal: model_proposed = model_current +/- 1
!--------------------------------------------------------------------------------------------------!
subroutine driver1(model_best)

use annealing                                                       ! Import the annealing module
implicit none

interface                                                           ! The driver subroutines and functions
    subroutine driver1_init(model,n)                                ! must be declared with an interface
        integer :: n                                                ! statement so they can be matched with
        integer :: model(n)                                         ! the generic routines called in anneal().
    end subroutine                                                  ! This is no longer the case if the
    subroutine driver1_propose(model_current,model_proposed,n)      ! driver routines are part of a module
        integer :: n                                                ! (see Example #3).
        integer :: model_current(n)
        integer :: model_proposed(n)
    end subroutine
    function driver1_objective(model,n)
        integer :: n
        integer :: model(n)
        double precision :: driver1_objective
    end function
    subroutine driver1_log(it,temp,obj,model_current,model_proposed,n,isAccepted,string)
        integer :: it, n
        double precision :: temp, obj
        integer :: model_current(n)
        integer :: model_proposed(n)
        logical :: isAccepted
        character(len=*) :: string
    end subroutine
end interface

integer :: n, maxit, resetit                                        ! These variable types must be exactly
integer :: model_best(2)                                            ! the same as in the corresponding
double precision :: tstart, tmin, cool                              ! version of anneal.
logical :: saveRejected

n = 2
model_best = 0
maxit = 200
resetit = -1
tstart = 1.0d0
tmin = -1.0d0
cool = 0.99d0
saveRejected = .true.

! Call anneal with specific driver routines
call anneal(n, model_best, &
            driver1_init, driver1_propose, driver1_objective, &
            maxit, resetit, tstart, tmin, cool, &
            driver1_log)

return
end subroutine

subroutine driver1_init(model,n)
!----
! Start model at origin, initialize seed to fixed value
!----
use random, only: iseed
implicit none
integer :: n                                                        ! As in the driver routine, these types
integer :: model(n)                                                 ! must be the same as in anneal
iseed = -52918
model = 0
return
end subroutine

subroutine driver1_propose(model_in,model_out,n)
!----
! Equal odds of adding or subtracting one from each model dimension
!----
use random, only: iseed, ran0
implicit none
integer :: n
integer :: model_in(n)
integer :: model_out(n)
integer :: i
do i = 1,n
    if (ran0(iseed).lt.0.50) then
        model_out(i) = model_in(i) + 1
    else
        model_out(i) = model_in(i) - 1
    endif
enddo
return
end subroutine

function driver1_objective(model,n)
!----
! Aiming for coordinate (10,6), with Gaussian probability distribution
!----
implicit none
integer :: n
integer :: model(n)
double precision :: driver1_objective
driver1_objective = 0.0d0
driver1_objective = driver1_objective + dble((model(1)-10)**2)/dble(20)
driver1_objective = driver1_objective + dble((model(2)-6)**2)/dble(8)
driver1_objective = -0.5d0*driver1_objective
return
end function

subroutine driver1_log(it,temp,obj,model_current,model_proposed,n,isAccepted,string)
integer :: it, n
double precision :: temp, obj
integer :: model_current(n)
integer :: model_proposed(n)
logical :: isAccepted
character(len=*) :: string
character(len=32) :: logfile
logfile = 'driver1.log'
if (string.eq.'init') then
    ! Open the log file
    open(unit=29,file=logfile,status='unknown')
    ! Write locked/unlocked, fault slip results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj
    do i = 1,n
        write(29,*) model_current(i)
    enddo
elseif (string.eq.'append') then
    ! Write locked/unlocked, fault slip, old model results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj,' isAccepted ',isAccepted
    do i = 1,n
        write(29,*) model_current(i),model_proposed(i)
    enddo
elseif (string.eq.'close') then
    ! Close the log file
    close(29)
endif
end subroutine

!--------------------------------------------------------------------------------------------------!
! EXAMPLE #2: FIND A DOUBLE PRECISION POINT IN TWO DIMENSIONS
! - Objective function: obj = -0.5*((x-x0)**2/sx+(y-y0)**2/sy)
! - Initialization: model = 0
! - Proposal: model_proposed = model_current + gaussian random number
!--------------------------------------------------------------------------------------------------!
subroutine driver2(model_best)

use annealing
implicit none

interface
    subroutine driver2_init(model,n)
        integer :: n
        double precision :: model(n)
    end subroutine
    subroutine driver2_propose(model_current,model_proposed,n)
        integer :: n
        double precision :: model_current(n)
        double precision :: model_proposed(n)
    end subroutine
    function driver2_objective(model,n)
        integer :: n
        double precision :: model(n)
        double precision :: driver2_objective
    end function
    subroutine driver2_log(it,temp,obj,model_current,model_proposed,n,isAccepted,string)
        integer :: it, n
        double precision :: temp, obj
        double precision :: model_current(n)
        double precision :: model_proposed(n)
        logical :: isAccepted
        character(len=*) :: string
    end subroutine
end interface

integer :: n, maxit, resetit
double precision :: model_best(2)
double precision :: tstart, tmin, cool
logical :: saveRejected

n = 2
model_best = 0.0d0
maxit = 200
resetit = -1
tstart = 1.0d0
tmin = -1.0d0
cool = 0.99d0
saveRejected = .true.

! Call anneal with specific driver routines
call anneal(n, model_best, &
            driver2_init, driver2_propose, driver2_objective, &
            maxit, resetit, tstart, tmin, cool, &
            driver2_log)

return
end subroutine

subroutine driver2_init(model,n)
!----
! Start model at origin, initialize seed to fixed value
!----
use random, only: iseed
implicit none
integer :: n
double precision :: model(n)
iseed = -829321
model = 0.0d0
return
end subroutine

subroutine driver2_propose(model_in,model_out,n)
!----
! Gaussian probability distribution with a standard deviation of 1 for each model dimension
!----
use random, only: iseed, r8_normal_ab
implicit none
integer :: n
double precision :: model_in(n)
double precision :: model_out(n)
integer :: i
do i = 1,n
    model_out(i) = model_in(i) + r8_normal_ab(0.0d0,1.0d0,iseed)
enddo
return
end subroutine

function driver2_objective(model,n)
!----
! Aiming for coordinate (10,6), with Gaussian probability distribution
!----
implicit none
integer :: n
double precision :: model(n)
double precision :: driver2_objective
driver2_objective = 0.0d0
driver2_objective = driver2_objective + (model(1)-10.0d0)**2/20.0d0
driver2_objective = driver2_objective + (model(2)-6.0d0)**2/8.0d0
driver2_objective = -0.5d0*driver2_objective
return
end function

subroutine driver2_log(it,temp,obj,model_current,model_proposed,n,isAccepted,string)
integer :: it, n
double precision :: temp, obj
double precision :: model_current(n)
double precision :: model_proposed(n)
logical :: isAccepted
character(len=*) :: string
character(len=32) :: logfile
logfile = 'driver2.log'
if (string.eq.'init') then
    ! Open the log file
    open(unit=29,file=logfile,status='unknown')
    ! Write locked/unlocked, fault slip results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj
    do i = 1,n
        write(29,*) model_current(i)
    enddo
elseif (string.eq.'append') then
    ! Write locked/unlocked, fault slip, old model results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj,' isAccepted ',isAccepted
    do i = 1,n
        write(29,*) model_current(i),model_proposed(i)
    enddo
elseif (string.eq.'close') then
    ! Close the log file
    close(29)
endif
end subroutine

!--------------------------------------------------------------------------------------------------!
! EXAMPLE #3: FIND A DOUBLE PRECISION POINT IN TWO DIMENSIONS, WITH DRIVERS IN A MODULE
! - Objective function: obj = -0.5*((x-x0)**2/x0+(y-y0)**2/y0)
! - Initialization: model = 0
! - Proposal: model_proposed = model_current + gaussian random number
!--------------------------------------------------------------------------------------------------!
module driver3_module

character(len=32) :: logfile

contains

subroutine driver3(model_best)

use annealing, only: anneal
implicit none

! interface                                                         ! When the driver routines are part
!     subroutine driver3_init(model,n)                              ! of a module, then they do not need
!         integer :: n                                              ! an explicit interface statement
!         double precision :: model(n)                              ! because they are no longer (implicitly)
!     end subroutine                                                ! external routines. (In fact, this will
!     subroutine driver3_propose(model_current,model_proposed,n)    ! not compile when these lines are
!         integer :: n                                              ! uncommented.)
!         double precision :: model_current(n)
!         double precision :: model_proposed(n)
!     end subroutine
!     function driver3_objective(model,n)
!         integer :: n
!         double precision :: model(n)
!         double precision :: driver3_objective
!     end function
! end interface

integer :: n, maxit, resetit
double precision :: model_best(2)
double precision :: tstart, tmin, cool
logical :: saveRejected

n = 2
model_best = 0.0d0
maxit = 200
resetit = -1
tstart = 1.0d0
tmin = -1.0d0
cool = 0.99d0
logfile = 'driver3.log'
saveRejected = .true.

! Call anneal with specific driver routines
call anneal(n, model_best, &
            driver3_init, driver3_propose, driver3_objective, &
            maxit, resetit, tstart, tmin, cool, &
            driver3_log)

return
end subroutine

subroutine driver3_init(model,n)
use random, only: iseed
implicit none
integer :: n
double precision :: model(n)
iseed = -829321
model = 0.0d0
return
end subroutine

subroutine driver3_propose(model_in,model_out,n)
use random, only: iseed, r8_normal_ab
implicit none
integer :: n
double precision :: model_in(n)
double precision :: model_out(n)
integer :: i
do i = 1,n
    model_out(i) = model_in(i) + r8_normal_ab(0.0d0,1.0d0,iseed)
enddo
return
end subroutine

function driver3_objective(model,n)
implicit none
integer :: n
double precision :: model(n)
double precision :: driver3_objective
driver3_objective = 0.0d0
driver3_objective = driver3_objective + (model(1)-10.0d0)**2/20.0d0
driver3_objective = driver3_objective + (model(2)-6.0d0)**2/8.0d0
driver3_objective = -0.5d0*driver3_objective
return
end function

subroutine driver3_log(it,temp,obj,model_current,model_proposed,n,isAccepted,string)
integer :: it, n
double precision :: temp, obj
double precision :: model_current(n)
double precision :: model_proposed(n)
logical :: isAccepted
character(len=*) :: string
if (string.eq.'init') then
    ! Open the log file
    open(unit=29,file=logfile,status='unknown')
    ! Write locked/unlocked, fault slip results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj
    do i = 1,n
        write(29,*) model_current(i)
    enddo
elseif (string.eq.'append') then
    ! Write locked/unlocked, fault slip, old model results to log file
    write(29,*) 'Iteration ',it,' Temperature ',temp,' Objective ',obj,' isAccepted ',isAccepted
    do i = 1,n
        write(29,*) model_current(i),model_proposed(i)
    enddo
elseif (string.eq.'close') then
    ! Close the log file
    close(29)
endif
end subroutine

end module

!==================================================================================================!
!==================================================================================================!
!==================================================================================================!
!==================================================================================================!
!==================================================================================================!

program main

use test, only: test_value
use io, only: stdout
use random

use annealing
use driver3_module, only: driver3

implicit none

integer, parameter :: nw = 10
integer :: j, ios, it, model_best_int(2), iselected(nw)
character(len=512) :: line
character(len=1) :: ch
double precision :: temp, obj, model_best_dp(2), weights(nw)

! driver1 (anneal_int_array)
call driver1(model_best_int)
open(unit=11,file='driver1.log',status='old')
do
    read(11,'(A)',iostat=ios) line
    if (ios.ne.0) then
        exit
    endif
    j = index(line,'Iteration')
    if (j.ne.0) then
        read(line,*) ch,it,ch,temp,ch,obj
    endif
enddo
call test_value(model_best_int(1),10,'driver1 (anneal_int_array): model_best_int(1)')
call test_value(model_best_int(2),6,'driver1 (anneal_int_array): model_best_int(2)')
call test_value(it,200,'driver1 (anneal_int_array): maxit')
call test_value(temp,0.13533300490703207d0,'driver1 (anneal_int_array): final temp')
call test_value(obj,-0.34999999999999998d0,'driver1 (anneal_int_array): final obj')
close(11,status='delete')
write(stdout,*) 'subroutine anneal_int_array() - driver1() passed unit test'
write(stdout,*)

! driver2 (anneal_dp_array)
call driver2(model_best_dp)
open(unit=12,file='driver2.log',status='old')
do
    read(12,'(A)',iostat=ios) line
    if (ios.ne.0) then
        exit
    endif
    j = index(line,'Iteration')
    if (j.ne.0) then
        read(line,*) ch,it,ch,temp,ch,obj
    endif
enddo
call test_value(model_best_dp(1),9.7829045374811461d0,'driver2 (anneal_dp_array): model_best_dp(1)')
call test_value(model_best_dp(2),6.0216229846947424d0,'driver2 (anneal_dp_array): model_best_dp(2)')
call test_value(it,200,'driver2 (anneal_dp_array): maxit')
call test_value(temp,0.13533300490703207d0,'driver2 (anneal_dp_array): final temp')
call test_value(obj,-5.8331140982675264d-002,'driver2 (anneal_dp_array): final obj')
close(12,status='delete')
write(stdout,*) 'subroutine anneal_dp_array() - driver2() passed unit test'
write(stdout,*)

! driver3 (anneal_dp_array; module)
call driver3(model_best_dp)
open(unit=13,file='driver3.log',status='old')
do
    read(13,'(A)',iostat=ios) line
    if (ios.ne.0) then
        exit
    endif
    j = index(line,'Iteration')
    if (j.ne.0) then
        read(line,*) ch,it,ch,temp,ch,obj
    endif
enddo
call test_value(model_best_dp(1),9.7829045374811461d0,'driver3 (anneal_dp_array): model_best_dp(1)')
call test_value(model_best_dp(2),6.0216229846947424d0,'driver3 (anneal_dp_array): model_best_dp(2)')
call test_value(it,200,'driver3 (anneal_dp_array): maxit')
call test_value(temp,0.13533300490703207d0,'driver3 (anneal_dp_array): final temp')
call test_value(obj,-5.8331140982675264d-002,'driver3 (anneal_dp_array): final obj')
close(13,status='delete')
write(stdout,*) 'subroutine anneal_dp_array() - driver3_module passed unit test'
write(stdout,*)

! resample(weights,iselected,n)
iseed = 495546918
weights(1)  = 1.0d0
weights(2)  = 1.0d0
weights(3)  = 1.0d0
weights(4)  = 1.0d0
weights(5)  = 1.0d0
weights(6)  = 1.0d0
weights(7)  = 2.0d0
weights(8)  = 2.0d0
weights(9)  = 2.0d0
weights(10) = 6.0d0
iselected = 0
call resample(weights,iselected,nw)
call test_value(iselected(1),  6,'resample(): iselected(1)')
call test_value(iselected(2),  8,'resample(): iselected(2)')
call test_value(iselected(3),  9,'resample(): iselected(3)')
call test_value(iselected(4),  6,'resample(): iselected(4)')
call test_value(iselected(5),  8,'resample(): iselected(5)')
call test_value(iselected(6), 10,'resample(): iselected(6)')
call test_value(iselected(7),  9,'resample(): iselected(7)')
call test_value(iselected(8),  4,'resample(): iselected(8)')
call test_value(iselected(9),  6,'resample(): iselected(9)')
call test_value(iselected(10),10,'resample(): iselected(10)')
write(stdout,*) 'subroutine resample() passed unit test'
write(stdout,*)

write(stdout,*) 'annealing_module unit test passed'
end


module test_stats
  use stats
  use stdlib_kinds, only: int32, dp
  use stdlib_stats, only: mean, var, moment
  use testdrive, only : error_type, unittest_type, new_unittest, check
  implicit none
  private

  public :: collect_stats

  contains

    subroutine collect_stats(testsuite)
      !> Collect all exported unit tests
      type(unittest_type), allocatable, intent(out) :: testsuite(:)

      testsuite = [                                              &
        new_unittest("test for mean", test_mean),                &
        new_unittest("test for sample variance", test_vars),     &
        new_unittest("test for population variance", test_varp)  &
      ]
    end subroutine collect_stats

    subroutine test_mean(error)
      !> Check accumulator real mean
      type(error_type), allocatable, intent(out) :: error

      type (acc)     :: t
      real(dp)       :: x(1000), avg1, avg2

      call random_init(.false., .false.)
      call random_number(x)
      call t%push(x)

      avg1 = t%mean()
      avg2 = mean(x, 1)

      call check(error, avg2, avg1, thr=epsilon(avg2)*100)

      if (allocated(error)) return

    end subroutine test_mean

    subroutine test_vars(error)
      !> Check accumulator sample variance
      type(error_type), allocatable, intent(out) :: error

      type (acc)     :: t
      real(dp)       :: x(1000), var1, var2

      call random_init(.false., .false.)
      call random_number(x)
      call t%push(x)

      var1 = t%vars()
      var2 = var(x, 1)

      call check(error, var2, var1, thr=epsilon(var2)*100)

      if (allocated(error)) return

    end subroutine test_vars

    subroutine test_varp(error)
      !> Check accumulator population variance
      type(error_type), allocatable, intent(out) :: error

      type (acc)     :: t
      real(dp)       :: x(1000), var1, var2

      call random_init(.false., .false.)
      call random_number(x)
      call t%push(x)

      var1 = t%varp()
      var2 = var(x, 1, corrected=.false.)

      call check(error, var2, var1, thr=epsilon(var2)*100)

      if (allocated(error)) return

    end subroutine test_varp

end module test_stats

program tester
  use, intrinsic :: iso_fortran_env, only : error_unit
  use testdrive, only : run_testsuite
  use test_stats, only : collect_stats
  implicit none
  integer :: stat

  stat = 0
  call run_testsuite(collect_stats, error_unit, stat)

  if (stat > 0) then
    write(error_unit, '(i0, 1x, a)') stat, "test(s) failed!"
    error stop
  end if

end program tester


!Routine to fill data gaps in a series
!Input:
!       time: the time series that is not linear in time
!       dt: expected time lag between succesive observations
!       tol: tolerance for dt
!       ai: A series to fill in the gaps alongside the time series
!       ln: length of ai
!       nmd: an approximation to extend the current length of time and ai by to
!       account for filling in the time gaps
!       fillval: the value to fill into ai when gaps are filled in time
!Output:
!       tn: filled time series
!       dn: filled ai series

subroutine fill_gaps(time, dt, tol, ai, ln, nmd, fillval, tn, dn,counter)
   double precision, intent(in) :: fillval, dt, tol
   integer*8, intent(in) :: ln, nmd
   integer*8, intent(out) :: counter
   double precision, intent(in), dimension(ln) :: ai, time
   double precision, intent(out), dimension(ln+nmd*2) :: tn, dn
   integer*8 :: i
   double precision :: del
   counter = 0
   do i=1,ln
      del = time(i+1) - time(i)
      if (abs(del-dt) <= tol) then
         dn(counter) = ai(i)
         tn(counter) = time(i)
         counter = counter + 1
      else
         do while (abs(time(i+1) - tn(counter-1)) > dt+tol)
            if (tn(counter-1)+dt > time(i+1)) then
               EXIT
            else
               tn(counter) = tn(counter-1)+dt
               dn(counter) = fillval
               counter = counter + 1
            endif
         enddo
      endif
   enddo
end subroutine fill_gaps


*> \brief \b DGET22
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE DGET22( TRANSA, TRANSE, TRANSW, N, A, LDA, E, LDE, WR,
*                          WI, WORK, RESULT )
*
*       .. Scalar Arguments ..
*       CHARACTER          TRANSA, TRANSE, TRANSW
*       INTEGER            LDA, LDE, N
*       ..
*       .. Array Arguments ..
*       DOUBLE PRECISION   A( LDA, * ), E( LDE, * ), RESULT( 2 ), WI( * ),
*      $                   WORK( * ), WR( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> DGET22 does an eigenvector check.
*>
*> The basic test is:
*>
*>    RESULT(1) = | A E  -  E W | / ( |A| |E| ulp )
*>
*> using the 1-norm.  It also tests the normalization of E:
*>
*>    RESULT(2) = max | m-norm(E(j)) - 1 | / ( n ulp )
*>                 j
*>
*> where E(j) is the j-th eigenvector, and m-norm is the max-norm of a
*> vector.  If an eigenvector is complex, as determined from WI(j)
*> nonzero, then the max-norm of the vector ( er + i*ei ) is the maximum
*> of
*>    |er(1)| + |ei(1)|, ... , |er(n)| + |ei(n)|
*>
*> W is a block diagonal matrix, with a 1 by 1 block for each real
*> eigenvalue and a 2 by 2 block for each complex conjugate pair.
*> If eigenvalues j and j+1 are a complex conjugate pair, so that
*> WR(j) = WR(j+1) = wr and WI(j) = - WI(j+1) = wi, then the 2 by 2
*> block corresponding to the pair will be:
*>
*>    (  wr  wi  )
*>    ( -wi  wr  )
*>
*> Such a block multiplying an n by 2 matrix ( ur ui ) on the right
*> will be the same as multiplying  ur + i*ui  by  wr + i*wi.
*>
*> To handle various schemes for storage of left eigenvectors, there are
*> options to use A-transpose instead of A, E-transpose instead of E,
*> and/or W-transpose instead of W.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] TRANSA
*> \verbatim
*>          TRANSA is CHARACTER*1
*>          Specifies whether or not A is transposed.
*>          = 'N':  No transpose
*>          = 'T':  Transpose
*>          = 'C':  Conjugate transpose (= Transpose)
*> \endverbatim
*>
*> \param[in] TRANSE
*> \verbatim
*>          TRANSE is CHARACTER*1
*>          Specifies whether or not E is transposed.
*>          = 'N':  No transpose, eigenvectors are in columns of E
*>          = 'T':  Transpose, eigenvectors are in rows of E
*>          = 'C':  Conjugate transpose (= Transpose)
*> \endverbatim
*>
*> \param[in] TRANSW
*> \verbatim
*>          TRANSW is CHARACTER*1
*>          Specifies whether or not W is transposed.
*>          = 'N':  No transpose
*>          = 'T':  Transpose, use -WI(j) instead of WI(j)
*>          = 'C':  Conjugate transpose, use -WI(j) instead of WI(j)
*> \endverbatim
*>
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The order of the matrix A.  N >= 0.
*> \endverbatim
*>
*> \param[in] A
*> \verbatim
*>          A is DOUBLE PRECISION array, dimension (LDA,N)
*>          The matrix whose eigenvectors are in E.
*> \endverbatim
*>
*> \param[in] LDA
*> \verbatim
*>          LDA is INTEGER
*>          The leading dimension of the array A.  LDA >= max(1,N).
*> \endverbatim
*>
*> \param[in] E
*> \verbatim
*>          E is DOUBLE PRECISION array, dimension (LDE,N)
*>          The matrix of eigenvectors. If TRANSE = 'N', the eigenvectors
*>          are stored in the columns of E, if TRANSE = 'T' or 'C', the
*>          eigenvectors are stored in the rows of E.
*> \endverbatim
*>
*> \param[in] LDE
*> \verbatim
*>          LDE is INTEGER
*>          The leading dimension of the array E.  LDE >= max(1,N).
*> \endverbatim
*>
*> \param[in] WR
*> \verbatim
*>          WR is DOUBLE PRECISION array, dimension (N)
*> \endverbatim
*>
*> \param[in] WI
*> \verbatim
*>          WI is DOUBLE PRECISION array, dimension (N)
*>
*>          The real and imaginary parts of the eigenvalues of A.
*>          Purely real eigenvalues are indicated by WI(j) = 0.
*>          Complex conjugate pairs are indicated by WR(j)=WR(j+1) and
*>          WI(j) = - WI(j+1) non-zero; the real part is assumed to be
*>          stored in the j-th row/column and the imaginary part in
*>          the (j+1)-th row/column.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is DOUBLE PRECISION array, dimension (N*(N+1))
*> \endverbatim
*>
*> \param[out] RESULT
*> \verbatim
*>          RESULT is DOUBLE PRECISION array, dimension (2)
*>          RESULT(1) = | A E  -  E W | / ( |A| |E| ulp )
*>          RESULT(2) = max | m-norm(E(j)) - 1 | / ( n ulp )
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date November 2011
*
*> \ingroup double_eig
*
*  =====================================================================
      SUBROUTINE DGET22( TRANSA, TRANSE, TRANSW, N, A, LDA, E, LDE, WR,
     $                   WI, WORK, RESULT )
*
*  -- LAPACK test routine (version 3.4.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     November 2011
*
*     .. Scalar Arguments ..
      CHARACTER          TRANSA, TRANSE, TRANSW
      INTEGER            LDA, LDE, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * ), E( LDE, * ), RESULT( 2 ), WI( * ),
     $                   WORK( * ), WR( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D0, ONE = 1.0D0 )
*     ..
*     .. Local Scalars ..
      CHARACTER          NORMA, NORME
      INTEGER            IECOL, IEROW, INCE, IPAIR, ITRNSE, J, JCOL,
     $                   JVEC
      DOUBLE PRECISION   ANORM, ENORM, ENRMAX, ENRMIN, ERRNRM, TEMP1,
     $                   ULP, UNFL
*     ..
*     .. Local Arrays ..
      DOUBLE PRECISION   WMAT( 2, 2 )
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH, DLANGE
      EXTERNAL           LSAME, DLAMCH, DLANGE
*     ..
*     .. External Subroutines ..
      EXTERNAL           DAXPY, DGEMM, DLASET
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, DBLE, MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Initialize RESULT (in case N=0)
*
      RESULT( 1 ) = ZERO
      RESULT( 2 ) = ZERO
      IF( N.LE.0 )
     $   RETURN
*
      UNFL = DLAMCH( 'Safe minimum' )
      ULP = DLAMCH( 'Precision' )
*
      ITRNSE = 0
      INCE = 1
      NORMA = 'O'
      NORME = 'O'
*
      IF( LSAME( TRANSA, 'T' ) .OR. LSAME( TRANSA, 'C' ) ) THEN
         NORMA = 'I'
      END IF
      IF( LSAME( TRANSE, 'T' ) .OR. LSAME( TRANSE, 'C' ) ) THEN
         NORME = 'I'
         ITRNSE = 1
         INCE = LDE
      END IF
*
*     Check normalization of E
*
      ENRMIN = ONE / ULP
      ENRMAX = ZERO
      IF( ITRNSE.EQ.0 ) THEN
*
*        Eigenvectors are column vectors.
*
         IPAIR = 0
         DO 30 JVEC = 1, N
            TEMP1 = ZERO
            IF( IPAIR.EQ.0 .AND. JVEC.LT.N .AND. WI( JVEC ).NE.ZERO )
     $         IPAIR = 1
            IF( IPAIR.EQ.1 ) THEN
*
*              Complex eigenvector
*
               DO 10 J = 1, N
                  TEMP1 = MAX( TEMP1, ABS( E( J, JVEC ) )+
     $                    ABS( E( J, JVEC+1 ) ) )
   10          CONTINUE
               ENRMIN = MIN( ENRMIN, TEMP1 )
               ENRMAX = MAX( ENRMAX, TEMP1 )
               IPAIR = 2
            ELSE IF( IPAIR.EQ.2 ) THEN
               IPAIR = 0
            ELSE
*
*              Real eigenvector
*
               DO 20 J = 1, N
                  TEMP1 = MAX( TEMP1, ABS( E( J, JVEC ) ) )
   20          CONTINUE
               ENRMIN = MIN( ENRMIN, TEMP1 )
               ENRMAX = MAX( ENRMAX, TEMP1 )
               IPAIR = 0
            END IF
   30    CONTINUE
*
      ELSE
*
*        Eigenvectors are row vectors.
*
         DO 40 JVEC = 1, N
            WORK( JVEC ) = ZERO
   40    CONTINUE
*
         DO 60 J = 1, N
            IPAIR = 0
            DO 50 JVEC = 1, N
               IF( IPAIR.EQ.0 .AND. JVEC.LT.N .AND. WI( JVEC ).NE.ZERO )
     $            IPAIR = 1
               IF( IPAIR.EQ.1 ) THEN
                  WORK( JVEC ) = MAX( WORK( JVEC ),
     $                           ABS( E( J, JVEC ) )+ABS( E( J,
     $                           JVEC+1 ) ) )
                  WORK( JVEC+1 ) = WORK( JVEC )
               ELSE IF( IPAIR.EQ.2 ) THEN
                  IPAIR = 0
               ELSE
                  WORK( JVEC ) = MAX( WORK( JVEC ),
     $                           ABS( E( J, JVEC ) ) )
                  IPAIR = 0
               END IF
   50       CONTINUE
   60    CONTINUE
*
         DO 70 JVEC = 1, N
            ENRMIN = MIN( ENRMIN, WORK( JVEC ) )
            ENRMAX = MAX( ENRMAX, WORK( JVEC ) )
   70    CONTINUE
      END IF
*
*     Norm of A:
*
      ANORM = MAX( DLANGE( NORMA, N, N, A, LDA, WORK ), UNFL )
*
*     Norm of E:
*
      ENORM = MAX( DLANGE( NORME, N, N, E, LDE, WORK ), ULP )
*
*     Norm of error:
*
*     Error =  AE - EW
*
      CALL DLASET( 'Full', N, N, ZERO, ZERO, WORK, N )
*
      IPAIR = 0
      IEROW = 1
      IECOL = 1
*
      DO 80 JCOL = 1, N
         IF( ITRNSE.EQ.1 ) THEN
            IEROW = JCOL
         ELSE
            IECOL = JCOL
         END IF
*
         IF( IPAIR.EQ.0 .AND. WI( JCOL ).NE.ZERO )
     $      IPAIR = 1
*
         IF( IPAIR.EQ.1 ) THEN
            WMAT( 1, 1 ) = WR( JCOL )
            WMAT( 2, 1 ) = -WI( JCOL )
            WMAT( 1, 2 ) = WI( JCOL )
            WMAT( 2, 2 ) = WR( JCOL )
            CALL DGEMM( TRANSE, TRANSW, N, 2, 2, ONE, E( IEROW, IECOL ),
     $                  LDE, WMAT, 2, ZERO, WORK( N*( JCOL-1 )+1 ), N )
            IPAIR = 2
         ELSE IF( IPAIR.EQ.2 ) THEN
            IPAIR = 0
*
         ELSE
*
            CALL DAXPY( N, WR( JCOL ), E( IEROW, IECOL ), INCE,
     $                  WORK( N*( JCOL-1 )+1 ), 1 )
            IPAIR = 0
         END IF
*
   80 CONTINUE
*
      CALL DGEMM( TRANSA, TRANSE, N, N, N, ONE, A, LDA, E, LDE, -ONE,
     $            WORK, N )
*
      ERRNRM = DLANGE( 'One', N, N, WORK, N, WORK( N*N+1 ) ) / ENORM
*
*     Compute RESULT(1) (avoiding under/overflow)
*
      IF( ANORM.GT.ERRNRM ) THEN
         RESULT( 1 ) = ( ERRNRM / ANORM ) / ULP
      ELSE
         IF( ANORM.LT.ONE ) THEN
            RESULT( 1 ) = ( MIN( ERRNRM, ANORM ) / ANORM ) / ULP
         ELSE
            RESULT( 1 ) = MIN( ERRNRM / ANORM, ONE ) / ULP
         END IF
      END IF
*
*     Compute RESULT(2) : the normalization error in E.
*
      RESULT( 2 ) = MAX( ABS( ENRMAX-ONE ), ABS( ENRMIN-ONE ) ) /
     $              ( DBLE( N )*ULP )
*
      RETURN
*
*     End of DGET22
*
      END


      SUBROUTINE DGBCO(ABD, LDA, N, ML, MU, IPVT, RCOND, Z)
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY:  DOUBLE
!...Translated by Pacific-Sierra Research 77to90  4.3E  21:52:17  11/14/01
!...Switches:
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE ddot_I
      USE dasum_I
      USE dgbfa_I
      USE dscal_I
      USE daxpy_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER  :: LDA
      INTEGER  :: N
      INTEGER  :: ML
      INTEGER  :: MU
      REAL(DOUBLE) , INTENT(OUT) :: RCOND
      INTEGER  :: IPVT(1)
      REAL(DOUBLE)  :: ABD(LDA,1)
      REAL(DOUBLE)  :: Z(1)
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: IS, INFO, J, JU, K, KB, KP1, L, LA, LM, LZ, M, MM
      REAL(DOUBLE) :: EK, T, WK, WKM, ANORM, S, SM, YNORM
!-----------------------------------------------
!
!     DGBCO FACTORS A DOUBLE PRECISION BAND MATRIX BY GAUSSIAN
!     ELIMINATION AND ESTIMATES THE CONDITION OF THE MATRIX.
!
!     IF  RCOND  IS NOT NEEDED, DGBFA IS SLIGHTLY FASTER.
!     TO SOLVE  A*X = B , FOLLOW DGBCO BY DGBSL.
!     TO COMPUTE  INVERSE(A)*C , FOLLOW DGBCO BY DGBSL.
!     TO COMPUTE  DETERMINANT(A) , FOLLOW DGBCO BY DGBDI.
!
!     ON ENTRY
!
!        ABD     DOUBLE PRECISION(LDA, N)
!                CONTAINS THE MATRIX IN BAND STORAGE.  THE COLUMNS
!                OF THE MATRIX ARE STORED IN THE COLUMNS OF  ABD  AND
!                THE DIAGONALS OF THE MATRIX ARE STORED IN ROWS
!                ML+1 THROUGH 2*ML+MU+1 OF  ABD .
!                SEE THE COMMENTS BELOW FOR DETAILS.
!
!        LDA     INTEGER
!                THE LEADING DIMENSION OF THE ARRAY  ABD .
!                LDA MUST BE .GE. 2*ML + MU + 1 .
!
!        N       INTEGER
!                THE ORDER OF THE ORIGINAL MATRIX.
!
!        ML      INTEGER
!                NUMBER OF DIAGONALS BELOW THE MAIN DIAGONAL.
!                0 .LE. ML .LT. N .
!
!        MU      INTEGER
!                NUMBER OF DIAGONALS ABOVE THE MAIN DIAGONAL.
!                0 .LE. MU .LT. N .
!                MORE EFFICIENT IF  ML .LE. MU .
!
!     ON RETURN
!
!        ABD     AN UPPER TRIANGULAR MATRIX IN BAND STORAGE AND
!                THE MULTIPLIERS WHICH WERE USED TO OBTAIN IT.
!                THE FACTORIZATION CAN BE WRITTEN  A = L*U  WHERE
!                L  IS A PRODUCT OF PERMUTATION AND UNIT LOWER
!                TRIANGULAR MATRICES AND  U  IS UPPER TRIANGULAR.
!
!        IPVT    INTEGER(N)
!                AN INTEGER VECTOR OF PIVOT INDICES.
!
!        RCOND   DOUBLE PRECISION
!                AN ESTIMATE OF THE RECIPROCAL CONDITION OF  A .
!                FOR THE SYSTEM  A*X = B , RELATIVE PERTURBATIONS
!                IN  A  AND  B  OF SIZE  EPSILON  MAY CAUSE
!                RELATIVE PERTURBATIONS IN  X  OF SIZE  EPSILON/RCOND .
!                IF  RCOND  IS SO SMALL THAT THE LOGICAL EXPRESSION
!                           1.0 + RCOND .EQ. 1.0
!                IS TRUE, THEN  A  MAY BE SINGULAR TO WORKING
!                PRECISION.  IN PARTICULAR,  RCOND  IS ZERO  IF
!                EXACT SINGULARITY IS DETECTED OR THE ESTIMATE
!                UNDERFLOWS.
!
!        Z       DOUBLE PRECISION(N)
!                A WORK VECTOR WHOSE CONTENTS ARE USUALLY UNIMPORTANT.
!                IF  A  IS CLOSE TO A SINGULAR MATRIX, THEN  Z  IS
!                AN APPROXIMATE NULL VECTOR IN THE SENSE THAT
!                NORM(A*Z) = RCOND*NORM(A)*NORM(Z) .
!
!     BAND STORAGE
!
!           IF  A  IS A BAND MATRIX, THE FOLLOWING PROGRAM SEGMENT
!           WILL SET UP THE INPUT.
!
!                   ML = (BAND WIDTH BELOW THE DIAGONAL)
!                   MU = (BAND WIDTH ABOVE THE DIAGONAL)
!                   M = ML + MU + 1
!                   DO 20 J = 1, N
!                      I1 = MAX0(1, J-MU)
!                      I2 = MIN0(N, J+ML)
!                      DO 10 I = I1, I2
!                         K = I - J + M
!                         ABD(K,J) = A(I,J)
!                10    CONTINUE
!                20 CONTINUE
!
!           THIS USES ROWS  ML+1  THROUGH  2*ML+MU+1  OF  ABD .
!           IN ADDITION, THE FIRST  ML  ROWS IN  ABD  ARE USED FOR
!           ELEMENTS GENERATED DURING THE TRIANGULARIZATION.
!           THE TOTAL NUMBER OF ROWS NEEDED IN  ABD  IS  2*ML+MU+1 .
!           THE  ML+MU BY ML+MU  UPPER LEFT TRIANGLE AND THE
!           ML BY ML  LOWER RIGHT TRIANGLE ARE NOT REFERENCED.
!
!     EXAMPLE..  IF THE ORIGINAL MATRIX IS
!
!           11 12 13  0  0  0
!           21 22 23 24  0  0
!            0 32 33 34 35  0
!            0  0 43 44 45 46
!            0  0  0 54 55 56
!            0  0  0  0 65 66
!
!      THEN  N = 6, ML = 1, MU = 2, LDA .GE. 5  AND ABD SHOULD CONTAIN
!
!            *  *  *  +  +  +  , * = NOT USED
!            *  * 13 24 35 46  , + = USED FOR PIVOTING
!            * 12 23 34 45 56
!           11 22 33 44 55 66
!           21 32 43 54 65  *
!
!     LINPACK. THIS VERSION DATED 08/14/78 .
!     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB.
!
!     SUBROUTINES AND FUNCTIONS
!
!     LINPACK DGBFA
!     BLAS DAXPY,DDOT,DSCAL,DASUM
!     FORTRAN DABS,DMAX1,MAX0,MIN0,DSIGN
!
!     INTERNAL VARIABLES
!
!
!
!     COMPUTE 1-NORM OF A
!
      ANORM = 0.0D0
      L = ML + 1
      IS = L + MU
      DO J = 1, N
         ANORM = DMAX1(ANORM,DASUM(L,ABD(IS,J),1))
         IF (IS > ML + 1) IS = IS - 1
         IF (J <= MU) L = L + 1
         IF (J < N - ML) CYCLE
         L = L - 1
      END DO
!
!     FACTOR
!
      CALL DGBFA (ABD, LDA, N, ML, MU, IPVT, INFO)
!
!     RCOND = 1/(NORM(A)*(ESTIMATE OF NORM(INVERSE(A)))) .
!     ESTIMATE = NORM(Z)/NORM(Y) WHERE  A*Z = Y  AND  TRANS(A)*Y = E .
!     TRANS(A)  IS THE TRANSPOSE OF A .  THE COMPONENTS OF  E  ARE
!     CHOSEN TO CAUSE MAXIMUM LOCAL GROWTH IN THE ELEMENTS OF W  WHERE
!     TRANS(U)*W = E .  THE VECTORS ARE FREQUENTLY RESCALED TO AVOID
!     OVERFLOW.
!
!     SOLVE TRANS(U)*W = E
!
      EK = 1.0D0
      Z(:N) = 0.0D0
      M = ML + MU + 1
      JU = 0
      DO K = 1, N
         IF (Z(K) /= 0.0D0) EK = DSIGN(EK,(-Z(K)))
         IF (DABS(EK - Z(K)) > DABS(ABD(M,K))) THEN
            S = DABS(ABD(M,K))/DABS(EK - Z(K))
            CALL DSCAL (N, S, Z, 1)
            EK = S*EK
         ENDIF
         WK = EK - Z(K)
         WKM = (-EK) - Z(K)
         S = DABS(WK)
         SM = DABS(WKM)
         IF (ABD(M,K) /= 0.0D0) THEN
            WK = WK/ABD(M,K)
            WKM = WKM/ABD(M,K)
         ELSE
            WK = 1.0D0
            WKM = 1.0D0
         ENDIF
         KP1 = K + 1
         JU = MIN0(MAX0(JU,MU + IPVT(K)),N)
         MM = M
         IF (KP1 <= JU) THEN
            DO J = KP1, JU
               MM = MM - 1
               SM = SM + DABS(Z(J)+WKM*ABD(MM,J))
               Z(J) = Z(J) + WK*ABD(MM,J)
               S = S + DABS(Z(J))
            END DO
            IF (S < SM) THEN
               T = WKM - WK
               WK = WKM
               MM = M
               DO J = KP1, JU
                  MM = MM - 1
                  Z(J) = Z(J) + T*ABD(MM,J)
               END DO
            ENDIF
         ENDIF
         Z(K) = WK
      END DO
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL (N, S, Z, 1)
!
!     SOLVE TRANS(L)*Y = W
!
      DO KB = 1, N
         K = N + 1 - KB
         LM = MIN0(ML,N - K)
         IF (K < N) Z(K) = Z(K) + DDOT(LM,ABD(M+1,K),1,Z(K+1),1)
         IF (DABS(Z(K)) > 1.0D0) THEN
            S = 1.0D0/DABS(Z(K))
            CALL DSCAL (N, S, Z, 1)
         ENDIF
         L = IPVT(K)
         T = Z(L)
         Z(L) = Z(K)
         Z(K) = T
      END DO
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL (N, S, Z, 1)
!
      YNORM = 1.0D0
!
!     SOLVE L*V = Y
!
      DO K = 1, N
         L = IPVT(K)
         T = Z(L)
         Z(L) = Z(K)
         Z(K) = T
         LM = MIN0(ML,N - K)
         IF (K < N) CALL DAXPY (LM, T, ABD(M+1,K), 1, Z(K+1), 1)
         IF (DABS(Z(K)) <= 1.0D0) CYCLE
         S = 1.0D0/DABS(Z(K))
         CALL DSCAL (N, S, Z, 1)
         YNORM = S*YNORM
      END DO
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL (N, S, Z, 1)
      YNORM = S*YNORM
!
!     SOLVE  U*Z = W
!
      DO KB = 1, N
         K = N + 1 - KB
         IF (DABS(Z(K)) > DABS(ABD(M,K))) THEN
            S = DABS(ABD(M,K))/DABS(Z(K))
            CALL DSCAL (N, S, Z, 1)
            YNORM = S*YNORM
         ENDIF
         IF (ABD(M,K) /= 0.0D0) THEN
            Z(K) = Z(K)/ABD(M,K)
         ELSE
            Z(K) = 1.0D0
         ENDIF
         LM = MIN0(K,M) - 1
         LA = M - LM
         LZ = K - LM
         T = -Z(K)
         CALL DAXPY (LM, T, ABD(LA,K), 1, Z(LZ), 1)
      END DO
!     MAKE ZNORM = 1.0
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL (N, S, Z, 1)
      YNORM = S*YNORM
      IF (ANORM /= 0.0D0) THEN
!
         RCOND = YNORM/ANORM
      ELSE
         RCOND = 0.0D0
      ENDIF
      RETURN
      END SUBROUTINE DGBCO


! =============================================================================
!                       Test the vorticity tendency
!
!  This unit test checks the calculation of the vorticity tendency using the
!  Beltrami flow:
!     u(x, y, z) = (k^2 + l^2)^(-1) * [k*m*sin(mz) - l*alpha*cos(m*z) * sin(k*x + l*y)]
!     v(x, y, z) = (k^2 + l^2)^(-1) * [l*m*sin(mz) + k*alpha*cos(m*z) * sin(k*x + l*y)]
!     w(x, y, z) = cos(m*z) * cos(k*x + l*y)
!  The vorticity of this flow is
!    xi(x, y, z) = alpha * u(x, y, z)
!   eta(x, y, z) = alpha * v(x, y, z)
!  zeta(x, y, z) = alpha * w(x, y, z)
!  For this setting the theoretical vorticity tendency is
!  vtend(z, y, x, 1) = alpha*k*m**2 * (k^2+l^2)^(-1)*sin(k*x+l*y)*cos(k*x+l*y)
!  vtend(z, y, x, 2) = alpha*l*m**2 * (k^2+l^2)^(-1)*sin(k*x+l*y)*cos(k*x+l*y)
!  vtend(z, y, x, 3) = -alpha * m * sin(m*z) * cos(m*z)
! =============================================================================
program test_vtend
    use unit_test
    use constants, only : one, two, pi, f12, f34, three
    use parameters, only : lower, update_parameters, dx, nx, ny, nz, extent
    use fields, only : vortg, velog, vtend, tbuoyg, field_default
    use inversion_utils, only : init_fft, fftxyp2s
    use inversion_mod, only : vor2vel, vor2vel_timer, vorticity_tendency, vtend_timer
    use timer
    implicit none

    double precision              :: error
    double precision, allocatable :: vtend_ref(:, :, :, :)
    integer                       :: ix, iy, iz, ik, il, im
    double precision              :: x, y, z, alpha, fk2l2, k, l, m
    double precision              :: cosmz, sinmz, sinkxly, coskxly

    call register_timer('vorticity', vor2vel_timer)
    call register_timer('vtend', vtend_timer)

    nx = 128
    ny = 128
    nz = 128

    lower  = -f12 * pi * (/one, one, one/)
    extent =  pi * (/one, one, one/)

    allocate(vtend_ref(-1:nz+1, 0:ny-1, 0:nx-1, 3))

    call update_parameters

    call field_default

    call init_fft

    do ik = 1, 3
        k = dble(ik)
        do il = 1, 3
            l = dble(il)
            do im = 1, 3, 2
                m = dble(im)

                alpha = dsqrt(k ** 2 + l ** 2 + m ** 2)
                fk2l2 = one / dble(k ** 2 + l ** 2)

                do ix = 0, nx-1
                    x = lower(1) + ix * dx(1)
                    do iy = 0, ny-1
                        y = lower(2) + iy * dx(2)
                        do iz = -1, nz+1
                            z = lower(3) + iz * dx(3)

                            cosmz = dcos(m * z)
                            sinmz = dsin(m * z)
                            sinkxly = dsin(k * x + l * y)
                            coskxly = dcos(k * x + l * y)

                            ! velocity
                            velog(iz, iy, ix, 1) = fk2l2 * (k * m * sinmz - l * alpha * cosmz) * sinkxly
                            velog(iz, iy, ix, 2) = fk2l2 * (l * m * sinmz + k * alpha * cosmz) * sinkxly
                            velog(iz, iy, ix, 3) = cosmz * coskxly

                            ! vorticity
                            vortg(iz, iy, ix, 1) = alpha * velog(iz, iy, ix, 1)
                            vortg(iz, iy, ix, 2) = alpha * velog(iz, iy, ix, 2)
                            vortg(iz, iy, ix, 3) = alpha * velog(iz, iy, ix, 3)

                            ! reference solution
                            vtend_ref(iz, iy, ix, 1) = alpha * k * m ** 2 * fk2l2 * sinkxly * coskxly
                            vtend_ref(iz, iy, ix, 2) = alpha * l * m ** 2 * fk2l2 * sinkxly * coskxly
                            vtend_ref(iz, iy, ix, 3) = -alpha * m * sinmz * cosmz
                        enddo
                    enddo
                enddo

                call vorticity_tendency(vortg, velog, tbuoyg, vtend)

                error = max(error, maxval(dabs(vtend_ref(0:nz, :, :, :) - vtend(0:nz, :, :, :))))
            enddo
        enddo
    enddo

    call print_result_dp('Test vorticity tendency', error, atol=5.0e-2)

    deallocate(vtend_ref)

end program test_vtend


      FUNCTION gammq(a,x)
      REAL a,gammq,x
CU    USES gcf,gser
      REAL gammcf,gamser,gln
      if(x.lt.0..or.a.le.0.)pause 'bad arguments in gammq'
      if(x.lt.a+1.)then
        call gser(gamser,a,x,gln)
        gammq=1.-gamser
      else
        call gcf(gammcf,a,x,gln)
        gammq=gammcf
      endif
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/gammq.for


!
! Copyright (C) 2001 PWSCF group
! This file is distributed under the terms of the
! GNU General Public License. See the file `License'
! in the root directory of the present distribution,
! or http://www.gnu.org/copyleft/gpl.txt .
!
!
!---------------------------------------------------------------------
subroutine volume (alat, a1, a2, a3, omega)
  !---------------------------------------------------------------------
  !
  !     Compute the volume of the unit cell
  !
  use kinds, ONLY: DP
  implicit none
  !
  !     First the I/O variables
  !
  real(DP) :: alat, a1 (3), a2 (3), a3 (3), omega
  ! input:  lattice parameter (unit length)
  ! input: the first lattice vector
  ! input: the second lattice vector
  ! input: the third lattice vector
  ! input: the volume of the unit cell
  !
  !    Here the local variables required by the routine
  !

  real(DP) :: s
  ! the sign of a permutation
  integer :: i, j, k, l, iperm
  !\
  ! \
  ! /   auxiliary indices
  !/
  ! counter on permutations
  !
  !   Compute the volume
  !
  omega = 0.d0
  s = 1.d0
  i = 1
  j = 2
  k = 3
101 do iperm = 1, 3
     omega = omega + s * a1 (i) * a2 (j) * a3 (k)
     l = i
     i = j
     j = k
     k = l
  enddo
  i = 2
  j = 1
  k = 3
  s = - s

  if (s.lt.0.d0) goto 101

  omega = abs (omega) * alat**3
  return
end subroutine volume


module m
implicit none
contains
!
subroutine disp_vec(v,title)
integer, intent(in) :: v(:)
character (len=*), intent(in) :: title
write (*,"(a)") title // " ="
write (*,"(*(i4))") v
write (*,*)
end subroutine disp_vec
!
subroutine disp_mat(m,title)
integer, intent(in) :: m(:,:)
character (len=*), intent(in) :: title
integer :: i1
write (*,"(a)") title // " ="
do i1=1,size(m,1)
   write (*,"(*(i4))") m(i1,:)
end do
write (*,*)
end subroutine disp_mat
!
pure subroutine twice(m)
integer, intent(in out) :: m(:,:)
m = 2*m
end subroutine twice
!
end module m
!
program vector_subscript ! analog to Numpy "fancy indexing"
use m, only: twice, disp_vec, disp_mat
implicit none
integer, parameter :: n1 = 3, n2 = 4
integer :: mat(n1,n2), vec(n2), i1, i2
forall (i1=1:n1,i2=1:n2) mat(i1,i2) = 10*i1 + i2
vec = mat(1,:)
call disp_vec(vec,"vec")
call disp_vec(vec([1,3]),"vec([1,3])")
call disp_mat(mat,"mat")
call disp_mat(mat([1,3],:),"mat([1,3],:)")
! Vector subscripts in multiple dimensions yield a
! tensor product.
call disp_mat(mat([1,3],[2,4]),"mat([1,3],[2,4])")
call twice(mat)
! line below invalid because intent(in out) dummy argument
! has actual argument with vector subscript
! call twice(mat([1],:))
end program vector_subscript
! output:
! vec =
!   11  12  13  14
!
! vec([1,3]) =
!   11  13
!
! mat =
!   11  12  13  14
!   21  22  23  24
!   31  32  33  34
!
! mat([1,3],:) =
!   11  12  13  14
!   31  32  33  34
!
! mat([1,3],[2,4]) =
!   12  14
!   32  34


!------------------------------------------------------
! Pijl et al, 4.1.2. Zalesak's rotating disk
!------------------------------------------------------

  FUNCTION InitZalesak( Model, n, t ) RESULT(f)
    USE Types
    USE SolverUtils
    IMPLICIT NONE

    TYPE(Model_t) :: Model
    LOGICAL :: Visited = .FALSE., GotIt
    INTEGER :: n,i,j
    REAL(KIND=dp) :: t,x,y,r,Lx,Ly,x0,y0,r0,w0,dist(4),f,a1,a2,b1,b2,f1,f2

    SAVE Visited, x0, y0, r0, w0, a1, a2, b1, b2

    IF(.NOT. Visited) THEN
      Lx = ListGetConstReal(Model % Simulation,'Zalesak Lx',GotIt)
      IF(.NOT. GotIt) Lx = MAXVAL( Model % Nodes % x )
      Ly = ListGetConstReal(Model % Simulation,'Zalesak Ly',GotIt)
      IF(.NOT. GotIt) Ly = MAXVAL( Model % Nodes % y )

      x0 = 0.5 * Lx
      y0 = 0.75 * Ly
      r0 = (3.0/20.0) * Lx
      w0 = r0 / 3.0d0

      a1 = x0 - w0/2
      a2 = x0 + w0/2
      b1 = -1.0
      b2 = y0 + r0 - w0
      Visited = .TRUE.
    END IF

    x = Model % Nodes % x(n)
    y = Model % Nodes % y(n)

    ! distance from horizontal lines
    IF( x < a1 ) THEN
      dist(1) = SQRT( (x-a1)**2.0 + (y-b1)**2.0)
      dist(2) = SQRT( (x-a1)**2.0 + (y-b2)**2.0)
    ELSE IF( x > a2 ) THEN
      dist(1) = SQRT( (x-a2)**2.0 + (y-b1)**2.0)
      dist(2) = SQRT( (x-a2)**2.0 + (y-b2)**2.0)
    ELSE
      dist(1) = ABS(y - b1)
      dist(2) = ABS(y - b2)
    END IF

    ! distance from vertical lines
    IF( y < b1 ) THEN
      dist(3) = SQRT( (x-a1)**2.0 + (y-b1)**2.0)
      dist(4) = SQRT( (x-a2)**2.0 + (y-b1)**2.0)
    ELSE IF( y > b2 ) THEN
      dist(3) = SQRT( (x-a1)**2.0 + (y-b2)**2.0)
      dist(4) = SQRT( (x-a2)**2.0 + (y-b2)**2.0)
    ELSE
      dist(3) = ABS(x - a1)
      dist(4) = ABS(x - a2)
    END IF

    ! minimum distance to the sides of the rectangle
    f1 = MINVAL( ABS( dist ) )

    ! set negative sign inside the rectangle
    IF( x > a1 .AND. x < a2 .AND. y > b1 .AND. y < b2) THEN
      f1 = -f1
    END IF

    ! inside of a circle
    f2 = r0 - SQRT( (x-x0)**2.0 + (y-y0)**2.0 )

    ! take a union of the rectangle and circle
    IF( f2 > 0.0 ) THEN
      IF( f1 < 0.0 ) THEN
        f = f1
      ELSE
        f = MIN(f1,f2)
      END IF
    ELSE
      IF( f1 > 0.0 ) THEN
        f = f2
      ELSE
        f = MIN(f1,f2)
      END IF
    END IF

  END FUNCTION InitZalesak

  FUNCTION RotateVeloX( Model, n, t ) RESULT(f)
    USE Types
    USE SolverUtils
    IMPLICIT NONE

    TYPE(Model_t) :: Model
    LOGICAL :: Visited = .FALSE., GotIt
    INTEGER :: n,i,j
    REAL(KIND=dp) :: t,x,y,r,Lx,Ly,x0,y0,f

    SAVE Visited, x0, y0

    IF(.NOT. Visited) THEN
      Lx = ListGetConstReal(Model % Simulation,'Zalesak Lx',GotIt)
      IF(.NOT. GotIt) Lx = MAXVAL( Model % Nodes % x )
      Ly = ListGetConstReal(Model % Simulation,'Zalesak Ly',GotIt)
      IF(.NOT. GotIt) Ly = MAXVAL( Model % Nodes % y )

      x0 = Lx / 2.0
      y0 = Ly / 2.0
      Visited = .TRUE.
    END IF

    x = Model % Nodes % x(n)
    y = Model % Nodes % y(n)

    f = -(y-y0)

  END FUNCTION RotateVeloX

  FUNCTION RotateVeloY( Model, n, t ) RESULT(f)
    USE Types
    USE SolverUtils
    IMPLICIT NONE

    TYPE(Model_t) :: Model
    LOGICAL :: Visited = .FALSE., GotIt
    INTEGER :: n,i,j
    REAL(KIND=dp) :: t,x,y,r,Lx,Ly,x0,y0,f

    SAVE Visited, x0, y0

    IF(.NOT. Visited) THEN
      Lx = ListGetConstReal(Model % Simulation,'Zalesak Lx',GotIt)
      IF(.NOT. GotIt) Lx = MAXVAL( Model % Nodes % x )
      Ly = ListGetConstReal(Model % Simulation,'Zalesak Ly',GotIt)
      IF(.NOT. GotIt) Ly = MAXVAL( Model % Nodes % y )

      x0 = Lx / 2.0
      y0 = Ly / 2.0
      Visited = .TRUE.
    END IF

    x = Model % Nodes % x(n)
    y = Model % Nodes % y(n)

    f = x - x0

  END FUNCTION RotateVeloY


    program demo_maxexponent
    use,intrinsic :: iso_fortran_env, only : dp=>real64,sp=>real32
    implicit none
    real(kind=sp) :: x
    real(kind=dp) :: y

       print *, minexponent(x), maxexponent(x)
       print *, minexponent(y), maxexponent(y)
    end program demo_maxexponent


program appendage_gen
!-------------------------------------------------------!
!	PROGRAM TO GENERATE DARPA SUBOFF APPENDAGES	!
!	FULL/MODEL RATIO = 24				!
!	POINTS CALCULATED BY EQUATIONS:	UNIT = FEET	!
!	AXIAL LOCATION  = X  = H			!
!			= POSITIONS OF TE ON THE HULL	!
!			H(1) = 12.729617		!
!			H(2) = 13.146284 = BASELINE	!
!			H(3) = 13.562950		!
!	CHORD LENGTH    = CY = -0.466308*RR + 0.88859	!
!	MOUNTING POSITION: 0,90,180,270 DEGREES		!
!			   0 = TOP-DEAD CENTER OF HULL	!
!	NO INTERSECTION WILL HULL PERFROMED		!
!	OUTPUT FILE IS ICEMCFD FORMATED DATA		!
!		Xiaolong Tang: tangxl@buaa.edu.cn	!
!				       01/26/2018	!
!		        	BEIHANG UNIVERSITY	!
!-------------------------------------------------------!
IMPLICIT NONE
REAL(8) XXI,H,RH,AK0,AK1,RMAX,XXX,RR,Z
REAL(8) X,Y,HH,DX,XIB,XI,CY
REAL(8) A,B,C,D,E,RHA,RHAS,DELR,SRS,XINIT,RBSMAX,RO
REAL(8) TE_TRIM_AT
INTEGER I,J,K,NP,ITR,I750
DIMENSION XXI(20),H(3)
CHARACTER(LEN=27) FILENAME
!-------------------------------!
!	DEFINE CONSTANTS	!
!-------------------------------!
! XXI: POSITION ALONG CHORD
! NP : TOTAL NUMBER OF POINTS TO CALCULATE
RH    =   0.1175
AK0   =  10.0
AK1   =  44.6244
NP    =  SIZE(XXI)
RMAX  =   0.833333
DATA XXI /0.0  , 0.005, 0.0125, 0.025, 0.050, 0.075, 0.100,&
         &0.150, 0.200, 0.2500, 0.300, 0.400, 0.500, 0.600,&
         &0.700, 0.800, 0.9000, 0.950, 0.975, 1.000/
WRITE(*,*) "APPENDAGE TRAILING EDGE TRIMED AT ?% CHORD(0-100):"
READ(*,*) TE_TRIM_AT
TE_TRIM_AT = TE_TRIM_AT/100
J=1
DO I=1,NP
  IF(TE_TRIM_AT .LE. XXI(I)) THEN
    XXI(I)=TE_TRIM_AT
    GOTO 1000
  ENDIF
  J=J+1
ENDDO
! NP AFTER TRIMING
1000 NP=J
DATA H   /12.729617, 13.146284, 13.562950/
!-------------------------------!
!    LOOP ON TRAILING EDGE      !
!-------------------------------!
DO 900 K=1,3
  HH = H(K)
  WRITE(6,1) HH
1 FORMAT(//2X,'TRAILING EDGE LOCATED AT X = ',F10.5)
  DX = 0.05
  X  =HH+DX
  WRITE(FILENAME,'(A16,F5.3,A5,I1)') "appdg-noitsct-te",TE_TRIM_AT,"-case",K
  OPEN(10*K,FILE=FILENAME,STATUS="UNKNOWN")
!-------------------------------!
!    LOOP ON AXIAL X 	        !
!    STARTING AT TE		!
!    ENDING AT LE		!
!-------------------------------!
  DO 800 J=1,32
    X=X-DX
    IF(X.GT.HH) GOTO 800
!-------------------------------!
!    DEFINE HULL RADIUS AT X    !
!-------------------------------!
    XIB = (13.979167-X)/3.333333
    A   =               RH*RH +     RH*AK0                *XIB*XIB
    B   = ( 20.0 - 20.0*RH*RH - 4.0*RH*AK0 - 0.333333*AK1)*XIB**3
    C   = (-45.0 + 45.0*RH*RH + 6.0*RH*AK0 +          AK1)*XIB**4
    D   = ( 36.0 - 36.0*RH*RH - 4.0*RH*AK0 -          AK1)*XIB**5
    E   = (-10.0 + 10.0*RH*RH +     RH*AK0 + 0.333333*AK1)*XIB**6
    RHA = RMAX*(A+B+C+D+E)**0.5
    RHAS = RHA*RHA
    RR   = 0.075
    DELR = 0.025
    ITR  = 0
!-------------------------------!
!    LOOP ON RADIUS		!
!    BEGIN WITH R = 0.1		!
!-------------------------------!
    DO 700 I=1,31
      RR=RR+DELR
      CY=-0.466308*RR+0.88859
      XI=(X-HH)/CY+1.0
      IF(XI.LT.0.0 .OR. XI.GT.1.0) GOTO 700
!-------------------------------!
!    DEFINE APPENDAGES          !
!-------------------------------!
      Z = 0.29690*SQRT(XI) - 0.12600*XI - 0.35160*XI*XI&
      & + 0.28520*XI**3    - 0.10450*XI**4
      Z = CY*Z
      SRS = RR*RR + Z*Z
700 CONTINUE
      GOTO 800
!-------------------------------!
!    CALCULATE APPENDAGE   	!
!    SECTION AT GIVEN RADIUS    !
!-------------------------------!
    CY=-0.466308*RR+0.88859
    I750=0
    XINIT=(X-HH)/CY+1.0
!-------------------------------!
!    LOOP ON XI			!
!-------------------------------!
    DO 750 I=1,NP
      XI=XXI(I)
      IF(XI.LT.XINIT) GOTO 750
740 CONTINUE
      XI=XXI(I)
      IF(I750.EQ.0) XI=XINIT
      XXX=(XI-1.0)*CY+HH
      IF(XI.LT.0.0 .OR. XI.GT.1.0) GOTO 750
      Z = 0.29690*SQRT(XI) - 0.12600*XI - 0.35160*XI*XI&
      & + 0.28520*XI**3    - 0.10450*XI**4
      Z = CY*Z
!-------------------------------!
!    WRITE X,Y,+/-Z TO FILE     !
!-------------------------------!
      IF(I750.EQ.0) WRITE(6,2)
2 FORMAT(//6X,1HX,8X,3HR/Y,6X,6H(+/-)Z)
      WRITE(6,3) XXX,RR,Z
      WRITE(10*K,3) XXX,Z,RR
3 FORMAT(3F10.5)
      I750=I750+1
      RBSMAX=RR
      IF(I750.EQ.1) GOTO 740
750 CONTINUE
800 CONTINUE
!-------------------------------!
!    CALCULATE INTERSECTION     !
!    WITH HULL, NOW RADIUS      !
!    LARGER THAN HULL RADIUS   	!
!-------------------------------!
  DELR = 0.05
  DO 850 I=1,NP
    IF(I==1) WRITE(10*k,*)NP,NP
    RO=RR
    RR=RBSMAX+I*DELR
    IF(RR.GT.RMAX) RR=RMAX
    IF(RR.EQ.RO) GOTO 900
    CY=-0.466308*RR+0.88859
    WRITE(6,2)
    DO 840 J=1,NP
    XI=XXI(J)
    XXX=(XI-1.0)*CY+HH
    Z = 0.29690*SQRT(XI) - 0.12600*XI - 0.35160*XI*XI&
    & + 0.28520*XI**3    - 0.10450*XI**4
    Z=CY*Z
    WRITE(6,3) XXX,RR,Z
    WRITE(10*K,3) XXX,Z,RR
840 CONTINUE
850 CONTINUE
    CLOSE(10*K)
900 CONTINUE
STOP
END


submodule(Mod_CauchyElement) SUPCauchyElement

   implicit none

contains

   module subroutine sup_elmVS(e,ndime,tn,dvol,P,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    B_t*P*Ñ_s
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: dvol,P(tn,tn)
      real(rp),             intent(inout) :: elmat(ndime,e%pnode,tn,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp) :: B(ndime,tn)
      real(rp) :: auxB(tn,ndime)
      real(rp) :: BPN(ndime,tn)
      real(rp) :: ii(tn,tn),N(tn,tn)
      real(rp) :: PN(tn,tn)

      call getDiagTensor2Txy(ndime,tn,ii)

      do inode=1,e%pnode

          call sld_calculateB(e,ndime,tn,inode,auxB)
          B   = transpose(auxB)

          do jnode=1,e%pnode

              N = e%shape(jnode,e%igaus)*ii

              PN  = matmul(P,N)
              BPN = matmul(B,PN)*dvol

              elmat(1:ndime,inode,1:tn,jnode) = elmat(1:ndime,inode,1:tn,jnode) + BPN(1:ndime,1:tn)

          end do
      end do

   end subroutine sup_elmVS

   module subroutine sup_elmVP(e,ndime,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    GN_p
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(ndime,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: G(ndime),GN(ndime)

      G  = 0.0_rp
      GN = 0.0_rp

      do inode=1,e%pnode

          call sld_calculateB_inditial(e,ndime,inode,G)

          do jnode=1,e%pnode

              GN = G*e%shape(jnode,e%igaus)*dvol

              elmat(1:ndime,inode,1,jnode) = elmat(1:ndime,inode,1,jnode) + GN(1:ndime)

          end do
      end do

   end subroutine sup_elmVP

   module subroutine sup_elmEU(e,ndime,tn,dvol,P,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    N_t*P*B,   (y,P:grad_s(u))
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: P(tn,tn),dvol
      real(rp),             intent(inout) :: elmat(tn,e%pnode,ndime,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp)    :: ii(tn,tn)
      real(rp)    :: N(tn,tn)
      real(rp)    :: B(tn,ndime)
      real(rp)    :: PB(tn,ndime)
      real(rp)    :: NPB(tn,ndime)

      call getDiagTensor2Txy(ndime,tn,ii)

      do inode=1,e%pnode

          N    = e%shape(inode,e%igaus)*(ii)

          do jnode=1,e%pnode

              call sld_calculateB(e,ndime,tn,jnode,B)

              PB  = matmul(P,B)
              NPB = matmul(N,PB)*dvol

              elmat(1:tn,inode,1:ndime,jnode) = elmat(1:tn,inode,1:ndime,jnode) + NPB(:,:)

          end do
      end do

   end subroutine sup_elmEU

   module subroutine sup_elmES(e,ndime,tn,dvol,D,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    -N_s_t*D*N_s,    (y,D:s)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: dvol,D(tn,tn)
      real(rp),             intent(inout) :: elmat(tn,e%pnode,tn,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp)    :: ii(tn,tn)
      real(rp)    :: N_i(tn,tn),N_j(tn,tn)
      real(rp)    :: B(tn,ndime)
      real(rp)    :: DN(tn,tn)
      real(rp)    :: NDN(tn,tn)

      call getDiagTensor2Txy(ndime,tn,ii)

      do inode=1,e%pnode

          N_i    = e%shape(inode,e%igaus)*(ii)

          do jnode=1,e%pnode

              N_j  = e%shape(jnode,e%igaus)*ii

              DN   = matmul(D,N_j)
              NDN  = matmul(N_i,DN)*dvol

              !Notice the minus sign
              elmat(1:tn,inode,1:tn,jnode) = elmat(1:tn,inode,1:tn,jnode) - NDN(1:tn,1:tn)

          end do
      end do

   end subroutine sup_elmES

   module subroutine sup_elmQU(e,ndime,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    N_iG_j
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,ndime,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: G(ndime),NG(ndime)

      G  = 0.0_rp
      NG = 0.0_rp

      do inode=1,e%pnode
          do jnode=1,e%pnode

              call sld_calculateB_inditial(e,ndime,jnode,G)
              NG = G*e%shape(inode,e%igaus)*dvol

              elmat(1,inode,1:ndime,jnode) = elmat(1,inode,1:ndime,jnode) + NG(1:ndime)

          end do
      end do

   end subroutine sup_elmQU

   module subroutine sup_elmQP(e,ndime,dvol,D_vol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    -D_vol*N_j_t*N_i
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol,D_vol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: DNN

      do inode=1,e%pnode
          do jnode=1,e%pnode

              !Notice the minus sign
              DNN = -D_vol*e%shape(inode,e%igaus)*e%shape(jnode,e%igaus)*dvol

              elmat(1,inode,1,jnode) = elmat(1,inode,1,jnode) + DNN

          end do
      end do

   end subroutine sup_elmQP

end submodule SUPCauchyElement


program molecular_dynamics_2d
use openacc

implicit none
integer N,i,j,p,myid
real*8 Lx,Ly,Vxmax,Vymax,tmax,dt,svx,svy,fx,fy,scl,KE,Temp,t,xdiff,ydiff,r,tau
real, dimension (100) :: x,y,vx,vy,ux,uy,ax,ay
integer, parameter :: seed = 99999999
character (len=90) :: filename
call srand(seed)

Temp = 0.010d0

 write ( *, '(a,i8)' ) &
    '  The number of processors available = ', omp_get_num_procs ( )
write ( *, '(a,i8)' ) &
    '  The number of threads available    = ', omp_get_max_threads ( )

N = 100

Lx = 1.0d0
Ly = 1.0d0

Vxmax = 1.0d0
Vymax = 1.0d0

tmax = 1.0d0
dt = 0.010d0

svx = 0.0d0
svy = 0.0d0

!=============== Initial Condition ==================

do i = 1,N
  x(i) = (rand())*2.0d0*Lx - Lx
  y(i) = (rand())*2.0d0*Ly - Ly
  vx(i) = (rand())*Vxmax - Vxmax/2.0d0
  vy(i) = (rand())*Vymax - Vymax/2.0d0
  svx = svx + vx(i)
  svy = svy + vy(i)
enddo

do i = 1,N
  vx(i) = vx(i) - svx/dfloat(N)
  vy(i) = vy(i) - svy/dfloat(N)
enddo

do i = 1,N
  ax(i) = 0.0d0
  ay(i) = 0.0d0
    do j = 1,N
      if (i .ne. j) then
        xdiff = ( x(i)-x(j) ) - nint ((x(i)-x(j))/(2.0d0*Lx)) * 2.0d0*Lx
        ydiff = ( y(i)-y(j) ) - nint ((y(i)-y(j))/(2.0d0*Ly)) * 2.0d0*Ly
        r = dsqrt (xdiff*xdiff + ydiff*ydiff)
        fx = xdiff/(r*r*r)
        fy = ydiff/(r*r*r)
        ax(i) = ax(i) + fx
        ay(i) = ay(i) + fy
      endif
    enddo
enddo

!=============== Time Loop =======================

do t = 0.0d0+dt, tmax, dt

KE = 0.0d0
!$acc parallel copyin(vx,vy,ax,ay,Lx,Ly) copyout(ux,uy,x,y)
!$acc kernels

  do i = 1,N
    ux(i) = vx(i) + ax(i) * dt / 2.0d0
    uy(i) = vy(i) + ay(i) * dt / 2.0d0
    x(i) = x(i) + ux(i) * dt
    y(i) = y(i) + uy(i) * dt
    x(i) = x(i) - (int(x(i)/(Lx)))  * 2.0d0 * Lx      ! Periodic Boundary Condition
    y(i) = y(i) - (int(y(i)/(Ly))) * 2.0d0 * Ly      ! Periodic Boundary Condition
  enddo
!$acc end kernals

!$acc kernals
  do i = 1,N
    ax(i) = 0.0d0
    ay(i) = 0.0d0
      do j = 1,N
        if (i .ne. j) then
          xdiff = ( x(i)-x(j) ) - nint ((x(i)-x(j))/(2.0d0*Lx)) * 2.0d0*Lx
          ydiff = ( y(i)-y(j) ) - nint ((y(i)-y(j))/(2.0d0*Ly)) * 2.0d0*Ly
          r = dsqrt (xdiff*xdiff + ydiff*ydiff)
          fx = xdiff/(r*r*r)
          fy = ydiff/(r*r*r)
          ax(i) = ax(i) + fx
          ay(i) = ay(i) + fy
        endif
      enddo
  enddo
!$acc end kernals
!$acc end parallel

!$acc parallel copyin(ux,uy,ax,ay) copyout(KE)
!$acc kernels

  do i = 1,N
    vx(i) = ux(i) + ax(i) * dt / 2.0d0
    vy(i) = uy(i) + ay(i) * dt / 2.0d0
    KE = KE + ( vx(i)*vx(i) + vy(i)*vy(i) ) / 2.0d0
  enddo
!$acc end kernals

!$acc kernals
  do i = 1,N
    p = int(t/dt)
    write(filename, '("output/fort.",I8.8)') p+010d0
    open(unit=p+100,file=filename,status='unknown')
    write (p+100,*) x(i), y(i)
  enddo
!$acc end kernals
!$acc end parallel
  close(p+100)

  tau = 10.0d0 * dt
  scl = dsqrt (1.0d0 + (dt/tau) * ((Temp/(2.0d0*KE/(3.0d0*dfloat(N)) )) -1.0d0))

  if (t .le. tmax/2.0d0) then
    do i = 1,N
      vx(i) = scl * vx(i)
      vy(i) = scl * vy(i)
    enddo
  else
    vx(i) = vx(i)
    vy(i) = vy(i)
  endif

enddo     !time

end program molecular_dynamics_2d


      subroutine chol1d ( p, v, qty, npoint, ncol, u, qu )
c  from  * a practical guide to splines *  by c. de boor
c  from  * a practical guide to splines *  by c. de boor
c to be called in  s m o o t h
constructs the upper three diags. in v(i,j), i=2,,npoint-1, j=1,3, of
c  the matrix  6*(1-p)*q-transp.*(d**2)*q + p*r, then computes its
c  l*l-transp. decomposition and stores it also in v, then applies
c  forward and backsubstitution to the right side q-transp.*y in  qty
c  to obtain the solution in  u .
      integer ncol,npoint,   i,npm1,npm2
      real p,qty(npoint),qu(npoint),u(npoint),v(npoint,7),   prev,ratio
     *	  ,six1mp,twop
      npm1 = npoint - 1
c     construct 6*(1-p)*q-transp.*(d**2)*q  +  p*r
      six1mp = 6.*(1.-p)
      twop = 2.*p
      do 2 i=2,npm1
	 v(i,1) = six1mp*v(i,5) + twop*(v(i-1,4)+v(i,4))
	 v(i,2) = six1mp*v(i,6) + p*v(i,4)
    2	 v(i,3) = six1mp*v(i,7)
      npm2 = npoint - 2
      if (npm2 .ge. 2)			go to 10
      u(1) = 0.
      u(2) = qty(2)/v(2,1)
      u(3) = 0.
					go to 41
c  factorization
   10 do 20 i=2,npm2
	 ratio = v(i,2)/v(i,1)
	 v(i+1,1) = v(i+1,1) - ratio*v(i,2)
	 v(i+1,2) = v(i+1,2) - ratio*v(i,3)
	 v(i,2) = ratio
	 ratio = v(i,3)/v(i,1)
	 v(i+2,1) = v(i+2,1) - ratio*v(i,3)
   20	 v(i,3) = ratio
c
c  forward substitution
      u(1) = 0.
      v(1,3) = 0.
      u(2) = qty(2)
      do 30 i=2,npm2
   30	 u(i+1) = qty(i+1) - v(i,2)*u(i) - v(i-1,3)*u(i-1)
c  back substitution
      u(npoint) = 0.
      u(npm1) = u(npm1)/v(npm1,1)
      i = npm2
   40	 u(i) = u(i)/v(i,1)-u(i+1)*v(i,2)-u(i+2)*v(i,3)
	 i = i - 1
	 if (i .gt. 1)			go to 40
c  construct q*u
   41 prev = 0.
      do 50 i=2,npoint
	 qu(i) = (u(i) - u(i-1))/v(i-1,4)
	 qu(i-1) = qu(i) - prev
   50	 prev = qu(i)
      qu(npoint) = -qu(npoint)
					return
      end


subroutine sw_test_case_1()
  implicit none

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Setup shallow water test case 1: Advection of Cosine Bell over the Pole
  !
  ! Reference: Williamson, D.L., et al., "A Standard Test Set for Numerical
  !            Approximations to the Shallow Water Equations in Spherical
  !            Geometry" J. of Comp. Phys., 102, pp. 211--224
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  ! ----- local variables -----
  real (kind=RKIND), parameter :: u0 = 2.0 * pii * a / (12.0 * 86400.0)
  real (kind=RKIND), parameter :: h0 = 1000.0
  real (kind=RKIND), parameter :: theta_c = 0.0
  real (kind=RKIND), parameter :: lambda_c = 3.0*pii/2.0
  real (kind=RKIND), parameter :: alpha = pii/4.0
  integer :: iCell, iEdge, iVtx
  real (kind=RKIND) :: r, v
  real (kind=RKIND), allocatable, dimension(:) :: psiVertex
  ! ----- end local variables -----

  xCell = xCell * a
  yCell = yCell * a
  zCell = zCell * a
  xVertex = xVertex * a
  yVertex = yVertex * a
  zVertex = zVertex * a
  xEdge = xEdge * a
  yEdge = yEdge * a
  zEdge = zEdge * a
  dvEdge = dvEdge * a
  dcEdge = dcEdge * a
  areaCell = areaCell * a**2.0
  areaTriangle = areaTriangle * a**2.0
  kiteAreasOnVertex = kiteAreasOnVertex * a**2.0
  !
  ! Initialize wind field
  !
  allocate(psiVertex(nVertices))
  do iVtx = 1, nVertices
     psiVertex(iVtx) = -a * u0 * ( &
          sin(latVertex(iVtx)) * cos(alpha) - &
          cos(lonVertex(iVtx)) * cos( latVertex(iVtx))*sin(alpha) )
  end do

  do iEdge = 1, nEdges
     u(1,1,iEdge) = -1.0 * ( &
          psiVertex(verticesOnEdge(2,iEdge)) - &
          psiVertex(verticesOnEdge(1,iEdge)) ) / dvEdge(iEdge)
  end do
  deallocate(psiVertex)

  !
  ! Initialize cosine bell at (theta_c, lambda_c)
  !
  do iCell = 1, nCells
     r = sphere_distance(theta_c, lambda_c, latCell(iCell), lonCell(iCell), a)
     if (r < a/3.0) then
        h(1,1,iCell) = (h0 / 2.0) * (1.0 + cos(pii*r*3.0/a))
     else
        h(1,1,iCell) = h0 / 2.0
     end if
  end do

end subroutine sw_test_case_1

subroutine sw_test_case_2()
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Setup shallow water test case 2: Global Steady State Nonlinear Zonal
  !                                  Geostrophic Flow
  !
  ! Reference: Williamson, D.L., et al., "A Standard Test Set for Numerical
  !            Approximations to the Shallow Water Equations in Spherical
  !            Geometry" J. of Comp. Phys., 102, pp. 211--224
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  real (kind=RKIND), parameter :: u0 = 2.0 * pii * a / (12.0 * 86400.0)
  real (kind=RKIND), parameter :: gh0 = 29400.0
  real (kind=RKIND), parameter :: alpha = 0.0

  integer :: iCell, iEdge, iVtx
  real (kind=RKIND) :: v
  real (kind=RKIND), allocatable, dimension(:) :: psiVertex

  !
  ! Scale all distances and areas from a unit sphere to one with radius a
  !
  xCell = xCell * a
  yCell = yCell * a
  zCell = zCell * a
  xVertex = xVertex * a
  yVertex = yVertex * a
  zVertex = zVertex * a
  xEdge = xEdge * a
  yEdge = yEdge * a
  zEdge = zEdge * a
  dvEdge = dvEdge * a
  dcEdge = dcEdge * a
  areaCell = areaCell * a**2.0
  areaTriangle = areaTriangle * a**2.0
  kiteAreasOnVertex = kiteAreasOnVertex * a**2.0

  !
  ! Initialize wind field
  !
  allocate(psiVertex(nVertices))
  do iVtx = 1, nVertices
     psiVertex(iVtx) = -a * u0 * ( &
          sin(latVertex(iVtx)) * cos(alpha) - &
          cos(lonVertex(iVtx)) * cos(latVertex(iVtx)) * sin(alpha) )
  end do
  do iEdge = 1,nEdges
     u(1,1,iEdge) = -1.0 * ( &
          psiVertex(verticesOnEdge(2,iEdge)) - &
          psiVertex(verticesOnEdge(1,iEdge)) ) / dvEdge(iEdge)
  end do
  deallocate(psiVertex)

  !
  ! Generate rotated Coriolis field
  !
  do iEdge = 1, nEdges
     fEdge(iEdge) = 2.0 * omega * &
          ( -cos(lonEdge(iEdge)) * cos(latEdge(iEdge)) * sin(alpha) + &
          sin(latEdge(iEdge)) * cos(alpha) )
  end do
  do iVtx = 1, nVertices
     fVertex(iVtx) = 2.0 * omega * &
          (-cos(lonVertex(iVtx)) * cos(latVertex(iVtx)) * sin(alpha) + &
          sin(latVertex(iVtx)) * cos(alpha) )
  end do

  !
  ! Initialize height field (actually, fluid thickness field)
  !
  do iCell = 1, nCells
     h(1,1,iCell) = (gh0 - (a * omega * u0 + 0.5 * u0**2.0) * &
          (-cos(lonCell(iCell)) * cos(latCell(iCell)) * sin(alpha) + &
          sin(latCell(iCell)) * cos(alpha) )**2.0 ) / gravity
  end do

end subroutine sw_test_case_2

subroutine sw_test_case_5()
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Setup shallow water test case 5: Zonal Flow over an Isolated Mountain
  !
  ! Reference: Williamson, D.L., et al., "A Standard Test Set for Numerical
  !            Approximations to the Shallow Water Equations in Spherical
  !            Geometry" J. of Comp. Phys., 102, pp. 211--224
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  real (kind=RKIND), parameter :: u0 = 20.
  real (kind=RKIND), parameter :: gh0 = 5960.0*gravity
  real (kind=RKIND), parameter :: hs0 = 2000.
  real (kind=RKIND), parameter :: theta_c = pii/6.0
  real (kind=RKIND), parameter :: lambda_c = 3.0*pii/2.0
  real (kind=RKIND), parameter :: rr = pii/9.0
  real (kind=RKIND), parameter :: alpha = 0.0

  integer :: iCell, iEdge, iVtx
  real (kind=RKIND) :: r, v
  real (kind=RKIND), allocatable, dimension(:) :: psiVertex

  !
  ! Scale all distances and areas from a unit sphere to one with radius a
  !
  xCell = xCell * a
  yCell = yCell * a
  zCell = zCell * a
  xVertex = xVertex * a
  yVertex = yVertex * a
  zVertex = zVertex * a
  xEdge = xEdge * a
  yEdge = yEdge * a
  zEdge = zEdge * a
  dvEdge = dvEdge * a
  dcEdge = dcEdge * a
  areaCell = areaCell * a**2.0
  areaTriangle = areaTriangle * a**2.0
  kiteAreasOnVertex = kiteAreasOnVertex * a**2.0

  !
  ! Initialize wind field
  !
  allocate(psiVertex(nVertices))
  do iVtx = 1, nVertices
     psiVertex(iVtx) = -a * u0 * ( &
          sin(latVertex(iVtx)) * cos(alpha) - &
          cos(lonVertex(iVtx)) * cos(latVertex(iVtx)) * sin(alpha) )
  end do
  do iEdge = 1, nEdges
     u(1,1,iEdge) = -1.0 * ( &
          psiVertex(verticesOnEdge(2,iEdge)) - &
          psiVertex(verticesOnEdge(1,iEdge)) ) / dvEdge(iEdge)
  end do
  deallocate(psiVertex)

  !
  ! Generate rotated Coriolis field
  !
  do iEdge = 1, nEdges
     fEdge(iEdge) = 2.0 * omega * &
          (-cos(lonEdge(iEdge)) * cos(latEdge(iEdge)) * sin(alpha) + &
          sin(latEdge(iEdge)) * cos(alpha) )
  end do
  do iVtx = 1, nVertices
     fVertex(iVtx) = 2.0 * omega * &
          (-cos(lonVertex(iVtx)) * cos(latVertex(iVtx)) * sin(alpha) + &
          sin(latVertex(iVtx)) * cos(alpha) )
  end do

  !
  ! Initialize mountain
  !
  do iCell = 1, nCells
     if (lonCell(iCell) < 0.0) lonCell(iCell) = lonCell(iCell) + 2.0 * pii
     r = sqrt(min(rr**2.0, (lonCell(iCell) - lambda_c)**2.0 &
          + (latCell(iCell) - theta_c)**2.0))
     h_s(iCell) = hs0 * (1.0 - r/rr)
  end do

  !
  ! Initialize tracer fields
  !
  do iCell = 1, nCells
     r = sqrt(min(rr**2.0, (lonCell(iCell) - lambda_c)**2.0 + &
          (latCell(iCell) - theta_c)**2.0))
     tracers(1,1,1,iCell) = 1.0 - r/rr
  end do
  if (nTracers > 1) then
     do iCell = 1, nCells
        r = sqrt(min(rr**2.0, (lonCell(iCell) - lambda_c)**2.0 + &
             (latCell(iCell) - theta_c - pii/6.0)**2.0 ) )
        tracers(1,2,1,iCell) = 1.0 - r/rr
     end do
  end if

  !
  ! Initialize height field (actually, fluid thickness field)
  !
  do iCell = 1, nCells
     h(1,1,iCell) = (gh0 - (a * omega * u0 + 0.5 * u0**2.0) * &
          (-cos(lonCell(iCell)) * cos(latCell(iCell)) * sin(alpha) + &
          sin(latCell(iCell)) * cos(alpha) )**2.0 ) / gravity
     h(1,1,iCell) = h(1,1,iCell) - h_s(iCell)
  end do

end subroutine sw_test_case_5

subroutine sw_test_case_6()
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Setup shallow water test case 6: Rossby-Haurwitz Wave
  !
  ! Reference: Williamson, D.L., et al., "A Standard Test Set for Numerical
  !            Approximations to the Shallow Water Equations in Spherical
  !            Geometry" J. of Comp. Phys., 102, pp. 211--224
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  real (kind=RKIND), parameter :: h0 = 8000.0
  real (kind=RKIND), parameter :: w = 7.848e-6
  real (kind=RKIND), parameter :: K = 7.848e-6
  real (kind=RKIND), parameter :: R = 4.0

  integer :: iCell, iEdge, iVtx, nTracers
  real (kind=RKIND) :: v
  real (kind=RKIND), allocatable, dimension(:) :: psiVertex

  !
  ! Scale all distances and areas from a unit sphere to one with radius a
  !
  xCell = xCell * a
  yCell = yCell * a
  zCell = zCell * a
  xVertex = xVertex * a
  yVertex = yVertex * a
  zVertex = zVertex * a
  xEdge = xEdge * a
  yEdge = yEdge * a
  zEdge = zEdge * a
  dvEdge = dvEdge * a
  dcEdge = dcEdge * a
  areaCell = areaCell * a**2.0
  areaTriangle = areaTriangle * a**2.0
  kiteAreasOnVertex = kiteAreasOnVertex * a**2.0

  !
  ! Initialize wind field
  !
  allocate(psiVertex(nVertices))
  do iVtx = 1, nVertices
     psiVertex(iVtx) = -a * a * w * sin(latVertex(iVtx)) + &
          a *a * K * (cos(latVertex(iVtx))**R) * &
          sin(latVertex(iVtx)) * cos(R * lonVertex(iVtx))
  end do
  do iEdge = 1, nEdges
     u(1,1,iEdge) = -1.0 * ( &
          psiVertex(verticesOnEdge(2,iEdge)) - &
          psiVertex(verticesOnEdge(1,iEdge)) ) / dvEdge(iEdge)
  end do
  deallocate(psiVertex)

  !
  ! Initialize height field (actually, fluid thickness field)
  !
  do iCell = 1, nCells
     h(1,1,iCell) = (gravity * h0 + a*a*aa(latCell(iCell)) + &
          a*a*bb(latCell(iCell)) * cos(R*lonCell(iCell)) + &
          a*a*cc(latCell(iCell)) * cos(2.0*R*lonCell(iCell)) ) / gravity
  end do

end subroutine sw_test_case_6

function sphere_distance(lat1, lon1, lat2, lon2, radius)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Compute the great-circle distance between (lat1, lon1) and (lat2, lon2) on a
  !   sphere with given radius.
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  integer, parameter :: RKIND  = selected_real_kind(12)
  real (kind=RKIND), intent(in) :: lat1, lon1, lat2, lon2, radius
  real (kind=RKIND) :: sphere_distance

  real (kind=RKIND) :: arg1

  arg1 = sqrt( sin(0.5*(lat2-lat1))**2 +  &
       cos(lat1)*cos(lat2)*sin(0.5*(lon2-lon1))**2 )
  sphere_distance = 2.*radius*asin(arg1)

end function sphere_distance

function aa(theta)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! A, used in height field computation for Rossby-Haurwitz wave
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  integer, parameter :: RKIND  = selected_real_kind(12)
  real (kind=RKIND), parameter :: w = 7.848e-6
  real (kind=RKIND), parameter :: K = 7.848e-6
  real (kind=RKIND), parameter :: R = 4.0

  real (kind=RKIND) :: aa
  real (kind=RKIND), intent(in) :: theta

  aa = 0.5 * w * (2.0 * omega + w) * cos(theta)**2.0 + &
       0.25 * K**2.0 * cos(theta)**(2.0*R) * ((R+1.0)*cos(theta)**2.0 &
       + 2.0*R**2.0 - R - 2.0 - 2.0*R**2.0 * cos(theta)**(-2.0))

end function aa

function bb(theta)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! B, used in height field computation for Rossby-Haurwitz wave
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none
  integer, parameter :: RKIND  = selected_real_kind(12)
  real (kind=RKIND), parameter :: w = 7.848e-6
  real (kind=RKIND), parameter :: K = 7.848e-6
  real (kind=RKIND), parameter :: R = 4.0

  real (kind=RKIND) :: bb
  real (kind=RKIND), intent(in) :: theta

  bb = (2.0*(omega + w)*K / ((R+1.0)*(R+2.0))) * cos(theta)**R &
       * ((R**2.0 + 2.0*R + 2.0) - ((R+1.0)*cos(theta))**2.0)

end function bb

function cc(theta)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! C, used in height field computation for Rossby-Haurwitz wave
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  implicit none

  integer, parameter :: RKIND  = selected_real_kind(12)
  real (kind=RKIND), parameter :: w = 7.848e-6
  real (kind=RKIND), parameter :: K = 7.848e-6
  real (kind=RKIND), parameter :: R = 4.0

  real (kind=RKIND), intent(in) :: theta
  real (kind=RKIND) :: cc

  cc = 0.25 * K**2.0 * cos(theta)**(2.0*R) * ((R+1.0)*cos(theta)**2.0 - R - 2.0)

end function cc


c
c********************************
c
        subroutine mkhat(s,nu,h)

        complex s(3),h(3)
        integer nu

        real*4 ss(4),det

        ss(1) = s(1)/nu
        ss(2) = s(3)/nu
        ss(3) = real(s(2))/nu
        ss(4) = aimag(s(2))/nu

        det = ss(1)*ss(2) - ss(3)*ss(3) - ss(4)*ss(4)

        h(1) = cmplx((ss(2)/det),0.)
        h(3) = cmplx((ss(1)/det),0.)
        h(2) = cmplx((-ss(3)/det),-ss(4)/det)

        return
        end