      SUBROUTINE DCOSQI (N,WSAVE)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION       WSAVE(1)
      DATA PIH /1.57079632679489661923D0/
      DT = PIH/FLOAT(N)
      FK = 0.0D0
      DO 101 K=1,N
         FK = FK+1.0D0
         WSAVE(K) = COS(FK*DT)
  101 CONTINUE
      CALL DFFTI (N,WSAVE(N+1))
      RETURN
      END


!##############################################################################
! PROGRAM HetAg_Sim
!
! ## The heterogeneous agent model with simulation methods
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
include "prog09_08m.f90"

program HetAg_Sim

    use globals

    implicit none

    ! initialize variables
    call initialize()

    ! set initial guess of the interest rate
    r = 0.040d0

    ! solve initial equilibrium
    call solve_model()

contains

    ! For solving the model
    subroutine solve_model

        use toolbox

        implicit none
        real*8 :: x_in
        logical :: check

        ! start timer
        call tic()

        write(*, '(a)')'       K/Y         r      diff_K'

        ! set tolerance level for outer optimization
        call settol_root(sig_out)

        ! set initial guess
        x_in = r
        check = .false.

        ! solve asset market equilibrium
        call fzero(x_in, asset_market, check)

        if(check)write(*,'(a)')'ERROR IN ROOTFINDING'

        call toc

        ! write output to the screen
        call output

    end subroutine

    ! For initializing variables
    subroutine initialize()

        use toolbox

        implicit none
        integer :: is

        ! calculate the shock process
        call discretize_AR(rho, 0d0, sigma_eps, eta, pi, weights)
        eta = exp(eta)

        ! calculate aggregate labor supply
        LL = sum(eta*weights)

        ! simulate draws from the AR(1) process
        call simulate_AR(pi, is_t)

        ! initialize grid
        call grid_Cons_Grow(a, a_l, a_u, a_grow)

        ! initialize policy function
        do is = 1, ns
            c(:, is) = 0.04d0*a(:) + eta(is)
            c_new(:, is) = c(:, is)
        enddo

    end subroutine

    ! For creating output plots
    subroutine output()

        implicit none
        real*8 :: CV_c, CV_a, CV_eta

        ! aggregate consumption and investment
        CC = sum(c_t(0:TT))/dble(TT+1)
        II = delta*KK

        ! calculate coefficients of variation
        CV_eta = sqrt(sum((eta_t(0:TT)-LL)**2)/dble(TT))/LL*100d0
        CV_c   = sqrt(sum((c_t(0:TT)-CC)**2)/dble(TT))/CC*100d0
        CV_a   = sqrt(sum((a_t(0:TT)-KK)**2)/dble(TT))/AA*100d0

        ! write output to screen
        write(*, '(//a)')'AIYAGARI ECONOMY OUTPUT'
        write(*,'(/a)')'PROD :       K/Y       L/Y         r         w'
        write(*,'(6x,4f10.4)')(/KK, LL/)/YY*100d0, r*100d0, w
        write(*,'(/a)')'GOODS:       C/Y       I/Y       D/Y'
        write(*,'(6x,3f10.4)')(/CC, II, CC+II/)/YY*100d0
        write(*,'(/a)')'               c         a       eta'
        write(*,'(a,4f10.4)')'LEV  :', CC, AA, LL
        write(*,'(a,3f10.4)')'CV   :', CV_c, CV_a, CV_eta
        write(*,'(//)')

    end subroutine

end program


SUBROUTINE calculate_rdf(&
        positions, istart, istop, stepsize,      & !, stepsize &
        radius, density, cell, invcell , indices1, indices2, nbins, rdf, integral, radii, &
        nstep, nat, nat1, nat2 &
    )
    IMPLICIT NONE

    integer, intent(in)         ::          istart, istop, stepsize ! The ionic step I start and I end
    real*8, intent(in)          ::          cell(3,3), invcell(3,3)

    integer, intent(in)         ::          nat, nat1, nat2  ! The total number of atoms, and number of atoms that I calculate the den
    integer, intent(in)         ::          nstep, nbins  ! The total number of atoms, and number of atoms that I calculate the den
    real*8, intent(in)          ::          positions(nstep, nat,3)

    integer, intent(in)         ::          indices1(nat1) ! The indices of the atoms that I calculate the density from
    integer, intent(in)         ::          indices2(nat2) ! The indices of the atoms that I calculate the density from

    real*8, intent(in)          ::          radius ! The maximum radius I should go for
    real*8, intent(in)          ::          density ! The density of the atoms
    real, parameter             ::          pi = 3.1415927

    real*8, intent(out) :: rdf(nbins), integral(nbins), radii(nbins) ! That's where I count
    real*8 :: distance_real(1:3), distance_crystal(1:3)
    real*8 :: binsize, distance_norm, vb, factor, r

    integer :: iat1, iat2, istep, bin, idim !, idim, iat
    ! This is the amount I augment the counter by, so that I correctly
    ! mean over the steps and the number of atoms
    factor = 1.0D0 * DBLE(stepsize) / DBLE(istop+1-istart)  / DBLE(nat1)
    rdf(:) = 0.0D0
    binsize = radius / DBLE (nbins)
    do istep=istart, istop, stepsize
        do iat1=1,nat1
            do iat2=1, nat2
                if ( indices2(iat2) .eq. indices1(iat1) ) CYCLE
                ! I calculate the distance between atom1 and atom2 in real space:
                distance_real(1:3) = positions(istep, indices2(iat2), 1:3) - &
                        positions(istep, indices1(iat1), 1:3)
                ! I multiply with invcell to calculate the distance
                ! in crystal coordinates, and take the modulo 1 to get the distance
                ! vector into the unit cell
                distance_crystal(1:3) = MOD(MATMUL(invcell, distance_real), 1.0)
                ! The following gets the shortest distance of atom1 to atom2 in
                ! an orthorhombic system. For accute cells, this might be wrong.
                DO idim=1, 3
                    IF ( distance_crystal(idim) < -0.5 ) THEN
                        distance_crystal(idim) = distance_crystal(idim) + 1.0D0
                    ELSEIF ( distance_crystal(idim) > 0.5 ) THEN
                        distance_crystal(idim) = 1.0D0 - distance_crystal(idim)
                    ENDIF
                ENDDO
                distance_real(1:3) = MATMUL(cell, distance_crystal)
                ! Calculate the norm of vector
                distance_norm = SQRT(SUM(distance_real(1:3)*distance_real(1:3)))
                ! and in which bin it belongs:
                ! Here I take the nearest bin.
                bin = nint( distance_norm / binsize )
                if ( bin > 0 .and. bin <= nbins ) rdf(bin) = rdf(bin)  + factor
            enddo
        end do
    enddo
    integral(1) = 0.0D0
    bin = 1
    integral(bin) = 0.0D0
    ! Below expression calculates the volume element for a certain bin,
    ! in this case the first:
    factor = 4.0D0/3.0D0 * pi
    vb = factor *( ((bin+0.5)*binsize)**3 - ((bin-0.5D0)*binsize)**3 )
    ! I normalize the RDF by the ideal gas density:
    rdf(bin) = rdf(bin) / (vb * density)
    radii(bin) = binsize
    do bin=2, nbins
        integral(bin) = integral(bin-1) + rdf(bin)
        r = DBLE(bin) * binsize
        radii(bin) = r
        vb = factor * (3.0D0 * r**2 *binsize + 0.75D0 * binsize**3)
        rdf(bin) = rdf(bin) / (vb * density) ! number of particles in a real gas
    end do

END SUBROUTINE calculate_rdf

SUBROUTINE calculate_angular_spec(&
        positions, istart, istop, stepsize,      & !, stepsize &
        radius, cell, invcell , indices1, indices2, indices3,  &
        nbins, angular_spec, radii, &
        nstep, nat, nat1, nat2, nat3 &
    )
    IMPLICIT NONE

    integer, intent(in)         ::          istart, istop, stepsize ! The ionic step I start and I end
    real*8, intent(in)          ::          cell(3,3), invcell(3,3)

    integer, intent(in)         ::          nat, nat1, nat2, nat3  ! The total number of atoms, and number of atoms that I calculate the den
    integer, intent(in)         ::          nstep, nbins  ! The total number of atoms, and number of atoms that I calculate the den
    real*8, intent(in)          ::          positions(nstep, nat,3)

    integer, intent(in)         ::          indices1(nat1) ! The indices of the atoms that I calculate the density from
    integer, intent(in)         ::          indices2(nat2)
    integer, intent(in)         ::          indices3(nat3)

    real*8, intent(in)          ::          radius ! The maximum radius I should go for
    real, parameter             ::          pi = 3.1415927

    real*8, intent(out) :: angular_spec(nbins), radii(nbins) ! That's where I count
    real*8 :: distance_real2(1:3), distance_real3(1:3), distance_crystal(1:3)
    real*8 :: binsize, distance_norm2, distance_norm3, factor, angle

    integer :: iat1, iat2, iat3, istep, bin, idim !, idim, iat
    ! This is the amount I augment the counter by, so that I correctly
    ! mean over the steps and the number of atoms
    factor = 1.0D0 * DBLE(stepsize) / DBLE(istop+1-istart)  / DBLE(nat1)
    angular_spec(:) = 0.0D0
!~     binsize = 180.0D0 / DBLE (nbins)
    binsize = 180.0D0 / DBLE (nbins)
    do istep=istart, istop, stepsize
        do iat1=1,nat1
            do iat2=1, nat2
                if ( indices2(iat2) .eq. indices1(iat1) ) CYCLE
                ! I calculate the distance between atom1 and atom2 in real space:
                distance_real2(1:3) = positions(istep, indices2(iat2), 1:3) - &
                        positions(istep, indices1(iat1), 1:3)
                ! I multiply with invcell to calculate the distance
                ! in crystal coordinates, and take the modulo 1 to get the distance
                ! vector into the unit cell
                distance_crystal(1:3) = MOD(MATMUL(invcell, distance_real2), 1.0)
                ! The following gets the shortest distance of atom1 to atom2 in
                ! an orthorhombic system. For accute cells, this might be wrong.
                DO idim=1, 3
                    IF ( distance_crystal(idim) < -0.5 ) THEN
                        distance_crystal(idim) = distance_crystal(idim) + 1.0D0
                    ELSEIF ( distance_crystal(idim) > 0.5 ) THEN
                        distance_crystal(idim) = 1.0D0 - distance_crystal(idim)
                    ENDIF
                ENDDO
                distance_real2(1:3) = MATMUL(cell, distance_crystal)
                ! Calculate the norm of vector
                distance_norm2 = SQRT(SUM(distance_real2(1:3)*distance_real2(1:3)))
                if ( distance_norm2 > radius ) CYCLE
                ! and in which bin it belongs:
                ! Here I take the nearest bin.
                do iat3=1, nat3
                    if ( indices3(iat3) .eq. indices1(iat1) ) CYCLE
                    if ( indices3(iat3) .eq. indices2(iat2) ) CYCLE
                    distance_real3(1:3) = positions(istep, indices3(iat3), 1:3) - &
                            positions(istep, indices1(iat1), 1:3)
                    ! I multiply with invcell to calculate the distance
                    ! in crystal coordinates, and take the modulo 1 to get the distance
                    ! vector into the unit cell
                    distance_crystal(1:3) = MOD(MATMUL(invcell, distance_real3), 1.0)
                    ! The following gets the shortest distance of atom1 to atom2 in
                    ! an orthorhombic system. For accute cells, this might be wrong.
                    DO idim=1, 3
                        IF ( distance_crystal(idim) < -0.5 ) THEN
                            distance_crystal(idim) = distance_crystal(idim) + 1.0D0
                        ELSEIF ( distance_crystal(idim) > 0.5 ) THEN
                            distance_crystal(idim) = 1.0D0 - distance_crystal(idim)
                        ENDIF
                    ENDDO
                    distance_real3(1:3) = MATMUL(cell, distance_crystal)
                    ! Calculate the norm of vector
                    distance_norm3 = SQRT(SUM(distance_real3(1:3)*distance_real3(1:3)))
                    if ( distance_norm3 > radius ) CYCLE
                    angle = 180.0D0/pi * acos(sum(distance_real2(1:3) * distance_real3(1:3)) / ( distance_norm2*distance_norm3))
!~                     if ( angle > 180.0D0 ) angle = 360.0D0 - angle
!~                     print*, istep, indices1(iat1), indices2(iat2), indices3(iat3), angle
                    bin = nint( angle / binsize )
                    if ( bin > 0 .and. bin <= nbins ) angular_spec(bin) = angular_spec(bin)  + &
                            factor  / distance_norm2**2 / distance_norm3**2
                enddo
            enddo
        end do
    enddo
    do bin=1, nbins
        radii(bin) = DBLE(bin) * binsize
    end do

END SUBROUTINE calculate_angular_spec


program test_rkmethod
  use RKMethod, only: dagrt_state_type, &
    timestep_initialize => initialize, &
    timestep_run => run, &
    timestep_shutdown => shutdown

  implicit none

  type(dagrt_state_type), target :: state
  type(dagrt_state_type), pointer :: state_ptr

  real*8, dimension(2) :: initial_condition, true_sol

  real*8 t_fin
  integer ntrips
  parameter (t_fin=1d0, ntrips=100)

  real*8 :: dt_value, error, atol

  integer stderr
  parameter(stderr=0)

  integer istep

  ! start code ----------------------------------------------------------------

  state_ptr => state

  initial_condition(1) = 2
  initial_condition(2) = 2.3

  dt_value = 1e-5
  atol = 1e-6

  call timestep_initialize( &
    dagrt_state=state_ptr, &
    state_y=initial_condition, &
    dagrt_t=0d0, &
    dagrt_dt=dt_value)

  do istep = 1, ntrips
    call timestep_run(dagrt_state=state_ptr)
  enddo

  true_sol = initial_condition * exp(-2*state%ret_time_y)
  error = sqrt(sum((true_sol-state%ret_state_y)**2))

  call timestep_shutdown(dagrt_state=state_ptr)
  write(*,*) 'done'

  if (error>atol) then
    write(stderr,*) 'ERROR: Max error exceeds tolerance:', error, ' < ', &
        atol
  endif

end program


!> \brief \b DLASSQ updates a sum of squares represented in scaled form.
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download DLASSQ + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE DLASSQ( N, X, INCX, SCALE, SUMSQ )
!
!       .. Scalar Arguments ..
!       INTEGER            INCX, N
!       DOUBLE PRECISION   SCALE, SUMSQ
!       ..
!       .. Array Arguments ..
!       DOUBLE PRECISION   X( * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> DLASSQ  returns the values  scl  and  smsq  such that
!>
!>    ( scl**2 )*smsq = x( 1 )**2 +...+ x( n )**2 + ( scale**2 )*sumsq,
!>
!> where  x( i ) = X( 1 + ( i - 1 )*INCX ). The value of  sumsq  is
!> assumed to be non-negative.
!>
!> scale and sumsq must be supplied in SCALE and SUMSQ and
!> scl and smsq are overwritten on SCALE and SUMSQ respectively.
!>
!> If scale * sqrt( sumsq ) > tbig then
!>    we require:   scale >= sqrt( TINY*EPS ) / sbig   on entry,
!> and if 0 < scale * sqrt( sumsq ) < tsml then
!>    we require:   scale <= sqrt( HUGE ) / ssml       on entry,
!> where
!>    tbig -- upper threshold for values whose square is representable;
!>    sbig -- scaling constant for big numbers; \see la_constants.f90
!>    tsml -- lower threshold for values whose square is representable;
!>    ssml -- scaling constant for small numbers; \see la_constants.f90
!> and
!>    TINY*EPS -- tiniest representable number;
!>    HUGE     -- biggest representable number.
!>
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The number of elements to be used from the vector x.
!> \endverbatim
!>
!> \param[in] X
!> \verbatim
!>          X is DOUBLE PRECISION array, dimension (1+(N-1)*abs(INCX))
!>          The vector for which a scaled sum of squares is computed.
!>             x( i )  = X( 1 + ( i - 1 )*INCX ), 1 <= i <= n.
!> \endverbatim
!>
!> \param[in] INCX
!> \verbatim
!>          INCX is INTEGER
!>          The increment between successive values of the vector x.
!>          If INCX > 0, X(1+(i-1)*INCX) = x(i) for 1 <= i <= n
!>          If INCX < 0, X(1-(n-i)*INCX) = x(i) for 1 <= i <= n
!>          If INCX = 0, x isn't a vector so there is no need to call
!>          this subroutine.  If you call it anyway, it will count x(1)
!>          in the vector norm N times.
!> \endverbatim
!>
!> \param[in,out] SCALE
!> \verbatim
!>          SCALE is DOUBLE PRECISION
!>          On entry, the value  scale  in the equation above.
!>          On exit, SCALE is overwritten with  scl , the scaling factor
!>          for the sum of squares.
!> \endverbatim
!>
!> \param[in,out] SUMSQ
!> \verbatim
!>          SUMSQ is DOUBLE PRECISION
!>          On entry, the value  sumsq  in the equation above.
!>          On exit, SUMSQ is overwritten with  smsq , the basic sum of
!>          squares from which  scl  has been factored out.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Edward Anderson, Lockheed Martin
!
!> \par Contributors:
!  ==================
!>
!> Weslley Pereira, University of Colorado Denver, USA
!> Nick Papior, Technical University of Denmark, DK
!
!> \par Further Details:
!  =====================
!>
!> \verbatim
!>
!>  Anderson E. (2017)
!>  Algorithm 978: Safe Scaling in the Level 1 BLAS
!>  ACM Trans Math Softw 44:1--28
!>  https://doi.org/10.1145/3061665
!>
!>  Blue, James L. (1978)
!>  A Portable Fortran Program to Find the Euclidean Norm of a Vector
!>  ACM Trans Math Softw 4:15--23
!>  https://doi.org/10.1145/355769.355771
!>
!> \endverbatim
!
!> \ingroup OTHERauxiliary
!
!  =====================================================================
subroutine DLASSQ( n, x, incx, scl, sumsq )
   use LA_CONSTANTS, &
      only: wp=>dp, zero=>dzero, one=>done, &
            sbig=>dsbig, ssml=>dssml, tbig=>dtbig, tsml=>dtsml
   use LA_XISNAN
!
!  -- LAPACK auxiliary routine --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!
!  .. Scalar Arguments ..
   integer :: incx, n
   real(wp) :: scl, sumsq
!  ..
!  .. Array Arguments ..
   real(wp) :: x(*)
!  ..
!  .. Local Scalars ..
   integer :: i, ix
   logical :: notbig
   real(wp) :: abig, amed, asml, ax, ymax, ymin
!  ..
!
!  Quick return if possible
!
   if( LA_ISNAN(scl) .or. LA_ISNAN(sumsq) ) return
   if( sumsq == zero ) scl = one
   if( scl == zero ) then
      scl = one
      sumsq = zero
   end if
   if (n <= 0) then
      return
   end if
!
!  Compute the sum of squares in 3 accumulators:
!     abig -- sums of squares scaled down to avoid overflow
!     asml -- sums of squares scaled up to avoid underflow
!     amed -- sums of squares that do not require scaling
!  The thresholds and multipliers are
!     tbig -- values bigger than this are scaled down by sbig
!     tsml -- values smaller than this are scaled up by ssml
!
   notbig = .true.
   asml = zero
   amed = zero
   abig = zero
   ix = 1
   if( incx < 0 ) ix = 1 - (n-1)*incx
   do i = 1, n
      ax = abs(x(ix))
      if (ax > tbig) then
         abig = abig + (ax*sbig)**2
         notbig = .false.
      else if (ax < tsml) then
         if (notbig) asml = asml + (ax*ssml)**2
      else
         amed = amed + ax**2
      end if
      ix = ix + incx
   end do
!
!  Put the existing sum of squares into one of the accumulators
!
   if( sumsq > zero ) then
      ax = scl*sqrt( sumsq )
      if (ax > tbig) then
!        We assume scl >= sqrt( TINY*EPS ) / sbig
         abig = abig + (scl*sbig)**2 * sumsq
      else if (ax < tsml) then
!        We assume scl <= sqrt( HUGE ) / ssml
         if (notbig) asml = asml + (scl*ssml)**2 * sumsq
      else
         amed = amed + scl**2 * sumsq
      end if
   end if
!
!  Combine abig and amed or amed and asml if more than one
!  accumulator was used.
!
   if (abig > zero) then
!
!     Combine abig and amed if abig > 0.
!
      if (amed > zero .or. LA_ISNAN(amed)) then
         abig = abig + (amed*sbig)*sbig
      end if
      scl = one / sbig
      sumsq = abig
   else if (asml > zero) then
!
!     Combine amed and asml if asml > 0.
!
      if (amed > zero .or. LA_ISNAN(amed)) then
         amed = sqrt(amed)
         asml = sqrt(asml) / ssml
         if (asml > amed) then
            ymin = amed
            ymax = asml
         else
            ymin = asml
            ymax = amed
         end if
         scl = one
         sumsq = ymax**2*( one + (ymin/ymax)**2 )
      else
         scl = one / ssml
         sumsq = asml
      end if
   else
!
!     Otherwise all values are mid-range or zero
!
      scl = one
      sumsq = amed
   end if
   return
end subroutine


MODULE HASHTABLE

  USE BIG_INTEGER_MODULE

CONTAINS

  SUBROUTINE KEY2KEY(KEY1, KEY2, REACTION, REACTIONKEY, RKEYSIGN, PD)
    USE BIG_INTEGER_MODULE
    IMPLICIT NONE

    INTEGER REACTION, PD, RKEYSIGN(PD)
    TYPE (BIG_INTEGER) :: KEY1, KEY2, REACTIONKEY(PD)

    IF (RKEYSIGN(REACTION).GT.0) THEN
       KEY2 = KEY1 + REACTIONKEY(REACTION)
    ELSE
       KEY2 = KEY1 - REACTIONKEY(REACTION)
    ENDIF
  END SUBROUTINE  KEY2KEY

  SUBROUTINE KEY2KEYBW(KEY1, KEY2, REACTION, REACTIONKEY, RKEYSIGN, PD)
    USE BIG_INTEGER_MODULE
    IMPLICIT NONE

    INTEGER REACTION, PD, RKEYSIGN(PD)
    TYPE (BIG_INTEGER) :: KEY1, KEY2, REACTIONKEY(PD)

    IF (RKEYSIGN(REACTION)>0) THEN
       IF (KEY1<REACTIONKEY(REACTION)) THEN
          KEY2 = 0
       ELSE
          KEY2 = KEY1 - REACTIONKEY(REACTION)
       ENDIF
    ELSE
       KEY2 = KEY1 + REACTIONKEY(REACTION)
    ENDIF
  END SUBROUTINE KEY2KEYBW

  SUBROUTINE STATE2KEY(KEY, STATE, SD, B)
    !---  RETURNS THE ENUMERATION OF THE STATE
    USE BIG_INTEGER_MODULE
    IMPLICIT NONE
    INTEGER SD, STATE(SD), B, K
    TYPE (BIG_INTEGER) :: J, KEY, C

    C = BIG(B)
    J = 2
    DO K = 1, SD
       J = J + STATE(K) * (C + 1)**(K - 1)
    ENDDO
    DO K = 1, SD
       !        CHECK STATE IS
       !        1.   NONNEGATIVE;  AND
       !        2.   INSIDE MAXIMUM ALLOWED;
       !        (OTHERWISE SET J=0 AS FLAG)
       IF (STATE(K).LT.0 .OR. STATE(K).GT.B) J = 0
    ENDDO
    KEY = J
  END SUBROUTINE STATE2KEY
  !----------------------------------------------------------------------|
  SUBROUTINE HASH(KEY, MODE, KA, FOUND, &
       KTLEN, KEYTAB, KVTAB)
    !     ADAPTED FOR OUR CME-SOLVER.
    !     USES BIG_INTEGER_MODULE
    !     SHOULD TAKE ADVANTAGE OF THE FACT THAT DELETE IS NOT USED.
    !     CHANGES ADDED:
    !     - A CONSTANT DELKEY FOR DELETED KEY (SINCE BIG_INTEGER DOES NOT
    !     SUPPORT NEGATIVE NUMBERS).
    !     - IMPLICIT NONE
    !===============BRENT'S HASHING ALGORITHM.
    !               SEE CACM 16(2):105-109.
    !
    !       THIS ROUTINE WILL LOOK UP AND INSERT OR DELETE
    !       INTEGER KEYS IN THE TABLE KEYTAB.  VALUES ASSOCIATED
    !       WITH THE KEYS MAY BE STORED IN THE TABLE KVTAB. THE
    !       ROUTINE IS DESIGNED TO BE EFFICIENT, EVEN IF THE TABLE
    !       IS NEARLY FULL, PROVIDED MOST ENTRIES ARE LOOKED UP
    !       SEVERAL TIMES.
    !  PARAMETERS:
    !  /BRENT/ COMMON BLOCK DESCRIBES:
    !          LEN:    THE TABLE LENGTH.  MUST BE GIVEN AS A PRIME NUMBER
    !                  BEFORE HASH IS CALLED. NO CHECK IS MADE FOR PRIMALITY
    !
    !          KEYTAB: IS GIVEN AS A LEN-VECTOR FOR STORING THE KEYS. IT
    !                  MUST BE INITIALIZED TO ALL ZEROS BEFORE THE FIRST
    !                  CALL TO HASH.
    !
    !          KVTAB:  IS USED BY BOTH HASH AND THE CALLING PROGRAM AS
    !                  A LEN-VECOTR OF VALUES ASSOCIATED WITH KEYS IN
    !                  THE HASH TABLE, KEYTAB.  KVTAB COULD ALSO BE A
    !                  POINTER VECTOR INITIALIZED TO THE INTEGERS 1,2,...LEN
    !                  IF MULTIPLE VALUES ARE ASSOCIATED WITH EACH KEY.
    !  (KEY,MODE,KA,FOUND):
    !          KEY:    IS GIVEN AS A POSITIVE INTEGER KEY TO BE ENTERED,
    !                  DELETED, OR PROBED FOR.
    !                  THE VALUES 0 AND -1 ARE RESERVED FOR EMPTY SPACE
    !                  OR DELETED ITEM RESPECTIVELY.
    !
    !          MODE:   IS GIVEN AS AN INDICATOR AS FOLLOWS...
    !          MODE=1: MEANS LOOK UP ONLY.  IF THE KEY IS IN KEYTAB, THEN
    !                  KA IS RETURNED AS THE SUBSCRIPT POINTER; OTHERWISE
    !                  KA IS RETURNED 0.  THE VALUE ASSOCIATED WITH
    !                  KEYTAB(KA) IS KVTAB(KA) WHEN KA IS NOT ZERO.
    !          MODE=2: MEANS LOOK UP AND ENTER.  THIS IS USED TO BUILD
    !                  THE TABLE.  OTHERWISE THE KEY IS ENTERED AT
    !                  KEYTAB(KA).  KA IS RETURNED AS THE POINTER SUBSCRIPT
    !                  IN EITHER CASE, OR KA=0 IF AN ENTRY IS ATTEMPTED AND
    !                  THE TABLE IS FULL OR KA=0 IF KEY=0.  UPON RETURN,
    !                  WHEN FOUND=.FALSE., THE CALLING PROGRAM MUST ENTER
    !                  THE VALUE ASSOCIATED WITH KEY WHEN MODE=2.
    !          MODE=3: MEANS LOOK UP AND DELETE.  IF THE KEY IS IN THE TABLE
    !                  IT IS DELETED AND ITS FORMER ADDRESS KA IS RETURNED.
    !                  IF THE KEY IS NOT THERE, KA=0 IS RETURNED.  (THE
    !                  SUBROUTINE CAN BE SIMPLIFIED CONSIDERABLY IF KEYS
    !                  NEVER DELETED).
    !
    !          KA:     IS RETUNED AS THE SUBSCRIPT OF KEYTAB SUCH THAT
    !                  KEY=KEYTAB(KA) OR KA IS RETURNED AS ZERO IF KEY=0
    !                  OR KEY IS NOT IN KEYTAB.
    !
    !          FOUND:  IS A LOGICAL FLAG RETURNED .TRUE. IF THE KEY WAS
    !                  FOUND IN KEYTAB; ELSE FOUND IS RETURNED .FALSE. .
    !
    !          KTLEN: LENGTH OF KEY TABLE

    USE BIG_INTEGER_MODULE

    IMPLICIT NONE

    INTEGER IQ, IR, IC, LEN2, IA, IX, JQ, JR
    LOGICAL FOUND, DEL
    !-----------THE COMMON STATEMENT  BELOW MUST BE CHANGED TO MEET
    !           THE PROBLEM SIZE.
    INTEGER KTLEN, MODE, KA
    INTEGER KVTAB(KTLEN)
    TYPE (BIG_INTEGER) :: KEY, KEYTAB(KTLEN), IS, KT, DELKEY
    !      INTEGER KEY, KEYTAB(KTLEN)

    DELKEY = 1

    LEN2 = KTLEN - 2
    IC = -1
    !--------COMPUTE ADDRESS OF FIRST PROBE(IR) AND INCREMENT(IQ).
    !        ANY INDEPENDENT PSEUDO-RANDOM FUNCTIONS OF THE KEY MAY BE USED,
    !        PROVIDED 0<IQ<LEN AND 0<IR<=LEN .
    IQ = MODULO(KEY, LEN2) + 1
    IR = MODULO(KEY, KTLEN) + 1
    !      IQ=MOD(IABS(KEY),LEN2)+1
    !      IR=MOD(IABS(KEY),KTLEN)+1
    KA = IR
    !-----------LOOK IN THE TABLE.
20  KT = KEYTAB(KA)
    !----------CHECK FOR AN EMPTY SPACE, A DELETE ENTRY, OR A MATCH.
    IF(KT.EQ.0) GO TO 30
    IF(KT.EQ.DELKEY) GO TO 40
    IF(KT.EQ.KEY) GO TO 60
    IC = IC + 1
    !---------COMPUTE ADDRESS OF NEXT PROBE.
    KA = KA + IQ
    IF(KA.GT.KTLEN)KA = KA - KTLEN
    !----------SEE IF WHOLE TABLE HAS BEEN SEARCHED.
    IF(KA.NE.IR) GO TO 20
    !---------THE KEY IS NOT IN THE TABLE.
30  FOUND = .FALSE.
    !---------RETURN WITH KA=0 UNLES AN ENTRY HAS TO BE MADE.
    IF((MODE.EQ.2).AND.(IC.LE.LEN2) .AND.(KEY.NE.0).AND.&
         (KEY.NE.DELKEY))&
         GO TO 70
    KA = 0
    RETURN
    !-----------A DELETED ENTRY HAS BEEN FOUND.
40  IA = KA
    !----------COMPUTE ADDRESS OF NEXT PROBE.
50  IA = IA + IQ
    IF(IA.GT.KTLEN) IA = IA - KTLEN
    IS = KEYTAB(IA)
    !----------CHECK FOR AN EMPTY SPACE OR A COMPLETE SCAN OF THE TABLE.
    IF((IS.EQ.0) .OR.(IA.EQ.IR)) GO TO 30
    !------------CHECK FOR A MISMATCH OR A DELETED ENTRY.
    IF((IS.NE.KEY).OR.(IS.EQ.DELKEY)) GO TO 50
    !----------KEY FOUND.  MOVE IT AND THE ASSOCIAATED VALUE TO SAVE PROBES
    !          ON THE NEXT SEARCH FOR THE SAME KEY.
    KVTAB(KA) = KVTAB(IA)
    KEYTAB(KA) = IS
    KEYTAB(IA) = DELKEY
    !----------THE KEY IS IN THE TABLE.
60  FOUND = .TRUE.
    !----------DELETE IT IF MODE=3
    IF(MODE.EQ.3) KEYTAB(KA) = DELKEY
    RETURN
    !-----------LOOK FOR THE BEST WAY TO MAKE AN ENTRY.
70  IF(IC.LE.0) GO TO 120
    !----------SET DEL IF A DELETED ENTRY HAS BEEN FOUND.
    DEL = KT.NE.0
    IA = KA
    IS = 0
    !-----------COMPUTE THE MAXIMUM LENGTH TO SEARCH ALONG CURRENT CHAIN.
80  IX = IC - IS
    !-----------COMPUTE INCREMENT JQ FOR CURRENT CHAIN.
    !      JQ=MOD(IABS(KEYTAB(IR)),LEN2)+1
    JQ = MODULO(KEYTAB(IR), LEN2) + 1
    JR = IR
    !----------LOOK ALONG THE CHAIN.
90  JR = JR + JQ
    IF(JR.GT.KTLEN) JR = JR - KTLEN
    KT = KEYTAB(JR)
    !----------CHECK FOR A HOLE (AN EMPTY SPACE OR DELETED ENTRY).
    IF((KT.EQ.0).OR.(KT.EQ.DELKEY)) GO TO 100
    IX = IX - 1
    IF(IX.GT.0) GO TO 90
    GO TO 110
    !----------SKIP IF THIS IS AN EMPTY SPACE AND A DELETED ENTRY HAS
    !          ALREADY BEEN FOUND.
100 IF(DEL.AND.(KT.EQ.0)) GO TO 110
    !------------CHECK FOR A DELETED ENTRY.
    IF(KT.NE.0) DEL = .TRUE.
    !-----------SAVE LOCATION OF HOLE.
    IA = JR
    KA = IR
    IC = IC - IX
    !------------MOVE DOWN TO THE NEXT CHAIN.
110 IS = IS + 1
    IR = IR + IQ
    IF(IR.GT.KTLEN) IR = IR - KTLEN
    !---------GO BACK IF A BETTER HOLE MIGHT STILL BE FOUND.
    IF(IC.GT.IS) GO TO 80
    !---------SKIP IF THERE IS NOTHING TO MOVE.
    IF(IA.EQ.KA) GO TO 120
    !---------MOVE AN OLD ENTRY AND ITS ASSOCIATED VALUE TO MAKE ROOM FOR
    !         THE NEW ENTRY.
    KVTAB(IA) = KVTAB(KA)
    KEYTAB(IA) = KEYTAB(KA)
    !---------ENTER THE NEW KEY, BUT NOT ITS ASSICIATED VALUE.
120 KEYTAB(KA) = KEY
    RETURN
  END SUBROUTINE HASH

END MODULE HASHTABLE


!*******************************************************************************
!> license: BSD
!
!  Test cases for the function parser module.

    program tests

    use function_parser
    use iso_fortran_env, only: wp => real64, output_unit

    implicit none

    call fptest()
    call fptest2()
    call fptest3()
    call fptest4()
    call fptest5()
    call fptest6()

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

!*******************************************************************************
    subroutine fptest()

    character(len=*),parameter :: func = '-x'
    integer,parameter :: nvar = 1  !! number of variables
    character(len=*),dimension(nvar),parameter :: var  = [ 'x' ]
    real(wp),dimension(nvar),parameter :: val  = [  2.0_wp  ]

    type(fparser) :: parser
    real(wp) :: res
    real(wp) :: x

    write(*,*) ''
    write(*,*) ' Test 1'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else

        write(*,*)'==> bytecode evaluation:'
        call parser%evaluate(val,res)  ! interprete bytecode representation of function
        if (parser%error()) then
            call parser%print_errors(output_unit)
        else
            write(*,*) func,'=',res
            write(*,*)'==> direct evaluation:'
            x  = val(1)
            write(*,*)'-x=',-x
        end if
    end if

    end subroutine fptest
!*******************************************************************************

!*******************************************************************************
    subroutine fptest2()

    implicit none

    integer, parameter :: nfunc = 3
    character (len=*), dimension(nfunc), parameter :: func = [  'a0*b0 ', &
                                                                'a1/b1 ', &
                                                                'a3**b3' ]
    integer, parameter :: nvar = 6
    character (len=*), dimension(nvar),  parameter :: var  = [ 'a0', &
                                                               'b0', &
                                                               'a1', &
                                                               'b1', &
                                                               'a3', &
                                                               'b3' ]
    real(wp),dimension(nvar),parameter :: val  = [ 1.0_wp, 2.0_wp, 3.0_wp, &
                                                   1.0_wp, 5.0_wp, 6.0_wp ]

    type(fparser_array) :: parser
    real(wp),dimension(nfunc) :: res
    integer :: i  !! counter
    real(wp) :: a0,b0,a1,b1,a3,b3

    write(*,*) ''
    write(*,*) ' Test 2'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else

        write(*,*)'==> bytecode evaluation:'
        call parser%evaluate(val,res)  ! interprete bytecode representation of function
        if (parser%error()) then
            call parser%print_errors(output_unit)
        else
            do i=1,nfunc
                write(*,*) func(i),'=',res(i)
            end do
            write(*,*)'==> direct evaluation:'
            a0 = val(1)
            b0 = val(2)
            a1 = val(3)
            b1 = val(4)
            a3 = val(5)
            b3 = val(6)
            write(*,*)'res=',a0*b0
            write(*,*)'res=',a1/b1
            write(*,*)'res=',a3**b3
        end if
    end if

    end subroutine fptest2
!*******************************************************************************

!*******************************************************************************
    subroutine fptest3()

    implicit none

    integer,                             parameter :: nfunc = 3
    character (len=*), dimension(nfunc), parameter :: func = [  'vel*cos(beta)           ', &
                                                                'vel*sin(beta)*cos(alpha)', &
                                                                'vel*sin(beta)*sin(alpha)' ]
    integer,                             parameter :: nvar = 3
    character (len=*), dimension(nvar),  parameter :: var  = [  'vel  ', &
                                                                'alpha', &
                                                                'beta ' ]
    real(wp),          dimension(nvar),  parameter :: val  = [  10., 1.5, 2.0  ]

    type(fparser_array) :: parser
    real(wp),dimension(nfunc) :: res
    integer :: i  !! counter
    real(wp) :: vel,alpha,beta

    write(*,*) ''
    write(*,*) ' Test 3'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else

        write(*,*)'==> bytecode evaluation:'
        call parser%evaluate(val,res)  ! interprete bytecode representation of function
        if (parser%error()) then
            call parser%print_errors(output_unit)
        else
            do i=1,nfunc
                write(*,*) func(i),'=',res(i)
            end do
            write(*,*)'==> direct evaluation:'
            vel   = val(1)
            alpha = val(2)
            beta  = val(3)
            write(*,*)'res=',vel*cos(beta)
            write(*,*)'res=',vel*sin(beta)*cos(alpha)
            write(*,*)'res=',vel*sin(beta)*sin(alpha)
        end if
    end if

    end subroutine fptest3
!*******************************************************************************

!*******************************************************************************
!>
!  Assesses how fast the interpreter is compared against a direct evaluation.

    subroutine fptest4()

    implicit none

    integer, parameter :: neval = 1000000
    integer, parameter :: nfunc = 3
    character (len=*), dimension(nfunc), parameter :: func = [ 'vel*cos(beta)           ', &
                                                               'vel*sin(beta)*cos(alpha)', &
                                                               'vel*sin(beta)*sin(alpha)' ]
    integer, parameter :: nvar = 3
    character (len=*), dimension(nvar),  parameter :: var  = [ 'vel  ', &
                                                               'alpha', &
                                                               'beta ' ]
    real(wp), dimension(nvar),  parameter :: val  = [  10., 1.5, 2.0  ]

    type(fparser_array) :: parser
    real(wp),dimension(nfunc) :: res
    integer  :: i,n
    real     :: rt1,rt2,rt3
    real(wp) :: vel,alpha,beta

    write(*,*) ''
    write(*,*) ' Test 4'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else

        vel   = val(1)
        alpha = val(2)
        beta  = val(3)
        call cpu_time (rt1)
        do n=1,neval
            call parser%evaluate(val,res)  ! interprete bytecode representation of function
            if (parser%error()) then
                call parser%print_errors(output_unit)
                return
            end if
        end do
        write(*,*)'==> bytecode evaluation:'
        write(*,*) 'res=',res
        call cpu_time (rt2)
        do n=1,neval
            res(1) = vel*cos(beta)
            res(2) = vel*sin(beta)*cos(alpha)
            res(3) = vel*sin(beta)*sin(alpha)
        end do
        write(*,*)'==> direct evaluation:'
        write(*,*) 'res=',res
        call cpu_time (rt3)
        write(*,*)'function evaluation:'
        write(*,*)'- bytecode interpreter cpu time = ',rt2-rt1
        write(*,*)'- machine code         cpu time = ',rt3-rt2,' = ',(rt3-rt2)/(rt2-rt1)*100.0_wp,'%'

    end if

    end subroutine fptest4
!*******************************************************************************

!*******************************************************************************
!>
!  An example with no variables.
!
!@warning This one fails on gfortran with `-O2` optimization enabled.

    subroutine fptest5()

    implicit none

    character(len=*),parameter :: func = '1.0e0 + 5.e1'
    character(len=*),dimension(1),parameter :: var  = 'a' !! not really used here
    real(wp),dimension(1),parameter :: val  = [0.0_wp]    !! not really used here

    type(fparser) :: parser
    real(wp) :: res
    real(wp) :: x

    write(*,*) ''
    write(*,*) ' Test 5'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else
        write(*,*)'==> bytecode evaluation:'
        call parser%evaluate(val,res)  ! interprete bytecode representation of function
        if (parser%error()) then
            call parser%print_errors(output_unit)
        else
            write(*,*) func,'=',res
        end if
    end if

    end subroutine fptest5
!*******************************************************************************

!*******************************************************************************
    subroutine fptest6()

    implicit none

    integer, parameter :: nfunc = 4
    character (len=*), dimension(nfunc), parameter :: func = [  '-1.0*x        ',  &
                                                                '-x            ',  &
                                                                'a*COS(b*x)+5  ',  &
                                                                'a*COS(b*x)+5.0' ]
    integer, parameter :: nvar = 3
    character (len=*), dimension(nvar),  parameter :: var  = [  'x', &
                                                                'a', &
                                                                'b'  ]
    real(wp), dimension(nvar),  parameter :: val  = [  2.0_wp, 3.0_wp, 4.0_wp ]

    type(fparser_array) :: parser
    real(wp),dimension(nfunc) :: res
    integer :: i  !! counter
    real(wp) :: x,a,b

    write(*,*) ''
    write(*,*) ' Test 6'
    write(*,*) ''

    call parser%parse(func, var, .false.)  ! parse and bytecompile function string
    if (parser%error()) then
        call parser%print_errors(output_unit)
    else

        write(*,*)'==> bytecode evaluation:'
        call parser%evaluate(val,res)  ! interprete bytecode representation of function
        if (parser%error()) then
            call parser%print_errors(output_unit)
        else
            do i=1,nfunc
                write(*,*) func(i),'=',res(i)
            end do
            write(*,*)'==> direct evaluation:'
            x  = val(1)
            a  = val(2)
            b  = val(3)
            write(*,*)'-1.0*x        =',-1.0_wp*x
            write(*,*)'-x            =',-x
            write(*,*)'a*cos(b*x)+5  =',a*cos(b*x)+5
            write(*,*)'a*cos(b*x)+5.0=',a*cos(b*x)+5.0_wp
        end if
    end if

    end subroutine fptest6
!*******************************************************************************

!*******************************************************************************
    end program tests
!*******************************************************************************


program hip_sgemv
  use iso_c_binding
  use hipfort
  use hipfort_check
  use hipfort_hipblas

  implicit none

  integer :: m = 6
  integer :: n = 5
  integer :: i, j

  real, parameter :: alpha = 1.0
  real, parameter :: beta = 0.0

  type(c_ptr) :: handle = c_null_ptr

  real(kind=4), allocatable, target, dimension(:) :: a, x, y

  type(c_ptr) :: da = c_null_ptr, dx = c_null_ptr, dy = c_null_ptr

  integer(c_size_t) :: Nabytes, Nxbytes, Nybytes
  integer, parameter :: bytes_per_element = 4 !float precision

  real :: error
  real, parameter :: error_max = 10*epsilon(error)

  Nabytes = m * n * bytes_per_element
  Nxbytes = n * bytes_per_element
  Nybytes = m * bytes_per_element

  allocate(x(n))
  allocate(y(m))
  allocate(a(m*n))

    a(:) = 1.0
    x(:) = 1.0
    y(:) = 1.0

  write(*,"(a)",advance="no") "-- Running test 'SGEMV' (Fortran 2003 interfaces) - "

  call hipblasCheck(hipblasCreate(handle))

  call hipCheck(hipMalloc(dx,Nxbytes))
  call hipCheck(hipMalloc(dy,Nybytes))
  call hipCheck(hipMalloc(da,Nabytes))

  call hipCheck(hipMemcpy(da, c_loc(a), Nabytes, hipMemcpyHostToDevice))
  call hipCheck(hipMemcpy(dx, c_loc(x), Nxbytes, hipMemcpyHostToDevice))
  call hipCheck(hipMemcpy(dy, c_loc(y), Nybytes, hipMemcpyHostToDevice))

  call hipCheck(hipblasSgemv(handle,HIPBLAS_OP_N,m,n,alpha,da,m,dx,1,beta,dy,1))

  call hipCheck(hipMemcpy(c_loc(y), dy, Nybytes, hipMemcpyDeviceToHost))

  do i = 1,m
    error = abs(5.0 - y(i))
      if( error > 10*epsilon(error) )then
        write(*,*) "FAILED! Error bigger than max! Error = ", error, "y(i) = ", y(i)
        call exit(1)
     end if
  end do

  call hipblasCheck(hipblasDestroy(handle))

  call hipCheck(hipFree(da))
  call hipCheck(hipFree(dx))
  call hipCheck(hipFree(dy))

  deallocate(a)
  deallocate(x)
  deallocate(y)

  write(*,*) "PASSED!"

end program hip_sgemv


!
! Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
! See https://llvm.org/LICENSE.txt for license information.
! SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
!

program testieee18
use ieee_arithmetic
real*4 a, ral, rsa, rone
real*4 aa1(140), aa2(140), aa3(140), aa4(140)
logical ll1(140), ll2(140), le1(140), le2(140)
logical luflow!, cg_support_underflow_control
logical lfsav(5), lfset(5)

lfset = .false.
call ieee_get_halting_mode(ieee_all, lfsav)
call ieee_set_halting_mode(ieee_all, lfset)

call ieee_get_underflow_mode(luflow)
call ieee_set_underflow_mode(.true.)

rone = 1.0
rlogb = 0.0
a = rone
do j = 1, 140
  ral = ieee_logb(a)
  aa1(j) = ral
  aa2(j) = rlogb
  i = int(ieee_rint(ral))
  rsa = ieee_scalb(rone,i)
  aa3(j) = rsa
  aa4(j) = a
  a = a * 0.5
  rlogb = rlogb - 1.0
end do

do j = 1, 140
  ll1(j) = aa1(j) .eq. aa2(j)
  ll2(j) = aa3(j) .eq. aa4(j)
end do

le1 = .true.
le2 = .true.

call check(ll1, le1, 140)
call check(ll2, le2, 140)

! This entire section was under a condition.
! We need to stop putting conditions in tests
!
! if (cg_support_underflow_control(a)) then

call ieee_set_underflow_mode(.false.)

rone = 1.0
rlogb = 0.0
a = rone
do j = 1, 140
  ral = ieee_logb(a)
  i = int(ieee_rint(ral))
  rsa = ieee_scalb(rone,i)
  aa3(j) = rsa
  aa4(j) = a
  a = a * 0.5
  rlogb = rlogb - 1.0
end do

do j = 1, 140
  ll2(j) = aa3(j) .eq. aa4(j)
end do

le2 = .true.

call check(ll2, le2, 140)
!end if

call ieee_set_underflow_mode(luflow)
call ieee_set_halting_mode(ieee_all, lfsav)
end

!logical function cg_support_underflow_control(a)
!use ieee_arithmetic
!real*4 a, b
!b = ieee_value(a,ieee_negative_inf)
!cg_support_underflow_control = ieee_is_normal(exp(b))
!return
!end


program pgm
  integer c
  integer i
  c = 10

  do i = 1,10
    if(i .gt. 4) then
      c = c + 1
    else if ( i .gt. 3) then
      c = c + 2
    else if ( i .gt. 2) then
      c = c + 2
    else
      c = c -1
    endif
  end do

  print *, c
  end


module model_mod
  implicit none

  type :: model
     real, dimension(2) :: theta
  end type model

contains

  subroutine model_init(self, theta)
    type(model), intent(inout) :: self
    real, dimension(2), intent(in) :: theta
    self%theta = theta
  end subroutine model_init

  subroutine model_obj(self, x, y)
    type(model), intent(inout) :: self
    real, dimension(:), intent(in) :: x
    real, intent(out) :: y
    y = (x(1) - self%theta(1))**2 + (x(2) - self%theta(2))**2
  end subroutine model_obj

  subroutine model_obj_wrapper(x, y, param)
    real, dimension(:), intent(in) :: x
    real, intent(out) :: y
    class(*), intent(inout), optional :: param

    call model_obj(param, x, y)

    if (present(param)) then
       select type(param)
       type is (model)
          call model_obj(param, x, y)
       end select
    end if
  end subroutine model_obj_wrapper

end module model_mod

module optimizer
  implicit none

contains

  subroutine optimize(func, x, param)
    real, dimension(:), intent(in) :: x
    class(*), intent(inout), optional :: param
    real :: y

    interface
       subroutine func(x, y, param)
         real, dimension(:), intent(in) :: x
         real, intent(out) :: y
         class(*), intent(inout), optional :: param
       end subroutine func
    end interface

    call func(x, y, param)
    print *, 'y = ', y

  end subroutine optimize

end module optimizer

program test_model
  use model_mod
  use optimizer
  implicit none

  type(model) :: mod
  real, dimension(2) :: theta = (/ 2.0d0, 1.0d0 /)
  real, dimension(2) :: x = (/ -1.0d0, 3.0d0 /)

  call model_init(mod, theta)

  call optimize(model_obj_wrapper, x, mod)

end program test_model


      subroutine runF_iiii(i1,i2,i3,i4,m0sq,Gr,Czero4,N0)
C---  Expression for rearrangement of extension of Eq. 5.69
C---  Calculates Diiii, requires D00iiii
C---  Small terms of order Gr(i,j)*Dijklmn
C---  Denominator m0sq
      implicit none
      include 'TRconstants.f'
      include 'pvDnames.f'
      include 'pvDv.f'
      include 'Darraydef.f'
      include 'Darrays.f'
      include 'pvweenumber.f'
      integer ep,N0,i1,i2,i3,i4,m,n,np
      parameter(np=3)
      double precision m0sq,Gr(np,np)
      double complex Czero4(z4max,-2:0),bit,pole

      do ep=-2,0
      bit=czip
      do n=1,np
      do m=1,np
      bit=bit+Gr(n,m)*Dv(diiiiii(z6(n,m,i1,i2,i3,i4))+N0,ep)
      enddo
      enddo

      pole=czip
      if (ep .gt. -2) pole=4d0*Dv(dzziiii(z4(i1,i2,i3,i4))+N0,ep-1)

      Dv(diiii(z4(i1,i2,i3,i4))+N0,ep)=
     . (24d0*Dv(dzziiii(z4(i1,i2,i3,i4))+N0,ep)
     . -pole
     . -2d0*Czero4(z4(i1,i2,i3,i4),ep)
     . +bit)/(2d0*m0sq)

      enddo

      return
      end


      SUBROUTINE AB09HD( DICO, JOB, EQUIL, ORDSEL, N, M, P, NR, ALPHA,
     $                   BETA, A, LDA, B, LDB, C, LDC, D, LDD, NS, HSV,
     $                   TOL1, TOL2, IWORK, DWORK, LDWORK, BWORK, IWARN,
     $                   INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute a reduced order model (Ar,Br,Cr,Dr) for an original
C     state-space representation (A,B,C,D) by using the stochastic
C     balancing approach in conjunction with the square-root or
C     the balancing-free square-root Balance & Truncate (B&T)
C     or Singular Perturbation Approximation (SPA) model reduction
C     methods for the ALPHA-stable part of the system.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DICO    CHARACTER*1
C             Specifies the type of the original system as follows:
C             = 'C':  continuous-time system;
C             = 'D':  discrete-time system.
C
C     JOB     CHARACTER*1
C             Specifies the model reduction approach to be used
C             as follows:
C             = 'B':  use the square-root Balance & Truncate method;
C             = 'F':  use the balancing-free square-root
C                     Balance & Truncate method;
C             = 'S':  use the square-root Singular Perturbation
C                     Approximation method;
C             = 'P':  use the balancing-free square-root
C                     Singular Perturbation Approximation method.
C
C     EQUIL   CHARACTER*1
C             Specifies whether the user wishes to preliminarily
C             equilibrate the triplet (A,B,C) as follows:
C             = 'S':  perform equilibration (scaling);
C             = 'N':  do not perform equilibration.
C
C     ORDSEL  CHARACTER*1
C             Specifies the order selection method as follows:
C             = 'F':  the resulting order NR is fixed;
C             = 'A':  the resulting order NR is automatically determined
C                     on basis of the given tolerance TOL1.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the original state-space representation,
C             i.e., the order of the matrix A.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs.  M >= 0.
C
C     P       (input) INTEGER
C             The number of system outputs.  P >= 0.
C             P <= M if BETA = 0.
C
C     NR      (input/output) INTEGER
C             On entry with ORDSEL = 'F', NR is the desired order of the
C             resulting reduced order system.  0 <= NR <= N.
C             On exit, if INFO = 0, NR is the order of the resulting
C             reduced order model. For a system with NU ALPHA-unstable
C             eigenvalues and NS ALPHA-stable eigenvalues (NU+NS = N),
C             NR is set as follows: if ORDSEL = 'F', NR is equal to
C             NU+MIN(MAX(0,NR-NU),NMIN), where NR is the desired order
C             on entry, and NMIN is the order of a minimal realization
C             of the ALPHA-stable part of the given system; NMIN is
C             determined as the number of Hankel singular values greater
C             than NS*EPS, where EPS is the machine precision
C             (see LAPACK Library Routine DLAMCH);
C             if ORDSEL = 'A', NR is the sum of NU and the number of
C             Hankel singular values greater than MAX(TOL1,NS*EPS);
C             NR can be further reduced to ensure that
C             HSV(NR-NU) > HSV(NR+1-NU).
C
C     ALPHA   (input) DOUBLE PRECISION
C             Specifies the ALPHA-stability boundary for the eigenvalues
C             of the state dynamics matrix A. For a continuous-time
C             system (DICO = 'C'), ALPHA <= 0 is the boundary value for
C             the real parts of eigenvalues, while for a discrete-time
C             system (DICO = 'D'), 0 <= ALPHA <= 1 represents the
C             boundary value for the moduli of eigenvalues.
C             The ALPHA-stability domain does not include the boundary.
C
C     BETA    (input) DOUBLE PRECISION
C             BETA > 0 specifies the absolute/relative error weighting
C             parameter. A large positive value of BETA favours the
C             minimization of the absolute approximation error, while a
C             small value of BETA is appropriate for the minimization
C             of the relative error.
C             BETA = 0 means a pure relative error method and can be
C             used only if rank(D) = P.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the state dynamics matrix A.
C             On exit, if INFO = 0, the leading NR-by-NR part of this
C             array contains the state dynamics matrix Ar of the reduced
C             order system.
C             The resulting A has a block-diagonal form with two blocks.
C             For a system with NU ALPHA-unstable eigenvalues and
C             NS ALPHA-stable eigenvalues (NU+NS = N), the leading
C             NU-by-NU block contains the unreduced part of A
C             corresponding to ALPHA-unstable eigenvalues in an
C             upper real Schur form.
C             The trailing (NR+NS-N)-by-(NR+NS-N) block contains
C             the reduced part of A corresponding to ALPHA-stable
C             eigenvalues.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the original input/state matrix B.
C             On exit, if INFO = 0, the leading NR-by-M part of this
C             array contains the input/state matrix Br of the reduced
C             order system.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the original state/output matrix C.
C             On exit, if INFO = 0, the leading P-by-NR part of this
C             array contains the state/output matrix Cr of the reduced
C             order system.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     D       (input/output) DOUBLE PRECISION array, dimension (LDD,M)
C             On entry, the leading P-by-M part of this array must
C             contain the original input/output matrix D.
C             On exit, if INFO = 0, the leading P-by-M part of this
C             array contains the input/output matrix Dr of the reduced
C             order system.
C
C     LDD     INTEGER
C             The leading dimension of array D.  LDD >= MAX(1,P).
C
C     NS      (output) INTEGER
C             The dimension of the ALPHA-stable subsystem.
C
C     HSV     (output) DOUBLE PRECISION array, dimension (N)
C             If INFO = 0, the leading NS elements of HSV contain the
C             Hankel singular values of the phase system corresponding
C             to the ALPHA-stable part of the original system.
C             The Hankel singular values are ordered decreasingly.
C
C     Tolerances
C
C     TOL1    DOUBLE PRECISION
C             If ORDSEL = 'A', TOL1 contains the tolerance for
C             determining the order of reduced system.
C             For model reduction, the recommended value of TOL1 lies
C             in the interval [0.00001,0.001].
C             If TOL1 <= 0 on entry, the used default value is
C             TOL1 = NS*EPS, where NS is the number of
C             ALPHA-stable eigenvalues of A and EPS is the machine
C             precision (see LAPACK Library Routine DLAMCH).
C             If ORDSEL = 'F', the value of TOL1 is ignored.
C             TOL1 < 1.
C
C     TOL2    DOUBLE PRECISION
C             The tolerance for determining the order of a minimal
C             realization of the phase system (see METHOD) corresponding
C             to the ALPHA-stable part of the given system.
C             The recommended value is TOL2 = NS*EPS.
C             This value is used by default if TOL2 <= 0 on entry.
C             If TOL2 > 0 and ORDSEL = 'A', then TOL2 <= TOL1.
C             TOL2 < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (MAX(1,2*N))
C             On exit with INFO = 0, IWORK(1) contains the order of the
C             minimal realization of the system.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK and DWORK(2) contains RCOND, the reciprocal
C             condition number of the U11 matrix from the expression
C             used to compute the solution X = U21*inv(U11) of the
C             Riccati equation for spectral factorization.
C             A small value RCOND indicates possible ill-conditioning
C             of the respective Riccati equation.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= 2*N*N + MB*(N+P) + MAX( 2, N*(MAX(N,MB,P)+5),
C                                    2*N*P+MAX(P*(MB+2),10*N*(N+1) ) ),
C             where MB = M if BETA = 0 and MB = M+P if BETA > 0.
C             For optimum performance LDWORK should be larger.
C
C     BWORK   LOGICAL array, dimension 2*N
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0:  no warning;
C             = 1:  with ORDSEL = 'F', the selected order NR is greater
C                   than NSMIN, the sum of the order of the
C                   ALPHA-unstable part and the order of a minimal
C                   realization of the ALPHA-stable part of the given
C                   system; in this case, the resulting NR is set equal
C                   to NSMIN;
C             = 2:  with ORDSEL = 'F', the selected order NR corresponds
C                   to repeated singular values for the ALPHA-stable
C                   part, which are neither all included nor all
C                   excluded from the reduced model; in this case, the
C                   resulting NR is automatically decreased to exclude
C                   all repeated singular values;
C             = 3:  with ORDSEL = 'F', the selected order NR is less
C                   than the order of the ALPHA-unstable part of the
C                   given system; in this case NR is set equal to the
C                   order of the ALPHA-unstable part.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  the computation of the ordered real Schur form of A
C                   failed;
C             = 2:  the reduction of the Hamiltonian matrix to real
C                   Schur form failed;
C             = 3:  the reordering of the real Schur form of the
C                   Hamiltonian matrix failed;
C             = 4:  the Hamiltonian matrix has less than N stable
C                   eigenvalues;
C             = 5:  the coefficient matrix U11 in the linear system
C                   X*U11 = U21 to determine X is singular to working
C                   precision;
C             = 6:  BETA = 0 and D has not a maximal row rank;
C             = 7:  the computation of Hankel singular values failed;
C             = 8:  the separation of the ALPHA-stable/unstable diagonal
C                   blocks failed because of very close eigenvalues;
C             = 9:  the resulting order of reduced stable part is less
C                   than the number of unstable zeros of the stable
C                   part.
C     METHOD
C
C     Let be the following linear system
C
C          d[x(t)] = Ax(t) + Bu(t)
C          y(t)    = Cx(t) + Du(t),                      (1)
C
C     where d[x(t)] is dx(t)/dt for a continuous-time system and x(t+1)
C     for a discrete-time system. The subroutine AB09HD determines for
C     the given system (1), the matrices of a reduced order system
C
C          d[z(t)] = Ar*z(t) + Br*u(t)
C          yr(t)   = Cr*z(t) + Dr*u(t),                  (2)
C
C     such that
C
C          INFNORM[inv(conj(W))*(G-Gr)] <=
C                       (1+HSV(NR+NS-N+1)) / (1-HSV(NR+NS-N+1)) + ...
C                       + (1+HSV(NS)) / (1-HSV(NS)) - 1,
C
C     where G and Gr are transfer-function matrices of the systems
C     (A,B,C,D) and (Ar,Br,Cr,Dr), respectively, W is the right, minimum
C     phase spectral factor satisfying
C
C         G1*conj(G1) = conj(W)* W,                      (3)
C
C     G1 is the NS-order ALPHA-stable part of G, and INFNORM(G) is the
C     infinity-norm of G. HSV(1), ... , HSV(NS) are the Hankel-singular
C     values of the stable part of the phase system (Ap,Bp,Cp)
C     with the transfer-function matrix
C
C          P = inv(conj(W))*G1.
C
C     If BETA > 0, then the model reduction is performed on [G BETA*I]
C     instead of G. This is the recommended approach to be used when D
C     has not a maximal row rank or when a certain balance between
C     relative and absolute approximation errors is desired. For
C     increasingly large values of BETA, the obtained reduced system
C     assymptotically approaches that computed by using the
C     Balance & Truncate or Singular Perturbation Approximation methods.
C
C     Note: conj(G)  denotes either G'(-s) for a continuous-time system
C           or G'(1/z) for a discrete-time system.
C           inv(G) is the inverse of G.
C
C     The following procedure is used to reduce a given G:
C
C     1) Decompose additively G as
C
C          G = G1 + G2,
C
C        such that G1 = (As,Bs,Cs,D) has only ALPHA-stable poles and
C        G2 = (Au,Bu,Cu) has only ALPHA-unstable poles.
C
C     2) Determine G1r, a reduced order approximation of the
C        ALPHA-stable part G1 using the balancing stochastic method
C        in conjunction with either the B&T [1,2] or SPA methods [3].
C
C     3) Assemble the reduced model Gr as
C
C           Gr = G1r + G2.
C
C     Note: The employed stochastic truncation algorithm [2,3] has the
C     property that right half plane zeros of G1 remain as right half
C     plane zeros of G1r. Thus, the order can not be chosen smaller than
C     the sum of the number of unstable poles of G and the number of
C     unstable zeros of G1.
C
C     The reduction of the ALPHA-stable part G1 is done as follows.
C
C     If JOB = 'B', the square-root stochastic Balance & Truncate
C     method of [1] is used.
C     For an ALPHA-stable continuous-time system (DICO = 'C'),
C     the resulting reduced model is stochastically balanced.
C
C     If JOB = 'F', the balancing-free square-root version of the
C     stochastic Balance & Truncate method [1] is used to reduce
C     the ALPHA-stable part G1.
C
C     If JOB = 'S', the stochastic balancing method is used to reduce
C     the ALPHA-stable part G1, in conjunction with the square-root
C     version of the Singular Perturbation Approximation method [3,4].
C
C     If JOB = 'P', the stochastic balancing method is used to reduce
C     the ALPHA-stable part G1, in conjunction with the balancing-free
C     square-root version of the Singular Perturbation Approximation
C     method [3,4].
C
C     REFERENCES
C
C     [1] Varga A. and Fasol K.H.
C         A new square-root balancing-free stochastic truncation model
C         reduction algorithm.
C         Proc. 12th IFAC World Congress, Sydney, 1993.
C
C     [2] Safonov M. G. and Chiang R. Y.
C         Model reduction for robust control: a Schur relative error
C         method.
C         Int. J. Adapt. Contr. Sign. Proc., vol. 2, pp. 259-272, 1988.
C
C     [3] Green M. and Anderson B. D. O.
C         Generalized balanced stochastic truncation.
C         Proc. 29-th CDC, Honolulu, Hawaii, pp. 476-481, 1990.
C
C     [4] Varga A.
C         Balancing-free square-root algorithm for computing
C         singular perturbation approximations.
C         Proc. 30-th IEEE CDC,  Brighton, Dec. 11-13, 1991,
C         Vol. 2, pp. 1062-1065.
C
C     NUMERICAL ASPECTS
C
C     The implemented methods rely on accuracy enhancing square-root or
C     balancing-free square-root techniques. The effectiveness of the
C     accuracy enhancing technique depends on the accuracy of the
C     solution of a Riccati equation. An ill-conditioned Riccati
C     solution typically results when [D BETA*I] is nearly
C     rank deficient.
C                                      3
C     The algorithm requires about 100N  floating point operations.
C
C     CONTRIBUTORS
C
C     A. Varga, German Aerospace Center, Oberpfaffenhofen, May 2000.
C     D. Sima, University of Bucharest, May 2000.
C     V. Sima, Research Institute for Informatics, Bucharest, May 2000.
C     Partly based on the RASP routine SRBFS, by A. Varga, 1992.
C
C     REVISIONS
C
C     A. Varga, Australian National University, Canberra, November 2000.
C     V. Sima, Research Institute for Informatics, Bucharest, Dec. 2000.
C              Oct. 2001.
C
C     KEYWORDS
C
C     Minimal realization, model reduction, multivariable system,
C     state-space model, state-space representation,
C     stochastic balancing.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO, TWOBY3, C100
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0,
     $                    TWOBY3 = TWO/3.0D0, C100 = 100.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         DICO, EQUIL, JOB, ORDSEL
      INTEGER           INFO, IWARN, LDA, LDB, LDC, LDD, LDWORK,
     $                  M, N, NR, NS, P
      DOUBLE PRECISION  ALPHA, BETA, TOL1, TOL2
C     .. Array Arguments ..
      INTEGER           IWORK(*)
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), D(LDD,*),
     $                  DWORK(*), HSV(*)
      LOGICAL           BWORK(*)
C     .. Local Scalars ..
      LOGICAL           BTA, DISCR, FIXORD, LEQUIL, SPA
      INTEGER           IERR, IWARNL, KB, KD, KT, KTI, KU, KW, KWI, KWR,
     $                  LW, LWR, MB, NMR, NN, NRA, NU, NU1, WRKOPT
      DOUBLE PRECISION  EPSM, MAXRED, RICOND, SCALEC, SCALEO
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH
      EXTERNAL          DLAMCH, LSAME
C     .. External Subroutines ..
      EXTERNAL          AB04MD, AB09HY, AB09IX, DLACPY, DLASET, TB01ID,
     $                  TB01KD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX, MIN
C     .. Executable Statements ..
C
      INFO   = 0
      IWARN  = 0
      DISCR  = LSAME( DICO,   'D' )
      FIXORD = LSAME( ORDSEL, 'F' )
      LEQUIL = LSAME( EQUIL,  'S' )
      BTA    = LSAME( JOB,    'B' ) .OR. LSAME( JOB, 'F' )
      SPA    = LSAME( JOB,    'S' ) .OR. LSAME( JOB, 'P' )
      MB = M
      IF( BETA.GT.ZERO ) MB = M + P
      LW = 2*N*N + MB*(N+P) + MAX( 2, N*(MAX( N, MB, P )+5),
     $                 2*N*P+MAX( P*(MB+2), 10*N*(N+1) ) )
C
C     Test the input scalar arguments.
C
      IF( .NOT. ( LSAME( DICO, 'C' ) .OR. DISCR ) ) THEN
         INFO = -1
      ELSE IF( .NOT. ( BTA .OR. SPA ) ) THEN
         INFO = -2
      ELSE IF( .NOT. ( LEQUIL .OR. LSAME( EQUIL, 'N' ) ) ) THEN
         INFO = -3
      ELSE IF( .NOT. ( FIXORD .OR. LSAME( ORDSEL, 'A' ) ) ) THEN
         INFO = -4
      ELSE IF( N.LT.0 ) THEN
         INFO = -5
      ELSE IF( M.LT.0 ) THEN
         INFO = -6
      ELSE IF( P.LT.0 .OR. ( BETA.EQ.ZERO .AND. P.GT.M ) ) THEN
         INFO = -7
      ELSE IF( FIXORD .AND. ( NR.LT.0 .OR. NR.GT.N ) ) THEN
         INFO = -8
      ELSE IF( ( DISCR .AND. ( ALPHA.LT.ZERO .OR. ALPHA.GT.ONE ) ) .OR.
     $    ( .NOT.DISCR .AND.   ALPHA.GT.ZERO ) ) THEN
         INFO = -9
      ELSE IF( BETA.LT.ZERO ) THEN
         INFO = -10
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -14
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -16
      ELSE IF( LDD.LT.MAX( 1, P ) ) THEN
         INFO = -18
      ELSE IF( TOL1.GE.ONE ) THEN
         INFO = -21
      ELSE IF( ( TOL2.GT.ZERO .AND. .NOT.FIXORD .AND. TOL2.GT.TOL1 )
     $         .OR. TOL2.GE.ONE ) THEN
         INFO = -22
      ELSE IF( LDWORK.LT.LW ) THEN
         INFO = -25
      END IF
C
      IF( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'AB09HD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( MIN( N, M, P ).EQ.0 .OR.
     $   ( BTA .AND. FIXORD .AND. NR.EQ.0 ) ) THEN
         NR = 0
         NS = 0
         IWORK(1) = 0
         DWORK(1) = TWO
         DWORK(2) = ONE
         RETURN
      END IF
C
      IF( LEQUIL ) THEN
C
C        Scale simultaneously the matrices A, B and C:
C        A <- inv(D)*A*D, B <- inv(D)*B and C <- C*D, where D is a
C        diagonal matrix.
C        Workspace: N.
C
         MAXRED = C100
         CALL TB01ID( 'All', N, M, P, MAXRED, A, LDA, B, LDB, C, LDC,
     $                DWORK, INFO )
      END IF
C
C     Allocate working storage.
C
      NN  = N*N
      KU  = 1
      KWR = KU + NN
      KWI = KWR + N
      KW  = KWI + N
      LWR = LDWORK - KW + 1
C
C     Reduce A to a block-diagonal real Schur form, with the
C     ALPHA-unstable part in the leading diagonal position, using a
C     non-orthogonal similarity transformation A <- inv(T)*A*T and
C     apply the transformation to B and C: B <- inv(T)*B and C <- C*T.
C
C     Workspace needed:      N*(N+2);
C     Additional workspace:  need   3*N;
C                            prefer larger.
C
      CALL TB01KD( DICO, 'Unstable', 'General', N, M, P, ALPHA, A, LDA,
     $             B, LDB, C, LDC, NU, DWORK(KU), N, DWORK(KWR),
     $             DWORK(KWI), DWORK(KW), LWR, IERR )
C
      IF( IERR.NE.0 ) THEN
         IF( IERR.NE.3 ) THEN
            INFO = 1
         ELSE
            INFO = 8
         END IF
         RETURN
      END IF
C
      WRKOPT = INT( DWORK(KW) ) + KW - 1
C
      IWARNL = 0
      NS = N - NU
      IF( FIXORD ) THEN
         NRA = MAX( 0, NR-NU )
         IF( NR.LT.NU )
     $      IWARNL = 3
      ELSE
         NRA = 0
      END IF
C
C     Finish if the system is completely unstable.
C
      IF( NS.EQ.0 ) THEN
         NR = NU
         IWORK(1) = NS
         DWORK(1) = WRKOPT
         DWORK(2) = ONE
         RETURN
      END IF
C
      NU1 = NU + 1
C
C     Allocate working storage.
C
      KB  = 1
      KD  = KB  + N*MB
      KT  = KD  + P*MB
      KTI = KT  + N*N
      KW  = KTI + N*N
C
C     Form [B 0] and [D BETA*I].
C
      CALL DLACPY( 'F', NS, M, B(NU1,1), LDB, DWORK(KB), N )
      CALL DLACPY( 'F', P, M, D, LDD, DWORK(KD), P )
      IF( BETA.GT.ZERO ) THEN
         CALL DLASET( 'F', NS, P, ZERO, ZERO, DWORK(KB+N*M), N )
         CALL DLASET( 'F', P,  P, ZERO, BETA, DWORK(KD+P*M), P )
      END IF
C
C     For discrete-time case, apply the discrete-to-continuous bilinear
C     transformation to the stable part.
C
      IF( DISCR ) THEN
C
C        Real workspace:    need  N, prefer larger;
C        Integer workspace: need  N.
C
         CALL AB04MD( 'Discrete', NS, MB, P, ONE, ONE, A(NU1,NU1), LDA,
     $                DWORK(KB), N, C(1,NU1), LDC, DWORK(KD), P,
     $                IWORK, DWORK(KT), LDWORK-KT+1, IERR )
         WRKOPT = MAX( WRKOPT, INT( DWORK(KT) ) + KT - 1 )
      END IF
C
C     Compute in DWORK(KTI) and DWORK(KT) the Cholesky factors S and R
C     of the controllability and observability Grammians, respectively.
C     Real workspace:    need  2*N*N + MB*(N+P)+
C                              MAX( 2, N*(MAX(N,MB,P)+5),
C                                   2*N*P+MAX(P*(MB+2), 10*N*(N+1) ) );
C                        prefer larger.
C     Integer workspace: need  2*N.
C
      CALL AB09HY( NS, MB, P, A(NU1,NU1), LDA, DWORK(KB), N,
     $             C(1,NU1), LDC, DWORK(KD), P, SCALEC, SCALEO,
     $             DWORK(KTI), N, DWORK(KT), N, IWORK, DWORK(KW),
     $             LDWORK-KW+1, BWORK, INFO )
      IF( INFO.NE.0 )
     $   RETURN
      WRKOPT = MAX( WRKOPT, INT( DWORK(KW) ) + KW - 1 )
      RICOND = DWORK(KW+1)
C
C     Compute a BTA or SPA of the stable part.
C     Real workspace:  need  2*N*N + MB*(N+P)+
C                            MAX( 1, 2*N*N+5*N, N*MAX(MB,P) ).
C
      EPSM = DLAMCH( 'Epsilon' )
      CALL AB09IX( 'C', JOB, 'Schur', ORDSEL, NS, MB, P, NRA, SCALEC,
     $             SCALEO, A(NU1,NU1), LDA, DWORK(KB), N, C(1,NU1), LDC,
     $             DWORK(KD), P, DWORK(KTI), N, DWORK(KT), N, NMR, HSV,
     $             MAX( TOL1, N*EPSM ), TOL2, IWORK, DWORK(KW),
     $             LDWORK-KW+1, IWARN, IERR )
      IWARN = MAX( IWARN, IWARNL )
      IF( IERR.NE.0 ) THEN
         INFO = 7
         RETURN
      END IF
      WRKOPT = MAX( WRKOPT, INT( DWORK(KW) ) + KW - 1 )
C
C     Check if the resulting order is greater than the number of
C     unstable zeros (this check is implicit by looking at Hankel
C     singular values equal to 1).
C
      IF( NRA.LT.NS .AND. HSV(NRA+1).GE.ONE-EPSM**TWOBY3 ) THEN
         INFO = 9
         RETURN
      END IF
C
C     For discrete-time case, apply the continuous-to-discrete
C     bilinear transformation.
C
      IF( DISCR ) THEN
         CALL AB04MD( 'Continuous', NRA, MB, P, ONE, ONE,
     $                A(NU1,NU1), LDA, DWORK(KB), N, C(1,NU1), LDC,
     $                DWORK(KD), P, IWORK, DWORK, LDWORK, IERR )
         WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
      END IF
C
      CALL DLACPY( 'F', NRA, M, DWORK(KB), N, B(NU1,1), LDB )
      CALL DLACPY( 'F', P, M, DWORK(KD), P, D, LDD )
C
      NR = NRA + NU
C
      IWORK(1) = NMR
      DWORK(1) = WRKOPT
      DWORK(2) = RICOND
C
      RETURN
C *** Last line of AB09HD ***
      END


c======================================================================
       subroutine rp1(maxm,meqn,mwaves,maux,mbc,mx,
     &                 ql,qr,auxl,auxr,fwave,s,amdq,apdq)
c======================================================================
c
c Solves normal Riemann problems for the 2D SHALLOW WATER equations
c     with topography:
c     #        h_t + (hu)_x + (hv)_y = 0                           #
c     #        (hu)_t + (hu^2 + 0.5gh^2)_x + (huv)_y = -ghb_x      #
c     #        (hv)_t + (huv)_x + (hv^2 + 0.5gh^2)_y = -ghb_y      #

c On input, ql contains the state vector at the left edge of each cell
c     qr contains the state vector at the right edge of each cell
c
c This data is along a slice in the x-direction if ixy=1
c     or the y-direction if ixy=2.

c  Note that the i'th Riemann problem has left state qr(i-1,:)
c     and right state ql(i,:)
c  From the basic clawpack routines, this routine is called with
c     ql = qr
c
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!                                                                           !
!      # This Riemann solver is for the shallow water adjoint               !
!             equations. It is modified from rpn2_geoclaw.f.                !
!                                                                           !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      implicit none

      !input
      integer maxm,meqn,maux,mwaves,mbc,mx,ixy

      double precision  fwave(meqn, mwaves, 1-mbc:maxm+mbc)
      double precision  s(mwaves, 1-mbc:maxm+mbc)
      double precision  ql(meqn, 1-mbc:maxm+mbc)
      double precision  qr(meqn, 1-mbc:maxm+mbc)
      double precision  apdq(meqn,1-mbc:maxm+mbc)
      double precision  amdq(meqn,1-mbc:maxm+mbc)
      double precision  auxl(maux,1-mbc:maxm+mbc)
      double precision  auxr(maux,1-mbc:maxm+mbc)

      !local only
      integer m,i,mw,maxiter
      double precision wall(2)
      double precision fw(2,2)
      double precision sw(2)
      double precision delta(2)

      double precision hR,hL,huR,huL
      double precision bR,bL,cL,cR,uhat,chat,grav
      double precision s1m,s2m
      double precision hstar,hstartest,hstarHLL,sLtest,sRtest
      double precision tw,dxdc
      double precision beta1, beta2, beta3, hRhat, hLhat

      logical rare1,rare2

!     # Gravity constant set in the setprob.f
      common /cparam/ grav

      !loop through Riemann problems at each grid cell
      do i=2-mbc,mx+mbc

!-----------------------Initializing-----------------------------------
         !inform of a bad riemann problem from the start
         if((qr(1,i-1).lt.0.d0).or.(ql(1,i) .lt. 0.d0)) then
            write(*,*) 'Negative input: hl,hr,i=',qr(1,i-1),ql(1,i),i
         endif

        !Initialize Riemann problem for grid interface
        do mw=1,mwaves
            s(mw,i)=0.d0
            fwave(1,mw,i)=0.d0
            fwave(2,mw,i)=0.d0

            fw(1,mw)=0.d0
            fw(2,mw)=0.d0
        enddo

         !zero (small) negative values if they exist
         if (qr(1,i-1).lt.0.d0) then
           qr(1,i-1)=0.d0
           qr(2,i-1)=0.d0
         endif

        if (ql(1,i).lt.0.d0) then
          ql(1,i)=0.d0
          ql(2,i)=0.d0
        endif

        !Riemann problem variables
        hL = qr(1,i-1)
        hR = ql(1,i)
        bL = auxr(1,i-1)
        bR = auxl(1,i)

        huL = qr(2,i-1)
        huR = ql(2,i)

        ! Linearizing
        hLhat = -bL
        hRhat = -bR

c       ! Check for wet/dry boundary
        if (hR <= 0.d0) then
            hR = 0.d0
            huR = 0.d0
            hRhat = 0.d0
        endif
        if (hL <= 0.d0) then
            hL = 0.d0
            huL = 0.d0
            hLhat = 0.d0
        endif

        wall(1) = 1.d0
        wall(2) = 1.d0

         !determine wave speeds
         cL=sqrt(grav*hLhat) ! 1 wave speed of left state
         cR=sqrt(grav*hRhat) ! 2 wave speed of right state

         !--------------------end initializing----------
         !solve Riemann problem.

c       # f-wave splitting
        delta(1) = -huR*cR**2 + huL*cL**2
        delta(2) = -(hR+bR) + (hL+bL)

        beta1 = (delta(1) + cR*delta(2))/(cL + cR)
        beta2 = (-delta(1) + cL*delta(2))/(cL + cR)

        if (cL + cR == 0.d0) then
            beta1 = (delta(1) + cR*delta(2))
            beta2 = (-delta(1) + cL*delta(2))
        endif

c       # Compute the waves.
        fw(1,1) = cL*beta1
        fw(2,1) = beta1
        sw(1) = -cL

        fw(1,2) = -cR*beta2
        fw(2,2) = beta2
        sw(2) = cR

c        !eliminate ghost fluxes for wall
         do mw=1,2
            sw(mw)=sw(mw)*wall(mw)

            fw(1,mw)=fw(1,mw)*wall(mw)
            fw(2,mw)=fw(2,mw)*wall(mw)
         enddo

         do mw=1,mwaves
            s(mw,i)=sw(mw)
            fwave(1,mw,i)=fw(1,mw)
            fwave(2,mw,i)=fw(2,mw)
         enddo

 30      continue
      enddo

 !        write(*,*) "Starting"
c============= compute fluctuations=============================================
         amdq(1:2,:) = 0.d0
         apdq(1:2,:) = 0.d0
         do i=2-mbc,mx+mbc
             do  mw=1,mwaves
!               write(*,*) "1", fwave(1:2,mw,i)
               if (s(mw,i) < 0.d0) then
                 amdq(1:2,i) = amdq(1:2,i) + fwave(1:2,mw,i)
               else if (s(mw,i) > 0.d0) then
                 apdq(1:2,i)  = apdq(1:2,i) + fwave(1:2,mw,i)
               else
                 amdq(1:2,i) = amdq(1:2,i) + 0.5d0 * fwave(1:2,mw,i)
                 apdq(1:2,i) = apdq(1:2,i) + 0.5d0 * fwave(1:2,mw,i)
               endif
             enddo
         enddo

         do i=2-mbc, mx+mbc
!             write(*,*) "2", fwave(1:meqn,1,i), fwave(1:meqn,2,i)
             amdq(1:meqn,i) = fwave(1:meqn,1,i)
             apdq(1:meqn,i) = fwave(1:meqn,2,i)
         enddo
!         write(*,*) "Finished"

      return
      end subroutine


c=======================================================================
c
c     subroutine CORLACK
c
c     Empirical correlation matrix with missing data (lack of data)
c
c     Returns data structure
c
c     X(n,p) = | x(1,1), ..., x(1,p) |
c              | x(2,1), ..., -1000  |
c              | x(3,1), ..., x(3,p) |
c              |   .   , ...,   .    |
c              |   .   , ...,   .    |
c              | x(n,1), ..., x(n,p) |
c
c     where x(2,p) = -1000 denote a missing return
c
c-----------------------------------------------------------------------
      SUBROUTINE corlack ( n, p, x, z, dwork, corr, ind, info )
c-----------------------------------------------------------------------
c
c     INPUT
c            n      : max. number of values                      integer
c            p      : number of assets (p > 1)                   integer
c            x      : asset(s) return(s) values (n*p)             double
c            z      : backup corr. matrix (p*p)                   double
c
c     WORKSPACE
c            dwork  : p*(n+p+1) + 4*n + 6                         double
c
c     OUTPUT
c            corr   : correlation matrix (p*p)                    double
c            ind    : index of used backup value(s) (p*p)        integer
c            info   : diagnostic argument                        integer
c
c     CALL
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, p, info
      DOUBLE PRECISION x(n,*), z(p,*), corr(p,*)
      INTEGER ind(p,*)
c
c     workspaces
      DOUBLE PRECISION dwork(*)
c
c     local variables
      INTEGER i, j, k, m, nb, pdwork, pdcorm, pdcorr, pdrho, pdx1,
     &        pdx2, pdx, out
      DOUBLE PRECISION missing
c
c-----------------------------------------------------------------------
c
c     initialization
      info = 0
      out = 0
      missing = -1000.0
      nb = 2
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdwork = 1
      pdcorm = pdwork
c     pdcorm : pointer CORM workspace (p*(n + p + 1)
      pdcorr = pdcorm + ( p*(n + p + 1) )
c     pdcorr : pointer for tmp corr (4)
      pdrho  = pdcorr + ( 4 )
c     pdrho  : pointer for rho vector (COVM), (2)
      pdx1   = pdrho + ( 2 )
c     pdx1   : pointer for X1, max(n)
      pdx2   = pdx1 + ( n )
c     pdx2   : pointer for X2, max(n)
      pdx    = pdx2 + ( n )
c     pdx    : pointer for X = [X1, X2], max( 2*n )
c
c     Total size of dwork array  = p(n+p+1) + 4*n + 6
c
c-----------------------------------------------------------------------
c
c     correlation diagonal
      DO j = 1,p
        corr(j,j) = 1.
      ENDDO
c
c     loop on j=1,...,p-1 and k=j+1,...,p
      DO j = 1,(p-1)
        DO k = (j+1),p
c
c           covariance of two assets (without missing returns)
            m = 0
            DO i = 1,n
                IF ((x(i,j) .GT. missing) .AND. (x(i,k) .GT. missing))
     &          THEN
                    m = m + 1
                    dwork(pdx1 + m - 1) = x(i,j)
                    dwork(pdx2 + m - 1) = x(i,k)
                ENDIF
            ENDDO
c
c           case m=0,1 no enough data for computation
            IF (m .LT. 2) THEN
                corr(k,j) = z(k,j)
                corr(j,k) = corr(k,j)
                ind(k,j) = 1
                ind(j,k) = 1
                out = out + 1
            ELSE
c
c               X = [x1, x2] with X(m,2)
                DO i = 1,m
                    dwork(pdx + i - 1)      = dwork(pdx1 + i - 1)
                    dwork(pdx + m + i - 1)  = dwork(pdx2 + i - 1)
                ENDDO
c
c               covariance (i,j)
                CALL cormvm ( m, nb, dwork(pdx), dwork(pdcorm),
     &                      dwork(pdrho), dwork(pdcorr), info)
                IF (info .LT. 0) RETURN
                corr(k,j) = dwork(pdcorr + 1)
                corr(j,k) = corr(k,j)
            ENDIF
        ENDDO
      ENDDO
c      info = out
c
      RETURN
      END


MODULE Var

INTEGER:: i,j,k,N=51,p,q,r,m1
DOUBLE PRECISION:: D,E,F,pm1,pp1,qm2,qm1,qp1,qp2,q0
DOUBLE PRECISION::alp,beta2,betaN,Nc,Ne,kh,pi,eta,eps2,eps4,k2,k4,Gr,Gi,a1,b1
DOUBLE PRECISION,DIMENSION(:,:):: A(51,51),B(51,51),C(51,51),G(314,50,50)
DOUBLE PRECISION,DIMENSION(:,:):: A2(51,51),B2(51,51),C2(51,51)
DOUBLE PRECISION,DIMENSION(:,:):: cnbyc1(314,50,50),cnbyc(314,50,50),dcnbyc1(314,50,50)
DOUBLE PRECISION,DIMENSION(:,:):: enbye(314,50,50),dcnbyc(314,50,50),denbye(314,50,50)
DOUBLE PRECISION,DIMENSION(:,:):: AI(51,51),X(51),AA(51,102),Dumm(51,51)
DOUBLE PRECISION,DIMENSION(:,:):: betaj(314,50,50),dbetaj(314,50,50)
DOUBLE PRECISION,DIMENSION(:,:):: term(314,50,50),dterm(314,50,50),vgnbyvg(314,50,50)
DOUBLE PRECISION,DIMENSION(:,:):: phaser(314,50,50),dphaser(314,50,50)
DOUBLE COMPLEX,DIMENSION(:,:):: Aj(314,50,50),Gj(314,50,50)
DOUBLE COMPLEX:: iota,sumC,sumC2
DOUBLE COMPLEX,DIMENSION(:):: C1(51)
character*30:: filename,filename1
END MODULE Var
!-----------------------------------------------------------------------------------------------------------------------------
Subroutine Define_AB
use Var
eta =  0.0d0
D = 0.3793894912d0
E = 1.57557379d0
F = 0.183205192d0
beta2 = -0.025d0
betaN = 0.09d0     ! for N-2
pm1 = D - eta/60.0d0
pp1 = D + eta/60.0d0
qm2 = -(F/4.0d0) + (eta/300.0d0)
qm1 = -(E/2.0d0) + (eta/30.0d0)
q0 = -11.0d0*eta/150.0d0
qp1 = (E/2.0d0) + (eta/30.0d0)
qp2 = (F/4.0d0) + (eta/300.0d0)
DO i = 1,N
DO j = 1,N
   A(i,j) = 0.0d0
   B(i,j) = 0.0d0
END DO
END DO
A(1,1) = 1.0d0
A(2,2) = 1.0d0
A(N-1,N-1) = 1.0d0
A(N,N) = 1.0d0
DO i = 3,N-2
 A(i,i) = 1.0d0
 A(i,i-1) = pm1    ! D - eta/60.0d0
 A(i,i+1) = pp1    ! D + eta/60.d0
END DO

B(1,1) = -1.5d0
B(1,2) = 2.0d0
B(1,3) =  -0.5d0
B(N,N) = 1.5d0
B(N,N-1) = -2.0d0
B(N,N-2) = 0.5d0
B(2,1) = (2.0d0*beta2/3.0d0) - (1.0d0/3.0d0)
B(2,2) = -((8.0d0*beta2/3.0d0) + (0.50d0))
B(2,3) = 4.0d0*beta2 + 1.0d0
B(2,4) = -((8.0d0*beta2/3.0d0) + (1.0d0/6.0d0))
B(2,5) = 2.0d0*beta2/3.0d0
B(N-1,N) = -((2.0d0*betaN/3.0d0) - (1.0d0/3.0d0))
B(N-1,N-1) = (8.0d0*betaN/3.0d0) + (0.50d0)
B(N-1,N-2) = -(4.0d0*betaN + 1.0d0)
B(N-1,N-3) = (8.0d0*betaN/3.0d0) + (1.0d0/6.0d0)
B(N-1,N-4) = -(2.0d0*betaN/3.0d0)
DO i = 3,N-2
B(i,i-2) = qm2   ! -(F/4.0d0) + (eta/300)
B(i,i-1) = qm1   ! -(E/2.0d0) + (eta/30)
B(i,i) = q0    ! -11.0d0*eta/150.0d0
B(i,i+1) = qp1   ! (E/2.0d0) + (eta/30)
B(i,i+2) =  qp2   ! (F/4.0d0) + (eta/300)
END DO

CALL Inverse_A
iota = (0.0d0,1.0d0)
 C  = matmul(AI,B)
!--------------------------------------------------
alp = 0.457    ! 0.47702202202203
a1 = -4.0d0*alp - 2.0d0
b1 = 16.0d0*alp + 8.0d0
DO i = 1,N
DO j = 1,N
   A(i,j) = 0.0d0
   B(i,j) = 0.0d0
END DO
END DO
A(1,1) = 1.0d0
A(2,2) = 1.0d0
A(N-1,N-1) = 1.0d0
A(N,N) = 1.0d0
DO i = 3,N-2
 A(i,i) = 1.0d0
 A(i,i-1) = alp
 A(i,i+1) = alp
END DO

B(1,1) = -1.5d0
B(1,2) = 2.0d0
B(1,3) =  -0.5d0
B(N,N) = 1.5d0
B(N,N-1) = -2.0d0
B(N,N-2) = 0.5d0
B(2,1) = (2.0d0*beta2/3.0d0) - (1.0d0/3.0d0)
B(2,2) = -((8.0d0*beta2/3.0d0) + (0.50d0))
B(2,3) = 4.0d0*beta2 + 1.0d0
B(2,4) = -((8.0d0*beta2/3.0d0) + (1.0d0/6.0d0))
B(2,5) = 2.0d0*beta2/3.0d0
B(N-1,N) = -((2.0d0*betaN/3.0d0) - (1.0d0/3.0d0))
B(N-1,N-1) = (8.0d0*betaN/3.0d0) + (0.50d0)
B(N-1,N-2) = -(4.0d0*betaN + 1.0d0)
B(N-1,N-3) = (8.0d0*betaN/3.0d0) + (1.0d0/6.0d0)
B(N-1,N-4) = -(2.0d0*betaN/3.0d0)
DO i = 3,N-2
B(i,i-2) = -b1/16.0d0
B(i,i-1) = -a1/2.0d0
B(i,i) = 0.0d0
B(i,i+1) = a1/2.0d0
B(i,i+2) = b1/16.0d0
END DO

CALL Inverse_A
 C2 = matmul(AI,B)

End Subroutine Define_AB

subroutine Inverse_A
use Var
       IMPLICIT NONE
       INTEGER:: L3,LI,LK,KP1,JJ,LJ,M2,M
       REAL:: P1,T,AB,BIG
         M = N + N
         M2 = N + 1
!      GENERATING THE AUGMENTED MATRIX AA
         DO I = 1, N            !ENTERING THE COEFFICIENT MATRIX A INTO
         DO  J = 1, N           !THE AUGMENTED MATRIX AA
           AA(I, J) = A(I, J)
         END DO
         END DO
        DO  I = 1, N
        DO  J = M2, M
           AA(I, J) = 0.0
        END DO
        END DO
        DO I = 1, N       !ENTERING THE IDENTITY MATRIX I INTO
            J = I + N         !THE AUGMENTED MATRIX AA
       AA(I, J) = 1.0
        END DO
!      STARTING ROW TRANSFORMATION
       DO LJ = 1, N
          K = LJ
          IF(K .LT. N)THEN
            JJ = K
            BIG = ABS(AA(K, K))
            KP1 = K + 1
            DO I = KP1, N
              AB = ABS(AA(I, K))
              IF((BIG - AB) .LT. 0.0)THEN
                 BIG = AB               !PERFORMING PIVOTING AND
                 JJ = I                 !ROW TRANSFORMATION OPERATIONS
              ENDIF
          END DO
            IF ((JJ - K) .GT. 0.0)THEN
              DO J = K, M
                 T = AA(JJ, J)
                 AA(JJ, J) = AA(K, J)
                 AA(K, J) = T
            END DO
            ENDIF
         ENDIF
          P1 = AA(LJ, LJ)
          DO I = LJ, M
             AA(LJ, I) = AA(LJ, I)/P1
          END DO
          DO LK = 1, N
            T = AA(LK, LJ)
            DO LI = LJ, M
              IF((LK - LJ) .NE. 0)THEN
                AA(LK, LI) = AA(LK, LI) - AA(LJ, LI)*T
              ENDIF
      END DO
      END DO
   END DO
         DO I = 1, N
         DO J = M2, M
            L3 = J - N
            AI(I, L3) = AA(I, J) !GENERATING THE INVERTED MATRIX
   END DO
   END DO
End subroutine Inverse_A

Subroutine Properties
use Var

!DO r = 1,N,1
 r = 26

DO k = 1,50
Ne = 0.01d0*k
DO j = 1,50
Nc = 0.01d0*j
DO i = 1,314
kh = 0.01d0*i

sumC = (0.0d0,0.0d0)
sumC2 = (0.0d0,0.0d0)

DO m1 = 1,N
sumC = sumC + C(r,m1)*exp(iota*kh*(m1-r))    ! node = r
END DO

DO m1 = 1,N
sumC2 = sumC2 + C2(r,m1)*exp(iota*kh*(m1-r))    ! node = r
END DO

Aj(i,j,k) =  Nc*sumC - Ne*sumC2

Gj(i,j,k) = 1.0d0 - Aj(i,j,k) + ((Aj(i,j,k))**2)/2.0d0 - ((Aj(i,j,k))**3)/6.0d0 + ((Aj(i,j,k))**4)/24.0d0

Gr = real(Gj(i,j,k))
Gi = aimag(Gj(i,j,k))

G(i,j,k) = sqrt(Gr**2 + Gi**2)

pi = 4.0d0*atan(1.0d0)

!betaj(i,j,k) = (atan(-Gi/Gr))

IF((Gr > 0.0d0) .and. (-Gi>0.0d0))THEN
 betaj(i,j,k) = (atan(-Gi/Gr))
ELSEIF((Gr < 0.0d0) .and. (-Gi > 0.0d0))THEN
 betaj(i,j,k) = pi + (atan(-Gi/Gr))        ! was pi +
ELSEIF((Gr < 0.0d0) .and. (-Gi < 0.0d0))THEN
 betaj(i,j,k) = pi + atan(-Gi/Gr)
ELSEIF((Gr > 0.0d0) .and. (-Gi < 0.0d0))THEN
 betaj(i,j,k) = 2.0d0*pi +  atan(-Gi/Gr)   ! was 2pi +
ENDIF

   cnbyc(i,j,k) = betaj(i,j,k)/(kh*Nc + Ne*kh*kh*kh)

   phaser(i,j,k) = (betaj(i,j,k)/kh) - Nc - Ne*kh*kh  ! this is dt*(cn-c)

   term(i,j,k) = Nc + Ne*kh*kh
END DO
END DO
END DO

DO k = 1,50
Ne = 0.01d0*k
DO j = 1,50
Nc = 0.01d0*j
DO i = 1,314
kh = 0.01d0*i
      IF(i.eq.1) THEN
      dphaser(i,j,k)= (phaser(i+1,j,k)-phaser(i,j,k))/0.01
      ELSEIF(i.eq.314) THEN
      dphaser(i,j,k)= (phaser(i,j,k)-phaser(i-1,j,k))/0.01
      ELSE
      dphaser(i,j,k)= (phaser(i+1,j,k)-phaser(i-1,j,k))/(2.0d0*0.01)
      ENDIF
     dcnbyc(i,j,k) = dphaser(i,j,k)    ! this is dt*[d(cn-c)/dk]
END DO
END DO
END DO

DO k = 1,50
Ne = 0.01d0*k
DO j = 1,50
Nc = 0.01d0*j
DO i = 1,314
kh = 0.01d0*i
      IF(i.eq.1) THEN
      dbetaj(i,j,k)= (betaj(i+1,j,k)-betaj(i,j,k))/0.01
      dterm(i,j,k)= (term(i+1,j,k)-term(i,j,k))/0.01
      ELSEIF(i.eq.314) THEN
      dbetaj(i,j,k)= (betaj(i,j,k)-betaj(i-1,j,k))/0.01
      dterm(i,j,k)= (term(i,j,k)-term(i-1,j,k))/0.01
      ELSE
      dbetaj(i,j,k)= (betaj(i+1,j,k)-betaj(i-1,j,k))/(2.0d0*0.01)
      dterm(i,j,k)= (term(i+1,j,k)-term(i-1,j,k))/(2.0d0*0.01)
      ENDIF
     vgnbyvg(i,j,k) = dbetaj(i,j,k)/ ((Nc+kh*kh*Ne) + kh*(dterm(i,j,k)))
END DO
END DO
END DO

write(filename,'(a,I2,a)') trim("O3_rk4_j_"),r,'.dat'
open(5,file = filename)
write(5,*)'Variables = Ne, Nc, kh, G, cn/c, (dt/h)*dcn/dk, (dt/h)*phase_err, vgnby/vg'
Write(5,*)'zone I=50 J=50 K=314 F=POINT'

DO i = 1,314
kh = 0.01d0*i
DO j = 1,50
Nc = 0.01d0*j
DO k = 1,50
Ne = 0.01d0*k
write(5,55) Ne, Nc, kh, G(i,j,k), cnbyc(i,j,k), dcnbyc(i,j,k), phaser(i,j,k), vgnbyvg(i,j,k)
55 FORMAT(3F12.8,F24.4,4F16.8)
END DO
END DO
END DO

print*,"node complete = ",r
!END DO
End Subroutine Properties
!-----------------------------------------------------------------------------------------------------------------------------
PROGRAM CNO3
use Var

CALL Define_AB
CALL Inverse_A
CALL Properties

END PROGRAM


!
!     CalculiX - A 3-dimensional finite element program
!              Copyright (C) 1998-2015 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
      subroutine shape8hr(xl,xsj,shp,gs,a)
!
!     shape functions and derivatives for a 8-node linear isoparametric
!     mean strain solid element with hourglass control
!
!     Reference: Flanagan, D.P., Belytschko, T.; "Uniform  strain hexahedron
!     and quadrilateral with orthogonal Hourglass control". Int. J. Num.
!     Meth. Engg., Vol. 17, 679-706, 1981.
!
!     author: Otto-Ernst Bernhardi
!
      implicit none
!
      integer i,j,k,l
      real*8 shp(4,20),xl(3,20),xsj,vol
      real*8 x1,x2,x3,x4,x5,x6,x7,x8
      real*8 y1,y2,y3,y4,y5,y6,y7,y8
      real*8 z1,z2,z3,z4,z5,z6,z7,z8
      real*8 gb(8,4),gs(8,4),s0,a
!
c      data gb / 1.0d0, 1.0d0,-1.0d0,-1.0d0,-1.0d0,-1.0d0, 1.0d0, 1.0d0,
c     &          1.0d0,-1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0, 1.0d0,-1.0d0,
c     &          1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0,
c     &         -1.0d0, 1.0d0,-1.0d0, 1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0 /
      gb = reshape((
     &      / 1.0d0, 1.0d0,-1.0d0,-1.0d0,-1.0d0,-1.0d0, 1.0d0, 1.0d0,
     &        1.0d0,-1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0, 1.0d0,-1.0d0,
     &        1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0,
     &       -1.0d0, 1.0d0,-1.0d0, 1.0d0, 1.0d0,-1.0d0, 1.0d0,-1.0d0 /),
     &        (/8,4/))
!
!     shape functions and their global derivatives
!
!     shape functions
!
      shp(4, 1)=1.0d0/8.0d0
      shp(4, 2)=1.0d0/8.0d0
      shp(4, 3)=1.0d0/8.0d0
      shp(4, 4)=1.0d0/8.0d0
      shp(4, 5)=1.0d0/8.0d0
      shp(4, 6)=1.0d0/8.0d0
      shp(4, 7)=1.0d0/8.0d0
      shp(4, 8)=1.0d0/8.0d0
!
!     extract node point coordinates of element
      x1=xl(1,1)
      x2=xl(1,2)
      x3=xl(1,3)
      x4=xl(1,4)
      x5=xl(1,5)
      x6=xl(1,6)
      x7=xl(1,7)
      x8=xl(1,8)
      y1=xl(2,1)
      y2=xl(2,2)
      y3=xl(2,3)
      y4=xl(2,4)
      y5=xl(2,5)
      y6=xl(2,6)
      y7=xl(2,7)
      y8=xl(2,8)
      z1=xl(3,1)
      z2=xl(3,2)
      z3=xl(3,3)
      z4=xl(3,4)
      z5=xl(3,5)
      z6=xl(3,6)
      z7=xl(3,7)
      z8=xl(3,8)
!
!     Average displacement derivative operator matrix,
!     using eqn 16 in the reference above.
!     Generated using maxima/wxmaxima and the following input lines.
!     Note that shp array must be divided by the element volume.
!     h1:(1-r)*(1-s)*(1-t)/8;
!     h2:(1+r)*(1-s)*(1-t)/8;
!     h3:(1+r)*(1+s)*(1-t)/8;
!     h4:(1-r)*(1+s)*(1-t)/8;
!     h5:(1-r)*(1-s)*(1+t)/8;
!     h6:(1+r)*(1-s)*(1+t)/8;
!     h7:(1+r)*(1+s)*(1+t)/8;
!     h8:(1-r)*(1+s)*(1+t)/8;
!     H:matrix([h1,h2,h3,h4,h5,h6,h7,h8])$
!     Bx:diff(H,r);
!     By:diff(H,s);
!     Bz:diff(H,t);
!     B:matrix([Bx[1,1],Bx[1,2],Bx[1,3],Bx[1,4],Bx[1,5],Bx[1,6],Bx[1,7],Bx[1,8]],
!              [By[1,1],By[1,2],By[1,3],By[1,4],By[1,5],By[1,6],By[1,7],By[1,8]],
!              [Bz[1,1],Bz[1,2],Bz[1,3],Bz[1,4],Bz[1,5],Bz[1,6],Bz[1,7],Bz[1,8]]);
!     x:matrix([x1,x2,x3,x4,x5,x6,x7,x8],
!              [y1,y2,y3,y4,y5,y6,y7,y8],
!              [z1,z2,z3,z4,z5,z6,z7,z8]);
!     xr:B.transpose(x);
!     det:determinant(xr);
!     vol:factor(integrate(integrate(integrate(det,r,-1,1),s,-1,1),t,-1,1));
!     shp: matrix( [0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0])$
!     shp[1][1]:ratsimp(diff(vol,x1));
!     shp[1][2]:ratsimp(diff(vol,x2));
!     shp[1][3]:ratsimp(diff(vol,x3));
!     ... (more lines)
!     shp[3][8]:ratsimp(diff(vol,z8));
!     fortran(shp);
!
!     local derivatives of the shape functions: xi-derivative
!
      shp(1,1) = ((y5-y4)*z8+(y2-y5)*z6+(-y8+y6-y4+y2)*z5+(y8+y5-y3-y2)
     1   *z4+(y4-y2)*z3+(-y6-y5+y4+y3)*z2)/1.2d+1
      shp(1,2) = -((y6-y3)*z7+(-y7+y5-y3+y1)*z6+(y1-y6)*z5+(y3-y1)*z4+(
     1   y7+y6-y4-y1)*z3+(-y6-y5+y4+y3)*z1)/1.2d+1
      shp(1,3) = -((y7-y4)*z8+(-y8+y6-y4+y2)*z7+(y2-y7)*z6+(y8+y7-y2-y1
     1   )*z4+(-y7-y6+y4+y1)*z2+(y4-y2)*z1)/1.2d+1
      shp(1,4) = -((y7-y5+y3-y1)*z8+(y3-y8)*z7+(y8-y1)*z5+(-y8-y7+y2+y1
     1   )*z3+(y1-y3)*z2+(y8+y5-y3-y2)*z1)/1.2d+1
      shp(1,5) = ((y7+y6-y4-y1)*z8+(y6-y8)*z7+(-y8-y7+y2+y1)*z6+(y8-y1)
     1   *z4+(y1-y6)*z2+(y8-y6+y4-y2)*z1)/1.2d+1
      shp(1,6) = ((y7-y5)*z8+(-y8-y5+y3+y2)*z7+(y8+y7-y2-y1)*z5+(y2-y7)
     1   *z3+(-y7+y5-y3+y1)*z2+(y5-y2)*z1)/1.2d+1
      shp(1,7) = -((y6+y5-y4-y3)*z8+(-y8-y5+y3+y2)*z6+(y6-y8)*z5+(y8-y3
     1   )*z4+(y8-y6+y4-y2)*z3+(y3-y6)*z2)/1.2d+1
      shp(1,8) = ((y6+y5-y4-y3)*z7+(y5-y7)*z6+(-y7-y6+y4+y1)*z5+(y7-y5+
     1   y3-y1)*z4+(y7-y4)*z3+(y4-y5)*z1)/1.2d+1
!
!     local derivatives of the shape functions: eta-derivative
!
      shp(2,1) = -((x5-x4)*z8+(x2-x5)*z6+(-x8+x6-x4+x2)*z5+(x8+x5-x3-x2
     1   )*z4+(x4-x2)*z3+(-x6-x5+x4+x3)*z2)/1.2d+1
      shp(2,2) = ((x6-x3)*z7+(-x7+x5-x3+x1)*z6+(x1-x6)*z5+(x3-x1)*z4+(x
     1   7+x6-x4-x1)*z3+(-x6-x5+x4+x3)*z1)/1.2d+1
      shp(2,3) = ((x7-x4)*z8+(-x8+x6-x4+x2)*z7+(x2-x7)*z6+(x8+x7-x2-x1)
     1   *z4+(-x7-x6+x4+x1)*z2+(x4-x2)*z1)/1.2d+1
      shp(2,4) = ((x7-x5+x3-x1)*z8+(x3-x8)*z7+(x8-x1)*z5+(-x8-x7+x2+x1)
     1   *z3+(x1-x3)*z2+(x8+x5-x3-x2)*z1)/1.2d+1
      shp(2,5) = -((x7+x6-x4-x1)*z8+(x6-x8)*z7+(-x8-x7+x2+x1)*z6+(x8-x1
     1   )*z4+(x1-x6)*z2+(x8-x6+x4-x2)*z1)/1.2d+1
      shp(2,6) = -((x7-x5)*z8+(-x8-x5+x3+x2)*z7+(x8+x7-x2-x1)*z5+(x2-x7
     1   )*z3+(-x7+x5-x3+x1)*z2+(x5-x2)*z1)/1.2d+1
      shp(2,7) = ((x6+x5-x4-x3)*z8+(-x8-x5+x3+x2)*z6+(x6-x8)*z5+(x8-x3)
     1   *z4+(x8-x6+x4-x2)*z3+(x3-x6)*z2)/1.2d+1
      shp(2,8) = -((x6+x5-x4-x3)*z7+(x5-x7)*z6+(-x7-x6+x4+x1)*z5+(x7-x5
     1   +x3-x1)*z4+(x7-x4)*z3+(x4-x5)*z1)/1.2d+1
!
!     local derivatives of the shape functions: zeta-derivative
!
      shp(3,1) = ((x5-x4)*y8+(x2-x5)*y6+(-x8+x6-x4+x2)*y5+(x8+x5-x3-x2)
     1   *y4+(x4-x2)*y3+(-x6-x5+x4+x3)*y2)/1.2d+1
      shp(3,2) = -((x6-x3)*y7+(-x7+x5-x3+x1)*y6+(x1-x6)*y5+(x3-x1)*y4+(
     1   x7+x6-x4-x1)*y3+(-x6-x5+x4+x3)*y1)/1.2d+1
      shp(3,3) = -((x7-x4)*y8+(-x8+x6-x4+x2)*y7+(x2-x7)*y6+(x8+x7-x2-x1
     1   )*y4+(-x7-x6+x4+x1)*y2+(x4-x2)*y1)/1.2d+1
      shp(3,4) = -((x7-x5+x3-x1)*y8+(x3-x8)*y7+(x8-x1)*y5+(-x8-x7+x2+x1
     1   )*y3+(x1-x3)*y2+(x8+x5-x3-x2)*y1)/1.2d+1
      shp(3,5) = ((x7+x6-x4-x1)*y8+(x6-x8)*y7+(-x8-x7+x2+x1)*y6+(x8-x1)
     1   *y4+(x1-x6)*y2+(x8-x6+x4-x2)*y1)/1.2d+1
      shp(3,6) = ((x7-x5)*y8+(-x8-x5+x3+x2)*y7+(x8+x7-x2-x1)*y5+(x2-x7)
     1   *y3+(-x7+x5-x3+x1)*y2+(x5-x2)*y1)/1.2d+1
      shp(3,7) = -((x6+x5-x4-x3)*y8+(-x8-x5+x3+x2)*y6+(x6-x8)*y5+(x8-x3
     1   )*y4+(x8-x6+x4-x2)*y3+(x3-x6)*y2)/1.2d+1
      shp(3,8) = ((x6+x5-x4-x3)*y7+(x5-x7)*y6+(-x7-x6+x4+x1)*y5+(x7-x5+
     1   x3-x1)*y4+(x7-x4)*y3+(x4-x5)*y1)/1.2d+1
!
!     computation of element volume (eqn 15)
!
      vol=0.0d0
      do k=1,8
        vol=vol+xl(1,k)*shp(1,k)
      enddo
!
!     computation of the average jacobian determinant
!
      xsj=vol/8.0d0
!
!     hourglass control vectors(see appendix 2 from the reference above).
!     divide shp array by element volume
      a=0.0d0
      do i=1,8
         do j=1,3
            a=a+shp(j,i)*shp(j,i)
            s0=shp(j,i)/vol
            shp(j,i)=s0
            do k=1,4
               gs(i,k)=gb(i,k)
               do l=1,8
                  gs(i,k)=gs(i,k)-s0*xl(j,l)*gb(l,k)
               enddo
            enddo
         enddo
      enddo
c
c     calculate hourglass control stiffness factor a
c     (to be used in hgstiffness() and hgforce())
c
c     in ABAQUS, a 0.005 is used as default value.
      a=0.0005*a/vol
c
      return
      end