! THIS VERSION: GALAHAD 2.1 - 22/03/2007 AT 09:00 GMT.

!-*-*-*-*-*-*-*-*-*-  G A L A H A D   U S E E Q P  *-*-*-*-*-*-*-*-*-*-*-

!  Nick Gould, Dominique Orban and Ph. L. Toint, for GALAHAD productions
!  Copyright reserved
!  March 25th 2004

    MODULE GALAHAD_USEEQP_double

!  CUTEr/AMPL interface to GALAHAD_EQP, an algorithm for solving
!  equality-constrained quadratic program using a projected conjugate
!  gradient method

      USE CUTEr_interface_double
!NOT95USE GALAHAD_CPU_time
      USE GALAHAD_QPT_double
      USE GALAHAD_EQP_double
      USE GALAHAD_SPECFILE_double
      USE GALAHAD_COPYRIGHT

      IMPLICIT NONE

      PRIVATE
      PUBLIC :: USE_EQP

    CONTAINS

!-*-*-*-*-*-*-*-*-*-   U S E _ E Q P  S U B R O U T I N E   -*-*-*-*-*-*-*-*-

     SUBROUTINE USE_EQP( input )

!  --------------------------------------------------------------------
!
!  Solve the equality-constrained quadratic program from CUTEr
!
!     minimize     1/2 x(T) H x + g(T) x
!
!     subject to     A x + c = 0
!
!  using the GALAHAD package GALAHAD_EQP
!
!  --------------------------------------------------------------------

!  Dummy argument

      INTEGER, INTENT( IN ) :: input

!  Parameters

      INTEGER, PARAMETER :: wp = KIND( 1.0D+0 )
      REAL ( KIND = wp ), PARAMETER :: zero = 0.0_wp
      REAL ( KIND = wp ), PARAMETER :: one = 1.0_wp
      REAL ( KIND = wp ), PARAMETER :: ten = 10.0_wp
      REAL ( KIND = wp ), PARAMETER :: infinity = ten ** 19

!     INTEGER, PARAMETER :: n_k = 100, k_k = 3, in = 28
!     REAL ( KIND = wp ), ALLOCATABLE, DIMENSION( :, : ) :: k_val
!     CHARACTER ( len = 10 ) :: filename = 'k.val'

!  Scalars

      INTEGER :: n, m, nmax, mmax, la, lh, iores
!     INTEGER :: np1, npm
      INTEGER :: i, j, l, neh, nea
!     INTEGER :: factorization_integer, factorization_real
      INTEGER :: status
      INTEGER :: alloc_stat, A_ne, H_ne, smt_stat
      INTEGER :: ntotal, natotal, nhtotal, nb, ni
      REAL :: time, timeo, times, timet
      REAL ( KIND = wp ) :: objf, h_max
      LOGICAL :: filexx, is_specfile

!  Specfile characteristics

      INTEGER, PARAMETER :: input_specfile = 34
      INTEGER, PARAMETER :: lspec = 13
      CHARACTER ( LEN = 16 ) :: specname = 'RUNEQP'
      TYPE ( SPECFILE_item_type ), DIMENSION( lspec ) :: spec
      CHARACTER ( LEN = 16 ) :: runspec = 'RUNEQP.SPC'

!  The default values for EQP could have been set as:

! BEGIN RUNEQP SPECIFICATIONS (DEFAULT)
!  write-problem-data                        NO
!  problem-data-file-name                    EQP.data
!  problem-data-file-device                  26
!  print-full-solution                       NO
!  write-solution                            NO
!  solution-file-name                        EQPSOL.d
!  solution-file-device                      62
!  write-result-summary                      NO
!  result-summary-file-name                  EQPRES.d
!  result-summary-file-device                47
!  write-oneline-result-summary              NO
!  result-oneline-summary-file-name          EQPRES_1line.d
!  result-oneline-summary-file-device        47
! END RUNEQP SPECIFICATIONS

!  Default values for specfile-defined parameters

      INTEGER :: dfiledevice = 26
      INTEGER :: rfiledevice = 47
      INTEGER :: lfiledevice = 48
      INTEGER :: sfiledevice = 62
      LOGICAL :: write_problem_data   = .FALSE.
      LOGICAL :: write_1line_summary  = .FALSE.
      LOGICAL :: write_solution       = .FALSE.
      LOGICAL :: write_result_summary = .FALSE.
      CHARACTER ( LEN = 30 ) :: dfilename = 'EQP.data'
!     CHARACTER ( LEN = 30 ) :: rfilename = 'EQPRES.d'
      CHARACTER ( LEN = 34 ) :: rfilename = '../results/EQP_IMPLICIT_fact.d'
!     CHARACTER ( LEN = 30 ) :: lfilename = 'EQPRES_1line.d'
      CHARACTER ( LEN = 36 ) :: lfilename ='../results/EQP_IMPLICIT_fact_1line.d'
      CHARACTER ( LEN = 30 ) :: sfilename = 'EQPSOL.d'
      LOGICAL :: fulsol = .FALSE.
      LOGICAL :: printo = .TRUE.

!  Output file characteristics

      INTEGER, PARAMETER :: out  = 6
      INTEGER :: errout = 6
      CHARACTER ( LEN = 10 ) :: pname

!  Arrays

      TYPE ( EQP_data_type ) :: data
      TYPE ( EQP_control_type ) :: EQP_control
      TYPE ( EQP_inform_type ) :: EQP_inform
      TYPE ( QPT_problem_type ) :: prob

!  Allocatable arrays

      CHARACTER ( LEN = 10 ), ALLOCATABLE, DIMENSION( : ) :: VNAME, CNAME
      REAL ( KIND = wp ), ALLOCATABLE, DIMENSION( : ) :: X0
      LOGICAL, ALLOCATABLE, DIMENSION( : ) :: EQUATN, LINEAR

      CALL CPU_TIME( time )

!  Determine the number of variables and constraints

      CALL CDIMEN( input, nmax, mmax )
      nmax = MAX( nmax, 1 ) ; mmax = MAX( mmax, 1 ) !  for fortran 77 CUTE tools

!  Allocate suitable arrays

      ALLOCATE( X0( nmax ), prob%X_l( nmax ), prob%X_u( nmax ),            &
                VNAME( nmax ), STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
        WRITE( out, 2150 ) 'X', alloc_stat ; STOP
      END IF

      ALLOCATE( prob%C_l( mmax ), prob%C_u( mmax ), prob%Y( mmax ),            &
                CNAME( mmax ), EQUATN( mmax ), LINEAR( mmax ),                 &
                STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
        WRITE( out, 2150 ) 'C', alloc_stat ; STOP
      END IF

!  Set up the data structures necessary to hold the group partially
!  separable function.

      CALL CSETUP( input, out, n, m, X0, prob%X_l, prob%X_u, nmax, EQUATN,     &
                   LINEAR, prob%Y, prob%C_l, prob%C_u, mmax, .FALSE.,          &
                   .FALSE., .FALSE. )
      DEALLOCATE( LINEAR )

      nb = COUNT( prob%X_l > - infinity .OR. prob%X_u < infinity )
      ni = COUNT( .NOT. EQUATN )
      ntotal = n + ni

!  Allocate derived types

      ALLOCATE( prob%X( ntotal ), prob%G( ntotal ), STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
        WRITE( out, 2150 ) 'X0', alloc_stat
        STOP
      END IF

      ALLOCATE( prob%C( m ), STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
        WRITE( out, 2150 ) 'C', alloc_stat
        STOP
      END IF

!  Determine the names of the problem, variables and constraints.

      CALL CNAMES( n, m, pname, VNAME, CNAME )
      ALLOCATE( prob%name( 10 ) )
      prob%name = TRANSFER( pname, prob%name )
      WRITE( out, "( /, ' Problem: ', A10 )" ) pname
!      WRITE( out, "( /, ' n   = ', i8, ' nmax = ', i8,                        &
!                      & ' m    = ', i8, ' mmax = ', i8 )" ) n, nmax, m, mmax

!  Set up the initial estimate of the solution and
!  right-hand-side of the Kuhn-Tucker system.

!  Determine the constant terms for the problem functions.

      prob%X( : n ) = MIN( prob%X_u( : n ), MAX( prob%X_l( : n ), X0( : n ) ) )

!  Set X0 to zero to determine the constant terms for the problem functions

      X0 = zero

!  Evaluate the constant terms of the objective (objf) and constraint
!  functions (C)

      CALL CFN( n, m, X0, objf, m, prob%C( : m ) )

!  Determine the number of nonzeros in the Jacobian

      CALL CDIMSJ( la ) ;
      natotal = la + ni
      la = MAX( la, 1 )

!  Allocate arrays to hold the Jacobian

      ALLOCATE( prob%A%row( natotal ), prob%A%col( natotal ),                 &
                prob%A%val( natotal ), STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
        WRITE( out, 2150 ) 'A', alloc_stat ; STOP
      END IF

!  Evaluate the linear terms of the constraint functions

      CALL CSGR( n, m, .FALSE., mmax, prob%Y, X0, nea, la,                    &
                 prob%A%val( : la ), prob%A%col( : la ), prob%A%row( : la ) )

      DEALLOCATE( X0 )

!  Exclude zeros; set the linear term for the objective function

      A_ne = 0
      prob%G( : ntotal ) = zero
      DO l = 1, nea
        IF ( prob%A%val( l ) /= zero ) THEN
          IF ( prob%A%row( l ) > 0 ) THEN
            A_ne = A_ne + 1
            prob%A%row( A_ne ) = prob%A%row( l )
            prob%A%col( A_ne ) = prob%A%col( l )
            prob%A%val( A_ne ) = prob%A%val( l )
          ELSE
            prob%G( prob%A%col( l ) ) = prob%A%val( l )
          END IF
        END IF
      END DO

!  Determine the number of nonzeros in the Hessian

      CALL CDIMSH( lh )
      nhtotal = lh + nb + ni
      lh = MAX( lh, 1 )

!  Allocate arrays to hold the Hessian

      ALLOCATE( prob%H%row( nhtotal ), prob%H%col( nhtotal ),                  &
                prob%H%val( nhtotal ), STAT = alloc_stat )
      IF ( alloc_stat /= 0 ) THEN
!       WRITE( out, "( ' nea = ', i8, ' la   = ', i8 )" ) nea, la
        WRITE( out, 2150 ) 'H', alloc_stat
        STOP
      END IF

!  Evaluate the Hessian of the Lagrangian function at the initial point.

      CALL CSH( n, m, prob%X( : n ), mmax, prob%Y, neh, lh,                    &
                prob%H%val( : lh ), prob%H%row( : lh ), prob%H%col( : lh ) )
!      WRITE( out, "( ' nea = ', i8, ' la   = ', i8,                           &
!     &               ' neh  = ', i8, ' lh   = ', i8 )" ) nea, la, neh, lh

!  Remove Hessian out of range

      h_max = one
      H_ne = 0
      DO l = 1, neh
        IF ( prob%H%val( l ) == zero ) CYCLE
        i = prob%H%row( l ) ; j = prob%H%col( l )
        IF ( i < 1 .OR. i > n .OR. j < 1 .OR. j > n ) CYCLE
        H_ne = H_ne + 1 ; prob%H%val( H_ne ) = prob%H%val( l )
        h_max = MAX( h_max, ABS( prob%H%val( l ) ) )
        IF ( i >= j ) THEN
          prob%H%row( H_ne ) = i
          prob%H%col( H_ne ) = j
        ELSE
          prob%H%row( H_ne ) = j
          prob%H%col( H_ne ) = i
        END IF
      END DO

!  Add penalty terms for bounded variables

      h_max = ten * h_max
      DO i = 1, n
        IF ( prob%X_l( i ) > - infinity .OR. prob%X_u( i ) < infinity ) THEN
          H_ne = H_ne + 1
          prob%H%row( H_ne ) = i
          prob%H%col( H_ne ) = i
          prob%H%val( H_ne ) = h_max
        END IF
      END DO

!  Add penalty terms for inequality constraints

      ni = 0
      DO i = 1, m
        IF ( .NOT. EQUATN( i ) ) THEN
          ni = ni + 1
          A_ne = A_ne + 1
          prob%A%row( A_ne ) = i
          prob%A%col( A_ne ) = n + ni
          prob%A%val( A_ne ) = - one
          H_ne = H_ne + 1
          prob%H%row( H_ne ) = n + ni
          prob%H%col( H_ne ) = n + ni
          prob%H%val( H_ne ) = h_max
        END IF
      END DO

!   ldummy = .TRUE.
!   IF ( .not. ldummy ) THEN

!     WRITE( out, "( ' maximum element of A = ', ES12.4,                       &
!    &                ' maximum element of H = ', ES12.4 )" )                  &
!      MAXVAL( ABS( prob%A%val( : A_ne ) ) ),                                  &
!      MAXVAL( ABS( prob%H%val( : H_ne ) ) )

!  Store the problem dimensions

      prob%n    = ntotal
      prob%m    = m
      prob%A%ne = A_ne
      IF ( ALLOCATED( prob%A%type ) ) DEALLOCATE( prob%A%type )
      CALL SMT_put( prob%A%type, 'COORDINATE', smt_stat )
      prob%H%ne = H_ne
      IF ( ALLOCATED( prob%H%type ) ) DEALLOCATE( prob%H%type )
      CALL SMT_put( prob%H%type, 'COORDINATE', smt_stat )
      prob%f    = objf

!  Print details

      WRITE( out, "( /, ' m    = ', I10, '  n    = ', I10,                     &
     &               ' (n_slack = ', I7, ')', /,                               &
     &               ' A_ne = ', I10, '  H_ne = ', I10 )" )                    &
                       m, ntotal, ni, A_ne, H_ne
!     WRITE( out, "( ' maximum element of A = ', ES12.4,                       &
!    &                ' maximum element of H = ', ES12.4 )" )                  &
!      MAXVAL( ABS( prob%A%val( : A_ne ) ) ),                                  &
!      MAXVAL( ABS( prob%H%val( : H_ne ) ) )
!   END IF

!  ------------------- problem set-up complete ----------------------

      CALL CPU_TIME( times )

!  ------------------ Open the specfile for runeqp ----------------

      INQUIRE( FILE = runspec, EXIST = is_specfile )
      IF ( is_specfile ) THEN
        OPEN( input_specfile, FILE = runspec, FORM = 'FORMATTED',              &
              STATUS = 'OLD' )

!   Define the keywords

        spec( 1 )%keyword = 'write-problem-data'
        spec( 2 )%keyword = 'problem-data-file-name'
        spec( 3 )%keyword = 'problem-data-file-device'
        spec( 4 )%keyword = 'write-oneline-result-summary'
        spec( 5 )%keyword = 'result-oneline-summary-file-name'
        spec( 6 )%keyword = 'result-oneline-summary-file-device'
        spec( 7 )%keyword = 'print-full-solution'
        spec( 8 )%keyword = 'write-solution'
        spec( 9 )%keyword = 'solution-file-name'
        spec( 10 )%keyword = 'solution-file-device'
        spec( 11 )%keyword = 'write-result-summary'
        spec( 12 )%keyword = 'result-summary-file-name'
        spec( 13 )%keyword = 'result-summary-file-device'

!   Read the specfile

        CALL SPECFILE_read( input_specfile, specname, spec, lspec, errout )

!   Interpret the result

        CALL SPECFILE_assign_logical( spec( 1 ), write_problem_data, errout )
        CALL SPECFILE_assign_string ( spec( 2 ), dfilename, errout )
        CALL SPECFILE_assign_integer( spec( 3 ), dfiledevice, errout )
        CALL SPECFILE_assign_logical( spec( 4 ), write_1line_summary, errout )
        CALL SPECFILE_assign_string ( spec( 5 ), lfilename, errout )
        CALL SPECFILE_assign_integer( spec( 6 ), lfiledevice, errout )
        CALL SPECFILE_assign_logical( spec( 7 ), fulsol, errout )
        CALL SPECFILE_assign_logical( spec( 8 ), write_solution, errout )
        CALL SPECFILE_assign_string ( spec( 9 ), sfilename, errout )
        CALL SPECFILE_assign_integer( spec( 10 ), sfiledevice, errout )
        CALL SPECFILE_assign_logical( spec( 11 ), write_result_summary, errout )
        CALL SPECFILE_assign_string ( spec( 12 ), rfilename, errout )
        CALL SPECFILE_assign_integer( spec( 13 ), rfiledevice, errout )
      END IF

!  If required, print out the (raw) problem data

      IF ( write_problem_data ) THEN
        INQUIRE( FILE = dfilename, EXIST = filexx )
        IF ( filexx ) THEN
           OPEN( dfiledevice, FILE = dfilename, FORM = 'FORMATTED',            &
                 STATUS = 'OLD', IOSTAT = iores )
        ELSE
           OPEN( dfiledevice, FILE = dfilename, FORM = 'FORMATTED',            &
                  STATUS = 'NEW', IOSTAT = iores )
        END IF
        IF ( iores /= 0 ) THEN
          write( out, 2160 ) iores, dfilename
          STOP
        END IF

        WRITE( dfiledevice, "( 'n, m = ', 2I6, ' obj = ', ES12.4 )" )          &
          ntotal, m, prob%f
        WRITE( dfiledevice, "( ' g ', /, ( 5ES12.4 ) )" ) prob%G( : ntotal )
        WRITE( dfiledevice, "( ' c ', /, ( 5ES12.4 ) )" ) prob%C( : m )
        WRITE( dfiledevice, "( ' A_row ', /, ( 10I6 ) )" ) prob%A%row( : A_ne )
        WRITE( dfiledevice, "( ' A_col ', /, ( 10I6 ) )" ) prob%A%col( : A_ne )
        WRITE( dfiledevice, "( ' A_val ', /, ( 5ES12.4 ) )" )                  &
          prob%A%val( : A_ne )
        WRITE( dfiledevice, "( ' H_row ', /, ( 10I6 ) )" ) prob%H%row( : H_ne )
        WRITE( dfiledevice, "( ' H_col ', /, ( 10I6 ) )" ) prob%H%col( : H_ne )
        WRITE( dfiledevice, "( ' H_val ', /, ( 5ES12.4 ) )" )                  &
          prob%H%val( : H_ne )

        CLOSE( dfiledevice )
      END IF

!  If required, append results to a file

      IF ( write_result_summary ) THEN
        INQUIRE( FILE = rfilename, EXIST = filexx )
        IF ( filexx ) THEN
           OPEN( rfiledevice, FILE = rfilename, FORM = 'FORMATTED',            &
                 STATUS = 'OLD', POSITION = 'APPEND', IOSTAT = iores )
        ELSE
           OPEN( rfiledevice, FILE = rfilename, FORM = 'FORMATTED',            &
                 STATUS = 'NEW', IOSTAT = iores )
        END IF
        IF ( iores /= 0 ) THEN
          WRITE( out,                                                          &
            "( ' IOSTAT = ', I6, ' when opening file ', A9, '. Stopping ' )" ) &
            iores, rfilename
          STOP
        END IF
        WRITE( rfiledevice, "( /, ' Problem ', ( 20A ) )" ) pname
      END IF

!  If required, open files for the results

      IF ( write_1line_summary ) THEN
        INQUIRE( FILE = lfilename, EXIST = filexx )
        IF ( filexx ) THEN
           OPEN( lfiledevice, FILE = lfilename, FORM = 'FORMATTED',            &
                 STATUS = 'OLD', POSITION = 'APPEND', IOSTAT = iores )
        ELSE
           OPEN( lfiledevice, FILE = lfilename, FORM = 'FORMATTED',            &
                 STATUS = 'NEW', IOSTAT = iores )
        END IF
        IF ( iores /= 0 ) THEN
          WRITE( out,                                                          &
            "( ' IOSTAT = ', I6, ' when opening file ', A9, '. Stopping ' )" ) &
            iores, lfilename
          STOP
        END IF
        WRITE( lfiledevice, "( 8A )" ) pname
      END IF

!  Set all default values, and override defaults if requested

      CALL EQP_initialize( data, EQP_control )
      IF ( is_specfile )                                                       &
        CALL EQP_read_specfile( EQP_control, input_specfile )

      WRITE( out, "( /, ' problem dimensions:  n = ', I7, ' m = ', I7,         &
     &            ' a_ne = ', I9, ' h_ne = ', I9 )" ) ntotal, m, A_ne, H_ne

      IF ( printo ) CALL COPYRIGHT( out, '2004' )

!  Call the optimizer

      CALL CPU_TIME( timeo )

      IF ( printo ) WRITE( out, " ( ' ** EQP solver used ** ' ) " )
      CALL EQP_solve( prob, data, EQP_control, EQP_inform )

      IF ( printo ) WRITE( out, " ( /, ' Exit from EQP solver' ) " )

      CALL CPU_TIME( timet )

      status = EQP_inform%status
!     factorization_integer = EQP_inform%factorization_integer
!     factorization_real = EQP_inform%factorization_real
      CALL EQP_terminate( data, EQP_control, EQP_inform )

!  Print details of the solution obtained

      WRITE( out, "( ' Stopping with inform%status = ', I3 )" ) status
      DEALLOCATE( VNAME, CNAME )

      WRITE( out, 2000 )                                                        &
        pname, EQP_control%preconditioner, EQP_inform%time%total,               &
        EQP_inform%time%factorize,                                              &
        EQP_inform%cg_iter_inter,                                               &
        EQP_inform%time%factorize + EQP_inform%time%solve_inter,                &
        EQP_control%inner_stop_inter,                                           &
        EQP_inform%cg_iter,                                                     &
        EQP_inform%time%factorize + EQP_inform%time%solve,                      &
        EQP_control%inner_stop_relative

!  If required, write results to  the appropriate files

     IF ( write_1line_summary ) THEN
       BACKSPACE( lfiledevice )
       IF ( status >= 0 ) THEN
         WRITE( lfiledevice, 2010 )                                             &
           pname, EQP_inform%time%total, EQP_inform%time%factorize,             &
           EQP_inform%cg_iter_inter,                                            &
           EQP_inform%time%factorize + EQP_inform%time%solve_inter,             &
           EQP_inform%cg_iter,                                                  &
           EQP_inform%time%factorize + EQP_inform%time%solve,                   &
           status, EQP_control%inner_stop_inter,                                &
           EQP_control%inner_stop_relative, EQP_control%preconditioner
        ELSE
          WRITE( lfiledevice, 2020 ) pname, status
        END IF
      END IF

     IF ( write_result_summary ) THEN
       WRITE( rfiledevice, "( ' Stopping with inform%status = ', I3 )" ) status
       WRITE( rfiledevice, 2000 )                                               &
         pname, EQP_control%preconditioner, EQP_inform%time%total,              &
         EQP_inform%time%factorize,                                             &
         EQP_inform%cg_iter_inter,                                              &
         EQP_inform%time%factorize + EQP_inform%time%solve_inter,               &
         EQP_control%inner_stop_inter,                                          &
         EQP_inform%cg_iter,                                                    &
         EQP_inform%time%factorize + EQP_inform%time%solve,                     &
         EQP_control%inner_stop_relative
      END IF
      IF ( is_specfile ) CLOSE( input_specfile )

      RETURN

!  Non-executable statements

 2000 FORMAT( /, ' Problem: ', A10, /,                                          &
                 ' Preconditioner type = ', I2, /,                              &
                 ' Total time = ', 0P, F8.2, /,                                 &
                 ' Factorization time = ', 0P, F8.2, /,                         &
                 '              cg its    time   reduction', /,                 &
                 ' intermediate ', I6, 0P, F8.2, ES12.4, /,                     &
                 ' total        ', I6, 0P, F8.2, ES12.4 )
 2010 FORMAT( A10, 2( 0P, F8.2 ), 2( I6, 0P, F8.2 ), I6, 2ES8.1, I3 )
 2020 FORMAT( A10, '       -       -', 2( '     -       -' ), I6 )
 2150 FORMAT( ' Allocation error, variable ', A8, ' status = ', I6 )
 2160 FORMAT( ' IOSTAT = ', I6, ' when opening file ', A9, '. Stopping ' )

!  End of subroutine USE_EQP

     END SUBROUTINE USE_EQP

!  End of module USEEQP_double

   END MODULE GALAHAD_USEEQP_double


use, intrinsic:: iso_fortran_env, only: dp=>real64
implicit none

real(dp) :: x
real(dp), external :: dnrm2

x = dnrm2(2, [2._dp, 2._dp], 1)

print *, x

end program


program main

use m_precision
use m_constants_math
use m_trajectory
use m_globals
use m_config_io
use mkl

implicit none

character(len=256) :: cla_buf
character(len=:), allocatable :: fn_sf, fn_sf_bb, fn_sf_sc
real(rp), dimension(:), allocatable :: r_bin !Binned |r|
real(rp), dimension(:), allocatable :: gr_bin !Binned pdf
real(rp), dimension(:), allocatable :: normalization
real(rp), dimension(3) :: ri, rj, rij
real(rp) :: rij_mag, rmin, rmax, bin_size
real(rp) :: rho
integer, dimension(:), allocatable :: count_bin
integer, dimension(:), allocatable :: sc_pos
integer :: natoms, numframes
integer :: num_bins
integer :: teq
integer :: istep, iatm, jatm, ibin, iframe, ierr, ios
integer :: fu_sf, fu_sf_bb, fu_sf_sc
integer :: ibr, ibeg, iend, ia_beg, ia_end
integer :: i, j, k
integer(ip_long) :: mode
real(rp) :: lbox
integer :: mflag

call get_command_argument(1, cla_buf)
fn_cfg = trim(adjustl(cla_buf))

call get_command_argument(2, cla_buf)
fn_traj = trim(adjustl(cla_buf))

call get_command_argument(3, cla_buf)
fn_sf = trim(adjustl(cla_buf))

call get_command_argument(4, cla_buf)
num_bins = int(str_to_d(cla_buf))

call get_command_argument(5, cla_buf)
rmax = str_to_d(cla_buf)

!call get_command_argument(6, cla_buf)
!natoms = int(str_to_d(cla_buf))

call get_command_argument(6, cla_buf)
teq = int(str_to_d(cla_buf))

call get_command_argument(7, cla_buf)
lbox = str_to_d(cla_buf)

bin_size = rmax/num_bins

!write(*,'(a,t15,g0.8)') 'BOXL', lbox
!write(*,'(a,t15,g0.8)') 'RMAX', rmax
!write(*,'(a,t15,g0.8)') 'NATOMS', natoms
!write(*,'(a,t15,g0.8)') 'BIN_SIZE', bin_size
!write(*,'(a,t15,g0.8)') 'NUM_BINS', num_bins
!write(*,'(a,t15,g0.8)') 'NUM_TRAJ', num_traj

allocate(r_bin(1:num_bins))
allocate(gr_bin(1:num_bins))
allocate(normalization(1:num_bins))

numframes = 0
gr_bin = 0.0
rmin = 0.0

!Read final (can be initial as well) configuration file
call read_config(fn_cfg)

natoms = num_atoms
rho = natoms/lbox**3

do i = 1, num_bins
    r_bin(i) = (i-1)*(rmax-rmin)/num_bins
end do

do j = 1, num_bins-1
    normalization(j) = ((r_bin(j+1))**3-(r_bin(j))**3)*(4.0/3.0*math_pi*rho)*(natoms-1)/2
end do

normalization(num_bins) = (rmax**3-(rmax-bin_size)**3)*(4.0/3.0*math_pi*rho)*(natoms-1)/2

!write(*,*) 'rbin', r_bin
!write(*,*) 'grbin', gr_bin
!write(*,*) 'normalization', normalization

mflag = 1
mode =  vmlSetMode(VML_LA)

!Open file for writing sf data & write header line
open(newunit=fu_sf, file=fn_sf, action='write', status='replace')
write(fu_sf, '(a16,2x,a16)') 'r', 'gr'

!Open file for reading trajectory
call traj%open(fn_traj, 'r', ierr)
write(*,*) 'numframes ', traj%num_frames
do iframe = 1, traj%num_frames
    call traj%read(iframe, nts, ierr, mflag, coordinates)
    !write(*,*) 'nts ', nts
    if (nts < teq) cycle
    write(*,*) 'iframe ', iframe
    numframes = numframes + 1
    do iatm = 1, num_atoms-1
        ri = coordinates(:,iatm)
        do jatm = iatm+1, num_atoms
            rj = coordinates(:,jatm)
            rij = rj - ri
            rij(1) = rij(1) - nint(rij(1)/lbox)*lbox
            rij(2) = rij(2) - nint(rij(2)/lbox)*lbox
            rij(3) = rij(3) - nint(rij(3)/lbox)*lbox
            rij_mag = sqrt(rij(1)*rij(1) + rij(2)*rij(2) + rij(3)*rij(3))
            if (rij_mag > rmax) cycle
            ibin = floor(rij_mag/bin_size) + 1
            !r_bin(ibin) = rij(ibin) + rij_mag
            gr_bin(ibin) = gr_bin(ibin) + 1
        end do
    end do
end do

!r_bin = r_bin/gr_bin
gr_bin = gr_bin/normalization/numframes

!write(*,*) 'gr_bin', gr_bin
do i = 1, num_bins
    write(fu_sf, '(es16.7,2x,es16.7)') r_bin(i), gr_bin(i)
end do

!Close files
close(fu_sf)

!*******************************************************************************

end program


C    Copyright(C) 1999-2020 National Technology & Engineering Solutions
C    of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C    NTESS, the U.S. Government retains certain rights in this software.
C
C    See packages/seacas/LICENSE for details

      SUBROUTINE ISOLAP (MXND, XN, YN, LXK, KXL, NXL, LXN, NNN, NNNOLD,
     &   WFAC, NIT, EPS, RO)
C***********************************************************************

C  SUBROUTINE ISOLAP = MESH SMOOTHING BY LAPLACE-S USING GAUSS-SEIDEL

C***********************************************************************

C  VARIABLES USED:
C     WFAC = WEIGHT (0. = LAPLACIAN, 1. = ISOPARAMETRIC)
C     NIT  = THE MAX NUMBER OF ITERATIONS TO DO.
C     EPS  = MINIMUM DISTANCE NODES MUST MOVE TO CONTINUE ITERATIONS
C     RO   = AN UNDER- OR OVER-RELAXATION FACTOR (NORMALLY 1.0)

C***********************************************************************

      DIMENSION KLIST(20), NODES(4)
      DIMENSION XN(MXND), YN(MXND)
      DIMENSION LXN(4, MXND), NXL(2, 3*MXND)
      DIMENSION LXK(4, MXND), KXL(2, 3*MXND)

      LOGICAL BIG, CCW, ERR

      IF (RO .LT. 0.01) RO = 1.
      EPS2 = (EPS*RO)**2

C  ITERATION LOOP

      DO 140 IT = 1, NIT
         BIG = .FALSE.

C  NODE LOOP

         DO 130 NODE = NNNOLD + 1, NNN

C  SKIP CONTINUATION AND BOUNDARY LINES

            IF ((LXN(1, NODE) .GT. 0) .AND. (LXN(2, NODE) .GT. 0)) THEN

C  FIND ELEMENTS ATTACHED TO NODE

               CALL GKXN (MXND, KXL, LXN, NODE, KS, KLIST, ERR)

               SUMX = 0.0
               SUMY = 0.0
               DO 120 KL = 1, KS
                  CCW = .FALSE.
                  KK = KLIST(KL)
                  CALL GNXKA (MXND, XN, YN, KK, NODES, AREA, LXK, NXL,
     &               CCW)

                  DO 100 IN = 1, 4
                     IF (NODES(IN) .EQ. NODE) THEN
                        J1 = IN + 1
                        IF (J1 .GT. 4) J1 = 1
                        GO TO 110
                     END IF
  100             CONTINUE
  110             CONTINUE
                  J2 = J1 + 1
                  IF (J2 .GT. 4) J2 = 1
                  J3 = J2 + 1
                  IF (J3 .GT. 4) J3 = 1

                  SUMX = SUMX + XN(NODES(J1)) + XN(NODES(J3))
     &               - WFAC*XN(NODES(J2))
                  SUMY = SUMY + YN(NODES(J1)) + YN(NODES(J3))
     &               - WFAC*YN(NODES(J2))
  120          CONTINUE

C  REDEFINE THIS NODE-S COORDINATES

               SUMX = SUMX/(DBLE(KS)*(2.0 - WFAC))
               SUMY = SUMY/(DBLE(KS)*(2.0 - WFAC))
               XDEL = RO*(SUMX-XN(NODE))
               YDEL = RO*(SUMY-YN(NODE))
               XN(NODE) = XN(NODE) + XDEL
               YN(NODE) = YN(NODE) + YDEL

C  CHECK FOR CONVERGENCE

               IF ((XDEL*XDEL + YDEL*YDEL) .GT. EPS2) BIG = .TRUE.
            ENDIF
  130    CONTINUE

C  IF NO SIGNIFICANT MOVEMENTS OCCURRED,  RETURN

         IF (.NOT.BIG) RETURN
  140 CONTINUE
      RETURN
      END


! This file is part of NHDS
! Copyright (C) 2018 Daniel Verscharen (d.verscharen@ucl.ac.uk)
!All rights reserved.
!
!Redistribution and use in source and binary forms, with or without
!modification, are permitted provided that the following conditions are met:
!
!1. Redistributions of source code must retain the above copyright notice, this
!   list of conditions and the following disclaimer.
!2. Redistributions in binary form must reproduce the above copyright notice,
!   this list of conditions and the following disclaimer in the documentation
!   and/or other materials provided with the distribution.
!
!THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
!ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
!WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
!DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
!ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
!(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
!LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
!ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
!(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
!SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
!
!The views and conclusions contained in the software and documentation are those
!of the authors and should not be interpreted as representing official policies,
!either expressed or implied, of the NHDS project.

double complex function dispfunct(zeta,kpos)
use input_params
implicit none

double complex :: zeta
logical :: flag,kpos
double precision :: U,V,XI,YI

XI=1.d0*real(zeta)
YI=1.d0*aimag(zeta)

if (kpos) then
   call WOFZ(XI,YI,U,V,flag)
   dispfunct = uniti*sqrt(M_PI)*(U+uniti*V)
else
   call WOFZ(-XI,-YI,U,V,flag)
   dispfunct = -uniti*sqrt(M_PI)*(U+uniti*V)
endif
end function

!Based on
!G.P.M. Poppe, C.M.J. Wijers, "More Efficient Computation of the Complex Error-Function",
!ACM Trans. Math. Software 16, 47 (1990).

!C      ALGORITHM 680, COLLECTED ALGORITHMS FROM ACM.
!C      THIS WORK PUBLISHED IN TRANSACTIONS ON MATHEMATICAL SOFTWARE,
!C      VOL. 16, NO. 1, PP. 47.
      SUBROUTINE WOFZ (XI, YI, U, V, FLAG)
!C
!C  GIVEN A COMPLEX NUMBER Z = (XI,YI), THIS SUBROUTINE COMPUTES
!C  THE VALUE OF THE FADDEEVA-FUNCTION W(Z) = EXP(-Z**2)*ERFC(-I*Z),
!C  WHERE ERFC IS THE COMPLEX COMPLEMENTARY ERROR-FUNCTION AND I
!C  MEANS SQRT(-1).
!C  THE ACCURACY OF THE ALGORITHM FOR Z IN THE 1ST AND 2ND QUADRANT
!C  IS 14 SIGNIFICANT DIGITS; IN THE 3RD AND 4TH IT IS 13 SIGNIFICANT
!C  DIGITS OUTSIDE A CIRCULAR REGION WITH RADIUS 0.126 AROUND A ZERO
!C  OF THE FUNCTION.
!C  ALL REAL VARIABLES IN THE PROGRAM ARE DOUBLE PRECISION.
!C
!C
!C  THE CODE CONTAINS A FEW COMPILER-DEPENDENT PARAMETERS :
!C     RMAXREAL = THE MAXIMUM VALUE OF RMAXREAL EQUALS THE ROOT OF
!C                RMAX = THE LARGEST NUMBER WHICH CAN STILL BE
!C                IMPLEMENTED ON THE COMPUTER IN DOUBLE PRECISION
!C                FLOATING-POINT ARITHMETIC
!C     RMAXEXP  = LN(RMAX) - LN(2)
!C     RMAXGONI = THE LARGEST POSSIBLE ARGUMENT OF A DOUBLE PRECISION
!C                GONIOMETRIC FUNCTION (DCOS, DSIN, ...)
!C  THE REASON WHY THESE PARAMETERS ARE NEEDED AS THEY ARE DEFINED WILL
!C  BE EXPLAINED IN THE CODE BY MEANS OF COMMENTS
!C
!C
!C  PARAMETER LIST
!C     XI     = REAL      PART OF Z
!C     YI     = IMAGINARY PART OF Z
!C     U      = REAL      PART OF W(Z)
!C     V      = IMAGINARY PART OF W(Z)
!C     FLAG   = AN ERROR FLAG INDICATING WHETHER OVERFLOW WILL
!C              OCCUR OR NOT; TYPE LOGICAL;
!C              THE VALUES OF THIS VARIABLE HAVE THE FOLLOWING
!C              MEANING :
!C              FLAG=.FALSE. : NO ERROR CONDITION
!C              FLAG=.TRUE.  : OVERFLOW WILL OCCUR, THE ROUTINE
!C                             BECOMES INACTIVE
!C  XI, YI      ARE THE INPUT-PARAMETERS
!C  U, V, FLAG  ARE THE OUTPUT-PARAMETERS
!C
!C  FURTHERMORE THE PARAMETER FACTOR EQUALS 2/SQRT(PI)
!C
!C  THE ROUTINE IS NOT UNDERFLOW-PROTECTED BUT ANY VARIABLE CAN BE
!C  PUT TO 0 UPON UNDERFLOW;
!C
!C  REFERENCE - GPM POPPE, CMJ WIJERS; MORE EFFICIENT COMPUTATION OF
!C  THE COMPLEX ERROR-FUNCTION, ACM TRANS. MATH. SOFTWARE.
!C
!*
!*
!*
!*
      IMPLICIT DOUBLE PRECISION (A-H, O-Z)

      LOGICAL A, B, FLAG
PARAMETER (FACTOR=1.12837916709551257388D0,RMAXREAL = 0.5D+154,RMAXEXP  = 708.503061461606D0,&
&RMAXGONI = 3.53711887601422D+15)

      FLAG = .FALSE.

      XABS = DABS(XI)
      YABS = DABS(YI)
      X    = XABS/6.3
      Y    = YABS/4.4
!*
!C
!C     THE FOLLOWING IF-STATEMENT PROTECTS
!C     QRHO = (X**2 + Y**2) AGAINST OVERFLOW
!C
      IF ((XABS.GT.RMAXREAL).OR.(YABS.GT.RMAXREAL)) GOTO 100

      QRHO = X**2 + Y**2

      XABSQ = XABS**2
      XQUAD = XABSQ - YABS**2
      YQUAD = 2*XABS*YABS

     A     = QRHO.LT.0.085264D0

      IF (A) THEN
!C
!C  IF (QRHO.LT.0.085264D0) THEN THE FADDEEVA-FUNCTION IS EVALUATED
!C  USING A POWER-SERIES (ABRAMOWITZ/STEGUN, EQUATION (7.1.5), P.297)
!C  N IS THE MINIMUM NUMBER OF TERMS NEEDED TO OBTAIN THE REQUIRED
!C  ACCURACY
!C
        QRHO  = (1-0.85*Y)*DSQRT(QRHO)
        N     = IDNINT(6 + 72*QRHO)
        J     = 2*N+1
        XSUM  = 1.0/J
        YSUM  = 0.0D0
        DO 10 I=N, 1, -1
          J    = J - 2
          XAUX = (XSUM*XQUAD - YSUM*YQUAD)/I
          YSUM = (XSUM*YQUAD + YSUM*XQUAD)/I
          XSUM = XAUX + 1.0/J
 10     CONTINUE
        U1   = -FACTOR*(XSUM*YABS + YSUM*XABS) + 1.0
        V1   =  FACTOR*(XSUM*XABS - YSUM*YABS)
        DAUX =  DEXP(-XQUAD)
        U2   =  DAUX*DCOS(YQUAD)
        V2   = -DAUX*DSIN(YQUAD)

        U    = U1*U2 - V1*V2
        V    = U1*V2 + V1*U2

      ELSE
!C
!C  IF (QRHO.GT.1.O) THEN W(Z) IS EVALUATED USING THE LAPLACE
!C  CONTINUED FRACTION
!C  NU IS THE MINIMUM NUMBER OF TERMS NEEDED TO OBTAIN THE REQUIRED
!C  ACCURACY
!C
!C  IF ((QRHO.GT.0.085264D0).AND.(QRHO.LT.1.0)) THEN W(Z) IS EVALUATED
!C  BY A TRUNCATED TAYLOR EXPANSION, WHERE THE LAPLACE CONTINUED FRACTION
!C  IS USED TO CALCULATE THE DERIVATIVES OF W(Z)
!C  KAPN IS THE MINIMUM NUMBER OF TERMS IN THE TAYLOR EXPANSION NEEDED
!C  TO OBTAIN THE REQUIRED ACCURACY
!C  NU IS THE MINIMUM NUMBER OF TERMS OF THE CONTINUED FRACTION NEEDED
!C  TO CALCULATE THE DERIVATIVES WITH THE REQUIRED ACCURACY
!

        IF (QRHO.GT.1.0) THEN
          H    = 0.0D0
          KAPN = 0
          QRHO = DSQRT(QRHO)
          NU   = IDINT(3 + (1442/(26*QRHO+77)))
        ELSE
          QRHO = (1-Y)*DSQRT(1-QRHO)
          H    = 1.88*QRHO
          H2   = 2*H
          KAPN = IDNINT(7  + 34*QRHO)
          NU   = IDNINT(16 + 26*QRHO)
        ENDIF

        B = (H.GT.0.0)

        IF (B) QLAMBDA = H2**KAPN

        RX = 0.0
        RY = 0.0
        SX = 0.0
        SY = 0.0

        DO 11 N=NU, 0, -1
          NP1 = N + 1
          TX  = YABS + H + NP1*RX
          TY  = XABS - NP1*RY
          C   = 0.5/(TX**2 + TY**2)
          RX  = C*TX
          RY  = C*TY
          IF ((B).AND.(N.LE.KAPN)) THEN
            TX = QLAMBDA + SX
            SX = RX*TX - RY*SY
            SY = RY*TX + RX*SY
            QLAMBDA = QLAMBDA/H2
          ENDIF
 11     CONTINUE

        IF (H.EQ.0.0) THEN
          U = FACTOR*RX
          V = FACTOR*RY
        ELSE
          U = FACTOR*SX
          V = FACTOR*SY
        END IF

        IF (YABS.EQ.0.0) U = DEXP(-XABS**2)

      END IF

!C  EVALUATION OF W(Z) IN THE OTHER QUADRANTS

      IF (YI.LT.0.0) THEN
        IF (A) THEN
          U2    = 2*U2
          V2    = 2*V2
        ELSE
          XQUAD =  -XQUAD

!         THE FOLLOWING IF-STATEMENT PROTECTS 2*EXP(-Z**2)
!        AGAINST OVERFLOW

          IF ((YQUAD.GT.RMAXGONI).OR. (XQUAD.GT.RMAXEXP)) GOTO 100

          W1 =  2*DEXP(XQUAD)
          U2  =  W1*DCOS(YQUAD)
          V2  = -W1*DSIN(YQUAD)
        END IF

        U = U2 - U
        V = V2 - V
        IF (XI.GT.0.0) V = -V
      ELSE
        IF (XI.LT.0.0) V = -V
      END IF

      RETURN

  100 FLAG = .TRUE.
      RETURN

      END


!        Generated by TAPENADE     (INRIA, Ecuador team)
!  tapenade 3.x
!
!  Differentiation of compw in reverse (adjoint) mode (with options noISIZE i8):
!   gradient     of useful results: compw t_h p_h
!   with respect to varying inputs: t_h p_h
! MIT License
!
! Copyright (c) 2020 SHEMAT-Suite
!
! Permission is hereby granted, free of charge, to any person obtaining a copy
! of this software and associated documentation files (the "Software"), to deal
! in the Software without restriction, including without limitation the rights
! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
! copies of the Software, and to permit persons to whom the Software is
! furnished to do so, subject to the following conditions:
!
! The above copyright notice and this permission notice shall be included in all
! copies or substantial portions of the Software.
!
! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
! SOFTWARE.
!> @brief compw calculates compressibility of pure water
!> @param[in] p_h pressure [MPa]
!> @param[in] t_h temperature [degC]
!> @return  compressibility                     compw  [1./Pa]
!> @details
!> compw calculates compressibility of pure water [1/Pa]
!> given temperature (t, in C), and pressure (p,in MPa)
!> at pressure/temperature (p_h,t).\n \n
!>
!> Method: \n
!>
!> compw = 1/rhow d/dP rhow, \n
!>
!> where rhow= water density.\n \n
!>
!>  Main source Zyvoloski1997: \n
!>
!> Zyvoloski, G.A., Robinson, B.A., Dash, Z.V., & Trease, L.L. Summary
!> of the models and methods for the FEHM application - a
!> finite-element heat- and mass-transfer code. United
!> States. doi:10.2172/565545. \n \n
!>
!> See Section 8.4.3. of Zyvoloski1997 for an explanation of the
!> "Rational function approximation" used in this subroutine. \n \n
!> The approximation uses the table of coefficients in Appendix 10 of
!> Zyvoloski1997.\n
!>
!> Alternative source (same text, more modern, without doi): \n
!> https://fehm.lanl.gov/orgs/ees/fehm/pdfs/fehm_mms.pdf \n \n
!>
!> The table of coefficients from Zyvoloski1997 describes the physical
!> values found in Haar1984: \n
!>
!> Lester Haar, John Gallagher, George Kell, NBS/NRC Steam Tables:
!> Thermodynamic and Transport Properties and Computer Programs for
!> Vapor and Liquid States of Water in SI Units, Hemisphere Publishing
!> Corporation, Washington, 1984. \n \n
!>
!>    range of validity:\n
!>    - pressures   0.001 - 110 MPa,\n
!>    - temperature   15 - 360 degC\n \n
!>
!> input:\n
!>   pressure                               p [MPa]\n
!>   temperature                         t in [degC]\n
SUBROUTINE COMPW_AD0(p_h, p_h_ad, t_h, t_h_ad, compw_ad)
  USE MOD_FLOW
  IMPLICIT NONE
! Input Pressure (MPa)
  DOUBLE PRECISION :: p_h
  DOUBLE PRECISION :: p_h_ad
! Input temperature (degc)
  DOUBLE PRECISION :: t_h
  DOUBLE PRECISION :: t_h_ad
! Monomials of temperature and pressure
  DOUBLE PRECISION :: t, t2, t3
  DOUBLE PRECISION :: t_ad, t2_ad, t3_ad
  DOUBLE PRECISION :: p, p2, p3, p4
  DOUBLE PRECISION :: p_ad, p2_ad, p3_ad
  DOUBLE PRECISION :: tp, t2p, tp2
  DOUBLE PRECISION :: tp_ad, t2p_ad, tp2_ad
! Coefficients of numerator of rational function approximation
  DOUBLE PRECISION, PARAMETER :: y0=0.10000000d+01
  DOUBLE PRECISION, PARAMETER :: y1=0.17472599d-01
  DOUBLE PRECISION, PARAMETER :: y2=-0.20443098d-04
  DOUBLE PRECISION, PARAMETER :: y3=-0.17442012d-06
  DOUBLE PRECISION, PARAMETER :: y4=0.49564109d-02
  DOUBLE PRECISION, PARAMETER :: y5=-0.40757664d-04
  DOUBLE PRECISION, PARAMETER :: y6=0.50676664d-07
  DOUBLE PRECISION, PARAMETER :: y7=0.50330978d-04
  DOUBLE PRECISION, PARAMETER :: y8=0.33914814d-06
  DOUBLE PRECISION, PARAMETER :: y9=-0.18383009d-06
! Coefficients of denominator of rational function approximation
  DOUBLE PRECISION, PARAMETER :: z0=0.10009476d-02
  DOUBLE PRECISION, PARAMETER :: z1=0.16812589d-04
  DOUBLE PRECISION, PARAMETER :: z2=-0.24582622d-07
  DOUBLE PRECISION, PARAMETER :: z3=-0.17014984d-09
  DOUBLE PRECISION, PARAMETER :: z4=0.48841156d-05
  DOUBLE PRECISION, PARAMETER :: z5=-0.32967985d-07
  DOUBLE PRECISION, PARAMETER :: z6=0.28619380d-10
  DOUBLE PRECISION, PARAMETER :: z7=0.53249055d-07
  DOUBLE PRECISION, PARAMETER :: z8=0.30456698d-09
  DOUBLE PRECISION, PARAMETER :: z9=-0.12221899d-09
! Numerator and denominator of rational function approximation
  DOUBLE PRECISION :: ta, tb
  DOUBLE PRECISION :: ta_ad, tb_ad
! Derivative of numerator wrt P
  DOUBLE PRECISION :: da
  DOUBLE PRECISION :: da_ad
! Derivative of denominator wrt P
  DOUBLE PRECISION :: db
  DOUBLE PRECISION :: db_ad
! Denominator squared
  DOUBLE PRECISION :: b2
  DOUBLE PRECISION :: b2_ad
! Water density (local)
  DOUBLE PRECISION :: rhow_loc
  DOUBLE PRECISION :: rhow_loc_ad
! Derivative of water density wrt P
  DOUBLE PRECISION :: drhodp
  DOUBLE PRECISION :: drhodp_ad
! Compressibiliy in Mpa
  DOUBLE PRECISION :: compw_mpa
  DOUBLE PRECISION :: compw_mpa_ad
  DOUBLE PRECISION :: temporary_ad
  DOUBLE PRECISION :: compw
  DOUBLE PRECISION :: compw_ad
! pressure [MPa]
  p = p_h
! temperature [degC]
  t = t_h
! Compute monomials in pressure and temperature
  p2 = p*p
  p3 = p2*p
  t2 = t*t
  t3 = t2*t
  tp = p*t
  t2p = t2*p
  tp2 = t*p2
! Numerator of rational function approximation
  ta = y0 + y1*p + y2*p2 + y3*p3 + y4*t + y5*t2 + y6*t3 + y7*tp + y8*tp2&
&   + y9*t2p
! Denominator of rational function approximation
  tb = z0 + z1*p + z2*p2 + z3*p3 + z4*t + z5*t2 + z6*t3 + z7*tp + z8*tp2&
&   + z9*t2p
! Water density
  rhow_loc = ta/tb
! Derivative of numerator
  da = y1 + 2.d0*y2*p + 3.d0*y3*p2 + y7*t + 2.d0*y8*tp + y9*t2
! Derivative of denominator
  db = z1 + 2.d0*z2*p + 3.d0*z3*p2 + z7*t + 2.0*z8*tp + z9*t2
! Denominator squared
  b2 = tb*tb
! Derivative, quotient rule
  drhodp = (da*tb-ta*db)/b2
! Compressibility: (1/rhow_loc) * drhodp [1/MPa]
! Compressibility [1/Pa]
  compw_mpa_ad = compw_ad/pa_conv
  drhodp_ad = compw_mpa_ad/rhow_loc
  rhow_loc_ad = -(drhodp*compw_mpa_ad/rhow_loc**2)
  temporary_ad = drhodp_ad/b2
  da_ad = tb*temporary_ad
  ta_ad = rhow_loc_ad/tb - db*temporary_ad
  db_ad = -(ta*temporary_ad)
  b2_ad = -((da*tb-ta*db)*temporary_ad/b2)
  tb_ad = da*temporary_ad + 2*tb*b2_ad - ta*rhow_loc_ad/tb**2
  tp_ad = z8*2.0*db_ad + y8*2.d0*da_ad + z7*tb_ad + y7*ta_ad
  p3_ad = z3*tb_ad + y3*ta_ad
  t3_ad = z6*tb_ad + y6*ta_ad
  tp2_ad = z8*tb_ad + y8*ta_ad
  p2_ad = z3*3.d0*db_ad + y3*3.d0*da_ad + z2*tb_ad + y2*ta_ad + t*tp2_ad&
&   + p*p3_ad
  t2p_ad = z9*tb_ad + y9*ta_ad
  p_ad = z2*2.d0*db_ad + y2*2.d0*da_ad + z1*tb_ad + y1*ta_ad + t2*t2p_ad&
&   + t*tp_ad + p2*p3_ad + 2*p*p2_ad
  t2_ad = z9*db_ad + y9*da_ad + z5*tb_ad + y5*ta_ad + p*t2p_ad + t*t3_ad
  t_ad = z7*db_ad + y7*da_ad + z4*tb_ad + y4*ta_ad + p2*tp2_ad + p*tp_ad&
&   + t2*t3_ad + 2*t*t2_ad
  t_h_ad = t_h_ad + t_ad
  p_h_ad = p_h_ad + p_ad
END SUBROUTINE COMPW_AD0


!# define MODULE_SOURCETERM 1

module setup_sourceterm_mod

use constants_mod

real(dp), save :: nodes(N_NODES),ws(N_NODES)

contains

subroutine hook_init_820

    use share_mod, only: mesh
    use interpol_mod, only: LegendreGaussNodesAndWeights

    integer :: level

    call LegendreGaussNodesAndWeights(N_NODES-1,nodes,ws)
    nodes = 0.5_dp*nodes
    ws = 0.5_dp*ws

end subroutine

pure subroutine calc_sourceterm(quad,rhs)

    use equations_const_mod
    use mesh_types_mod, only: quad_t
    use quad_utils_mod, only: quad_get_coords
    use setup_manufacsol_mod

    use share_mod, only: rt => runtime

    type(quad_t), intent(inout) :: quad
    real(dp), intent(out)       :: rhs(N_NODES,N_NODES,N_NODES,N_VARS)

    real(dp) :: coords(N_NODES,N_NODES,N_NODES,N_DIMS)

    real(dp) :: xs(N_NODES)
    real(dp) :: ys(N_NODES)
    real(dp) :: zs(N_NODES)

    real(dp) :: time

    integer :: i,j,k, ii,jj,kk

    ! rhs = quad%aux%manufacsol%st
    ! return

    coords = quad_get_coords(quad)

    time = rt%rksimtime
    ! time = 0.0_dp

    rhs = 0.0_dp
    do k = 1,N_NODES; do j = 1,N_NODES; do i = 1,N_NODES;
        xs = coords(i,j,k,1) + quad%delta(1)/REAL(N_NODES,dp)*nodes
        ys = coords(i,j,k,2) + quad%delta(2)/REAL(N_NODES,dp)*nodes
        zs = coords(i,j,k,3) + quad%delta(3)/REAL(N_NODES,dp)*nodes

        do kk = 1,N_NODES; do jj = 1,N_NODES; do ii = 1,N_NODES;
            rhs(i,j,k,:) = rhs(i,j,k,:) + ws(ii)*ws(jj)*ws(kk)*calc_manufacsol(time,xs(ii),ys(jj),zs(kk))
        end do; end do; end do
    end do; end do; end do

end subroutine

function setup_sourceterm_dt(quad) result(dt)

    use mesh_types_mod, only: quad_t

    type(quad_t), intent(in)    :: quad
    real(dp)                    :: dt

    dt = HUGE(1.0_dp)

end function

end module


      DOUBLE PRECISION FUNCTION CCF(M_L, M_H, WAV, SPEC,
     +     WEIGHT, SN, V_R, SNW, N, M)
c     This function calculates the cross-correlation function
c     of a spectrum (SPE, at wavlenths WAV) with a mask. M_L
c     and M_H contain the beginning and end of regions where the mask
c     equals 1, ordered.

      IMPLICIT NONE

c     Speed of light, km/s
      DOUBLE PRECISION C
      PARAMETER (C=2.99792458D5)

      INTEGER N, M, COND
      DOUBLE PRECISION M_L(N), M_H(N), WEIGHT(N)
      DOUBLE PRECISION WAV(M), SPEC(M), SN(M)
      DOUBLE PRECISION V_R, GAMMA, SNW
      DOUBLE PRECISION FRACTION, PIX_INIT, PIX_END
      DOUBLE PRECISION M_LLOC(N), M_HLOC(N)

      INTEGER I, J

C     DOPPLER FACTOR
      GAMMA = DSQRT(1. + V_R / C) / DSQRT(1. - V_R / C)

C     DOPPLER SHIFT MASK
      DO I=1,N
         M_LLOC(I) = M_L(I) * GAMMA
         M_HLOC(I) = M_H(I) * GAMMA
      END DO

C     I marks where we are in terms of masks
      I = 1
      CCF = 0.0D0

      SNW = 0.0D0
      COND = 0
      DO J=2,(M-1)
         PIX_INIT = 0.5*(WAV(J-1) + WAV(J))
         PIX_END  = 0.5*(WAV(J) + WAV(J+1))
         DO WHILE ((M_HLOC(I) < PIX_INIT) .and. (COND .eq. 0))
            IF (I .eq. N) THEN
               COND = 1
            END IF
            IF (COND .eq. 0) THEN
               I = I + 1
            END IF
         END DO
         IF ((PIX_END < M_HLOC(I)) .AND.
     +      (PIX_INIT > M_LLOC(I))) THEN
            CCF = CCF + SPEC(J) * WEIGHT(I) * SN(J)
            SNW = SNW + SN(J)*WEIGHT(I)
         ELSE IF ((PIX_END < M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I)) .AND.
     +           (PIX_END > M_LLOC(I))) THEN
            FRACTION = (PIX_END - M_LLOC(I)) / (PIX_END - PIX_INIT)
            CCF = CCF + SPEC(J) * WEIGHT(I) * FRACTION * SN(J)
            SNW = SNW + FRACTION*SN(J)*WEIGHT(I)
          ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT > M_LLOC(I)) .AND.
     +           (PIX_INIT < M_HLOC(I))) THEN
            FRACTION = (M_HLOC(I) - PIX_INIT) / (PIX_END - PIX_INIT)
            CCF = CCF + SPEC(J) * WEIGHT(I) * FRACTION * SN(J)
            SNW = SNW + FRACTION*SN(J)*WEIGHT(I)
         ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I))) THEN
            FRACTION = (M_HLOC(I) - M_LLOC(I)) / (PIX_END - PIX_INIT)
            CCF = CCF + SPEC(J) * WEIGHT(I) * FRACTION * SN(J)
            SNW = SNW + FRACTION*SN(J)*WEIGHT(I)
         END IF
      END DO

C      CCF = CCF / SNW

      END

      DOUBLE PRECISION FUNCTION CCFPIX(M_L, M_H, X, THAR,
     +     DELTA, N, M)
c     This function calculates the cross-correlation function

      IMPLICIT NONE

      INTEGER N, M, COND
      DOUBLE PRECISION M_L(N), M_H(N)
      DOUBLE PRECISION X(M), THAR(M)
      DOUBLE PRECISION DELTA
      DOUBLE PRECISION FRACTION, PIX_INIT, PIX_END

      DOUBLE PRECISION M_LLOC(N), M_HLOC(N)

      INTEGER I, J

C     SHIFT MASK BY DELTA
      DO I=1,N
         M_LLOC(I) = M_L(I) + DELTA
         M_HLOC(I) = M_H(I) + DELTA
      END DO

C     I marks where we are in terms of masks
      I = 1
      CCFPIX = 0.0D0

      COND = 0
      DO J=2,(M-1)
         PIX_INIT = 0.5*(X(J-1) + X(J))
         PIX_END  = 0.5*(X(J) + X(J+1))
         DO WHILE ((M_HLOC(I) < PIX_INIT) .and. (COND .eq. 0))
            IF (I .eq. N) THEN
               COND = 1
            END IF
            IF (COND .eq. 0) THEN
               I = I + 1
            END IF
         END DO
         IF ((PIX_END < M_HLOC(I)) .AND.
     +      (PIX_INIT > M_LLOC(I))) THEN
            CCFPIX = CCFPIX + THAR(J)
         ELSE IF ((PIX_END < M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I)) .AND.
     +           (PIX_END > M_LLOC(I))) THEN
            FRACTION = (PIX_END - M_LLOC(I)) / (PIX_END - PIX_INIT)
            CCFPIX = CCFPIX + THAR(J) * FRACTION
         ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT > M_LLOC(I)) .AND.
     +           (PIX_INIT < M_HLOC(I))) THEN
            FRACTION = (M_HLOC(I) - PIX_INIT) / (PIX_END - PIX_INIT)
            CCFPIX = CCFPIX + THAR(J) * FRACTION
         ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I))) THEN
            FRACTION = (M_HLOC(I) - M_LLOC(I)) / (PIX_END - PIX_INIT)
            CCFPIX = CCFPIX + THAR(J) * FRACTION
         END IF
      END DO

      END

      DOUBLE PRECISION FUNCTION CCFCOS(M_L, M_H, WAV, SPEC,
     +     WEIGHT, SN, V_R, SNW, N, M)
c     This function calculates the cross-correlation function
c     of a spectrum (SPE, at wavelengths WAV) with a mask. M_L
c     and M_H contain the beginning and end of regions where the mask
c     equals 1, ordered.
c     This version uses a sine function instead of a box --> better lobes
c     The function used is sin((lambda-lambda_init)*pi/(2*half_width))

      IMPLICIT NONE

c     Speed of light, km/s
      DOUBLE PRECISION C
      PARAMETER (C=2.99792458D5)

      INTEGER N, M, COND
      DOUBLE PRECISION M_L(N), M_H(N), WEIGHT(N)
      DOUBLE PRECISION WAV(M), SPEC(M), SN(M)
      DOUBLE PRECISION V_R, GAMMA, SNW
      DOUBLE PRECISION PIX_INIT, PIX_END
      DOUBLE PRECISION M_LLOC(N), M_HLOC(N), HW(N)
      DOUBLE PRECISION WD, NORM, ARG

      INTEGER I, J

      DOUBLE PRECISION PI
      PARAMETER (PI=3.1415926535897931D0)

C     DOPPLER FACTOR
      GAMMA = DSQRT(1. + V_R / C) / DSQRT(1. - V_R / C)

C     DOPPLER SHIFT MASK
      DO I=1,N
         M_LLOC(I) = M_L(I) * GAMMA
         M_HLOC(I) = M_H(I) * GAMMA
         HW(I)     = 0.5D0*( M_HLOC(I) - M_LLOC(I) )
      END DO

C     I marks where we are in terms of masks
      I = 1
      CCFCOS = 0.0D0
      SNW = 0.0D0
      COND = 0
      DO J=2,(M-1)
         PIX_INIT = 0.5*(WAV(J-1) + WAV(J))
         PIX_END  = 0.5*(WAV(J) + WAV(J+1))
         DO WHILE ((M_HLOC(I) < PIX_INIT) .and. (COND .eq. 0))
            IF (I .eq. N) THEN
               COND = 1
            END IF
            IF (COND .eq. 0) THEN
               I = I + 1
            END IF
         END DO
         NORM = HW(I)
         ARG  = PI / (2.0D0*HW(I))
         IF ((PIX_END < M_HLOC(I)) .AND.
     +      (PIX_INIT > M_LLOC(I))) THEN
            WD = NORM*( DCOS((PIX_INIT-M_LLOC(I))*ARG)
     +           - DCOS((PIX_END-M_LLOC(I))*ARG) )
            WD = WD / (PIX_END-PIX_INIT)
            CCFCOS = CCFCOS + WD*SPEC(J)
            SNW = SNW + WD
         ELSE IF ((PIX_END < M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I)) .AND.
     +           (PIX_END > M_LLOC(I))) THEN
            WD = NORM*( 1.0D0
     +           - DCOS((PIX_END-M_LLOC(I))*ARG) )
            WD = WD / (PIX_END-PIX_INIT)
            CCFCOS = CCFCOS + SPEC(J) * WD
            SNW = SNW + WD
          ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT > M_LLOC(I)) .AND.
     +           (PIX_INIT < M_HLOC(I))) THEN
            WD = NORM*( DCOS((PIX_INIT-M_LLOC(I))*ARG)
     +           + 1.0D0)
            WD = WD / (PIX_END-PIX_INIT)
            CCFCOS = CCFCOS + SPEC(J) * WD
            SNW = SNW + WD
         ELSE IF ((PIX_END > M_HLOC(I)) .AND.
     +           (PIX_INIT < M_LLOC(I))) THEN
            WD = 2 * NORM
            WD = WD / (PIX_END-PIX_INIT)
            CCFCOS = CCFCOS + SPEC(J) * WD
            SNW = SNW + WD
         END IF
      END DO

C      CCF = CCF / SNW

      END


C SUBROUTINE PHMINV.F
C
C SOURCE
C   BEVINGTON, PAGES 302-303.
C
C PURPOSE
C   INVERT A SYMMETRIC MATRIX AND CALCULATE ITS DETERMINANT
C
C USAGE
C   CALL PHMINV (ARRAY, IK, JK, NORDER, DET)
C
C DESCRIPTION OF PARAMETERS
C   ARRAY  - INPUT MATRIX WHICH IS REPLACED BY ITS INVERSE
C   IK     - WORK ARRAY REQUIRED BY PHMINV
C   JK     - WORK ARRAY REQUIRED BY PHMINV
C   NORDER - DEGREE OF MATRIX (ORDER OF DETERMINANT)
C   DET    - DETERMINANT OF INPUT MATRIX
C
C SUBROUTINES AND FUNCTION SUBPROGRAMS REQUIRED
C   NONE
C
C COMMENT
C   DIMENSION STATEMENT VALID FOR NORDER UP TO 10
C
	SUBROUTINE PHMINV (ARRAY,IK,JK,NORDER,DET)
	DOUBLE PRECISION ARRAY,AMAX,SAVE
	DIMENSION ARRAY(NORDER,NORDER),IK(NORDER),JK(NORDER)
C
10	DET=1.
11	DO 100 K=1,NORDER
C
C FIND LARGEST ELEMENT ARRAY(I,J) IN REST OF MATRIX
C
	AMAX=0.
21	CONTINUE
	IK(K) = K
	JK(K) = K
	DO 30 I=K,NORDER
	DO 30 J=K,NORDER
23	IF (DABS(AMAX)-DABS(ARRAY(I,J))) 24,24,30
24	AMAX=ARRAY(I,J)
	IK(K)=I
	JK(K)=J
30	CONTINUE
C
C INTERCHANGE ROWS AND COLUMNS TO PUT AMAX IN ARRAY(K,K)
C
41	I=IK(K)
	IF (I-K) 21,51,43
43	DO 50 J=1,NORDER
	SAVE=ARRAY(K,J)
	ARRAY(K,J)=ARRAY(I,J)
50	ARRAY(I,J)=-SAVE
51	J=JK(K)
	IF (J-K) 21,600,53
53	DO 60 I=1,NORDER
	SAVE=ARRAY(I,K)
	ARRAY(I,K)=ARRAY(I,J)
60	ARRAY(I,J)=-SAVE

C
C CHECK FOR SINGULAR MATRIX
C
600  	IF (AMAX) 61,610,61
610	DET=0.
	DO 700 I=1,NORDER
	IF (I-K) 630,700,630
630	ARRAY(I,K)=0.
700	CONTINUE
	DO 800 J=1,NORDER
	IF (J-K) 730,800,730
730	ARRAY(K,J)=0.
800	CONTINUE
	ARRAY(K,K)=0.
	GOTO 100
C
C ACCUMULATE ELEMENTS OF INVERSE MATRIX
C
61	DO 70 I=1,NORDER
	IF (I-K) 63,70,63
63	ARRAY(I,K)=-ARRAY(I,K)/AMAX
70	CONTINUE
71	DO 80 I=1,NORDER
	DO 80 J=1,NORDER
	IF (I-K) 74,80,74
74	IF (J-K) 75,80,75
75	ARRAY(I,J)=ARRAY(I,J)+ARRAY(I,K)*ARRAY(K,J)
80	CONTINUE
81	DO 90 J=1,NORDER
	IF (J-K) 83,90,83
83	ARRAY(K,J)=ARRAY(K,J)/AMAX
90	CONTINUE
	ARRAY(K,K)=1./AMAX
100	DET=DET*AMAX
C
C RESTORE ORDERING OF MATRIX
C
101	DO 130 L=1,NORDER
	K=NORDER-L+1
	J=IK(K)
	IF (J-K) 111,111,105
105	DO 110 I=1,NORDER
	SAVE=ARRAY(I,K)
	ARRAY(I,K)=-ARRAY(I,J)
110	ARRAY(I,J)=SAVE
111	I=JK(K)
	IF (I-K) 130,130,113
113	DO 120 J=1,NORDER
	SAVE=ARRAY(K,J)
	ARRAY(K,J)=-ARRAY(I,J)
120	ARRAY(I,J)=SAVE
130	CONTINUE
140	RETURN
	END


subroutine strloc(pnode,ndime,npoin,&
     lnods,lpoin,kpoin,elcod,elvel,strea,&
     nocha,noinv,westr)

!-----------------------------------------------------------------------
!****f* Mathru/strloc
! NAME
!    strloc
! DESCRIPTION
!    This routine computes the stream-function for each element
! USES
! USED BY
!    stream
!***
!-----------------------------------------------------------------------
!  use      def_kintyp
  use   typre
  implicit none
  integer(ip), intent(in)    :: pnode,ndime,npoin
  real(rp),    intent(in)    :: elcod(ndime,pnode),elvel(ndime*pnode)
  integer(ip), intent(in)    :: lnods(pnode)
  integer(ip), intent(in)    :: nocha(pnode),noinv(pnode)
  real(rp),    intent(in)    :: westr(pnode-1)
  integer(ip), intent(inout) :: lpoin(npoin)
  real(rp),    intent(inout) :: strea(npoin)
  integer(ip)                :: kpoin,inoco,itouc,ipoco,mnodb,mnoda
  integer(ip)                :: icoun,ivxin,ipoi1,ipoi2
  integer(ip)                :: ivxfi,jpoin,ipini,ipfin,inode,lapoi
  integer(ip)                :: ipoi3,ipoi4,ipo13,ipo14,ipo15,ipo16
  real(rp)                   :: xcomp,ycomp,unori,unorf,strfa,preno
  real(rp)                   :: strep,w1,w2,w3,w4
  logical(lg)                :: lofin
!
! Identify first node where the streamfunction is known
!
  inoco=0
  itouc=0
  do while(itouc==0)
     inoco=inoco+1
     ipoco=lnods(inoco)
     if(lpoin(ipoco)>=1) itouc=1
  end do
!
! Compute the streamfuntion at the nodes on the edges of the element
!
  inoco=nocha(inoco)
  do icoun=1,pnode
     mnodb=inoco+icoun
     lofin=.false.
     do while((.not.lofin).and.(mnodb<=pnode))
        if (nocha(mnodb)==0) then
           mnodb=mnodb+1
        else
           lofin=.true.
        end if
     end do
     if(mnodb>pnode) mnodb=mod(mnodb,pnode)
     if(mnodb==0) mnodb=pnode
     mnoda=mnodb-1
     if(mnoda==0) then
        mnoda=pnode
        do while(nocha(mnoda)==0)
           mnoda=mnoda-1
        end do
     end if
     mnodb=noinv(mnodb)
     mnoda=noinv(mnoda)
     xcomp=elcod(1,mnodb)-elcod(1,mnoda)
     ycomp=elcod(2,mnodb)-elcod(2,mnoda)
     ivxin=(mnoda-1)*2+1
     ivxfi=(mnodb-1)*2+1
     unori=-elvel(ivxin)*ycomp+elvel(ivxin+1)*xcomp
     unorf=-elvel(ivxfi)*ycomp+elvel(ivxfi+1)*xcomp
     ipini=lnods(mnoda)
     ipfin=lnods(mnodb)
     strfa=strea(ipini)/real(lpoin(ipini))
     strea(ipfin)=strea(ipfin)+strfa-0.5*(unori+unorf)
     if(lpoin(ipfin).eq.0) kpoin=kpoin+1
     lpoin(ipfin)=lpoin(ipfin)+1
  end do
!
! Compute the streamfunction at the interior nodes. For the cubic elements
! P3 and Q3 (10 and 16 nodes), a special routine is called. If the same
! algorithm as for the rest of elements is to be used, only the corner
! nodes are employed to compute the streamfunction (see routine CHANUM)
!
  if((pnode==10).or.(pnode==16)) then
     call strcub(pnode,npoin,kpoin,lpoin,lnods,strea)
  else
     if(nocha(pnode)==0) then
        preno=0.0
        do inode=1,min(pnode-1,8)
           jpoin=lnods(inode)
           strep=strea(jpoin)/real(lpoin(jpoin))
           preno=preno+westr(inode)*strep
        end do
        lapoi=lnods(pnode)
        strea(lapoi)=preno
        lpoin(lapoi)=lpoin(lapoi)+1
        kpoin=kpoin+1
     end if
  end if

end subroutine strloc


! @@name:	declare_target.5f
! @@type:	F-free
! @@compilable:	yes
! @@linkable:	no
! @@expect:	success
module my_global_array
!$omp declare target (N,Q)
integer, parameter :: N=10000, M=1024
real               :: Q(N,N)
contains
function P(k,i)
!$omp declare simd uniform(i) linear(k) notinbranch
!$omp declare target
real               :: P
integer,intent(in) :: k,i
   P=(Q(k,i) * Q(i,k))
end function
end module
function accum() result(tmp)
use my_global_array
real    :: tmp, tmp1
integer :: i
   tmp = 0.0e0
   !$omp target
   !$omp parallel do private(tmp1) reduction(+:tmp)
   do i=1,N
      tmp1 = 0.0e0
      !$omp simd reduction(+:tmp1)
      do k = 1,M
         tmp1 = tmp1 + P(k,i)
      end do
      tmp = tmp + tmp1
   end do
   !$omp end target
end function


subroutine units(scale_l,scale_t,scale_d,scale_v,scale_nH,scale_T2)
  use amr_commons
  use hydro_commons
  use cooling_module
  real(dp)::scale_nH,scale_T2,scale_t,scale_v,scale_d,scale_l
  !-----------------------------------------------------------------------
  ! Conversion factors from user units into cgs units
  ! For gravity runs, make sure that G=1 in user units.
  !-----------------------------------------------------------------------

  ! scale_d converts mass density from user units into g/cc
  scale_d = 3.9234d-18

  ! scale_t converts time from user units into seconds
  scale_t = 1.0/sqrt(6.67d-8*scale_d)

  ! scale_l converts distance from user units into cm
  scale_l = 4.947d+16

  ! scale_v convert velocity in user units into cm/s
  scale_v = scale_l / scale_t

  ! scale_T2 converts (P/rho) in user unit into (T/mu) in Kelvin
  scale_T2 = mH/kB * scale_v**2

  ! scale_nH converts rho in user units into nH in H/cc
  scale_nH = X/mH * scale_d

end subroutine units


! REV PROCESSED 213 LINES OF CODE. PROGRAM DONE.
REAL FUNCTION GAMMA1(X)
   use da_control, only : pi
!D    DOUBLE PRECISION FUNCTION DGAMMA(X)
!----------------------------------------------------------------------
!
! THIS ROUTINE CALCULATES THE GAMMA FUNCTION FOR A REAL ARGUMENT X.
!   COMPUTATION IS BASED ON AN ALGORITHM OUTLINED IN REFERENCE 1.
!   THE PROGRAM USES RATIONAL FUNCTIONS THAT APPROXIMATE THE GAMMA
!   FUNCTION TO AT LEAST 20 SIGNIFICANT DECIMAL DIGITS.  COEFFICIENTS
!   FOR THE APPROXIMATION OVER THE INTERVAL (1,2) ARE UNPUBLISHED.
!   THOSE FOR THE APPROXIMATION FOR X .GE. 12 ARE FROM REFERENCE 2.
!   THE ACCURACY ACHIEVED DEPENDS ON THE ARITHMETIC SYSTEM, THE
!   COMPILER, THE INTRINSIC FUNCTIONS, AND PROPER SELECTION OF THE
!   MACHINE-DEPENDENT CONSTANTS.
!
!
!*******************************************************************
!*******************************************************************
!
! EXPLANATION OF MACHINE-DEPENDENT CONSTANTS
!
! BETA   - RADIX FOR THE FLOATING-POINT REPRESENTATION
! MAXEXP - THE SMALLEST POSITIVE POWER OF BETA THAT OVERFLOWS
! XBIG   - THE LARGEST ARGUMENT FOR WHICH GAMMA(X) IS REPRESENTABLE
!          IN THE MACHINE, I.E., THE SOLUTION TO THE EQUATION
!                  GAMMA(XBIG) = BETA**MAXEXP
! XINF   - THE LARGEST MACHINE REPRESENTABLE FLOATING-POINT NUMBER;
!          APPROXIMATELY BETA**MAXEXP
! EPS    - THE SMALLEST POSITIVE FLOATING-POINT NUMBER SUCH THAT
!          1.0+EPS .GT. 1.0
! XMININ - THE SMALLEST POSITIVE FLOATING-POINT NUMBER SUCH THAT
!          1/XMININ IS MACHINE REPRESENTABLE
!
!     APPROXIMATE VALUES FOR SOME IMPORTANT MACHINES ARE:
!
!                            BETA       MAXEXP        XBIG
!
! CRAY-1         (S.P.)        2         8191        966.961
! CYBER 180/855
!   UNDER NOS    (S.P.)        2         1070        177.803
! IEEE (IBM/XT,
!   SUN, ETC.)   (S.P.)        2          128        35.040
! IEEE (IBM/XT,
!   SUN, ETC.)   (D.P.)        2         1024        171.624
! IBM 3033       (D.P.)       16           63        57.574
! VAX D-FORMAT   (D.P.)        2          127        34.844
! VAX G-FORMAT   (D.P.)        2         1023        171.489
!
!                            XINF         EPS        XMININ
!
! CRAY-1         (S.P.)   5.45E+2465   7.11E-15    1.84E-2466
! CYBER 180/855
!   UNDER NOS    (S.P.)   1.26E+322    3.55E-15    3.14E-294
! IEEE (IBM/XT,
!   SUN, ETC.)   (S.P.)   3.40E+38     1.19E-7     1.18E-38
! IEEE (IBM/XT,
!   SUN, ETC.)   (D.P.)   1.79D+308    2.22D-16    2.23D-308
! IBM 3033       (D.P.)   7.23D+75     2.22D-16    1.39D-76
! VAX D-FORMAT   (D.P.)   1.70D+38     1.39D-17    5.88D-39
! VAX G-FORMAT   (D.P.)   8.98D+307    1.11D-16    1.12D-308
!
!*******************************************************************
!*******************************************************************
!
! ERROR RETURNS
!
!  THE PROGRAM RETURNS THE VALUE XINF FOR SINGULARITIES OR
!     WHEN OVERFLOW WOULD OCCUR.  THE COMPUTATION IS BELIEVED
!     TO BE FREE OF UNDERFLOW AND OVERFLOW.
!
!
!  INTRINSIC FUNCTIONS REQUIRED ARE:
!
!     INT, DBLE, EXP, LOG, REAL, SIN
!
!
! REFERENCES:  AN OVERVIEW OF SOFTWARE DEVELOPMENT FOR SPECIAL
!              FUNCTIONS   W. J. CODY, LECTURE NOTES IN MATHEMATICS,
!              506, NUMERICAL ANALYSIS DUNDEE, 1975, G. A. WATSON
!              (ED.), SPRINGER VERLAG, BERLIN, 1976.
!
!              COMPUTER APPROXIMATIONS, HART, ET. AL., WILEY AND
!              SONS, NEW YORK, 1968.
!
!  LATEST MODIFICATION: OCTOBER 12, 1989
!
!  AUTHORS: W. J. CODY AND L. STOLTZ
!           APPLIED MATHEMATICS DIVISION
!           ARGONNE NATIONAL LABORATORY
!           ARGONNE, IL 60439
!
!----------------------------------------------------------------------
      INTEGER I,N
      LOGICAL PARITY
!D    DOUBLE PRECISION
      REAL :: &
         C,CONV,EPS,FACT,HALF,ONE,P,Q,RES,SQRTPI,SUM,TWELVE,  &
         TWO,X,XBIG,XDEN,XINF,XMININ,XNUM,Y,Y1,YSQ,Z,ZERO
      DIMENSION C(7),P(8),Q(8)
!----------------------------------------------------------------------
!  MATHEMATICAL CONSTANTS
!----------------------------------------------------------------------
      DATA ONE,HALF,TWELVE,TWO,ZERO/1.0E0,0.5E0,12.0E0,2.0E0,0.0E0/,  &
           SQRTPI/0.9189385332046727417803297E0/
!D    DATA ONE,HALF,TWELVE,TWO,ZERO/1.0D0,0.5D0,12.0D0,2.0D0,0.0D0/,
!D   1     SQRTPI/0.9189385332046727417803297D0/,
!----------------------------------------------------------------------
!  MACHINE DEPENDENT PARAMETERS
!----------------------------------------------------------------------
      DATA XBIG,XMININ,EPS/35.040E0,1.18E-38,1.19E-7/,  &
          XINF/3.4E38/
!D    DATA XBIG,XMININ,EPS/171.624D0,2.23D-308,2.22D-16/,
!D   1     XINF/1.79D308/
!----------------------------------------------------------------------
!  NUMERATOR AND DENOMINATOR COEFFICIENTS FOR RATIONAL MINIMAX
!     APPROXIMATION OVER (1,2).
!----------------------------------------------------------------------
      DATA P/-1.71618513886549492533811E+0,2.47656508055759199108314E+1, &
             -3.79804256470945635097577E+2,6.29331155312818442661052E+2, &
             8.66966202790413211295064E+2,-3.14512729688483675254357E+4, &
             -3.61444134186911729807069E+4,6.64561438202405440627855E+4/
      DATA Q/-3.08402300119738975254353E+1,3.15350626979604161529144E+2, &
            -1.01515636749021914166146E+3,-3.10777167157231109440444E+3, &
              2.25381184209801510330112E+4,4.75584627752788110767815E+3, &
            -1.34659959864969306392456E+5,-1.15132259675553483497211E+5/
!D    DATA P/-1.71618513886549492533811D+0,2.47656508055759199108314D+1,
!D   1       -3.79804256470945635097577D+2,6.29331155312818442661052D+2,
!D   2       8.66966202790413211295064D+2,-3.14512729688483675254357D+4,
!D   3       -3.61444134186911729807069D+4,6.64561438202405440627855D+4/
!D    DATA Q/-3.08402300119738975254353D+1,3.15350626979604161529144D+2,
!D   1      -1.01515636749021914166146D+3,-3.10777167157231109440444D+3,
!D   2        2.25381184209801510330112D+4,4.75584627752788110767815D+3,
!D   3      -1.34659959864969306392456D+5,-1.15132259675553483497211D+5/
!----------------------------------------------------------------------
!  COEFFICIENTS FOR MINIMAX APPROXIMATION OVER (12, INF).
!----------------------------------------------------------------------
      DATA C/-1.910444077728E-03,8.4171387781295E-04, &
           -5.952379913043012E-04,7.93650793500350248E-04, &
           -2.777777777777681622553E-03,8.333333333333333331554247E-02, &
            5.7083835261E-03/
!D    DATA C/-1.910444077728D-03,8.4171387781295D-04,
!D   1     -5.952379913043012D-04,7.93650793500350248D-04,
!D   2     -2.777777777777681622553D-03,8.333333333333333331554247D-02,
!D   3      5.7083835261D-03/
!----------------------------------------------------------------------
!  STATEMENT FUNCTIONS FOR CONVERSION BETWEEN INTEGER AND FLOAT
!----------------------------------------------------------------------
      CONV(I) = REAL(I)
!D    CONV(I) = DBLE(I)
      PARITY=.FALSE.
      FACT=ONE
      N=0
      Y=X
      IF(Y.LE.ZERO)THEN
!----------------------------------------------------------------------
!  ARGUMENT IS NEGATIVE
!----------------------------------------------------------------------
        Y=-X
        Y1=AINT(Y)
        RES=Y-Y1
        IF(RES.NE.ZERO)THEN
          IF(Y1.NE.AINT(Y1*HALF)*TWO)PARITY=.TRUE.
          FACT=-PI/SIN(PI*RES)
          Y=Y+ONE
        ELSE
          RES=XINF
          GOTO 900
        ENDIF
      ENDIF
!----------------------------------------------------------------------
!  ARGUMENT IS POSITIVE
!----------------------------------------------------------------------
      IF(Y.LT.EPS)THEN
!----------------------------------------------------------------------
!  ARGUMENT .LT. EPS
!----------------------------------------------------------------------
        IF(Y.GE.XMININ)THEN
          RES=ONE/Y
        ELSE
          RES=XINF
          GOTO 900
        ENDIF
      ELSEIF(Y.LT.TWELVE)THEN
        Y1=Y
        IF(Y.LT.ONE)THEN
!----------------------------------------------------------------------
!  0.0 .LT. ARGUMENT .LT. 1.0
!----------------------------------------------------------------------
          Z=Y
          Y=Y+ONE
        ELSE
!----------------------------------------------------------------------
!  1.0 .LT. ARGUMENT .LT. 12.0, REDUCE ARGUMENT IF NECESSARY
!----------------------------------------------------------------------
          N=INT(Y)-1
          Y=Y-CONV(N)
          Z=Y-ONE
        ENDIF
!----------------------------------------------------------------------
!  EVALUATE APPROXIMATION FOR 1.0 .LT. ARGUMENT .LT. 2.0
!----------------------------------------------------------------------
        XNUM=ZERO
        XDEN=ONE
        DO 260 I=1,8
          XNUM=(XNUM+P(I))*Z
          XDEN=XDEN*Z+Q(I)
  260   CONTINUE
        RES=XNUM/XDEN+ONE
        IF(Y1.LT.Y)THEN
!----------------------------------------------------------------------
!  ADJUST RESULT FOR CASE  0.0 .LT. ARGUMENT .LT. 1.0
!----------------------------------------------------------------------
          RES=RES/Y1
        ELSEIF(Y1.GT.Y)THEN
!----------------------------------------------------------------------
!  ADJUST RESULT FOR CASE  2.0 .LT. ARGUMENT .LT. 12.0
!----------------------------------------------------------------------
          DO 290 I=1,N
            RES=RES*Y
            Y=Y+ONE
  290     CONTINUE
        ENDIF
      ELSE
!----------------------------------------------------------------------
!  EVALUATE FOR ARGUMENT .GE. 12.0,
!----------------------------------------------------------------------
        IF(Y.LE.XBIG)THEN
          YSQ=Y*Y
          SUM=C(7)
          DO 350 I=1,6
            SUM=SUM/YSQ+C(I)
  350     CONTINUE
          SUM=SUM/Y-Y+SQRTPI
          SUM=SUM+(Y-HALF)*LOG(Y)
          RES=EXP(SUM)
        ELSE
          RES=XINF
          GOTO 900
        ENDIF
      ENDIF
!----------------------------------------------------------------------
!  FINAL ADJUSTMENTS AND RETURN
!----------------------------------------------------------------------
      IF(PARITY)RES=-RES
      IF(FACT.NE.ONE)RES=FACT/RES
  900 GAMMA1=RES
!D900 DGAMMA = RES
! ---------- LAST LINE OF GAMMA ----------
END FUNCTION GAMMA1


program saturn_grv
use param
implicit none
CALL GETARG(1,arg1)

select case (arg1)
case('1')
	call test_sph
case('2')
	call test_orbit
case default
	stop
end select

end program saturn_grv

!真实轨道仿真
subroutine test_orbit

end subroutine test_orbit

!土星环的引力场与土星带谐项相关分析
subroutine test_sph
use param
use grav
use lib_array
implicit none

integer :: i,j,nr,np
!real(dp),parameter:: rs=60356000.d0 !环半径(m)
real(dp)::x(3),xbf(3),fs(3),ufs(3),fr(3),ufr(3),r1,r2,p1,p2,dtemp,dtemp1,vdot
real(dp),allocatable:: rho(:),phi(:)
!rho 	:径向
!phi	:纬向
CALL GETARG(2,arg2)
CALL GETARG(3,arg3)
CALL GETARG(4,arg4)
CALL GETARG(5,arg5)
CALL GETARG(6,arg6)
CALL GETARG(7,arg7)
CALL GETARG(8,arg8)

read(arg3,*)nr
read(arg4,*)np
read(arg5,*)r1
read(arg6,*)r2
read(arg7,*)p1
read(arg8,*)p2

r1=r1*rsat
r2=r2*rsat
p1=p1*pi/180.d0
p2=p2*pi/180.d0

allocate(rho(nr),phi(np))
!xbf(1)=-47174.707491d3
!xbf(2)=-145456.356920d3
!xbf(3)=152.790709d3
grv_file=trim(arg2)
grv_file_id=20
grv_body_id=699
grv_order=8
call getgrvp

!call linspace(66900000.d0,136775000.d0,rho)
call linspace(r1,r2,rho)
!call linspace(-pi/2,pi/2,phi)
call linspace(p1,p2,phi)
!rho=rs+rho

do i=1,nr
	do j=1,np
		xbf(1)=0.d0
		xbf(2)=rho(i)*dcos(phi(j))
		xbf(3)=rho(i)*dsin(phi(j))
!		write(*,*)rho(i)
		xbf(1)=0.d0
		xbf(2)=116775000.d0
		xbf(3)=1000.d0
		call gforcebf(xbf,fs)
		call UNORM ( fs, ufs, dtemp )
		call ringbf(xbf,fr)
		write(*,*)fr
		stop
		call UNORM ( fr, ufr, dtemp1 )
		write(*,'(5e)')rho(i)/rsat,phi(j)*180.d0/pi,dtemp,dtemp1,dabs(vdot(ufs,ufr))
	enddo
enddo

end subroutine test_sph


Program test_util
  Use Iso_fortran_env , Only: real64
  Use g3d_module, Only : readg_g3d, bfield_geq_bicub, g_type
  Implicit None

  ! gfile name and type
  Character(len=128) :: gfilename = '../gfiles/g148712.04101'
  Type(g_type) :: g

  ! Evaluation parameters
  Integer, Parameter :: Ntest = 20
  Real(real64) :: R(Ntest), Z(Ntest),  Bvec(Ntest,3)
  Real(real64), Parameter :: Rstart = 1.2_real64, Rend = 2.2_real64
  Real(real64), Parameter :: Ztest = 0.1_real64
  Integer :: ierr, ii

  Write(*,'(/a)') '-----------------------------------'

  Call readg_g3d(gfilename,g)

  Write(*,'(a/)') '-----------------------------------'

  Do ii = 1, Ntest
     R(ii) = (Real(ii,real64)-1._real64)*(Rend-Rstart)  &
          / (Real(Ntest,real64)-1._real64) + Rstart
  End Do
  Z(:) = Ztest
  Bvec = 0._real64

  ! Returns Bvec(:,[Br, Bz, Bphi])
  Call bfield_geq_bicub(g,R,Z,Ntest,Bvec,ierr)

  Write(*,*) '   R (m)             Z (m)             Br (T)' &
       // '            Bz (T)            Bphi (T)'
  Do ii = 1, Ntest
     Write(*,'(5f18.12)') R(ii),Z(ii),Bvec(ii,:)
  End Do

End program test_util


        SUBROUTINE YCLPPC(FRAME,NAME,N,DET,FILL,DATA,ERR,EPS,
     *                    PEDGRE,DESCRP,ISTAT)
*
*  Module number:
*
*  Module name: YCLPPC
*
*  Keyphrase:
*  ----------
*       Perform FOS paired pulse correction
*  Description:
*  ------------
*       This routine performs the paired pulse correction
*       of FOS data.  On the first call the paired pulse parameters
*       the paired pulse parameters are read from table ccg2.
*       if q0 is not equal to zero then the following equation
*       is used:
*                      y
*               x = --------
*                   (1 - yt)
*
*       where:
*               x is the true count rate
*               y is the observed count rate
*               t = q0 for y less than or equal to F
*               t = q0 + q1*(y-F) for y greater than F
*               q0, q1, and F are coefficients in CCG2
*
*       IF q0 is equal to zero then the following equation is used:
*
*               x = log(1-ty)/(-t)
*
*       where:
*               t = tau1 ( a coefficient in CCG2)
*               the log is a natural logarithm
*
*
*  FORTRAN name: YCLPPC.FOR
*
*  Keywords of accessed files and tables:
*  --------------------------------------
*  Name                         I/O     Description / Comments
*       ccg2                    I       paired pulse coefficient table
*  Subroutines Called:
*  -------------------
*  CDBS:
*       yrccg2, ymsput
*
*  History:
*  --------
*  Version      Date        Author          Description
*       1       jul 89  D. Lindler      Designed and coded
*	2	Mar 94	H. Bushouse	Mods to handle PEDIGREE keywords
*-------------------------------------------------------------------------------
*
* INPUTS:
*       frame - frame number
*       name - name of the ccg2 reference table
*       n - number of points to calibrate
*       det - detector name
*       fill - flag value for fill data
*
* INPUT/OUTPUT:
*       data - data array
*       err - error array
*       eps - data quality vector
*
* OUTPUT:
*	pedgre - CCG2 PEDIGREE keyword string
*	descrp - CCG2 DESCRIP  keyword string
*       istat - error status
*
*----------------------------------------------------------------------------
        CHARACTER*64 NAME
        INTEGER N,FRAME,ISTAT
        CHARACTER*5 DET
	CHARACTER*68 PEDGRE,DESCRP
        REAL DATA(N),ERR(N),EPS(N),FILL
C
C     UMSPUT DESTINATIONS -- CB, DAO, 4-SEP-87
C
      INTEGER STDOUT
      PARAMETER (STDOUT = 1)
      INTEGER STDERR
      PARAMETER (STDERR = 2)
C
C LOCAL VARIABLES ------------------------------------------------------
C
C saturation epsilons
C
        REAL EPSSAT
        PARAMETER (EPSSAT = 300)
        REAL EPSR20
        PARAMETER (EPSR20 = 190)
        REAL EPSR5
        PARAMETER (EPSR5 = 130)
        REAL Q0,Q1,F,THRESH,TAU1,T,RSAT,X,VALUE,R20,R5
        INTEGER I
        CHARACTER*130 CONTXT
C
C-----------------------------------------------------------------------
C
C READ COEFFICIENTS IF FIRST CALL
C
        IF(FRAME.EQ.1)THEN
            CALL YRCCG2(NAME,DET,TAU1,THRESH,Q0,Q1,F,RSAT,R20,R5,
     *                  PEDGRE,DESCRP,ISTAT)
            IF(ISTAT.NE.0)GO TO 1000
C
C If the reference table contains dummy data, then skip the correction
C
	    IF(PEDGRE(1:5).EQ.'DUMMY')THEN
	       CONTXT='WARNING: PEDIGREE = DUMMY for CCG2 '//NAME
	       CALL YMSPUT(CONTXT,STDOUT+STDERR,0,ISTAT)
	       CONTXT='         Paired pulse correction will be '//
     *                'skipped'
	       CALL YMSPUT(CONTXT,STDOUT+STDERR,0,ISTAT)
	       GO TO 1000
	    END IF
C
C Report the coefficients being used
C
            IF(Q0.EQ.0.0)THEN
                WRITE(CONTXT,99)TAU1
99              FORMAT('Paired pulse coef.  tau1=',
     *                  E16.8)
              ELSE
                CALL YMSPUT('Deadtime coefficients',STDOUT,0,
     *                           ISTAT)
                WRITE(CONTXT,199)Q0,Q1,F
199             FORMAT('  Q0=',E16.8,'  Q1=',E16.8,
     *                 ' F=',F10.0)
            ENDIF
            CALL YMSPUT(CONTXT,STDOUT,0,ISTAT)
            WRITE(CONTXT,299)RSAT
299         FORMAT('Saturation limit = ',F12.0,
     *                          ' observed counts/second')
            CALL YMSPUT(CONTXT,STDOUT,0,ISTAT)
            IF(THRESH.LT.0.0)THRESH=0.0
        ENDIF
C-------------------- end of frame 1 only processing --------------------------
C
C loop on data
C
        DO 500 I=1,N
            VALUE=DATA(I)
            IF((EPS(I).NE.FILL).AND.(VALUE.GT.THRESH))THEN
C
C Saturated?
C
                IF(VALUE.GT.RSAT)THEN
                        DATA(I)=0.0
                        ERR(I)=0.0
                        IF(EPS(I).LT.EPSSAT)EPS(I)=EPSSAT
                    ELSE
C
C Flag level of saturation
C
                        IF(VALUE.GT.R5)THEN
                          IF(VALUE.LT.R20)THEN
                                IF(EPS(I).LT.EPSR5)EPS(I)=EPSR5
                              ELSE
                                IF(EPS(I).LT.EPSR20)EPS(I)=EPSR20
                          ENDIF
                        ENDIF
C
C CASE 1  Q0 is not zero
C
                        IF (Q0 .NE. 0.0) THEN
                                T=Q0
                                IF(VALUE.GT.F)T=T+(VALUE-F)*Q1
                                X=(1.0-VALUE*T)
                                IF(X .LT. 1.0E-12) X=1.0E-12
                                VALUE=VALUE/X
                                ERR(I)=ERR(I)/X
                            ELSE
C
C CASE 2  Q0 is zero
C
                                X=(1.0-VALUE*TAU1)
                                IF(X .LT. 1.0E-12) X=1.0E-12
                                VALUE=LOG(X)/(-TAU1)
                                ERR(I)=ERR(I)*(VALUE/DATA(I))
                        ENDIF
                        DATA(I) = VALUE
               ENDIF
            ENDIF
500     CONTINUE
C
        ISTAT=0
1000    RETURN
        END


!==========================================================================
elemental function gsw_sound_speed_t_exact (sa, t, p)
!==========================================================================
!
! Calculates the sound speed of seawater
!
! sa     : Absolute Salinity                               [g/kg]
! t      : in-situ temperature                             [deg C]
! p      : sea pressure                                    [dbar]
!
! gsw_sound_speed_t_exact : sound speed                    [m/s]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_gibbs

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, t, p

real (r8) :: gsw_sound_speed_t_exact

real (r8) :: g_tt, g_tp

integer, parameter :: n0=0, n1=1, n2=2

g_tt = gsw_gibbs(n0,n2,n0,sa,t,p)
g_tp = gsw_gibbs(n0,n1,n1,sa,t,p)

gsw_sound_speed_t_exact = gsw_gibbs(n0,n0,n1,sa,t,p) * &
               sqrt(g_tt/(g_tp*g_tp - g_tt*gsw_gibbs(n0,n0,n2,sa,t,p)))

return
end function

!--------------------------------------------------------------------------