module integrators
    implicit none
    contains

    function vec_mag(n_dim, rel_vec)
        integer, intent(in) :: n_dim
        real(8), dimension(n_dim), intent(in) :: rel_vec
        real(8) :: vec_mag
        vec_mag = sqrt(sum(rel_vec**2))
    end function vec_mag

    subroutine euler_forward(n, masses, positions, velocities, dt, g, poses, vels)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), intent(in) :: g
        real(8), dimension(n, 3) :: accels
        real(8), dimension(n, 3), intent(out):: poses, vels
        real(8), intent(in) :: dt
        integer(8) :: i, j
        real(8) :: rel_mag
        accels = 0.
        poses = 0.
        vels = 0.
        !    f2py intent(in) n, masses, positions, velocities, dt
        !    f2py intent(out) poses, vels
        do i=1, n
            do j=1, n
                if (i /= j) then
                    if (masses(j) /= 0.) then
                        rel = positions(j, :) - positions(i, :)
                        rel_mag = vec_mag(3, rel)
                        accels(i, :) = accels(i, :) + g * masses(j) / rel_mag**3 * rel(:)
                    end if
                end if
            end do
            poses(i, :) = positions(i, :) + velocities(i, :) * dt
            vels(i, :) = velocities(i, :) + accels(i, :) * dt
        end do
!        print *, "Accels"
!        print *, velocities(2, :) + accels(2, :) * dt
!        poses(:, :) = positions + velocities * dt
!        vels(:, :) = velocities(:, :) + accels(:, :) * dt
    end subroutine euler_forward

    subroutine kdk(n, masses, positions, velocities, dt, g, poses, vels)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), dimension(n, 3) :: accels
        real(8), intent(in) :: g
        real(8), dimension(n, 3), intent(out) :: poses, vels
        integer(8) :: i, j
        real(8), intent(in) :: dt
        real(8) :: rel_mag
        rel = 0.
        rel_mag = 0.
        accels = 0.
        do i=1, n
            do j=1, n
                if (i /= j) then
                    if (masses(j) /= 0) then
                        rel = positions(j, :) - positions(i, :)
                        rel_mag = vec_mag(3, rel)
                        accels(i, :) = accels(i, :) + g * masses(j) / rel_mag**3 * rel(:)
                    end if
                end if
            end do
            vels(i, :) = velocities(i, :) + accels(i, :) * dt / 2.  ! Kick
            poses(i, :) = positions(i, :) + vels(i, :) * dt  ! Drift
        end do
        accels = 0.  ! Reset accels for second kick

        do i=1, n
            do j=1, n
                if (i /= j) then
                    if (masses(j) /= 0) then
                        rel = poses(j, :) - poses(i, :)
                        rel_mag = vec_mag(3, rel)
                        accels(i, :) = accels(i, :) + g * masses(j) / rel_mag**3 * rel(:)
                    end if
                end if
            end do
            vels(i, :) = vels(i, :) + accels(i, :) * dt / 2.  ! Kick 2
        end do

    end subroutine kdk

    subroutine kdk_gal(n, masses, positions, velocities, dt, g, poses, vels)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), intent(in) :: g
        real(8), dimension(n, 3) :: accels
        real(8), dimension(n, 3), intent(out) :: poses, vels
        integer(8) :: i
        real(8), intent(in) :: dt
        real(8) :: rel_mag
        rel = 0.
        accels = 0.
        rel_mag = 1.
        do i=1, n
            rel = positions(i, :)
            rel_mag = vec_mag(3, rel)
            accels(i, :) = (-log(1 + rel_mag) / rel_mag ** 2 + 1 / rel_mag / (1 + rel_mag)) * rel(:) / rel_mag
!            print*, accels(i, :)
            vels(i, :) = velocities(i, :) + accels(i, :) * dt / 2.  ! Kick
!            print *, vels(i, :)
            poses(i, :) = positions(i, :) + vels(i, :) * dt  ! Drift
        end do
        accels = 0.  ! Reset accels for second kick

        do i=1, n
            rel = poses(i, :)
            rel_mag = vec_mag(3, rel)
            accels(i, :) = (-log(1 + rel_mag) / rel_mag ** 2 + 1 / rel_mag / (1 + rel_mag)) * rel(:) / rel_mag
            vels(i, :) = vels(i, :) + accels(i, :) * dt / 2.  ! Kick 2
        end do

    end subroutine kdk_gal

    subroutine rk_pde(n, masses, positions, velocities, g, vels, accels)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), intent(in) :: g
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), dimension(n, 3), intent(out) :: vels, accels
        integer(8) :: i, j
        real(8) :: rel_mag
    !    f2py intent(in) masses, positions, velocities, dt
    !    f2py intent(out) poses, vels
        vels = 0.
        accels = 0.

        do i=1, n
            do j=1, n
                if (i /= j) then
                    if (masses(j) /= 0.) then
                        rel = positions(j, :) - positions(i, :)
!                        print *, "rel and accels", rel, accels(i, :)
                        rel_mag = vec_mag(3, rel)
                        accels(i, :) = accels(i, :) + g * masses(j) / rel_mag**3 * rel(:)
!                        print *, "masse and accels", masses(i), accels(i, :)
                    end if
                end if
            end do
        end do
        vels = velocities

    end subroutine rk_pde

    subroutine rk3_classic(n, masses, positions, velocities, dt, g, poses, vels)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), dimension(n, 3), intent(out) :: poses, vels
        real(8), intent(in) :: g
        integer(8) :: i, j, k, l
        real(8), intent(in) :: dt
        real(8), dimension(3, 3) :: coefficients
        real(8), dimension(4) :: weights
        real(8), dimension(4, n, 3) :: k_poses, k_vels
        real(8), dimension(n, 3) :: temp_p, temp_v
        k_poses = 0.
        k_vels = 0.
        temp_p = 0.
        temp_v = 0.
        coefficients(1, :) = (/0.5, 0., 0./)
        coefficients(2, :) = (/0., 0.5, 0./)
        coefficients(3, :) = (/0., 0., 1./)
        weights(:) = (/1., 2., 2., 1./) / 6.
        temp_p = 0.
        temp_v = 0.

        call rk_pde(n, masses, positions, velocities, g, &
                    k_poses(1, :, :), k_vels(1, :, :))
        do i=1, 3
            temp_p = positions
            temp_v = velocities
            do l=1, i
                temp_p = temp_p + k_poses(i, :, :) * coefficients(i, l) * dt
                temp_v = temp_v + k_vels(i, :, :) * coefficients(i, l) * dt
            end do
            call rk_pde(n, masses, temp_p, temp_v, g, &
                        k_poses(i + 1, :, :), k_vels(i + 1, :, :))
        end do

        poses = positions
        vels = velocities
        do i=1, 4
!            print "(e22.16)", k_poses(i, :, :)
!            print *, k_poses(i, 2, 2)
            poses(:, :) = poses(:, :) + dt * weights(i) * k_poses(i, :, :)
            vels(:, :) = vels(:, :) + dt * weights(i) * k_vels(i, :, :)
        end do

    end subroutine rk3_classic

    subroutine kdkrk3(n, masses, positions, velocities, dt, g, poses_low, &
            vels_low, poses_high, vels_high)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        !        real(8), dimension(n, 3) :: accels
        real(8), dimension(n, 3), intent(out) :: poses_low, vels_low, poses_high, &
                vels_high
        real(8), intent(in) :: g
        integer(8) :: i, j, k, l
        real(8), intent(in) :: dt
        real(8) :: rel_mag
        real(8), dimension(3, 3) :: coefficients
        real(8), dimension(4) :: weights
        real(8), dimension(4, n, 3) :: k_poses, k_vels
        real(8), dimension(n, 3) :: temp_p, temp_v
        k_poses = 0.
        k_vels = 0.
        temp_p = 0.
        temp_v = 0.

        coefficients(1, :) = (/0.5, 0., 0./)
        coefficients(2, :) = (/0., 0.5, 0./)
        coefficients(3, :) = (/0., 0., 1./)
        weights(:) = (/1., 2., 2., 1./) / 6.

!        call rk_pde(n, masses, positions, velocities, &
!                k_poses(1, :, :), k_vels(1, :, :))
!        do i=1, 5
!            temp_p = positions
!            temp_v = velocities
!            do l=1, i
!                temp_p = temp_p + k_poses(i, :, :) * coefficients(i, l) * dt
!                temp_v = temp_v + k_vels(i, :, :) * coefficients(i, l) * dt
!            end do
!            call rk_pde(n, masses, temp_p, temp_v, &
!                    k_poses(i + 1, :, :), k_vels(i + 1, :, :))
!        end do
!
!        poses_low = positions
!        vels_low = velocities
!        poses_high = positions
!        vels_high = velocities
!
!        do i=1, 6
!            poses_low(:, :) = poses_low(:, :) + dt * weights_4(i) * k_poses(i, :, :)
!            vels_low(:, :) = vels_low(:, :) + dt * weights_4(i) * k_vels(i, :, :)
!            poses_high(:, :) = poses_high(:, :) + dt * weights_5(i) * k_poses(i, :, :)
!            vels_high(:, :) = vels_high(:, :) + dt * weights_5(i) * k_vels(i, :, :)
!        end do

    end subroutine kdkrk3

    subroutine rkf45(n, masses, positions, velocities, dt, g, poses_low, &
            vels_low, poses_high, vels_high)
        implicit none
        integer(8), intent(in) :: n
        real(8), dimension(n), intent(in) :: masses
        real(8), dimension(3) :: rel
        real(8), dimension(n, 3), intent(in) :: positions, velocities
        real(8), intent(in) :: g
!        real(8), dimension(n, 3) :: accels
        real(8), dimension(n, 3), intent(out) :: poses_low, vels_low, poses_high, &
                vels_high
        integer(8) :: i, j, k, l
        real(8), intent(in) :: dt
        real(8) :: rel_mag
        real(8), dimension(5, 5) :: coefficients
        real(8), dimension(6) :: weights_4, weights_5
        real(8), dimension(6, n, 3) :: k_poses, k_vels
        real(8), dimension(n, 3) :: temp_p, temp_v
        k_poses = 0.
        k_vels = 0.
        temp_p = 0.
        temp_v = 0.

        coefficients(1, :) = (/1. / 4., 0., 0., 0., 0./)
        coefficients(2, :) = (/3. / 32., 9. / 32., 0., 0., 0./)
        coefficients(3, :) = (/1932. / 2197., -7200. / 2197., 7296. / 2197., &
                               0., 0./)
        coefficients(4, :) = (/439. / 216., -8., 3680. / 513., -845. / 4104., &
                               0./)
        coefficients(5, :) = (/-8. / 27., 2., -3544. / 2565., 1859. / 4104., &
                            -11. / 40./)
        weights_4(:) = (/25. / 216., 0., 1408. / 2565., 2197. / 4104., &
                         -1. / 5., 0./)
        weights_5(:) = (/16. / 135., 0., 6656. / 12825., 28561. / 56430.,&
                         -9 / 50., 2. / 55./)

        temp_p = 0.
        temp_v = 0.

        call rk_pde(n, masses, positions, velocities, g, &
                    k_poses(1, :, :), k_vels(1, :, :))
        do i=1, 5
            temp_p = positions
            temp_v = velocities
            do l=1, i
                temp_p = temp_p + k_poses(i, :, :) * coefficients(i, l) * dt
                temp_v = temp_v + k_vels(i, :, :) * coefficients(i, l) * dt
            end do
            call rk_pde(n, masses, temp_p, temp_v, g, &
                        k_poses(i + 1, :, :), k_vels(i + 1, :, :))
        end do

        poses_low = positions
        vels_low = velocities
        poses_high = positions
        vels_high = velocities

        do i=1, 6
            poses_low(:, :) = poses_low(:, :) + dt * weights_4(i) * k_poses(i, :, :)
            vels_low(:, :) = vels_low(:, :) + dt * weights_4(i) * k_vels(i, :, :)
            poses_high(:, :) = poses_high(:, :) + dt * weights_5(i) * k_poses(i, :, :)
            vels_high(:, :) = vels_high(:, :) + dt * weights_5(i) * k_vels(i, :, :)
        end do

    end subroutine rkf45

end module integrators


!--------------------------------------------------------------------------------
! Copyright (c) 2016 Peter Grünberg Institut, Forschungszentrum Jülich, Germany
! This file is part of FLEUR and available as free software under the conditions
! of the MIT license as expressed in the LICENSE file in more detail.
!--------------------------------------------------------------------------------

      MODULE m_cylbes
      use m_juDFT
c********************************************************************
c generates cylindrical Bessel functions for a given x and for the orders
c from mmax to -mmax
c                                  Y. Mokrousov
c********************************************************************
      CONTAINS
      SUBROUTINE cylbes(
     >                  mmax,x,
     <                  fJ)

      IMPLICIT NONE
!     ..
!     ..Arguments ..
      INTEGER, INTENT  (IN) :: mmax
      REAL,    INTENT  (IN) :: x
      REAL,    INTENT (OUT) :: fJ(-mmax:mmax)
!
!     .. Parameters ..
      REAL,    PARAMETER :: zero = 0.0
!     ..Locals ..
      INTEGER :: m,i,mass
      REAL :: quot
      REAL, ALLOCATABLE :: aux(:)
!     ..

      IF (x.LT.zero)  CALL juDFT_error("cylbes2",calledby="cylbes")

      IF (x.EQ.zero) THEN
        fJ(0) = 1.

        DO m=1,mmax
         fJ(m) = 0.
         fJ(-m) = 0.
        END DO
        RETURN
      END IF

      mass = INT( mmax + 50 + x )
      ALLOCATE ( aux(0:mass) )
      aux(mass) = 0.0
      aux(mass-1) = 1.0e-22

      DO i=mass-2,0,-1
        aux(i) = 2*(i+1)*aux(i+1)/x - aux(i+2)
      END DO

      quot = aux(0)

      DO i=1,INT( mass/2. )
        quot = quot + 2*aux(2*i)
      END DO

      fJ(0) = aux(0)/quot

      DO m=1,mmax
        fJ(m) = aux(m)/quot
        fJ(-m) = ((-1)**m)*fJ(m)
      END DO

      DEALLOCATE ( aux )

      RETURN
      END SUBROUTINE cylbes
      END MODULE m_cylbes


      REAL             FUNCTION SLAPY3( X, Y, Z )
*
*  -- LAPACK auxiliary routine (version 3.2) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      REAL               X, Y, Z
*     ..
*
*  Purpose
*  =======
*
*  SLAPY3 returns sqrt(x**2+y**2+z**2), taking care not to cause
*  unnecessary overflow.
*
*  Arguments
*  =========
*
*  X       (input) REAL
*  Y       (input) REAL
*  Z       (input) REAL
*          X, Y and Z specify the values x, y and z.
*
*  =====================================================================
*
*     .. Parameters ..
      REAL               ZERO
      PARAMETER          ( ZERO = 0.0E0 )
*     ..
*     .. Local Scalars ..
      REAL               W, XABS, YABS, ZABS
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX, SQRT
*     ..
*     .. Executable Statements ..
*
      XABS = ABS( X )
      YABS = ABS( Y )
      ZABS = ABS( Z )
      W = MAX( XABS, YABS, ZABS )
      IF( W.EQ.ZERO ) THEN
*     W can be zero for max(0,nan,0)
*     adding all three entries together will make sure
*     NaN will not disappear.
         SLAPY3 =  XABS + YABS + ZABS
      ELSE
         SLAPY3 = W*SQRT( ( XABS / W )**2+( YABS / W )**2+
     $            ( ZABS / W )**2 )
      END IF
      RETURN
*
*     End of SLAPY3
*
      END


datatype Pair (X:*) (Y:*) =
      Pcons of (x:X) (y:Y);;

let LTInt (hi:Int) :* = (x:Int. (x <= hi));;

let RInt (hi:Int)  (lo:(LTInt hi)):* = (x:Int. (and  (lo<=x)  (x<hi)));;

/*let exact (d:Int) : (RInt (d+1) (d+0)) = cast Int (RInt (d+1) (d+0)) d;;*/

datatype RangeList (max:Int) (min:(LTInt max)) =
  RNull of (u:Unit)
| RCons of (hd:(RInt max min)) (tl:RangeList max min);;

datatype SortedList (max:Int) (min:(LTInt max)) =
  SNull of (u:Unit)
| SCons of (hd:(RInt max min)) (tl:(SortedList max hd));;

let rec sortAppend (ub:Int) (mid:(LTInt ub)) (lb:(LTInt mid))
      (l1:SortedList mid lb) (l2:SortedList ub mid):
      (SortedList ub lb) =
      caseSortedList mid lb l1 (SortedList ub lb)
        (fn u => l2)
        (fn x y => SCons ub lb x (sortAppend ub mid x y l2));;

let rec partition (ub:Int) (lb:(LTInt ub)) (pivot:(RInt ub lb))
      (l:(RangeList ub lb)) (l1:(RangeList pivot (lb+0)))
      (l2:(RangeList ub pivot)):
      (Pair (RangeList pivot (lb+0)) (RangeList ub pivot)) =
      caseRangeList ub lb l
      (Pair (RangeList pivot (lb+0)) (RangeList ub pivot))
      (fn u => (Pcons (RangeList pivot (lb+0)) (RangeList ub pivot) l1 l2))
      (fn x y =>
         if [(Pair (RangeList pivot (lb+0)) (RangeList ub pivot))] x >= pivot
         then (partition ub lb pivot y l1 (RCons ub pivot (x+0) l2))
         else (partition ub lb pivot y (RCons pivot (lb+0) (x+0) l1) l2));;

let rec quicksort (ub:Int) (lb:(LTInt ub)) (l:(RangeList ub lb)):
      (SortedList ub lb) =
      caseRangeList ub lb l (SortedList ub lb)
      (fn u1 => (SNull ub lb unit))
      (fn x1 y1 =>
           (caseRangeList ub lb y1 (SortedList ub lb)
           (fn u2 => (SCons ub lb x1 (SNull ub x1 unit)))
           (fn x2 y2 =>
                (let pivot = x1 in
                (let pairLists =
                   (partition ub lb pivot y1
                   (RNull pivot (lb+0) unit) (RNull ub pivot unit)) in
                (casePair (RangeList pivot (lb+0)) (RangeList ub pivot)
                          pairLists (SortedList ub lb)
                (fn x y => (sortAppend ub pivot (lb+0)
                            /* BUG: switched lb and pivot */
                            (quicksort (lb+0) pivot x)
                            (SCons ub pivot (pivot+0) (quicksort ub pivot y))))
                            ))))));;


subroutine array_division(x, a, nrow, ncol, y)
    use env_precision
    implicit none
    integer, intent(in)   :: ncol, nrow
    real(r8k), intent(in)  :: x(nrow, ncol)
    real(r8k), intent(in)  :: a
    real(r8k), intent(out) :: y(nrow, ncol)
    integer               :: i, j
    character(len=100)    :: fmt1, fmt2

    write(6, *) "(array_division) Input array = "
    call print_array2d(x, nrow, ncol)

    do j = 1, ncol
        do i = 1, nrow
            y(i, j) = x(i, j) / a
        end do
    end do

end subroutine

subroutine sub_dgemv(A, x, y, alpha, beta, nrow, ncol, res)
    implicit none
    external :: dgemv
    integer, parameter    :: dp = kind(1.0d0)
    integer, intent(in)   :: ncol, nrow
    real(dp), intent(in)  :: alpha, beta
    real(dp), intent(in)  :: A(nrow, ncol)
    real(dp), intent(in)  :: x(ncol), y(nrow)
    real(dp), intent(out) :: res(nrow)
    integer :: i, j
    do i = 1, nrow
        res(i) = y(i)
    end do
    call dgemv('N', nrow, ncol, alpha, A, nrow, x, 1, beta, res, 1)
end subroutine

subroutine sub_dgesvd(A, k, nrow, ncol, U, S, VT)
    implicit none
    external :: dgesvd
    integer, parameter    :: dp = kind(1.0d0)
    integer, intent(in)   :: nrow, ncol, k
    real(dp), intent(in)  :: A(nrow, ncol)
    real(dp), intent(out) :: U(nrow, k), S(k), VT(k, ncol)

!   local variables
    integer(kind = 4) :: ldmax, lwork, info, i, j
    real(dp), allocatable :: work(:)
    real(dp) :: Acopy(nrow, ncol)

    do j = 1, ncol
        do i = 1, nrow
            Acopy(i, j) = A(i, j)
        end do
    end do

    ldmax = max(nrow, ncol)
    lwork = max(1, 3 * k + ldmax, 5 * k)
    allocate(work(lwork))
    call dgesvd('S', 'S', nrow, ncol, Acopy, nrow, S, U, nrow, VT, k, work, lwork, info)
end subroutine

subroutine print_array2d(x, m, n)
!
!   x is an input array of size (m, n)
!
    implicit none
    integer, parameter    :: dp = kind(1.0d0)
    integer, intent(in)   :: m, n
    real(dp), dimension(m, n), intent(in)  :: x
    character(len=100)    :: fmt1
!
!   Format for each row should be:
!   fmt1 = '(n(2X,F7.2))'
!
    write (fmt1, '(A,I0,A)') '(', n, '(2X, F7.3))'
    write (6, fmt1) transpose(x)
!
end subroutine print_array2d

subroutine print_vector(x, m)
    implicit none
    integer, parameter    :: dp = kind(1.0d0)
    integer, intent(in)   :: m
    real(dp), dimension(m), intent(in)  :: x
    character(len=100)    :: fmt1
!
!   Format for each row should be:
!   fmt1 = '(m(2X,F7.2))'
!
    write (fmt1, '(A,I0,A)') '(', m, '(2X, F7.3))'
    write (6, fmt1) x
!
end subroutine print_vector


C     a functional parameter is used. See the calls to relax in the main.
c------------------------------------------------------------------------------
      subroutine init
      parameter (l=2, m=3, n=4)
c
      real*8 a(l,m), b(m,n)
c
      common /d/ a, b
c
      real*8 drand
c
      do i = 1, l
         do j = 1, m
            a(i,j) = srand(0)
         enddo
      enddo
c
      do i = 1, m
         do j = 1, n
            b(i,j) = drand(0)
         enddo
      enddo
c
      return
      end
c------------------------------------------------------------------------------
      program linear
c
      parameter (l=2, m=3, n=4)
      parameter (niter=100)
c
      real*8 a(l,m), b(m,n)
      real*8 c(l,n)
c
      common /d/ a, b
      common /r/ c
c
      real*8 mean1, mean2
      external mean1, mean2
c
      call init
c
      call prmat(a, l, m)
      call prmat(b, m, n)
c
      call matmul(a, b, c, l, m, n)
c
      call prmat(c, l, n)
c
      call relax(c, l, n, niter, mean1)
      call relax(c, l, n, niter, mean2)
c
      call prmat(c, l, n)
c
      stop 'end of program'
      end
c------------------------------------------------------------------------------
      subroutine matmul(a, b, c, l, m, n)
c
      integer l, m, n
      real*8 a(l,m), b(m,n)
      real*8 c(l,n)
      real*8 t
c
      do i = 1, l
         do j = 1, n
            t = 0.0
            do k = 1, m
               t = t + a(i,k)*b(k,j)
            enddo
            c(i,j) = t
         enddo
      enddo
c
      return
      end

c------------------------------------------------------------------------------
      real*8 function mean1(q1, q2)
c
      real*8 q1, q2
c
      mean1 = 0.5*(q1+q2)
c
      return
      end

c------------------------------------------------------------------------------
      real*8 function mean2(q1, q2)
c
      real*8 q1, q2
c
      mean2 = 0.75*q1 + .25*q2
c
      return
      end

c------------------------------------------------------------------------------
      subroutine prmat(mat, d1, d2)
c
      real*8 mat(d1, d2)
      integer d1, d2
c
      do i = 1, d1
         do j = 1, d2
            write (6, 1000) i, j, mat(i,j)
         enddo
      enddo
c
 1000 format('i = ', i3, '   j = ', i3, '   mat(i,j) = ', e18.7)
c
      return
      end

c------------------------------------------------------------------------------
      subroutine relax(mat, d1, d2, n, f)
c
      integer d1, d2
      real*8 mat(d1, d2)
      real*8 f
c
      if (n .le. 0) stop 'nombre d iterations incorrect'
c
      do iter = 1, n
         do i = 2, d1-1
            do j = 2, d2-1
               q = (mat(i-1,j)+mat(i+1,j)+mat(i,j-1)+mat(i,j+1))/4.0
               mat(i,j) = f(mat(i,j), q)
            enddo
         enddo
      enddo
c
      return
      end


      subroutine tmpcft
c
c     + + + PURPOSE + + +
c
c     Fit an air temperature curve form longterm monthly temperature data
c      Codes of Newton method was adapted from Li Wang
c
c     Called from: SR WINIT
c     Author(s): Li Wang, Shuhui Dun, WSU
c     Reference in User Guide: choose minimum square root error
c
c     Version: 2008.
c     Date recoded: January 07, 2008
c     Verified by : Joan Wu, WSU
c

c
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
      include 'pmxpln.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
c
c
c     + + + ARGUMENT DEFINITIONS + + +
c
c
c     + + + COMMON BLOCKS + + +
c
      include 'cobclim.inc'
c     Read: obmaxt, obmint
c
      include 'ctcurv.inc'
c     Modify: YavgT,YampT,YpshfT,obavgT
c
c     + + + LOCAL VARIABLES + + +
c
      integer  i,nir
      real Fpcal,Fmin,Mmaxt,Mmint,tol,dpshft,fsdsqe,sddsqe,ThetaT,pai
      real tmpfun,tday
c
c     + + + LOCAL DEFINITIONS + + +
c
c     Fp0, Fp1,Fpcal: function value (mean square error) of corresponding points
c     Fmin: minimum mean square error
c     tol: tolerance
c      Mmaxt: Maximum monthly temperature
c     Mmint: Minimum monthly temperature
c      dpshft: Delta phase shift
c      fsdsqe: First order derivative of the square error
c     sddsqe: second order derivative of the square error
c
c     + + + SAVES + + +
c
c     + + + SUBROUTINES CALLED + + +
c
c
c     + + + DATA INITIALIZATIONS + + +
      tol = 0.00001
      Mmaxt = -100
      Mmint = 100
      pai=acos(-1.0)
c
c     + + + END SPECIFICATIONS + + +
c
cd      Added by S. Dun, Jan 05, 2008
c      For a long term yearly average temperature in frost simulation
      yavgt = 0
c
      do 10 i = 1, 12
         obavgt(i) = (obmaxt(i) + obmint(i))/2.0
         if (Mmaxt.lt.obavgt(i))  Mmaxt = obavgt(i)
         if (Mmint.gt.obavgt(i))  Mmint = obavgt(i)
         yavgt = yavgt + obavgt(i)
10    continue
c
      yavgt = yavgt/12.
      YampT = (Mmaxt - Mmint)/2.0
c
      if (YpshfT.gt. 0) then
c      check if the Ypshft could be directly used
            if (tmpfun(YpshfT) .lt. 2.0) return
      endif
c
c     Newton method
      dpshft = 1
      nir = 0
      YpshfT = 0

      Do while ((abs(dpshft).gt. tol).and. (nir.lt.20))

         fsdsqe = 0.0
         sddsqe = 0.0
         nir = nir +1

         Do 30 i = 1, 12
             tday = 15. + (i-1)* 30.5
             ThetaT = 2.*pai/365. *(tday-YpshfT)

             fsdsqe = fsdsqe -(YavgT + YampT * sin(thetaT) -obavgt(i))
     1                * cos(thetaT)
c
             sddsqe = sddsqe - 2.*pai/365.*
     1        ((YavgT - obavgt(i))*sin(thetaT) - YampT*cos(2.*thetaT))
30        continue
c
            if(sddsqe .lt. 0.) then
              YpshfT = YpshfT + 365.0/2.0
            else
c
                dpshft = - fsdsqe/sddsqe
                Ypshft = ypshft + dpshft
c
              Ypshft = mod(Ypshft,365.0)
              if (Ypshft.lt.0) Ypshft = Ypshft + 365.0
          endif
c
40      enddo
c
cd    The alternative method if Newton Mehtod would not converge after 20 iterations
      if(nir.ge.20) then
c      chose minimum square root error
          Fmin = tmpfun(0.)
          YpshfT = 0
c
          do 20 i = 1, 365
              Fpcal = tmpfun(float(i))
                if (Fpcal.lt.Fmin) then
                  Fmin = Fpcal
                  YpshfT = i
              endif
20        continue
      endif

      return
      end


FUNCTION getAccumulation(  Model, Node, InputArray) RESULT(accum)
  ! provides you with most Elmer functionality
  USE DefUtils
  ! saves you from stupid errors
  IMPLICIT NONE
  ! the external variables
  !----------------------------------------------------------------------------
  TYPE(Model_t) :: Model     ! the access point to everything about the model
  INTEGER :: Node            ! the current Node number
  REAL(KIND=dp) :: InputArray(2) ! Contains the arguments passed to the function
  REAL(KIND=dp) :: accum      ! the result
  !----------------------------------------------------------------------------
  ! internal variables
  !----------------------------------------------------------------------------
  REAL(KIND=dp) :: lapserate, ela0, dElaDt, elaT, accumulationAtSl,&
       inittime, time, elevation, cutoff, offset
  LOGICAL :: FirstTime=.TRUE.
  ! Remember this value
  SAVE FirstTime, inittime

  ! lets hard-code our values (if we have time we can later make them being read from SIF)
  lapserate = 11.0_dp/2750.0_dp
  ela0 = 400.0_dp
  dElaDt = -0.05_dp
  cutoff = 600.0_dp
  offset = 1500.0

  ! copy input (should match the arguments!)
  elevation = InputArray(1)
  time = InputArray(2)
  WRITE (Message, '(A,E10.2,A,E10.2)')  "elevation=", elevation, "time=", time
  CALL INFO("getAccumulation", Message, Level=9)

  ! store the initial time, to be sure to have relative times
  IF (FirstTime) THEN
     inittime = time
     FirstTime = .FALSE.
  END IF

  ! get change of ELA with time
  IF (time > offset) THEN
     elaT = ela0 - dElaDt * (time - offset)
  ELSE
     elaT = ela0
  END IF

  ! lets do the math
  accumulationAtSl = -elaT*lapserate
  IF (elevation > cutoff) elevation = cutoff
  accum = lapserate*elevation + accumulationAtSl

  RETURN

END FUNCTION getAccumulation


      SUBROUTINE MB03QX( N, T, LDT, WR, WI, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute the eigenvalues of an upper quasi-triangular matrix.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix T.  N >= 0.
C
C     T       (input) DOUBLE PRECISION array, dimension(LDT,N)
C             The upper quasi-triangular matrix T.
C
C     LDT     INTEGER
C             The leading dimension of the array T.  LDT >= max(1,N).
C
C     WR, WI  (output) DOUBLE PRECISION arrays, dimension (N)
C             The real and imaginary parts, respectively, of the
C             eigenvalues of T. The eigenvalues are stored in the same
C             order as on the diagonal of T. If T(i:i+1,i:i+1) is a
C             2-by-2 diagonal block with complex conjugated eigenvalues
C             then WI(i) > 0 and WI(i+1) = -WI(i).
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
C     CONTRIBUTOR
C
C     A. Varga, German Aerospace Center, DLR Oberpfaffenhofen,
C     March 1998. Based on the RASP routine SEIG.
C
C     ******************************************************************
C     .. Parameters ..
      DOUBLE PRECISION ZERO
      PARAMETER        ( ZERO = 0.0D0 )
C     .. Scalar Arguments ..
      INTEGER          INFO, LDT, N
C     .. Array Arguments ..
      DOUBLE PRECISION T(LDT, *), WI(*), WR(*)
C     .. Local Scalars ..
      INTEGER          I, I1, INEXT
      DOUBLE PRECISION A11, A12, A21, A22, CS, SN
C     .. External Subroutines ..
      EXTERNAL         DLANV2, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC        MAX
C     .. Executable Statements ..
C
      INFO = 0
C
C     Test the input scalar arguments.
C
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( LDT.LT.MAX( 1, N ) ) THEN
         INFO = -3
      END IF
C
      IF( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB03QX', -INFO )
         RETURN
      END IF
C
      INEXT = 1
      DO 10 I = 1, N
         IF( I.LT.INEXT )
     $      GO TO 10
         IF( I.NE.N ) THEN
            IF( T(I+1,I).NE.ZERO ) THEN
C
C              A pair of eigenvalues.
C
               INEXT = I + 2
               I1 = I + 1
               A11 = T(I,I)
               A12 = T(I,I1)
               A21 = T(I1,I)
               A22 = T(I1,I1)
               CALL DLANV2( A11, A12, A21, A22, WR(I), WI(I), WR(I1),
     $                      WI(I1), CS, SN )
               GO TO 10
            END IF
         END IF
C
C        Simple eigenvalue.
C
         INEXT = I + 1
         WR(I) = T(I,I)
         WI(I) = ZERO
   10 CONTINUE
C
      RETURN
C *** Last line of MB03QX ***
      END


MODULE WAVELENGTH
  !Author: Dang Huu Chung

  USE GLOBAL,ONLY:RKD
  IMPLICIT NONE

  REAL(RKD),PRIVATE,PARAMETER :: G=9.81
  REAL(RKD),PRIVATE,PARAMETER :: PI=3.14159265358979

  CONTAINS

  FUNCTION DISRELATION(RLS,TP,HD,U,PHI) RESULT(FWL)
    !Dispersion Relation:          FWL=0
    !RLS: Wave length              [m]
    !TP : Wave period              [s]
    !HD : Water depth              [m]
    !U  : Depth-average velocity   [m/s]
    !PHI: Angle of (wave,current)  [Rad]
    REAL(RKD) :: FWL,RLS,TP,HD,U,PHI
    FWL=(RLS/TP-U*COS(PHI))-SQRT(G*RLS/2._8/PI*TANH(2._8*PI*HD/RLS))
  END FUNCTION

  RECURSIVE SUBROUTINE BISEC(FUN,A0,B0,TOL,TP,HD,U,PHI,X)
    REAL(RKD),INTENT(IN)       :: A0,B0,TOL,TP,HD,U,PHI
    REAL(RKD),EXTERNAL         :: FUN
    REAL(RKD),INTENT(OUT)      :: X
    INTEGER                    :: IST
    REAL(RKD)                  :: A,B,FA,FB,FX
    A=A0
    B=B0
    IST=1
    FA=FUN(A,TP,HD,U,PHI)
    FB=FUN(B,TP,HD,U,PHI)
    IF( FA*FB < 0 )THEN
      X=0.5*(A+B)
      FX=FUN(X,TP,HD,U,PHI)
      IF( FA*FX < 0 )THEN
        B=X
      ELSE
        A=X
      ENDIF
      IF( ABS(A-B) <= TOL )THEN
        RETURN
      ELSE
        CALL BISEC(FUN,A,B,TOL,TP,HD,U,PHI,X)
      ENDIF
    ELSEIF (FA == 0 )THEN
      X=A
    ELSEIF (FB == 0 )THEN
      X=B
    ELSE
      IST=-1
      STOP 'DISPERSION RELATION: FA.FB>0'
    ENDIF
  END SUBROUTINE BISEC

  FUNCTION RTBIS(FUNC,X1,X2,XACC,TP,HD,U,PHI)
    REAL(RKD) :: RTBIS,X1,X2,XACC,TP,HD,U,PHI
    REAL(RKD),EXTERNAL :: FUNC
    INTEGER   :: J,JMAX
    REAL(RKD) :: DX,F,FMID,XMID
    PARAMETER (JMAX=40)

    FMID=FUNC(X2,TP,HD,U,PHI)
    F=FUNC(X1,TP,HD,U,PHI)
    IF( F*FMID >= 0.) PAUSE 'ROOT MUST BE BRACKETED IN RTBIS'
    IF( F<0. )THEN
      RTBIS=X1
      DX=X2-X1
    ELSE
      RTBIS=X2
      DX=X1-X2
    ENDIF
    DO J=1,JMAX
      DX=DX*.5
      XMID=RTBIS+DX
      FMID=FUNC(XMID,TP,HD,U,PHI)
      IF( FMID <= 0.)RTBIS=XMID
      IF( ABS(DX)<XACC .OR. FMID == 0. ) RETURN
    ENDDO
    PAUSE 'TOO MANY BISECTIONS IN RTBIS'
  END FUNCTION

END MODULE


!=======================================================================
!
!  Calculate the level populations at LTE for the given species.
!
!-----------------------------------------------------------------------
SUBROUTINE CALCULATE_LTE_POPULATIONS(NLEVEL,ENERGY,WEIGHT,DENSITY, &
                                   & TEMPERATURE,POPULATION)

   USE HEALPIX_TYPES
   IMPLICIT NONE

   INTEGER(KIND=I4B), INTENT(IN)  :: NLEVEL
   REAL(KIND=DP),     INTENT(IN)  :: ENERGY(:),WEIGHT(:)
   REAL(KIND=DP),     INTENT(IN)  :: DENSITY,TEMPERATURE
   REAL(KIND=DP),     INTENT(OUT) :: POPULATION(:)

   INTEGER(KIND=I4B) :: ILEVEL
   REAL(KIND=DP) :: PARTITION_FUNCTION

!  Initialize the level populations
   POPULATION=0.0D0

   PARTITION_FUNCTION=0.0D0
   DO ILEVEL=1,NLEVEL
      POPULATION(ILEVEL)=WEIGHT(ILEVEL)*EXP(-ENERGY(ILEVEL)/(KB*TEMPERATURE))
      PARTITION_FUNCTION=PARTITION_FUNCTION+POPULATION(ILEVEL)
   END DO
   POPULATION=POPULATION*DENSITY/PARTITION_FUNCTION

!  Check that the sum of the level populations matches the total density to within 0.1%
   IF(ABS(DENSITY-SUM(POPULATION))/DENSITY.GT.1.0D-3) THEN
      WRITE(6,"('ERROR! Sum of LTE level populations differs from the total density by',F4.1,'%')") &
         & 1.0D2*ABS(SUM(POPULATION)-DENSITY)/DENSITY
      STOP
   END IF

   RETURN
END SUBROUTINE CALCULATE_LTE_POPULATIONS
!=======================================================================


c polynom interpolation
			SUBROUTINE polint(xa,ya,n,x,y,dy)
			INTEGER n,NMAX
			REAL dy,x,y,xa(n),ya(n)
c			Largest anticipated value of n.
			PARAMETER (NMAX=10)
c			Given arrays xa and ya, each of length n, and given a value x, this routine returns a
c			value y, and an error estimate dy. If P(x) is the polynomial of degree N ? 1 such that
c			P(xai) = yai, i = 1, . . . , n, then the returned value y = P(x).
			INTEGER i,m,ns
			REAL den,dif,dift,ho,hp,w,c(NMAX),d(NMAX)
			ns=1
			dif=abs(x-xa(1))
c			Here we find the index ns of the closest table entry,
			do i=1,n
				dift=abs(x-xa(i))
				if (dift.lt.dif) then
					ns=i
					dif=dift
				endif
c			and initialize the tableau of c?s and d?s.
				c(i)=ya(i)
				d(i)=ya(i)
			enddo
c			This is the initial approximation to y.
			y=ya(ns)
			ns=ns-1
c			For each column of the tableau,
			do m=1,n-1
c			we loop over the current c?s and d?s and update them.
				do i=1,n-m
					ho=xa(i)-x
					hp=xa(i+m)-x
					w=c(i+1)-d(i)
					den=ho-hp
					if(den.eq.0.) write (*,*) 'failure in polint'
c			This error can occur only if two input xa?s are (to within roundoff) identical.
					den=w/den
c			Here the c?s and d?s are updated.
					d(i)=hp*den
					c(i)=ho*den
				enddo
				if (2*ns.lt.n-m) then
c			After each column in the tableau is completed, we decide
c			which correction, c or d, we want to add to our accumulating
c			value of y, i.e., which path to take through
c			the tableau?forking up or down. We do this in such a
c			way as to take the most ?straight line? route through the
c			tableau to its apex, updating ns accordingly to keep track
c			of where we are. This route keeps the partial approximations
c			centered (insofar as possible) on the target x. T he
c			last dy added is thus the error indication.
					dy=c(ns+1)
				else
					dy=d(ns)
					ns=ns-1
				endif
				y=y+dy
			enddo
			return
			END

c integration par la methode de Romberg
			SUBROUTINE qromb(func,a,b,ss)
			INTEGER JMAX,JMAXP,K,KM
			REAL a,b,func,ss,EPS
			EXTERNAL func
			PARAMETER (EPS=1.e-6, JMAX=20, JMAXP=JMAX+1, K=5, KM=K-1)
c			USES polint,trapzd
c			Returns as ss the integral of the function func from a to b. Integration is performed by
c			Romberg's method of order 2K, where, e.g., K=2 is Simpson?s rule.
c			Parameters: EPS is the fractional accuracy desired, as determined by the extrapolation
c			error estimate; JMAX limits the total number of steps; K is the number of points used in
c			the extrapolation.
			INTEGER j
c			These store the successive trapezoidal approximations
			REAL dss,h(JMAXP),s(JMAXP)
c			and their relative stepsizes.
			h(1)=1.
			do j=1,JMAX
				call trapzd(func,a,b,s(j),j)
				if (j.ge.K) then
					call polint(h(j-KM),s(j-KM),K,0.,ss,dss)
					if (abs(dss).le.EPS*abs(ss)) return
				endif
				s(j+1)=s(j)

c			This is a key step: The factor is 0.25 even though
c			the stepsize is decreased by only 0.5. This makes
c			the extrapolation a polynomial in h2 as allowed
c			by equation (4.2.1), not just a polynomial in h.

				h(j+1)=0.25*h(j)
			enddo
c			write (*,*) 'too many steps in qromb'
			END

c subroutine d'integration classique (trapeze)
			SUBROUTINE trapzd(func,a,b,s,n)
			INTEGER n
			REAL a,b,s,func
			EXTERNAL func
c This routine computes the nth stage of refinement of an extended trapezoidal rule. func is
c input as the name of the function to be integrated between limits a and b, also input. When
c called with n=1, the routine returns as s the crudest estimate of integral from a to 0 of f(x)dx.
c Subsequent calls with n=2,3,... (in that sequential order) will improve the accuracy of s by adding 2^(n-2)
c additional interior points. s should not be modified between sequential calls.
			INTEGER it,j
			REAL del,sum,tnm,x
			if (n.eq.1) then
				s=0.5*(b-a)*(func(a)+func(b))
			else
				it=2**(n-2)
				tnm=it
c				This is the spacing of the points to be added.
				del=(b-a)/tnm
				x=a+0.5*del
				sum=0.
				do j=1,it
					sum=sum+func(x)
					x=x+del
				enddo
c				This replaces s by its refined value.
				s=0.5*(s+(b-a)*sum/tnm)
			endif
			return
			END

c *********************************************************
			SUBROUTINE qtrap(func,a,b,s)
			INTEGER JMAX
			REAL a,b,func,s,EPS
			EXTERNAL func
c MODIF JH : JMAX=12 au lieu de 20 (pour acc�l�rer les calculs au d�triment de la convergence)
			PARAMETER (EPS=1.e-6, JMAX=20)
c USES trapzd
c			Returns as s the integral of the function func from a to b. The parameters EPS can be set
c			to the desired fractional accuracy and JMAX so that 2 to the power JMAX-1 is the maximum
c			allowed number of steps. Integration is performed by the trapezoidal rule.
			INTEGER j
			REAL olds
c 		Any number that is unlikely to be the average of the function
c			at its endpoints will do here.
			olds=-1.e30
			do j=1,JMAX
				call trapzd(func,a,b,s,j)
c			Avoid spurious early convergence.
				if (j.gt.5) then
					if (abs(s-olds).lt.EPS*abs(olds).or.(s.eq.0.and.olds.eq.0.)) return
				endif
				olds=s
			enddo
c			pause 'too many steps in qtrap'
c			write (*,*) '[S_grill_version6] (WARNING) : too many steps in qtrap'
			END


!> This module contains a function to perform unit conversions
MODULE unit_conversions
  IMPLICIT NONE
  PRIVATE
  PUBLIC convert

CONTAINS

    !> Function that converts units from "a" to "b"
  FUNCTION convert(a,b) RESULT(res)
      USE parameters
    USE input_output
    IMPLICIT NONE
    CHARACTER(len=*), INTENT(in) :: a,b
    REAL(dp) :: res

    ! Femtoseconds converted to atomic units of time
    IF ( a == 'fs' .AND. b == 'au_time' ) THEN
       res = fs2s * s2autime
    ELSE IF ( a == 'au_time' .AND. b == 'fs' ) THEN
       res = 1.0_dp / ( fs2s * s2autime )

    ! Kelvin converted to atomic units of energy
    ELSE IF ( a == 'kelvin' .AND. b == 'au_energy' ) THEN
       res = kb * joules2au
    ELSE IF ( a == 'au_energy' .AND. b == 'kelvin' ) THEN
       res = 1.0_dp / ( kb * joules2au )

    ! Wavenumbers converted to atomic units of angular frequency
    ELSE IF ( a == 'wvnbr' .AND. b == 'au_ang_freq' ) THEN
       res = wvnbr2Hz/s2autime*2.0_dp*pi
    ELSE IF ( a == 'au_ang_freq' .AND. b == 'wvnbr' ) THEN
       res = 1.0_dp/(wvnbr2Hz/s2autime*2.0_dp*pi)

    ! Wavenumbers converted to atomic units of energy
    ELSE IF ( a == 'wvnbr' .AND. b == 'au_energy' ) THEN
       res = wvnbr2joules*joules2au
    ELSE IF ( a == 'au_energy' .AND. b == 'wvnbr' ) THEN
       res = 1.0_dp/(wvnbr2joules*joules2au)

    ELSE
       OPEN(UNIT=10, FILE=ERRORLOG)
       WRITE(10,*) 'Undefined unit conversion attempted, see file unit_conversions.f90'
       CLOSE(10)
    END IF

  END FUNCTION convert

END MODULE unit_conversions


C     LAST UPDATE 10/12/92
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C
      SUBROUTINE SVDVAR(V,MA,NP,W,CVM,NCVM)
      IMPLICIT NONE
C
C Purpose: Evaluates the covariancs matrix CVM of the fit for MA
C          parameters obtained by SVDFIT. Call this routine with
C          matrices V,W as returned from SVDFIT. NP,NCVM give the
C          physical dimensions of V,W,CVM.
C
      INTEGER MA,NP,NCVM
      REAL V(NP,NP),W(NP),CVM(NCVM,NCVM)
C
C Calls   0:
C Called by:
C
C-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-
C Local variables:
C
      INTEGER MMAX
      PARAMETER(MMAX=5000)
      REAL WTI(MMAX)
      REAL SUM
      INTEGER I,J,K
C
C---------------------------------------------------------------------
      DO 10 I=1,MA
         WTI(I) = 0.0
         IF(W(I).NE.0.0)WTI(I) = 1.0/(W(I)*W(I))
 10   CONTINUE
      DO 40 I=1,MA
         DO 30 J=1,I
            SUM = 0.0
            DO 20 K=1,MA
               SUM = SUM + V(I,K)*V(J,K)*WTI(K)
 20         CONTINUE
            CVM(I,J) = SUM
            CVM(J,I) = SUM
 30      CONTINUE
 40   CONTINUE
      RETURN
      END


	SUBROUTINE OAGSPC  ( gltln, slat, slon, number, dscomp, dsunif,
     +			     iret )
C************************************************************************
C* OAGSPC								*
C*									*
C* This subroutine computes the average minimum station spacing		*
C* and the uniform station spacing.					*
C*									*
C* OAGSPC  ( GLTLN, SLAT, SLON, NUMBER, DSCOMP, DSUNIF, IRET )		*
C*									*
C* Input parameters:							*
C*	GLTLN (4)	REAL		Grid bounds			*
C*	SLAT (NUMBER)	REAL		Station latitudes		*
C*	SLON (NUMBER)	REAL		Station longitudes		*
C*	NUMBER		INTEGER		Number of stations		*
C*									*
C* Output parameters:							*
C*	DSCOMP		REAL		Average min station spacing	*
C*	DSUNIF		REAL		Uniform station spacing		*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					 -9 = insufficient stations	*
C**									*
C* Log:									*
C* M. desJardins/GSFC	 8/85						*
C* M. desJardins/GSFC	11/88	GEMPAK 4.1				*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
	INCLUDE		'oagcmn.cmn'
C*
	REAL		slat (*), slon (*), gltln (*)
C*
	PARAMETER	( EPS = 1.E-10, DIST = 8., BIG = 100000. )
C*
C------------------------------------------------------------------------
	iret = 0
C
C*	Check for insufficient stations.
C
	IF  ( number .lt. 4 )  THEN
	    iret = -9
	    CALL ER_WMSG  ( 'OAGRID', iret, ' ', ier )
	    RETURN
	END IF
C
C*	Check each station and find the closest station.
C
	ns   = 0
	dsum = 0.
C*
	DO  i = 1, number
	    tlat  = slat (i)
	    tlon  = slon (i)
	    amind = big
	    tlatp = tlat + DIST
	    tlatm = tlat - DIST
	    tlonp = tlon + DIST
	    tlonm = tlon - DIST
	    DO  j = 1, number
		IF  ( i .ne. j )  THEN
		    ulat = slat (j)
		    ulon = slon (j)
	            IF ( ( ulat .gt. tlatm ) .and. ( ulat .lt. tlatp )
     +			 .and. ( ulon .gt. tlonm ) .and.
     +                   ( ulon .lt. tlonp ) )  THEN
		        dc = COS  ( ( ( tlat + ulat ) / 2. ) * DTR )
		        d  = ( tlat - ulat ) ** 2 +
     +                       ( dc * ( tlon - ulon ) ) ** 2
	  	        IF  ( ( d .lt. amind ) .and.
     +                        ( d .gt. EPS ) )  amind = d
	            END IF
		END IF
	    END DO
C
C*	    Check that information is not missing.
C
	    IF  ( amind .lt. BIG )  THEN
	        dsum = dsum + SQRT ( amind )
	        ns   = ns + 1
	    END IF
	END DO
C
C*	Check that there are enough stations.
C
	IF  ( ns .lt. 2 )  THEN
	    iret = -9
	    CALL ER_WMSG  ( 'OAGRID', iret, ' ', ier )
	  ELSE
C
C*	    Compute the computed station spacing.
C
	    dscomp = dsum / ns
C
C*	    Compute the uniform station spacing.
C
	    dn = COS  ( ( gltln (3) + gltln (1) ) * DTR / 2. )
	    d1 = gltln (3) - gltln (1)
	    d2 = ( gltln (4) - gltln (2) ) * dn
	    a  = 1 - number
	    b  = d1 + d2
	    c  = d1 * d2
	    ds = SQRT ( b**2 - 4.*a*c )
	    dsunif = ( -b - ds ) / ( 2. * a )
	END IF
C*
	RETURN
	END


********************************************************************************
!     Input Variables:
!     S        Amount of susceptible members at the current timestep
!     I        Amount of infected members at the current timestep
!     R        Amount of recovered members at the current timestep
!     beta     Rate of transmission via contact
!     gamma    Rate of recovery from infection
!     dt       Next inter-event time
!
!     State Variables:
!     infected    Increase in infected at the current timestep
!     recovered   Increase in recovered at the current timestep
********************************************************************************
      subroutine sir(S, I, R, beta, gamma, dt)
        implicit none
        double precision S, I, R, beta, gamma, dt
        double precision infected, recovered

        infected = (-(beta*S*I) / (S + I + R)) * dt
        recovered = (gamma*I) * dt

        S = S - infected
        I = I + infected - recovered
        R = R + recovered
      end subroutine sir


! { dg-do run }
! { dg-options "-fcheck=recursion" }
!
! PR fortran/39577
!
! Recursive but valid program
! Contributed by Dominique Dhumieres
!
recursive function fac(i) result (res)
  integer :: i, j, k, res
  k = 1
  goto 100
entry bifac(i,j) result (res)
  k = j
100 continue
  if (i < k) then
    res = 1
  else
    res = i * bifac(i-k,k)
  end if
end function

program test
interface
  recursive function fac(n) result (res)
    integer :: res
    integer :: n
  end function fac
  recursive function bifac(m,n) result (res)
    integer :: m, n, res
  end function  bifac
end interface

  print *, fac(5)
  print *, bifac(5,2)
  print*, fac(6)
  print *, bifac(6,2)
  print*, fac(0)
  print *, bifac(1,2)
end program test


      SUBROUTINE MATRIX_MUL_UNROLLED (A, B, C, L, M, N)
      DIMENSION A(L,M), B(M,N), C(L,N)

      DO 100 K = 1, N
        DO 100 I = 1, L
          C(I,K) = 0.
100     CONTINUE
      DO 110 J = 1, M, 4
        DO 110 K = 1, N
          DO 110 I = 1, L
            C(I,K) = C(I,K) + A(I,J) * B(J,K)
     $             + A(I,J+1) * B(J+1,K) + A(I,J+2) * B(J+2,K)
     $             + A(I,J+3) * B(J+3,K)
110   CONTINUE

      RETURN
      END

! Disabled for now as it requires delinearization.
! { dg-final { scan-tree-dump-times "number of SCoPs: 2" 1 "graphite" { xfail *-*-* } } }


!   Program to solve the two-dimensional Ising model
!   with zero external field using MPI
!   The coupling constant J = 1
!   Boltzmann's constant = 1, temperature has thus dimension energy
!   Metropolis sampling is used. Periodic boundary conditions.
!
!   The program uses a single pseudorandom number or an array of pseudorandom
!   numbers from the uniform distribution over the range 0 \leq x < 1.
!   The runtime-library implements the xorshift1024* random number
!   generator (RNG). This generator has a period of 2^1024 - 1, and when
!   using multiple threads up to 2^512 threads can each generate 2^512
!   random numbers before any aliasing occurs.
!   Note that in a multi-threaded program (e.g. using OpenMP directives),
!   each thread will have its own random number state. For details of the
!   seeding procedure, see the documentation for the RANDOM_SEED
!   intrinsic.

!    This module contains all constants and declarations
!    of variables read in by the function read_data. These
!    variables are used by many functions.

MODULE constants
  INTEGER,  PARAMETER :: dp = KIND(1.0D0)
  INTEGER, PARAMETER :: dpc = KIND((1.0D0,1.0D0))
END MODULE constants

!
! definition of MPI constants
!
MODULE mpi_constants
  INCLUDE 'mpif.h'
  INTEGER, PUBLIC :: no_intervalls, my_rank, myloop_begin, myloop_end, numprocs, ierror

END MODULE mpi_constants

!
!   Main program starts here, standard Metropolis algorithm
!

PROGRAM ising2dim
  USE constants
  USE mpi_constants
  INTEGER :: n_spins, de, t, tstep, i
  INTEGER, ALLOCATABLE, DIMENSION(:,:) :: spin_matrix
  REAL(dp) :: w(-8:8), local_average(5), total_average(5)
  REAL(DP) :: initial_temp, final_temp, E, M, temp_step, temperature
  REAL(DP) :: time_start, time_end

  ! initialize mpi
  !
  CALL MPI_INIT(ierror)
  CALL MPI_COMM_SIZE( MPI_COMM_WORLD, numprocs, ierror )
  CALL MPI_COMM_RANK( MPI_COMM_WORLD, my_rank, ierror )
  ! Only master writes and reads to file
  IF ( my_rank == 0 ) THEN
     OPEN(UNIT=6,FILE='mc_calc.dat')
     OPEN(UNIT=5,FILE='input.dat')
     CALL read_input(n_spins, mcs, initial_temp, final_temp, temp_step)
  ENDIF
  ! Broadcats to all processes common values
  CALL MPI_Bcast(mcs, 1, MPI_INT, 0, MPI_COMM_WORLD,ierror)
  CALL MPI_Bcast(n_spins, 1, MPI_INT, 0, MPI_COMM_WORLD,ierror)
  CALL MPI_Bcast(initial_temp, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD, ierror)
  CALL MPI_Bcast(final_temp, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD, ierror)
  CALL MPI_Bcast(temp_step, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD, ierror)
  ! Find temperature step
  tstep = INT((final_temp-initial_temp)/temp_step); temperature = initial_temp
  ! Start look over time steps
  ALLOCATE(spin_matrix(n_spins,n_spins))
    ! All spins point up, cold start, we don't use temp here
  spin_matrix = 1
  time_start = MPI_Wtime()
  DO t = 1, tstep
     !   initialise energy, magnetization and array for energy changes
     w = 0.0_dp
     DO de =-8, 8, 4
        w(de) = EXP(-de/temperature)
     ENDDO
     ! initialise array for expectation values
     total_average = 0.0_dp; local_average  =0.0_dp
     CALL Metropolis(mcs, n_spins, my_rank, w, local_average, spin_matrix)
     ! collect all results
     CALL  MPI_REDUCE(local_average, total_average, 5, MPI_DOUBLE_PRECISION, MPI_SUM, 0, MPI_COMM_WORLD, ierror)
     ! print results
     IF (my_rank == 0) THEN
        CALL WritetoFile(n_spins, mcs*numprocs, temperature, total_average)
     ENDIF
     ! update temp
     temperature = temperature + temp_step
  ENDDO
  DEALLOCATE(spin_matrix)
  time_end = MPI_Wtime()
  IF (my_rank == 0) THEN
     WRITE(*,*) 'Total time =', time_end-time_start,' in seconds used  on number of processes = ', numprocs
  ENDIF
  CALL MPI_FINALIZE(ierror)

END PROGRAM ising2dim

! read in input data
!
SUBROUTINE  read_input(n_spins, mcs, initial_temp, final_temp, temp_step)
  USE constants
  IMPLICIT NONE
  INTEGER, INTENT(inout) :: n_spins, mcs
  REAL(dp), INTENT(inout) :: initial_temp, final_temp, temp_step

  ! 'Number of Monte Carlo trials, lattice, init temp, final temp and temp step
  READ(5,*) mcs, n_spins, initial_temp, final_temp, temp_step

END SUBROUTINE read_input
!
! function to initialise energy and magnetization
!
SUBROUTINE  initialize(n_spins, spin_matrix, E, M)
  USE constants
  IMPLICIT NONE
  INTEGER, INTENT(IN) :: n_spins
  INTEGER, INTENT(IN) :: spin_matrix(n_spins, n_spins)
  REAL(dp), INTENT(INOUT) :: E, M
  INTEGER :: x, y, right, left, up, down

  ! setup initial energy and magnetization
  DO y =1, n_spins
     DO x= 1, n_spins
        right = x+1 ; IF(x == n_spins  ) right = 1
        left = x-1 ; IF(x == 1  ) left = n_spins
        up = y+1 ; IF(y == n_spins  ) up = 1
        down = y-1 ; IF(y == 1  ) down = n_spins
        e= e-spin_matrix(x,y)*(spin_matrix(right,y)+&
             spin_matrix(left,y)+spin_matrix(x,up)+ &
             spin_matrix(x,down) )
        m = m + spin_matrix(x,y)
     ENDDO
  ENDDO

  e = e*0.5_dp

END SUBROUTINE  initialize
!
!  Here we perform the sampling
!
SUBROUTINE  Metropolis(mcs, n_spins, my_rank, w, average, spin_matrix)
  USE constants
  IMPLICIT NONE
  INTEGER, INTENT(in) :: n_spins, my_rank, mcs
  INTEGER :: spins, ix, iy,  deltae, right, left, up, down, allspins, cycles
  REAL(dp) :: e, m
  REAL(dp), INTENT(IN) :: w(-8:8)
  REAL(dp), INTENT(INOUT) :: average(5)
  INTEGER, INTENT(INOUT) :: spin_matrix(n_spins,n_spins)
  REAL(DP) :: RandomNumber(3)
  INTEGER :: i, n, clock
  INTEGER, DIMENSION(:), ALLOCATABLE :: seed
  INTEGER :: idum

  ! Initialize RNG
  CALL RANDOM_SEED(size = n)
  ALLOCATE(seed(n))
  CALL SYSTEM_CLOCK(COUNT=clock)
  seed = clock + 37 * (/ (i - 1, i = 1, n) /)-my_rank
  CALL RANDOM_SEED(PUT = seed)
  ! initialize energy, magnetic moment
  e = 0.0_dp; m = 0.0_dp
  CALL initialize(n_spins, spin_matrix, e, m)
  ! start Monte Carlo computation
  allspins = n_spins*n_spins
  DO cycles = 1, mcs
     ! loop over all spins, that is a sweep over the lattice
     DO spins =1, allspins
        CALL RANDOM_NUMBER(RandomNumber)
        ix = INT(RandomNumber(1)*n_spins)+1
        iy = INT(RandomNumber(2)*n_spins)+1
        right = ix+1 ; IF(ix == n_spins  ) right = 1
        left = ix-1 ; IF(ix == 1  ) left = n_spins
        up = iy+1 ; IF(iy == n_spins  ) up = 1
        down = iy-1 ; IF(iy == 1  ) down = n_spins
        deltae = 2*spin_matrix(ix,iy)*(spin_matrix(right,iy)+&
             spin_matrix(left,iy)+spin_matrix(ix,up)+ &
             spin_matrix(ix,down) )
        IF ( RandomNumber(3) <= w(deltae) ) THEN
           spin_matrix(ix,iy) = -spin_matrix(ix,iy)  !flip one spin and accept new spin config
           m = m+2*spin_matrix(ix,iy)
           e = e+deltaE
        ENDIF
     ENDDO
     ! update expectation values
     average(1) = average(1) + e;     average(2) = average(2) + e*e
     average(3) = average(3)+m;     average(4) = average(4)+m*m
     average(5) = average(5)+ABS(m)
  ENDDO
  DEALLOCATE(seed)

END SUBROUTINE metropolis
!
!  Write out <E>, <M>, Heat capacity and susceptibility.
!
SUBROUTINE WritetoFile(n_spins, mcs, temperature, average)
  USE constants
  IMPLICIT NONE
  INTEGER, INTENT(in) :: n_spins, mcs
  REAL(dp), INTENT(in) :: temperature, average(5)
  REAL(dp) :: norm, Eaverage, E2average, Maverage, M2average, Mabsaverage, &
       Evariance, Mvariance

  norm = 1.0_dp/mcs  ! divided by total number of cycles
  Eaverage = average(1)*norm
  E2average = average(2)*norm
  Maverage = average(3)*norm
  M2average = average(4)*norm
  Mabsaverage = average(5)*norm
  ! all expectation values are per spin, divide by 1/n_spins/n_spins
  Evariance = (E2average- Eaverage*Eaverage)/n_spins/n_spins
  Mvariance = (M2average - Maverage*Maverage)/n_spins/n_spins
  WRITE(6,'(6F12.6)')temperature, Eaverage/n_spins/n_spins, Evariance/temperature/temperature, &
       Maverage/n_spins/n_spins, Mvariance/temperature, Mabsaverage/n_spins/n_spins

END SUBROUTINE WritetoFile


      module funcs58

      contains

      function arctanII(x)

      real :: prevxp, presxp, pi

      pi = 3.1415927

      if (x) 2,3,3

!2     stop  ! instead, return a value outside plausible range (common Fortran trick)
2     arctanII=-99
      return

3     arctanII = 0.

      if (x - 1.) 10, 10, 5

5     term = -1. / x
      arctanII = pi / 2
      goto 11

10    term = x
11    prevxp = 1.
      y = term ** 2.

12    arctanII = arctanII + term
      presxp = prevxp + 2.

      term = -prevxp / presxp * y * term
      prevxp = presxp

      if(term - 0.00005) 15, 12, 12
15    if(-term - 0.00005) 16, 12, 12

16    end

      end module funcs58


program stock_volatility

  use mod_arrays, only: average, std, moving_average, moving_std, reverse
  use mod_io, only: read_stock, write_stock

  implicit none

  character(len=4), allocatable :: symbols(:)
  character(len=:), allocatable :: time(:)
  real, allocatable :: open(:), high(:), low(:), close(:), adjclose(:), volume(:)
  integer :: i, im, n

  symbols = ['AAPL', 'AMZN', 'CRAY', 'CSCO', 'HPQ ',&
             'IBM ', 'INTC', 'MSFT', 'NVDA', 'ORCL']

  do n = 1, size(symbols)

    call read_stock('data/' // trim(symbols(n)) //  '.csv', time,&
      open, high, low, close, adjclose, volume)

    im = size(time)
    adjclose = reverse(adjclose)

    if (n == 1) then
      print *, time(im) // ' through ' // time(1)
      print *, 'Symbol, Average (USD), Volatility (USD), Relative Volatility (%)'
      print *, '----------------------------------------------------------------'
    end if

    print *, symbols(n), average(adjclose), std(adjclose),&
      nint(std(adjclose) / average(adjclose) * 100)

    time = time(im:1:-1)

    call write_stock(trim(symbols(n)) // '_volatility.txt', time, adjclose,&
      moving_average(adjclose, 30), moving_std(adjclose, 30))

  end do

end program stock_volatility


Subroutine polyleg(x,pn,dpn)

implicit none

integer,parameter :: npt=100000
integer	:: n,i,nb_point,ndeg
real*8  :: x(:),pn(:,0:),dpn(:,0:),xm
real*8	:: x2(npt),x3(npt),x4(npt),x5(npt)
real*8	:: pn_1(npt),pn_2(npt),dpn_1(npt),dpn_2(npt)

ndeg=size(pn,2)-1
nb_point=size(x)
do n=0,ndeg
  select case (n)
    case(0)
      pn (1:nb_point,n)= 1.d0
      dpn(1:nb_point,n)= 0.d0
    case(1)
      pn (1:nb_point,n) = x(:)
      dpn(1:nb_point,n) = 1.d0
    case(2)
      x2(1:nb_point)=x(:)*x(:)
      pn (1:nb_point,n)=1.5d0*x2(1:nb_point)-.5d0
      dpn(1:nb_point,n)=3.d0*x(:)
    case(3)
      x2(1:nb_point)=x(:)*x(:)
      x3(1:nb_point)=x2(1:nb_point)*x(:)
      pn(1:nb_point,n)=2.5d0*x3(1:nb_point)-1.5d0*x(:)
      dpn(1:nb_point,n)=7.5d0*x2(1:nb_point)-1.5d0
    case(4)
      x2(1:nb_point)=x(:)*x(:)
      x3(1:nb_point)=x2(1:nb_point)*x(:)
      x4(1:nb_point)=x3(1:nb_point)*x(:)
      pn (1:nb_point,n)=4.375d0*x4(1:nb_point)-3.75d0*x2(1:nb_point)+.375d0
      dpn(1:nb_point,n)=17.5d0*x3(1:nb_point)-7.5d0*x(:)
    case(5)
      x2(1:nb_point)=x(:)*x(:)
      x3(1:nb_point)=x2(1:nb_point)*x(:)
      x4(1:nb_point)=x3(1:nb_point)*x(:)
      x5(1:nb_point)=x4(1:nb_point)*x(:)
      pn (1:nb_point,n)=7.875d0*x5(1:nb_point) - 8.75d0*x3(1:nb_point)+1.875d0*x(:)
      dpn(1:nb_point,n)=39.375d0*x4(1:nb_point)-26.25d0*x2(1:nb_point)+1.875d0
    case default
      x2(1:nb_point)=x(:)*x(:)
      x3(1:nb_point)=x2(1:nb_point)*x(:)
      x4(1:nb_point)=x3(1:nb_point)*x(:)
      x5(1:nb_point)=x4(1:nb_point)*x(:)
      pn_2 (1:nb_point)=4.375d0*x4(1:nb_point)-3.75d0*x2(1:nb_point)+.375d0
      dpn_2(1:nb_point)=17.5d0*x3(1:nb_point)-7.5d0*x(:)
      pn_1 (1:nb_point)=7.875d0*x5(1:nb_point) - 8.75d0*x3(1:nb_point)+1.875d0*x(:)
      dpn_1(1:nb_point)=39.375d0*x4(1:nb_point)-26.25d0*x2(1:nb_point)+1.875d0
      xm=5.d0
      do i=1,n-5
        xm=xm+1.d0
        pn (1:nb_point,n)=(2.d0-1.d0/xm)*x(:)*pn_1(1:nb_point)-(1.d0-1.d0/xm)*pn_2(1:nb_point)
        dpn(1:nb_point,n)=(2.d0-1.d0/xm)*(pn_1(1:nb_point)+x(:)*dpn_1(1:nb_point))-(1.d0-1.d0/xm)*dpn_2(1:nb_point)
        pn_2 (1:nb_point)=pn_1 (1:nb_point)
        dpn_2(1:nb_point)=dpn_1(1:nb_point,n)
        pn_1 (1:nb_point)=pn   (1:nb_point)
        dpn_1(1:nb_point)=dpn  (1:nb_point,n)
      enddo
  end select
enddo

end subroutine polyleg


      SUBROUTINE TB01UD( JOBZ, N, M, P, A, LDA, B, LDB, C, LDC, NCONT,
     $                   INDCON, NBLK, Z, LDZ, TAU, TOL, IWORK, DWORK,
     $                   LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To find a controllable realization for the linear time-invariant
C     multi-input system
C
C             dX/dt = A * X + B * U,
C                Y  = C * X,
C
C     where A, B, and C are N-by-N, N-by-M, and P-by-N matrices,
C     respectively, and A and B are reduced by this routine to
C     orthogonal canonical form using (and optionally accumulating)
C     orthogonal similarity transformations, which are also applied
C     to C.  Specifically, the system (A, B, C) is reduced to the
C     triplet (Ac, Bc, Cc), where Ac = Z' * A * Z, Bc = Z' * B,
C     Cc = C * Z,  with
C
C             [ Acont     *    ]         [ Bcont ]
C        Ac = [                ],   Bc = [       ],
C             [   0    Auncont ]         [   0   ]
C
C        and
C
C                [ A11 A12  . . .  A1,p-1 A1p ]         [ B1 ]
C                [ A21 A22  . . .  A2,p-1 A2p ]         [ 0  ]
C                [  0  A32  . . .  A3,p-1 A3p ]         [ 0  ]
C        Acont = [  .   .   . . .    .     .  ],   Bc = [ .  ],
C                [  .   .     . .    .     .  ]         [ .  ]
C                [  .   .       .    .     .  ]         [ .  ]
C                [  0   0   . . .  Ap,p-1 App ]         [ 0  ]
C
C     where the blocks  B1, A21, ..., Ap,p-1  have full row ranks and
C     p is the controllability index of the pair.  The size of the
C     block  Auncont is equal to the dimension of the uncontrollable
C     subspace of the pair (A, B).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBZ    CHARACTER*1
C             Indicates whether the user wishes to accumulate in a
C             matrix Z the orthogonal similarity transformations for
C             reducing the system, as follows:
C             = 'N':  Do not form Z and do not store the orthogonal
C                     transformations;
C             = 'F':  Do not form Z, but store the orthogonal
C                     transformations in the factored form;
C             = 'I':  Z is initialized to the unit matrix and the
C                     orthogonal transformation matrix Z is returned.
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, or of columns of B.  M >= 0.
C
C     P       (input) INTEGER
C             The number of system outputs, or of rows of C.  P >= 0.
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 original state dynamics matrix A.
C             On exit, the leading NCONT-by-NCONT part contains the
C             upper block Hessenberg state dynamics matrix Acont in Ac,
C             given by Z' * A * Z, of a controllable realization for
C             the original system. The elements below the first block-
C             subdiagonal are set to zero. The leading N-by-N part
C             contains the matrix Ac.
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 input matrix B.
C             On exit, the leading NCONT-by-M part of this array
C             contains the transformed input matrix Bcont in Bc, given
C             by Z' * B, with all elements but the first block set to
C             zero. The leading N-by-M part contains the matrix Bc.
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 output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed output matrix Cc, given by C * Z.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     NCONT   (output) INTEGER
C             The order of the controllable state-space representation.
C
C     INDCON  (output) INTEGER
C             The controllability index of the controllable part of the
C             system representation.
C
C     NBLK    (output) INTEGER array, dimension (N)
C             The leading INDCON elements of this array contain the
C             the orders of the diagonal blocks of Acont.
C
C     Z       (output) DOUBLE PRECISION array, dimension (LDZ,N)
C             If JOBZ = 'I', then the leading N-by-N part of this
C             array contains the matrix of accumulated orthogonal
C             similarity transformations which reduces the given system
C             to orthogonal canonical form.
C             If JOBZ = 'F', the elements below the diagonal, with the
C             array TAU, represent the orthogonal transformation matrix
C             as a product of elementary reflectors. The transformation
C             matrix can then be obtained by calling the LAPACK Library
C             routine DORGQR.
C             If JOBZ = 'N', the array Z is not referenced and can be
C             supplied as a dummy array (i.e. set parameter LDZ = 1 and
C             declare this array to be Z(1,1) in the calling program).
C
C     LDZ     INTEGER
C             The leading dimension of array Z. If JOBZ = 'I' or
C             JOBZ = 'F', LDZ >= MAX(1,N); if JOBZ = 'N', LDZ >= 1.
C
C     TAU     (output) DOUBLE PRECISION array, dimension (N)
C             The elements of TAU contain the scalar factors of the
C             elementary reflectors used in the reduction of B and A.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determination when
C             transforming (A, B). If the user sets TOL > 0, then
C             the given value of TOL is used as a lower bound for the
C             reciprocal condition number (see the description of the
C             argument RCOND in the SLICOT routine MB03OD);  a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by  TOLDEF = N*N*EPS,  is used instead, where EPS
C             is the machine precision (see LAPACK Library routine
C             DLAMCH).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (M)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(1, N, 3*M, P).
C             For optimum performance LDWORK should be larger.
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
C     METHOD
C
C     Matrix B is first QR-decomposed and the appropriate orthogonal
C     similarity transformation applied to the matrix A. Leaving the
C     first rank(B) states unchanged, the remaining lower left block
C     of A is then QR-decomposed and the new orthogonal matrix, Q1,
C     is also applied to the right of A to complete the similarity
C     transformation. By continuing in this manner, a completely
C     controllable state-space pair (Acont, Bcont) is found for the
C     given (A, B), where Acont is upper block Hessenberg with each
C     subdiagonal block of full row rank, and Bcont is zero apart from
C     its (independent) first rank(B) rows.
C     All orthogonal transformations determined in this process are also
C     applied to the matrix C, from the right.
C     NOTE that the system controllability indices are easily
C     calculated from the dimensions of the blocks of Acont.
C
C     REFERENCES
C
C     [1] Konstantinov, M.M., Petkov, P.Hr. and Christov, N.D.
C         Orthogonal Invariants and Canonical Forms for Linear
C         Controllable Systems.
C         Proc. 8th IFAC World Congress, Kyoto, 1, pp. 49-54, 1981.
C
C     [2] Paige, C.C.
C         Properties of numerical algorithms related to computing
C         controllablity.
C         IEEE Trans. Auto. Contr., AC-26, pp. 130-138, 1981.
C
C     [3] Petkov, P.Hr., Konstantinov, M.M., Gu, D.W. and
C         Postlethwaite, I.
C         Optimal Pole Assignment Design of Linear Multi-Input Systems.
C         Leicester University, Report 99-11, May 1996.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations and is backward stable.
C
C     FURTHER COMMENTS
C
C     If the system matrices A and B are badly scaled, it would be
C     useful to scale them with SLICOT routine TB01ID, before calling
C     the routine.
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1998.
C
C     REVISIONS
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, May 1999, Nov. 2003,
C     Mar. 2017.
C     A. Varga, DLR Oberpfaffenhofen, March 2002, Nov. 2003.
C
C     KEYWORDS
C
C     Controllability, minimal realization, orthogonal canonical form,
C     orthogonal transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOBZ
      INTEGER           INDCON, INFO, LDA, LDB, LDC, LDWORK, LDZ, M, N,
     $                  NCONT, P
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*), TAU(*),
     $                  Z(LDZ,*)
      INTEGER           IWORK(*), NBLK(*)
C     .. Local Scalars ..
      LOGICAL           LJOBF, LJOBI, LJOBZ
      INTEGER           IQR, ITAU, J, MCRT, NBL, NCRT, NI, NJ, RANK,
     $                  WRKOPT
      DOUBLE PRECISION  ANORM, BNORM, FNRM, TOLDEF
C     .. Local Arrays ..
      DOUBLE PRECISION  SVAL(3)
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANGE
      EXTERNAL          DLAMCH, DLANGE, LSAME
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DLACPY, DLAPMT, DLASET, DORGQR, DORMQR,
     $                  MB01PD, MB03OY, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, INT, MAX, MIN
C     ..
C     .. Executable Statements ..
C
      INFO = 0
      LJOBF = LSAME( JOBZ, 'F' )
      LJOBI = LSAME( JOBZ, 'I' )
      LJOBZ = LJOBF.OR.LJOBI
C
C     Test the input scalar arguments.
C
      IF( .NOT.LJOBZ .AND. .NOT.LSAME( JOBZ, 'N' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( P.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -10
      ELSE IF( .NOT.LJOBZ .AND. LDZ.LT.1 .OR.
     $              LJOBZ .AND. LDZ.LT.MAX( 1, N ) ) THEN
         INFO = -15
      ELSE IF(  LDWORK.LT.MAX( 1, N, 3*M, P ) ) THEN
         INFO = -20
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'TB01UD', -INFO )
         RETURN
      END IF
C
      NCONT  = 0
      INDCON = 0
C
C     Calculate the absolute norms of A and B (used for scaling).
C
      ANORM = DLANGE( 'M', N, N, A, LDA, DWORK )
      BNORM = DLANGE( 'M', N, M, B, LDB, DWORK )
C
C     Quick return if possible.
C
      IF ( MIN( N, M ).EQ.0 .OR. BNORM.EQ.ZERO ) THEN
         IF( N.GT.0 ) THEN
            IF ( LJOBI ) THEN
               CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ )
            ELSE IF ( LJOBF ) THEN
               CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ )
               CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N )
            END IF
         END IF
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Scale (if needed) the matrices A and B.
C
      CALL MB01PD( 'S', 'G', N, N, 0, 0, ANORM, 0, NBLK, A, LDA, INFO )
      CALL MB01PD( 'S', 'G', N, M, 0, 0, BNORM, 0, NBLK, B, LDB, INFO )
C
C     Compute the Frobenius norm of [ B  A ] (used for rank estimation).
C
      FNRM = DLANGE( 'F', N, M, B, LDB, DWORK )
C
      TOLDEF = TOL
      IF ( TOLDEF.LE.ZERO ) THEN
C
C        Use the default tolerance in controllability determination.
C
         TOLDEF = DBLE( N*N )*DLAMCH( 'Precision' )
      END IF
C
      IF ( FNRM.LT.TOLDEF )
     $   FNRM = ONE
C
      WRKOPT = 1
      NI = 0
      ITAU = 1
      NCRT = N
      MCRT = M
      IQR  = 1
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
   10 CONTINUE
C
C        Rank-revealing QR decomposition with column pivoting.
C        The calculation is performed in NCRT rows of B starting from
C        the row IQR (initialized to 1 and then set to rank(B)+1).
C        Workspace: 3*MCRT.
C
         CALL MB03OY( NCRT, MCRT, B(IQR,1), LDB, TOLDEF, FNRM, RANK,
     $                SVAL, IWORK, TAU(ITAU), DWORK, INFO )
C
         IF ( RANK.NE.0 ) THEN
            NJ = NI
            NI = NCONT
            NCONT = NCONT + RANK
            INDCON = INDCON + 1
            NBLK(INDCON) = RANK
C
C           Premultiply and postmultiply the appropriate block row
C           and block column of A by Q' and Q, respectively.
C           Workspace: need   NCRT;
C                      prefer NCRT*NB.
C
            CALL DORMQR( 'Left', 'Transpose', NCRT, NCRT, RANK,
     $                   B(IQR,1), LDB, TAU(ITAU), A(NI+1,NI+1), LDA,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           Workspace: need   N;
C                      prefer N*NB.
C
            CALL DORMQR( 'Right', 'No transpose', N, NCRT, RANK,
     $                   B(IQR,1), LDB, TAU(ITAU), A(1,NI+1), LDA,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           Postmultiply the appropriate block column of C by Q.
C           Workspace: need   P;
C                      prefer P*NB.
C
            CALL DORMQR( 'Right', 'No transpose', P, NCRT, RANK,
     $                   B(IQR,1), LDB, TAU(ITAU), C(1,NI+1), LDC,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           If required, save transformations.
C
            IF ( LJOBZ.AND.NCRT.GT.1 ) THEN
               CALL DLACPY( 'L', NCRT-1, MIN( RANK, NCRT-1 ),
     $                      B(IQR+1,1), LDB, Z(NI+2,ITAU), LDZ )
            END IF
C
C           Zero the subdiagonal elements of the current matrix.
C
            IF ( RANK.GT.1 )
     $         CALL DLASET( 'L', RANK-1, RANK-1, ZERO, ZERO, B(IQR+1,1),
     $                      LDB )
C
C           Backward permutation of the columns of B or A.
C
            IF ( INDCON.EQ.1 ) THEN
               CALL DLAPMT( .FALSE., RANK, M, B(IQR,1), LDB, IWORK )
               IQR = RANK + 1
               FNRM = DLANGE( 'F', N, N, A, LDA, DWORK )
            ELSE
               DO 20 J = 1, MCRT
                  CALL DCOPY( RANK, B(IQR,J), 1, A(NI+1,NJ+IWORK(J)),
     $                        1 )
   20          CONTINUE
            END IF
C
            ITAU = ITAU + RANK
            IF ( RANK.NE.NCRT ) THEN
               MCRT = RANK
               NCRT = NCRT - RANK
               CALL DLACPY( 'G', NCRT, MCRT, A(NCONT+1,NI+1), LDA,
     $                      B(IQR,1), LDB )
               CALL DLASET( 'G', NCRT, MCRT, ZERO, ZERO,
     $                      A(NCONT+1,NI+1), LDA )
               GO TO 10
            END IF
         END IF
C
C     If required, accumulate transformations.
C     Workspace: need N;  prefer N*NB.
C
      IF ( LJOBI ) THEN
         CALL DORGQR( N, N, ITAU-1, Z, LDZ, TAU, DWORK,
     $                LDWORK, INFO )
         WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
      END IF
C
C     Annihilate the trailing blocks of B.
C
      IF( IQR.LE.N )
     $   CALL DLASET( 'G', N-IQR+1, M, ZERO, ZERO, B(IQR,1), LDB )
C
C     Annihilate the trailing elements of TAU, if JOBZ = 'F'.
C
      IF ( LJOBF ) THEN
         DO 30 J = ITAU, N
            TAU(J) = ZERO
   30    CONTINUE
      END IF
C
C     Undo scaling of A and B.
C
      IF ( INDCON.LT.N ) THEN
         NBL = INDCON + 1
         NBLK(NBL) = N - NCONT
      ELSE
         NBL = 0
      END IF
      CALL MB01PD( 'U', 'H', N, N, 0, 0, ANORM, NBL, NBLK, A, LDA,
     $             INFO )
      CALL MB01PD( 'U', 'G', NBLK(1), M, 0, 0, BNORM, 0, NBLK, B, LDB,
     $             INFO )
C
C     Set optimal workspace dimension.
C
      DWORK(1) = WRKOPT
      RETURN
C *** Last line of TB01UD ***
      END


!  Copyright (C) 2002 Regents of the University of Michigan, portions used with permission
!  For more information, see http://csem.engin.umich.edu/tools/swmf
module ModConstants

!-------------------------------------------------------------------
! Useful physical constants
!-------------------------------------------------------------------

  real, parameter :: Avogadros_Number       = 6.02e23               ! mol./mole
  real, parameter :: AMU                    = 1.6726e-27            ! Kg
  real, parameter :: Mass_Ion               = 1.6726e-27            ! Kg
  real, parameter :: Mass_Proton            = Mass_Ion              ! Kg
  real, parameter :: Mass_Electron          = 9.1094e-31            ! Kg
  real, parameter :: Boltzmanns_Constant    = 1.3807e-23            ! J/K
  real, parameter :: Planck_Constant        = 6.6261e-34            ! Js
  real, parameter :: Element_Charge         = 1.6022e-19            ! C (J/eV)
  real, parameter :: Speed_Light            = 2.9979e8              ! m/s
  real, parameter :: Univ_Gas_Constant      = Avogadros_Number*   &
                                              Boltzmanns_Constant   ! J/(moleK)
  real, parameter :: RGAS                   = Univ_Gas_Constant*   &
                                              1.0E+07               !erg/(moleK)
  real, parameter :: pi                     = 3.141592653589793
  real, parameter :: twopi = 2*pi

  real, parameter :: P00                    = 1.0e5                   ! Pa
  real, parameter :: T00                    = 273.0                   ! K

  real, parameter :: SpecificHeatVolume     = 3./2.
  real, parameter :: SpecificHeatPressure   = SpecificHeatVolume + 1.0
  real, parameter :: Gamma_const                  = &
       SpecificHeatPressure/SpecificHeatVolume

end module ModConstants


MODULE volterra

! Subroutines for volterra integral equations using dsplines

USE dspline

USE SpecFuns

IMPLICIT NONE

CONTAINS

  SUBROUTINE buildcofs(q,nnodes,tspan,Ker,m,dt,c)
!
! Build the coefficients of the dspline implicit time stepping
! scheme for the Volterra equation
!
! u(t)+int_0^tspan Ker(s)*u(t-s) ds = f(t)
!
! Assuming u,f=0 t <= -tspan
!
! Time integral using Gaussian quadrature with nnodes
!
  INTEGER, INTENT(IN) :: q,nnodes,m  ! Choose (m-q-1)*dt = tspan
  DOUBLE PRECISION, INTENT(IN) :: tspan
  DOUBLE PRECISION, DIMENSION(0:m), INTENT(OUT) :: c
  DOUBLE PRECISION, INTENT(OUT) :: dt
  INTERFACE
    DOUBLE PRECISION FUNCTION Ker(t)
      DOUBLE PRECISION, INTENT(IN) :: t
    END FUNCTION Ker
  END INTERFACE
!
  DOUBLE PRECISION, DIMENSION(0:nnodes) :: s,w,uwgts
  DOUBLE PRECISION, DIMENSION(nnodes+1) :: z
  DOUBLE PRECISION, DIMENSION(:,:), ALLOCATABLE :: umat,upmat
  INTEGER :: jmax,jmin,j,jr,k
!
  dt=tspan/DBLE(m-q-1)
  CALL GaussQCofs(s,w,nnodes)
  s=.5d0*tspan*(s+1.d0)
  w=.5d0*tspan*w
!
! Compute weights
!
  DO j=0,nnodes
    jr=nnodes-j
    uwgts(jr)=Ker(s(j))*w(j)
    z(jr+1)=-s(j)
  END DO
!
! Compute interpolation matrix
!
  CALL InterpMat(nnodes+1,z,dt,2*q,jmax,jmin,umat,upmat)
!
  c=0.d0
  DO j=1,jmax-jmin+1
    jr=j+m+jmin-jmax-1
    DO k=0,nnodes
      c(jr)=c(jr)+umat(k+1,j)*uwgts(k)
    END DO
  END DO
  c(m)=c(m)+1.d0
!
!  DO j=0,m
!    PRINT *,j,c(j)
!  END DO
  DEALLOCATE(umat,upmat)
!
  END SUBROUTINE BuildCofs
!
END MODULE volterra


      PROGRAM xqrsolv
C     driver for routine qrsolv
      INTEGER NP
      PARAMETER (NP=20)
      REAL a(NP,NP),ai(NP,NP),b(NP,NP),c(NP),d(NP),x(NP)
      INTEGER j,k,l,m,n
      CHARACTER txt*3
      LOGICAL sing
      open(7,file='MATRX1.DAT',status='old')
      read(7,*)
10    read(7,*)
      read(7,*) n,m
      read(7,*)
      read(7,*) ((a(k,l), l=1,n), k=1,n)
      read(7,*)
      read(7,*) ((b(k,l), k=1,n), l=1,m)
C     save matrix a for later testing
      do 12 l=1,n
        do 11 k=1,n
          ai(k,l)=a(k,l)
11      continue
12    continue
C     do qr decomposition
      call qrdcmp(a,n,NP,c,d,sing)
      if (sing) write(*,*) 'Singularity in QR decomposition.'
C     solve equations for each right-hand vector
      do 16 k=1,m
        do 13 l=1,n
          x(l)=b(l,k)
13      continue
        call qrsolv(a,n,NP,c,d,x)
C     test results with original matrix
        write(*,*) 'Right-hand side vector:'
        write(*,'(1x,6f12.6)') (b(l,k), l=1,n)
        write(*,*) 'Result of matrix applied to sol''n vector'
        do 15 l=1,n
          b(l,k)=0.0
          do 14 j=1,n
            b(l,k)=b(l,k)+ai(l,j)*x(j)
14        continue
15      continue
        write(*,'(1x,6f12.6)') (b(l,k), l=1,n)
        write(*,*) '***********************************'
16    continue
      write(*,*) 'Press RETURN for next problem:'
      read(*,*)
      read(7,'(a3)') txt
      if (txt.ne.'END') goto 10
      close(7)
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xqrsolv.for


! fortran_free_source
!
!   This module is part of Perfcode, written by Bradley J Eck
!
MODULE geom_funcs
implicit none
contains
!
!   1. Function  F_L_xi
!   2. Function unmap_x
!   3. Function unmap_y

!===========================================================================
!   \\\\\\\\\\     B E G I N       F U N C T I O N       //////////
!   //////////             F _ L _ X I                   \\\\\\\\\\
!===========================================================================
Function F_L_xi(xi, eta, seg) Result(L_xi)
!   Computes the METRIC COEFFICIENT for the length mapping.
!Function F_length_xi(xi, eta, xcc1, ycc1, dx, dy, R1, dR, W, theta1, dtheta)
! GEOMETRY MAPPING FUNCTIONS from Geometry.xlsb
use shared, only: CLSEG
implicit none
!   Arguments
real xi, eta
type(CLSEG) :: seg
!   Result
real L_xi
!   Internal Variables
real angle, dx_dxi, dy_dxi
real xcc1, ycc1, dx, dy, R1, dR, W, theta1, dtheta
!------------------------------------------------------------------------
! Assign parts of the derived type to local variables
!  to keep the formulas cleaner
xcc1   = seg%xcc1
ycc1   = seg%ycc1
dx     = seg%dx
dy     = seg%dy
R1     = seg%R1
dR     = seg%dR
W      = seg%W
theta1 = seg%theta1
dtheta = seg%dtheta

! compute intermediate variables
Angle = theta1 + xi * dtheta

dx_dxi = dx + dR * Cos(Angle) - dtheta * Sin(Angle) * &
         (R1 + W * (eta - 0.5) + xi * dR)

dy_dxi = dx + dR * Sin(Angle) + dtheta * Cos(Angle) * &
         (R1 + W * (eta - 0.5) + xi * dR)

! Calculate metric coefficient
L_xi = sqrt( (dx_dxi ** 2 + dy_dxi ** 2) )
!---------------------------------------------------------------------------
End Function F_L_xi
!===========================================================================
!   \\\\\\\\\\     E N D           F U N C T I O N       //////////
!   //////////             F _ L _ X I                   \\\\\\\\\\
!===========================================================================

!===========================================================================
!   \\\\\\\\\\     B E G I N       F U N C T I O N       //////////
!   //////////             U N M A P _ X                 \\\\\\\\\\
!===========================================================================
Function unmap_x(xi, eta, seg) Result( X )
!   Calculates the physical X coordinate of a point in the grid based on
!       its xi and eta coordinates.
use shared, only: CLSEG
implicit none
!   Arguments
real xi, eta
type(CLSEG) :: seg
!   Result
real X
!   Internal Variables
real xcc1, dx, R1, dR, W, theta1, dtheta
!------------------------------------------------------------------------
! Assign parts of the derived type to local variables
!  to keep the formulas cleaner
xcc1   = seg%xcc1
dx     = seg%dx
R1     = seg%R1
dR     = seg%dR
W      = seg%W
theta1 = seg%theta1
dtheta = seg%dtheta

! Compute the X coordinate
X = (xcc1 + xi * dx) + &
          (R1 + xi * dR + (eta - 0.5) * W) * Cos(theta1 + xi * dtheta)

!------------------------------------------------------------------------
end function unmap_x
!===========================================================================
!   \\\\\\\\\\     E N D           F U N C T I O N       //////////
!   //////////             U N M A P _ X                 \\\\\\\\\\
!===========================================================================

!===========================================================================
!   \\\\\\\\\\     B E G I N       F U N C T I O N       //////////
!   //////////             U N M A P _ Y                 \\\\\\\\\\
!===========================================================================
Function unmap_y(xi, eta, seg) result( Y )
!   Calculates the physical Y coordinate of a point on the grid
!       from its computational xi,eta coordinate
use shared, only: CLSEG
implicit none
real xi, eta
type(CLSEG) :: seg
!   Result
real Y
!   Internal Variables
real ycc1, dy, R1, dR, W, theta1, dtheta
!------------------------------------------------------------------------
! Assign parts of the derived type to local variables
!  to keep the formulas cleaner
ycc1   = seg%ycc1
dy     = seg%dy
R1     = seg%R1
dR     = seg%dR
W      = seg%W
theta1 = seg%theta1
dtheta = seg%dtheta

Y = (ycc1 + xi * dy) + &
          (R1 + xi * dR + (eta - 0.5) * W) * Sin(theta1 + xi * dtheta)

!------------------------------------------------------------------------
end function unmap_y
!===========================================================================
!   \\\\\\\\\\     E N D           F U N C T I O N       //////////
!   //////////             U N M A P _ Y                 \\\\\\\\\\
!===========================================================================

END MODULE geom_funcs


! ----------------------------------------------------------------------
!
!     this subroutine computes the local energy and potential energy
!     of a configuration.
!
! ----------------------------------------------------------------------

      subroutine local(ndim,ntraj,x, env, dv)

      implicit double precision (a-h, o-z)

      real (kind = 8) x(ndim,ntraj)

      real*8, intent(out) :: env(ntraj),dv(ndim,ntraj)

      ak1 = 5.0
      ak2 = 15.0
      b = 0.5d0
      v0 = 16d0
      g = 1.3624d0

      do i=1,ntraj

      ak = 0.5*(ak1+ak2)+0.5*(ak2-ak1)*tanh(b*x(2,i))

      env(i) = 0.5*ak*x(1,i)**2 + v0/cosh(g*x(2,i))**2

      dv(1,i) = ak*x(1,i)
      dv(2,i) = x(1,i)**2*(ak2-ak1)*(1d0-tanh(b*x(2,i))**2)*b/4d0- &
                2d0*v0*g*sinh(g*x(2,i))/cosh(g*x(2,i))**3
      enddo

      return
      end subroutine