! chk_gamma.f90 --
!     Check if the compiler support the new Gamma and log Gamma functions
!
!     Note: there is a whole list of such special functions defined for the Fortran 2008 standard, not
!     all of them are tested here
!
program chk_gamma
    implicit none

    real, dimension(11) :: x = (/ -2.5, -1.5, -0.5, 0.1, 0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 6.0 /)
    integer             :: i, n

    write( *, '(a)' ) 'Selected values for the Gamma and log Gamma functions:'

    write( *, '(5a12)' ) 'x', 'Gamma', 'log Gamma'
    do i = 1,size(x)
        write( *, '(3f12.4)' ) x(i), gamma(x(i)), log_gamma(x(i))
    enddo
end program chk_gamma


module special

! Special functions

use types, only: dp
use constants, only: pi
use utils, only: stop_error
use optimize, only: bisect
implicit none
private
public spherical_bessel_jn, spherical_bessel_jn_zeros, SPHY

contains

real(dp) function spherical_bessel_jn(n, x) result(r)
integer, intent(in) :: n
real(dp), intent(in) :: x
integer :: nm
real(dp) :: sj(0:n), dj(0:n)
call sphj(n, x, nm, sj, dj)
if (nm /= n) call stop_error("spherical_bessel_jn: sphj didn't converge")
r = sj(n)
end function

function spherical_bessel_jn_zeros(nmax, nzeros, eps) result(zeros)
! Calculates 'nzeros' zeros of spherical_bessel_jn() for all n=0, 1, ..., nmax
! It uses the fact that zeros of j_{n-1}(x) lie between zeros of j_n(x) and
! uses bisection to calculate them.
integer, intent(in) :: nmax, nzeros
real(dp), intent(in) :: eps
! zeros(i, n) is the i-th zero of spherical_bessel_jn(n, x)
real(dp) :: zeros(nzeros, 0:nmax)
! points holds all zeros of j_{n-1} needed for j_n
real(dp) :: points(nmax+nzeros)
integer :: n, j
points = [ (n*pi, n = 1, nmax + nzeros) ]
zeros(:, 0) = points(:nzeros)
do n = 1, nmax
    do j = 1, nmax + nzeros - n
        points(j) = bisect(spherical_bessel_jn, points(j), points(j+1), eps, n)
    end do
    zeros(:, n) = points(:nzeros)
end do

end function

! The SPHJ, SPHY, MSTA1, MSTA2 routines below are taken from SciPy's specfun.f.
! Authors: Shanjie Zhang and Jianming Jin
! Copyrighted but permission granted to use code in programs.

        SUBROUTINE SPHJ(N,X,NM,SJ,DJ)
!       =======================================================
!       Purpose: Compute spherical Bessel functions jn(x) and
!                their derivatives
!       Input :  x --- Argument of jn(x)
!                n --- Order of jn(x)  ( n = 0,1,… )
!       Output:  SJ(n) --- jn(x)
!                DJ(n) --- jn'(x)
!                NM --- Highest order computed
!       Routines called:
!                MSTA1 and MSTA2 for computing the starting
!                point for backward recurrence
!       =======================================================
!
        IMPLICIT DOUBLE PRECISION (A-H,O-Z)
        IMPLICIT integer (I-N)
        DIMENSION SJ(0:N),DJ(0:N)
        NM=N
        IF (DABS(X).LT.1.0D-100) THEN
           DO 10 K=0,N
              SJ(K)=0.0D0
              DJ(K)=0.0D0
10         END DO
           SJ(0)=1.0D0
           IF (N.GT.0) THEN
              DJ(1)=.3333333333333333D0
           ENDIF
           RETURN
        ENDIF
        SJ(0)=DSIN(X)/X
        DJ(0)=(DCOS(X)-DSIN(X)/X)/X
        IF (N.LT.1) THEN
           RETURN
        ENDIF
        SJ(1)=(SJ(0)-DCOS(X))/X
        IF (N.GE.2) THEN
           SA=SJ(0)
           SB=SJ(1)
           M=MSTA1(X,200)
           IF (M.LT.N) THEN
              NM=M
           ELSE
              M=MSTA2(X,N,15)
           ENDIF
           F=0.0D0
           F0=0.0D0
           F1=1.0D0-100
           DO 15 K=M,0,-1
              F=(2.0D0*K+3.0D0)*F1/X-F0
              IF (K.LE.NM) SJ(K)=F
              F0=F1
              F1=F
15         END DO
           CS=0.0D0
           IF (DABS(SA).GT.DABS(SB)) CS=SA/F
           IF (DABS(SA).LE.DABS(SB)) CS=SB/F0
           DO 20 K=0,NM
              SJ(K)=CS*SJ(K)
20         END DO
        ENDIF
        DO 25 K=1,NM
           DJ(K)=SJ(K-1)-(K+1.0D0)*SJ(K)/X
25      END DO
        RETURN
        END SUBROUTINE

        SUBROUTINE SPHY(N,X,NM,SY,DY)
!       ======================================================
!       Purpose: Compute spherical Bessel functions yn(x) and
!                their derivatives
!       Input :  x --- Argument of yn(x) ( x ≥ 0 )
!                n --- Order of yn(x) ( n = 0,1,… )
!       Output:  SY(n) --- yn(x)
!                DY(n) --- yn'(x)
!                NM --- Highest order computed
!       ======================================================
!
        IMPLICIT DOUBLE PRECISION (A-H,O-Z)
        IMPLICIT integer (I-N)
        DIMENSION SY(0:N),DY(0:N)
        NM=N
        IF (X.LT.1.0D-60) THEN
           DO 10 K=0,N
              SY(K)=-1.0D+300
              DY(K)=1.0D+300
10         END DO
           RETURN
        ENDIF
        SY(0)=-DCOS(X)/X
        F0=SY(0)
        DY(0)=(DSIN(X)+DCOS(X)/X)/X
        IF (N.LT.1) THEN
           RETURN
        ENDIF
        SY(1)=(SY(0)-DSIN(X))/X
        F1=SY(1)
        DO 15 K=2,N
           F=(2.0D0*K-1.0D0)*F1/X-F0
           SY(K)=F
           IF (DABS(F).GE.1.0D+300) GO TO 20
           F0=F1
           F1=F
15      END DO
20      NM=K-1
        DO 25 K=1,NM
           DY(K)=SY(K-1)-(K+1.0D0)*SY(K)/X
25      END DO
        RETURN
        END SUBROUTINE

        INTEGER FUNCTION MSTA1(X,MP)
!       ===================================================
!       Purpose: Determine the starting point for backward
!                recurrence such that the magnitude of
!                Jn(x) at that point is about 10^(-MP)
!       Input :  x     --- Argument of Jn(x)
!                MP    --- Value of magnitude
!       Output:  MSTA1 --- Starting point
!       ===================================================
!
        IMPLICIT DOUBLE PRECISION (A-H,O-Z)
        IMPLICIT integer (I-N)
        A0=DABS(X)
        N0=INT(1.1D0*A0)+1
        F0=ENVJ(N0,A0)-MP
        N1=N0+5
        F1=ENVJ(N1,A0)-MP
        DO 10 IT=1,20
           NN=int(N1-(N1-N0)/(1.0D0-F0/F1))
           F=ENVJ(NN,A0)-MP
           IF(ABS(NN-N1).LT.1) GO TO 20
           N0=N1
           F0=F1
           N1=NN
           F1=F
 10     END DO
 20     MSTA1=NN
        RETURN
        END FUNCTION

        INTEGER FUNCTION MSTA2(X,N,MP)
!       ===================================================
!       Purpose: Determine the starting point for backward
!                recurrence such that all Jn(x) has MP
!                significant digits
!       Input :  x  --- Argument of Jn(x)
!                n  --- Order of Jn(x)
!                MP --- Significant digit
!       Output:  MSTA2 --- Starting point
!       ===================================================
!
        IMPLICIT DOUBLE PRECISION (A-H,O-Z)
        IMPLICIT integer (I-N)
        A0=DABS(X)
        HMP=0.5D0*MP
        EJN=ENVJ(N,A0)
        IF (EJN.LE.HMP) THEN
           OBJ=MP
           N0=INT(1.1D0*A0)+1
        ELSE
           OBJ=HMP+EJN
           N0=N
        ENDIF
        F0=ENVJ(N0,A0)-OBJ
        N1=N0+5
        F1=ENVJ(N1,A0)-OBJ
        DO 10 IT=1,20
           NN=int(N1-(N1-N0)/(1.0D0-F0/F1))
           F=ENVJ(NN,A0)-OBJ
           IF (ABS(NN-N1).LT.1) GO TO 20
           N0=N1
           F0=F1
           N1=NN
           F1=F
10      END DO
20      MSTA2=NN+10
        RETURN
        END FUNCTION

        real(dp) function envj(n, x) result(r)
        integer, intent(in) :: n
        real(dp), intent(in) :: x
        r = log10(6.28_dp*n)/2 - n*log10(1.36_dp*x/n)
        end function

end module


!-------------------------------------------------------------------------------
! Copyright (c) 2019 FrontISTR Commons
! This software is released under the MIT License, see LICENSE.txt
!-------------------------------------------------------------------------------
!>  \brief   This module provides interface for elastic or hyperelastic
!>  calculation
module mUElastic
  implicit none
contains

  !> This subroutine calculates constitutive relation
  subroutine uElasticMatrix( matl, strain, D )
    use hecmw
    real(kind=kreal), intent(in)  :: matl(:)   !< material properties
    real(kind=kreal), intent(in)  :: strain(6) !< Green-Lagrangen strain
    real(kind=kreal), intent(out) :: D(6,6)    !< constitutive matrix

    ! following examples of linear elasticicty
    real(kind=kreal) :: EE, PP

    D(:,:)=0.d0

    EE = matl(1)
    PP = matl(2)
    D(1,1)=EE*(1.d0-PP)/(1.d0-2.d0*PP)/(1.d0+PP)
    D(1,2)=EE*PP/(1.d0-2.d0*PP)/(1.d0+PP)
    D(1,3)=D(1,2)
    D(2,1)=D(1,2)
    D(2,2)=D(1,1)
    D(2,3)=D(1,2)
    D(3,1)=D(1,3)
    D(3,2)=D(2,3)
    D(3,3)=D(1,1)
    D(4,4)=EE/(1.d0+PP)*0.5d0
    D(5,5)=EE/(1.d0+PP)*0.5d0
    D(6,6)=EE/(1.d0+PP)*0.5d0
  end subroutine

  !> This subroutine calculate updated strain and stress
  subroutine uElasticUpdate( matl, strain, stress )
    use hecmw
    real(kind=kreal), intent(in)  :: matl(:)   !< material properties
    real(kind=kreal), intent(in)  :: strain(6) !< strain
    real(kind=kreal), intent(out) :: stress(6) !< stress

    ! following examples of linear elasticicty
    real(kind=kreal) :: D(6,6)
    call uElasticMatrix( matl(:), strain, D )
    stress = matmul( D, strain )
  end subroutine

end module


!--------------------------------------------------------------------------------
! 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_cylpts
      CONTAINS
      SUBROUTINE cylpts(x,n,rad)
c     *********************************************************
c     YM: generate random points on the cylindrical vacuum boundary
c     *********************************************************
      USE m_constants, ONLY : tpi_const
      USE m_qranf
      IMPLICIT NONE

      INTEGER n
      REAL    rad
      REAL    x(3,n)
      REAL    phi,t,tc,y1,x1,xr,yr
      INTEGER r,j,i

      INTRINSIC sqrt

      yr = sqrt(7.)

      j = 0

      DO 10 i = 1,n
         x(3,i) = qranf(yr,j) - 0.5
   10 CONTINUE

      j = 0
      xr = sqrt(13.e0)
      yr = sqrt(7.e0)
      DO 20 i = 1,n
         tc = 2.e0*qranf(xr,j) - 1.e0
         phi = tpi_const*qranf(yr,j)
         t = sqrt(1.e0-tc*tc)
         x1 = cos(phi)
         y1 = sin(phi)
         x(1,i) = rad*x1
         x(2,i) = rad*y1
 20   CONTINUE

      END SUBROUTINE cylpts
      END MODULE m_cylpts


program calc_real
  implicit none   !forces the programmer to declare all variables
  real(8) :: w,l
  real(8) :: prmtr, area, diag1, diag2

  w = 3.0                    !Define a
  l = 4.0                    !Define b
  prmtr = 2.0*(w + l)        !Calculate the perimeter of rectangle
  area  = w*l              !Calculate the area of rectangle
  diag1  = sqrt(w**2.0+l**2.0) !Calculate diagonal
  diag2 =(w**2.0+l**2.0)**0.5  !Calculate diagonal

  print*, 'Perimeter = ',prmtr
  print*, 'Area = ' ,area
  print*, 'diag1 = ',diag1
  print*, 'diag2 = ',diag2

end program calc_real


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!   EVB-QMDFF - RPMD molecular dynamics and rate constant calculations on
!               black-box generated potential energy surfaces
!
!   Copyright (c) 2021 by Julien Steffen (steffen@pctc.uni-kiel.de)
!                         Stefan Grimme (grimme@thch.uni-bonn.de) (QMDFF code)
!
!   Permission is hereby granted, free of charge, to any person obtaining a
!   copy of this software and associated documentation files (the "Software"),
!   to deal in the Software without restriction, including without limitation
!   the rights to use, copy, modify, merge, publish, distribute, sublicense,
!   and/or sell copies of the Software, and to permit persons to whom the
!   Software is furnished to do so, subject to the following conditions:
!
!   The above copyright notice and this permission notice shall be included in
!   all copies or substantial portions of the Software.
!
!   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
!   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
!   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
!   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
!   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
!   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
!   DEALINGS IN THE SOFTWARE.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

!
!     subroutine lin_reg: Performs a linear regression for
!      a pair of data arrays (x and f(x) values).
!      It returns the slope, the y-intercept and the
!      correlation coefficient
!
!      part of EVB
!

subroutine lin_reg(x_data,y_data,npts,slope,y_inter,corre)

implicit none
!     Number of points in the given data arrays
integer,intent(in) :: npts
!     The x coordinates of the given data
real(kind=8), intent(in) :: x_data(npts)
!     The y coordinates of the given data
real(kind=8), intent(in) :: y_data(npts)
!     The slope of the y data
real(kind=8), intent(out) :: slope
!     The y-intercept for the y data
real(kind=8), intent(out) :: y_inter
!     The correlation coefficient of the given data
real(kind=8), intent(out) :: corre
!     squared correlation coefficient
real(kind=8) ::  r
!     intermediate sums for calculations
real(kind=8) :: sumx,sumy,sumx2,sumy2,sumxy
!     actual x and y coordinates
real(kind=8) :: x,y
!     loop index
integer :: i
!
!     Set default values for variables
!
slope=0.d0
y_inter=0.d0
corre=0.d0
sumx=0.d0
sumy=0.d0
sumx2=0.d0
sumy2=0.d0
sumxy=0.d0
!
!     Perform loop over all data pairs and calculate values
!
do i=1,npts
!
!     read out actual data from array
!
   x=x_data(i)
   y=y_data(i)
!
!     compute sum of x
!
   sumx  = sumx + x
!
!     compute sum of x**2
!
   sumx2 = sumx2 + x * x
!
!     compute sum of x * y
!
   sumxy = sumxy + x * y
!
!     compute sum of y
!
   sumy  = sumy + y
!
!     compute sum of y**2
!
   sumy2 = sumy2 + y * y
end do
!
!     compute slope
!
slope = (npts * sumxy  -  sumx * sumy) / (npts * sumx2 - sumx**2)
!
!     compute y-intercept
!
y_inter = (sumy * sumx2  -  sumx * sumxy) / (npts * sumx2  -  sumx**2)
!
!     compute correlation coefficient
!
corre = (sumxy-sumx*sumy/npts)/sqrt((sumx2-sumx**2/npts)*(sumy2-sumy**2/npts))

return
end subroutine lin_reg


submodule(forlab_math) forlab_math_is_close

contains

    elemental module function is_close_rsp(a, b, rel_tol, abs_tol) result(close)
        real(sp), intent(in) :: a, b
        real(sp), intent(in), optional :: rel_tol, abs_tol
        logical :: close

        close = abs(a - b) <= max(abs(optval(rel_tol, 1.0e-9_sp)*max(abs(a), abs(b))), &
                                  abs(optval(abs_tol, 0.0_sp)))

    end function is_close_rsp
    elemental module function is_close_rdp(a, b, rel_tol, abs_tol) result(close)
        real(dp), intent(in) :: a, b
        real(dp), intent(in), optional :: rel_tol, abs_tol
        logical :: close

        close = abs(a - b) <= max(abs(optval(rel_tol, 1.0e-9_dp)*max(abs(a), abs(b))), &
                                  abs(optval(abs_tol, 0.0_dp)))

    end function is_close_rdp

    elemental module function is_close_csp(a, b, rel_tol, abs_tol) result(close)
        complex(sp), intent(in) :: a, b
        real(sp), intent(in), optional :: rel_tol, abs_tol
        logical :: close

        close = is_close_rsp(a%re, b%re, rel_tol, abs_tol) .and. &
                is_close_rsp(a%im, b%im, rel_tol, abs_tol)

    end function is_close_csp
    elemental module function is_close_cdp(a, b, rel_tol, abs_tol) result(close)
        complex(dp), intent(in) :: a, b
        real(dp), intent(in), optional :: rel_tol, abs_tol
        logical :: close

        close = is_close_rdp(a%re, b%re, rel_tol, abs_tol) .and. &
                is_close_rdp(a%im, b%im, rel_tol, abs_tol)

    end function is_close_cdp

end submodule forlab_math_is_close


module test_math_signum

    use forlab_math, only: signum, is_close
    use stdlib_error, only: check

contains

    subroutine test_math_signum_real

        real :: x(3) = [-2.0, 0.0, 3.0]
        call check(all(is_close(signum(x), [-1.0, 0.0, 1.0])), "signum for `real` failed.")

    end subroutine test_math_signum_real

    subroutine test_math_signum_integer

        integer :: x(3) = [-2, 0, 3]
        call check(all(signum(x) == [-1, 0, 1]), "signum for `integer` failed.")

    end subroutine test_math_signum_integer

    subroutine test_math_signum_complex

        complex :: x(3) = [(-2.0, 1.0), (0.0, 0.0), (3.0, 4.0)]
        call check(all(is_close(signum(x), [(-0.894427180, 0.447213590), (0.0, 0.0), (0.600000024, 0.800000012)])), &
                   "signum for `complex` failed.")

    end subroutine test_math_signum_complex

end module test_math_signum

program tester

    use test_math_signum
    call test_math_signum_real
    call test_math_signum_integer
    call test_math_signum_complex
    print *, "All tests in `test_math_signum` passed."

end program tester


c     program DRDBLAS3
c>> 2001-05-22 DRDBLAS3 Krogh Minor change for making .f90 version.
c>> 1996-05-28 DRDBLAS3 Krogh Added external statement.
c>> 1994-10-19 DRDBLAS3 Krogh  Changes to use M77CON
c>> 1992-03-16 DRDBLAS3 CLL
c>> 1991-07-25 DRDBLAS3 CLL
c
c     Demonstrate usage of
c     DASUM, DNRM2, DSCAL, DSWAP, and IDAMAX
c     from the BLAS.
c     Also uses DAXPY.
c     ------------------------------------------------------------------
c--D replaces "?": DR?BLAS3, ?AXPY, ?ASUM, ?NRM2
c--&                 ?SCAL, ?SWAP, I?AMAX
c     ------------------------------------------------------------------
      external DASUM, DNRM2, IDAMAX
      double precision DASUM, DNRM2
      integer IDAMAX
      integer M3, M4, N3, N4
      parameter ( M3=10, M4=12 )
      parameter ( N3=3, N4=4 )
      integer IT1, J
      double precision E(M3,M4)
      double precision B(M3), C(5), C2(5), C3(5), D(5), D2(5), D3(5)
      double precision T1
c
      data (B(J),J=1,N3)   /  7.0d0, -3.0d0, 5.0d0 /
      data (C(J),J=1,5)   /  7.0d0, -3.0d0,  5.0d0, -4.0d0, 1.0d0 /
      data (C2(J),J=1,5)   /  7.0d0, -3.0d0,  5.0d0, -4.0d0, 1.0d0 /
      data (C3(J),J=1,5)   /  7.0d0, -3.0d0,  5.0d0, -4.0d0, 1.0d0 /
      data (D(J),J=1,5)   / 14.0d0, -6.0d0, 10.0d0, -8.0d0, 2.0d0 /
      data (D2(J),J=1,5)   / 14.0d0, -6.0d0, 10.0d0, -8.0d0, 2.0d0 /
      data (D3(J),J=1,5)   / 14.0d0, -6.0d0, 10.0d0, -8.0d0, 2.0d0 /

      data (E(1,J),J=1,N4) / -4.0d0, 2.0d0,  3.0d0, -6.0d0 /
      data (E(2,J),J=1,N4) /  7.0d0, 5.0d0, -6.0d0, -3.0d0 /
      data (E(3,J),J=1,N4) /  3.0d0, 4.0d0, -2.0d0,  5.0d0 /

c     ---------------------------------------------------------------
c
      T1 = DASUM(N3, B, 1) - 15.0d0
      print*,'Test of DASUM(): ',T1
c
      T1 = DNRM2(5, C, 1) - 10.0d0
      print*,'Test of DNRM2(): ',T1
c
      call DSCAL(5, 2.0d0, C2, 1)
      call DAXPY(5, -1.0d0, C2, 1, D2, 1)
      T1 = DASUM(5, D2, 1)
      print*,'Test of DSCAL(): ',T1
c
      call DSWAP(5, C3, 1, D3, 1)
      call DAXPY(5, -1.0d0, C, 1, D3, 1)
      call DAXPY(5, -1.0d0, D, 1, C3, 1)
      T1 = DASUM(5, C3, 1) + DASUM(5, D3, 1)
      print*,'Test of DSWAP(): ',T1
c
      IT1 = IDAMAX(N4, E(1,1), M3) - 4
      print*,'Test of IDAMAX(): ',IT1
      stop
      end


subroutine getBasisPtSurface(u, v, tu, tv, ku, kv, vals, col_ind, istart, l_index, nctlu, nctlv, nnz)
  use precision
  implicit none

  ! Input/Output
  integer            , intent(in)          :: ku, kv, nctlu, nctlv, nnz, istart
  real(kind=realType), intent(in)          :: u, v
  real(kind=realType), intent(in)          :: tu(nctlu+ku), tv(nctlv+kv)
  integer            , intent(in)          :: l_index(Nctlv, Nctlu)
  real(kind=realType), intent(inout)       :: vals(nnz)
  integer            , intent(inout)       :: col_ind(nnz)

  ! Working
  real(kind=realType)                      :: basisu(ku), basisv(kv)
  integer                                  :: ileftu, ileftv
  integer                                  :: ii, jj, counter, start

  ! Get u interval
  call findSpan(u, ku, tu, nctlu, ileftu)
  call basis(tu, nctlu, ku, u, ileftu, basisu)

  ! Get v interval
  call findSpan(v, kv, tv, nctlv, ileftv)
  call basis(tv, nctlv, kv, v, ileftv, basisv)

  counter = 0
  do ii=1, ku
     do jj = 1, kv
        ! Get the local row/col for this surface
        start = istart + counter + 1
        col_ind(start) = l_index(ileftv-kv+jj, ileftu-ku+ii)
        vals(start) = basisu(ii)*basisv(jj)
        counter = counter + 1
    end do
 end do
end subroutine getBasisPtSurface

subroutine getBasisPtVolume(u, v, w, tu, tv, tw, ku, kv, kw, vals, col_ind, istart, l_index, nctlu, nctlv, nctlw, nnz)
  use precision
  implicit none

  ! Input
  integer            , intent(in)          :: ku, kv, kw, nctlu, nctlv, nctlw, nnz, istart
  real(kind=realType), intent(in)          :: u, v, w
  real(kind=realType), intent(in)          :: tu(nctlu+ku), tv(nctlv+kv), tw(nctlw+kw)
  integer            , intent(in)          :: l_index(Nctlw, Nctlv, Nctlu)

  ! Output
  real(kind=realType), intent(inout)       :: vals(nnz)
  integer            , intent(inout)       :: col_ind(nnz)

  ! Working
  real(kind=realType)                      :: basisu(ku), basisv(kv), basisw(kw)
  integer                                  :: ileftu, ileftv, ileftw
  integer                                  :: ii, jj, kk, counter, start

  ! Get u interval
  call findSpan(u, ku, tu, nctlu, ileftu)
  call basis(tu, nctlu, ku, u, ileftu, basisu)

  ! Get v interval
  call findSpan(v, kv, tv, nctlv, ileftv)
  call basis(tv, nctlv, kv, v, ileftv, basisv)

  ! Get w interval
  call findSpan(w, kw, tw, nctlw, ileftw)
  call basis(tw, nctlw, kw, w, ileftw, basisw)

  counter = 0
  do ii=1, ku
     do jj = 1, kv
        do kk = 1, kw
           ! Get the local row/col for this surface
           start = istart + counter + 1
           col_ind(start) = l_index(ileftw-kw+kk, ileftv-kv+jj, ileftu-ku+ii)

           vals(start) = basisu(ii)*basisv(jj)*basisw(kk)
           counter = counter + 1
        end do
     end do
  end do
end subroutine getBasisPtVolume


!*==snrm2.f90  processed by SPAG 7.51RB at 20:37 on  3 Mar 2022
!> \brief \b SNRM2
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!  Definition:
!  ===========
!
!       REAL FUNCTION SNRM2(N,X,INCX)
!
!       .. Scalar Arguments ..
!       INTEGER INCX,N
!       ..
!       .. Array Arguments ..
!       REAL X(*)
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> SNRM2 returns the euclidean norm of a vector via the function
!> name, so that
!>
!>    SNRM2 := sqrt( x'*x ).
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>         number of elements in input vector(s)
!> \endverbatim
!>
!> \param[in] X
!> \verbatim
!>          X is REAL array, dimension ( 1 + ( N - 1 )*abs( INCX ) )
!> \endverbatim
!>
!> \param[in] INCX
!> \verbatim
!>          INCX is INTEGER
!>         storage spacing between elements of SX
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date November 2017
!
!> \ingroup single_blas_level1
!
!> \par Further Details:
!  =====================
!>
!> \verbatim
!>
!>  -- This version written on 25-October-1982.
!>     Modified on 14-October-1993 to inline the call to SLASSQ.
!>     Sven Hammarling, Nag Ltd.
!> \endverbatim
!>
!  =====================================================================
      REAL FUNCTION SNRM2(N,X,Incx)
      IMPLICIT NONE
!*--SNRM278
!
!  -- Reference BLAS level1 routine (version 3.8.0) --
!  -- Reference BLAS is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     November 2017
!
!     .. Scalar Arguments ..
      INTEGER Incx , N
!     ..
!     .. Array Arguments ..
      REAL X(*)
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      REAL ONE , ZERO
      PARAMETER (ONE=1.0E+0,ZERO=0.0E+0)
!     ..
!     .. Local Scalars ..
      REAL absxi , norm , scale , ssq
      INTEGER ix
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS , SQRT
!     ..
      IF ( N<1 .OR. Incx<1 ) THEN
         norm = ZERO
      ELSEIF ( N==1 ) THEN
         norm = ABS(X(1))
      ELSE
         scale = ZERO
         ssq = ONE
!        The following loop is equivalent to this call to the LAPACK
!        auxiliary routine:
!        CALL SLASSQ( N, X, INCX, SCALE, SSQ )
!
         DO ix = 1 , 1 + (N-1)*Incx , Incx
            IF ( X(ix)/=ZERO ) THEN
               absxi = ABS(X(ix))
               IF ( scale<absxi ) THEN
                  ssq = ONE + ssq*(scale/absxi)**2
                  scale = absxi
               ELSE
                  ssq = ssq + (absxi/scale)**2
               ENDIF
            ENDIF
         ENDDO
         norm = scale*SQRT(ssq)
      ENDIF
!
      SNRM2 = norm
!
!     End of SNRM2.
!
      END FUNCTION SNRM2


	SUBROUTINE VC_MSHT  ( psym, t, nxy, z, iret )
C************************************************************************
C* VC_MSHT								*
C*									*
C* This subroutine computes height from Montgomery Stream Function	*
C* and temperature.  This code always assumes that Montgomery Stream	*
C* Function is in the standard MKS units (m**2/s**2).			*
C*									*
C* VC_MSHT ( PSYM, T, NXY, Z, IRET )					*
C*									*
C* Input parameters:							*
C*	PSYM (NXY)	REAL		Montgomery stream function	*
C*	T    (NXY)	REAL		Temperature (K)			*
C*	NXY		INTEGER		Number of points		*
C* Output parameters:							*
C*	Z(*)		REAL		Height (m)			*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					 +1 = all missing		*
C**									*
C* Log:									*
C* K. Brill/NMC      05/92						*
C* K. Brill/NMC	     08/94	Documentation				*
C************************************************************************
	INCLUDE 	'GEMPRM.PRM'
	PARAMETER	( CP = AKAPPA * RDGAS )
C*
	REAL		psym (*), t (*), z (*)
C*
	INCLUDE		'ERMISS.FNC'
C-----------------------------------------------------------------------
	iret = 0
C*
	icnt = 0
	DO i = 1, nxy
	    IF ( ERMISS ( psym (i) ) .or. ERMISS ( t (i) ) ) THEN
		z (i) = RMISSD
		icnt = icnt + 1
	    ELSE
	   	z (i) = ( psym (i) - CP * t (i) ) / GRAVTY
	    END IF
	END DO
C*
	IF ( icnt .eq. nxy ) iret = +1
C*
	RETURN
	END


      PROGRAM xmmid
C     driver for routine mmid
      INTEGER NVAR
      REAL HTOT,X1
      PARAMETER(NVAR=4,X1=1.0,HTOT=0.5)
      INTEGER i
      REAL bessj,bessj0,bessj1
      REAL b1,b2,b3,b4,xf,y(NVAR),yout(NVAR),dydx(NVAR)
      EXTERNAL derivs
      y(1)=bessj0(X1)
      y(2)=bessj1(X1)
      y(3)=bessj(2,X1)
      y(4)=bessj(3,X1)
      call derivs(X1,y,dydx)
      xf=X1+HTOT
      b1=bessj0(xf)
      b2=bessj1(xf)
      b3=bessj(2,xf)
      b4=bessj(3,xf)
      write(*,'(1x,a/)') 'First four Bessel functions'
      do 11 i=5,50,5
        call mmid(y,dydx,NVAR,X1,HTOT,i,yout,derivs)
        write(*,'(1x,a,f6.4,a,f6.4,a,i2,a)') 'X = ',X1,
     *       ' to ',X1+HTOT,' in ',i,' steps'
        write(*,'(1x,t5,a,t20,a)') 'Integration','BESSJ'
        write(*,'(1x,2f12.6)') yout(1),b1
        write(*,'(1x,2f12.6)') yout(2),b2
        write(*,'(1x,2f12.6)') yout(3),b3
        write(*,'(1x,2f12.6)') yout(4),b4
        write(*,'(/1x,a)') 'press RETURN to continue...'
        read(*,*)
11    continue
      END
      SUBROUTINE derivs(x,y,dydx)
      REAL x,y(*),dydx(*)
      dydx(1)=-y(2)
      dydx(2)=y(1)-(1.0/x)*y(2)
      dydx(3)=y(2)-(2.0/x)*y(3)
      dydx(4)=y(3)-(3.0/x)*y(4)
      END

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


	FUNCTION PR_TVRK  ( tmpc, dwpc, pres )
C************************************************************************
C* PR_TVRK 								*
C*									*
C* This function computes TVRK from TMPC, DWPC and PRES, where DWPC	*
C* and PRES are used to compute MIXR.  The following equation is	*
C* used:								*
C*									*
C*   TVRK = TMPK * (1 + .001 * MIXR / .62197) / (1 + .001 * MIXR)	*
C*	                                                                *
C* If DWPC is missing, dry air is assumed and TMPK is returned.		*
C*									*
C* REAL PR_TVRK  ( TMPC, DWPC, PRES )					*
C*									*
C* Input parameters:							*
C*	TMPC		REAL		Temperature in Celsius		*
C*	DWPC		REAL		Dewpoint in Celsius		*
C*	PRES		REAL		Pressure in millibars		*
C*									*
C* Output parameters:							*
C*	PR_TVRK		REAL		Virtual temperature in Kelvin	*
C**	                                                                *
C* Log:									*
C* P. Kocin/CSC		1980    Original source PR_TVIRT		*
C* M. Goodman/RDS	 8/84	Renamed 				*
C* M. desJardins/GSFC	 9/86	Return temp if dewpoint is missing	*
C* I. Graffman/RDS	12/87	GEMPAK4					*
C* G. Huffman/GSC	 8/88	Documentation; RMIX test		*
C************************************************************************
        INCLUDE    'GEMPRM.PRM'
        INCLUDE    'ERMISS.FNC'
C------------------------------------------------------------------------
C*	Check if temperature or pressure is missing.
C
	IF  ( ERMISS ( tmpc ) .or. ERMISS ( pres ) )  THEN
	    PR_TVRK = RMISSD
C
C*	    If dewpoint is missing, return temperature.
C
	  ELSE IF  ( ERMISS ( dwpc ) )  THEN
	    PR_TVRK = PR_TMCK ( tmpc )
	  ELSE
C
C*	    Change temperature to Kelvin.
C
	    tmpk = PR_TMCK ( tmpc )
C
C*	    Find mixing ratio in g/kg; if missing, return temperature.
C
	    rmix = PR_MIXR  ( dwpc, pres )
	    IF  ( ERMISS ( rmix ) )  THEN
	        PR_TVRK = PR_TMCK ( tmpc )
	      ELSE
	        PR_TVRK =
     *           tmpk * (1. + .001*rmix/.62197) / (1. + .001*rmix)
	    END IF
	END IF
C*
	RETURN
	END


c
c-----------------------------------------------------------------------
c subroutine: fourea
c performs cooley-tukey fast fourier transform
c-----------------------------------------------------------------------
c
      subroutine fourea(data, n, isi)
c
c the cooley-tukey fast fourier transform in ansi fortran
c
c data is a one-dimensional complex array whose length, n, is a
c power of two.  isi is +1 for an inverse transform and -1 for a
c forward transform.  transform values are returned in the input
c array, replacing the input.
c transform(j)=sum(data(i)*w**((i-1)*(j-1))), where i and j run
c from 1 to n and w = exp (isi*2*pi*sqrt(-1)/n).  program also
c computes inverse transform, for which the defining expression
c is invtr(j)=(1/n)*sum(data(i)*w**((i-1)*(j-1))).
c running time is proportional to n*log2(n), rather than to the
c classical n**2.
c after program by brenner, june 1967. this is a very short version
c of the fft and is intended mainly for demonstration. programs
c are available in this collection which run faster and are not
c restricted to powers of 2 or to one-dimensional arrays.
c see -- ieee trans audio (june 1967), special issue on fft.
c
      complex data(1)
      complex temp, w
      ioutd = i1mach(2)
c
c check for power of two up to 15
c
      nn = 1
      do 10 i=1,15
        m = i
        nn = nn*2
        if (nn.eq.n) go to 20
  10  continue
      write (ioutd,9999)
9999  format (30h n not a power of 2 for fourea)
      stop
  20  continue
c
      pi = 4.*atan(1.)
      fn = n
c
c this section puts data in bit-reversed order
c
      j = 1
      do 80 i=1,n
c
c at this point, i and j are a bit reversed pair (except for the
c displacement of +1)
c
        if (i-j) 30, 40, 40
c
c exchange data(i) with data(j) if i.lt.j.
c
  30    temp = data(j)
        data(j) = data(i)
        data(i) = temp
c
c implement j=j+1, bit-reversed counter
c
  40    m = n/2
  50    if (j-m) 70, 70, 60
  60    j = j - m
        m = (m+1)/2
        go to 50
  70    j = j + m
  80  continue
c
c now compute the butterflies
c
      mmax = 1
  90  if (mmax-n) 100, 130, 130
 100  istep = 2*mmax
      do 120 m=1,mmax
        theta = pi*float(isi*(m-1))/float(mmax)
        w = cmplx(cos(theta),sin(theta))
        do 110 i=m,n,istep
          j = i + mmax
          temp = w*data(j)
          data(j) = data(i) - temp
          data(i) = data(i) + temp
 110    continue
 120  continue
      mmax = istep
      go to 90
 130  if (isi) 160, 140, 140
c
c for inv trans -- isi=1 -- multiply output by 1/n
c
 140  do 150 i=1,n
        data(i) = data(i)/fn
 150  continue
 160  return
      end


!*******************************************************************************
!>  @file File functions.f
!>  @brief Contains module @ref functions.
!
!  Note separating the Doxygen comment block here so detailed decription is
!  found in the Module not the file.
!
!>  This module containes functions used by the profiles.
!*******************************************************************************
      MODULE functions
      USE stel_kinds

!*******************************************************************************
!  INTERFACE BLOCKS
!*******************************************************************************
      PUBLIC :: two_power, two_power_gs, function_test
      PRIVATE :: check

      CONTAINS

!*******************************************************************************
!  PUBLIC FUNCTIONS AND SUBROUTINES
!*******************************************************************************
!*******************************************************************************
!>  @brief A two power profile.
!>
!>  A two power profiles is defined using 3 parameters.
!>
!>    b(0) * (1.0 - x ** b(1)) ** b(2)                                       (1)
!>
!>  b(0) controls the profile amplitude.
!>  b(1) controls the profile width.
!>  b(2) controls the profile tail.
!>
!>  @param[in] x Point to evaluate function from.
!>  @param[in] b Array of profile parameters.
!>  @returns The function evaluated at x.
!*******************************************************************************
      REAL(rprec) PURE FUNCTION two_power(x, b)

      IMPLICIT NONE

!  Declare Arguments
      REAL(rprec), INTENT(in)                  :: x
      REAL(rprec), DIMENSION(0:20), INTENT(in) :: b

!  Start of executable code
      two_power = b(0)*((1.0 - x**b(1))**b(2))

      END FUNCTION

!*******************************************************************************
!>  @brief A two power profile with added guassian peaks.
!>
!>  The power profile is defined as @ref functions::two_power. Then up to 9
!>  additional guassians peaks can be added.
!>
!>    two_power(x)*(1 + Sum[b(i)*Exp(-(x - b(i+1))/b(i+2)) ** 2])            (1)
!>
!>  Parameters beyond the first three control the additional guassians.
!>  b(i) controls the amplitude.
!>  b(i+1) controls the mean.
!>  b(i+2) controls the width.
!>
!>  @param[in] x Point to evaluate function from.
!>  @param[in] b Array of profile parameters.
!>  @returns The function evaluated at x.
!*******************************************************************************
      REAL(rprec) PURE FUNCTION two_power_gs(x, b)

      IMPLICIT NONE

!  Declare Arguments
      REAL(rprec), INTENT(in)                  :: x
      REAL(rprec), DIMENSION(0:20), INTENT(in) :: b

!  local variables
      INTEGER                                  :: i

!  Start of executable code
      two_power_gs = 1.0
      DO i = 3, 18, 3
         two_power_gs = two_power_gs                                           &
     &                + b(i)*exp(-((x - b(i+1))/b(i+2))**2.0)
      END DO
      two_power_gs = two_power_gs*two_power(x,b)

      END FUNCTION

!*******************************************************************************
!>  @brief A profile with a single guassian peak.
!>
!>  This defines a profile with a single guassian peak.
!>
!>    b(0)*Exp(-(x - b(1))/b(2)) ** 2                                        (1)
!>
!>  b(0) controls the amplitude.
!>  b(1) controls the mean.
!>  b(2) controls the width.
!>
!>  @param[in] x Point to evaluate function from.
!>  @param[in] b Array of profile parameters.
!>  @returns The function evaluated at x.
!*******************************************************************************
      REAL(rprec) PURE FUNCTION guassian(x, b)

      IMPLICIT NONE

!  Declare Arguments
      REAL(rprec), INTENT(in)                  :: x
      REAL(rprec), DIMENSION(0:20), INTENT(in) :: b

!  Start of executable code
      guassian = b(0)*exp(-((x - b(1))/b(2))**2.0)

      END FUNCTION

!*******************************************************************************
!>  @brief A profile with a integrated single guassian peak.
!>
!>  This defines a profile with a with an integarted single guassian peak.
!>
!>    Sqrt(2Pi)/2*b(0)*b(1)*(Erf(b(1)/b(2)) - Erf((b(1) - x)/b(2)))          (1)
!>
!>  b(0) controls the amplitude.
!>  b(1) controls the mean.
!>  b(2) controls the width.
!>
!>  @note Fortran 90 does not have a built in Error function must use the C
!>        function instead.
!>
!>  @param[in] x Point to evaluate function from.
!>  @param[in] b Array of profile parameters.
!>  @returns The function evaluated at x.
!*******************************************************************************
      REAL(rprec) PURE FUNCTION guassian_int(x, b)
      USE stel_constants, only : pi

      IMPLICIT NONE

!  Declare Arguments
      REAL(rprec), INTENT(in)                  :: x
      REAL(rprec), DIMENSION(0:20), INTENT(in) :: b

!  local parameters
      REAL(rprec), PARAMETER                   :: sqrpi = 0.5*SQRT(pi)

!  Start of executable code

      guassian_int = sqrpi*b(0)*b(1)*(ERF(b(1)/b(2)) -                         &
     &                                ERF((b(1) - x)/b(2)))

      END FUNCTION

!*******************************************************************************
!  UNIT TESTS
!*******************************************************************************
!-------------------------------------------------------------------------------
!>  @brief Profile unit test function.
!>
!>  This runs the associated unit tests and returns the result.
!>
!>  @returns True if the tests pass and false otherwise.
!-------------------------------------------------------------------------------
      FUNCTION function_test()

      IMPLICIT NONE

!  Declare Arguments
      LOGICAL                      :: function_test

!  local variables
      REAL(rprec)                  :: result
      REAL(rprec), DIMENSION(0:20) :: b(0:20) = 0

!  Start of executable code
! Test two_power function for x = 0, b = {1,10,2} is 1
      b(0:2) = (/ 1.0d+0, 10.0d+0, 2.0d+0 /)
      result = two_power(0.0d+0, b)
      function_test = check(1.0d+0,result,1,"two_power()")
      IF (.not.function_test) RETURN

! Test two_power function for x = 1, b = {1,10,2} is 0
      result = two_power(1.0d+0, b)
      function_test = check(0.0d+0,result,2,"two_power()")
      IF (.not.function_test) RETURN

! Test two_power function for x = 0.5, b = {1,1,1} is 0.5
      b(0:2) = (/ 1.0d+0, 1.0d+0, 1.0d+0 /)
      result = two_power(0.5d+0, b)
      function_test = check(0.5d+0,result,3,"two_power()")
      IF (.not.function_test) RETURN

! Test two_power function for x = 0.5, b = {1,1,2} is 0.25
      b(0:2) = (/ 1.0d+0, 1.0d+0, 2.0d+0 /)
      result = two_power(0.5d+0, b)
      function_test = check(0.25d+0,result,4,"two_power()")
      IF (.not.function_test) RETURN

! Test two_power_gs function for x = 0.4, b = {1,1,1,0,0,1} is twopower(x,b)
      b(0:5) = (/ 1.0d+0, 1.0d+0, 1.0d+0, 0.0d+0, 0.0d+0, 1.0d+0 /)
      result = two_power_gs(0.4d+0, b)
      function_test = check(two_power(0.4d+0, b),                              &
     &                      result,1,"two_power_gs")
      IF (.not.function_test) RETURN

! Test two_power_gs function for x = 0.8, b = {1,1,0,1,0.8,0.1} is 2
      b(0:5) = (/ 1.0d+0, 1.0d+0, 0.0d+0, 1.0d+0, 0.8d+0, 0.1d+0 /)
      result = two_power_gs(0.8d+0, b)
      function_test = check(2.0d+0,result,1,"two_power_gs")
      IF (.not.function_test) RETURN

      END FUNCTION

!*******************************************************************************
!  CHECK FUNCTIONS
!*******************************************************************************
!-------------------------------------------------------------------------------
!>  @brief Check a value.
!>
!>  Checks that the expected value matches the recieved. Otherwise report an
!>  error.
!>
!>  @param[in] expected The known value.
!>  @param[in] received The known test value.
!>  @param[in] testNum  The number of the test.
!>  @param[in] name     The name of the test.
!>  @returns True if the check passes and false otherwise.
!-------------------------------------------------------------------------------
      FUNCTION check(expected, received, testNum, name)

      IMPLICIT NONE

!  Declare Arguments
      LOGICAL                       :: check
      REAL(rprec), INTENT(in)       :: expected, received
      INTEGER, INTENT(in)           :: testNum
      CHARACTER (LEN=*), INTENT(in) :: name

!  Start of executable code
      check = expected .eq. received
      IF (.not.check) THEN
         write(*,*) "functions.f: ", name, " test", testNum,                   &
     &              "failed."
         write(*,*) "Expected", expected, "Received", received
      END IF

      END FUNCTION

      END MODULE


program test_mcmc

  use mo_kind,       only: dp, i4, i8
  use mo_ansi_colors, only: color, c_red, c_green
  use mo_likelihood, only: setmeas, loglikelihood_dp, loglikelihood_stddev_dp
  use mo_mcmc,       only: mcmc, mcmc_stddev
  use mo_moment,     only: mean, stddev

  ! for running mcmc
  real(dp)                              :: p
  real(dp)                              :: likeli_new
  real(dp)                              :: stddev_new
  real(dp), dimension(3)                :: parabest
  real(dp), dimension(3,2)              :: rangePar
  logical,  dimension(3)                :: maskpara
  real(dp), dimension(:,:), allocatable :: burnin_paras
  real(dp), dimension(:,:), allocatable :: mcmc_paras
  integer(i8)                           :: seed
  ! for result handling
  integer(i4)                           :: i, samples
  real(dp)                              :: dtmp1, dtmp2, dtmp3

  ! loading the data points observed
  call setmeas()

  ! best parameter set found for dataset using e.g. Simlated Annealing or DDS
  parabest = (/ 0.9999815795293934_dp, 2.0035191695594499_dp, 2.8675479872139675_dp /)

  write(*,*) ''
  write(*,*) '---------------------------------------------------------------------------------------------'
  write(*,*) ' (A1) "real" likelihood  (sigma is an error model or given) --> e.g. loglikelihood of mo_likelihood'
  write(*,*) '      full run '
  write(*,*) '---------------------------------------------------------------------------------------------'
  p = loglikelihood_dp(parabest)
  write(*,*) 'log-likelihood = ',p

  ! initializing the ranges of parameters
  rangePar(:,1) = -10.0_dp
  rangePar(:,2) =  10.0_dp

  ! which parameter will be sampled
  maskpara = (/ .true., .true., .true. /)

  ! fixing seeds for test case:
  seed = 62001519_i8

  ! starting MCMC:
  !     (1) Burn-in will be performed to optimize settings for MCMC
  !     (2) posterior distribution of the parameters at the minimum (best parameterset)
  !         will be sampled by MCMC
  call mcmc(loglikelihood_dp, parabest, rangePar, mcmc_paras, burnin_paras, &
       ParaSelectMode_in=2_i4,tmp_file='A_make_check_test_file',              &
       restart=.false., restart_file='restart_make_check_test_file', &
       seed_in=seed, loglike_in=.true., maskpara_in=maskpara, printflag_in=.true.)

  write(*,*)''
  write(*,*) '--------------------------------------------------'
  write(*,*) 'Results '
  write(*,*) '--------------------------------------------------'
  write(*,*) ''
  write(*,*) 'Number of parameter sets sampled'
  ! number of samples per chain x number of chains
  samples = size(mcmc_paras,1)
  write(*,*) samples

  write(*,*) ''
  write(*,*) 'Mean parameter value +/- standard deviation'
  do i=1,size(parabest)
     if (maskpara(i)) then
        write(*,*) 'para #',i, ' = ',mean(mcmc_paras(:,i)), '+/-',stddev(mcmc_paras(:,i), ddof=1)
     end if
  end do

  ! Does it run properly?
  ! para # 1  =    0.9999637290554980 +/-   5.8265791202762377E-05
  ! para # 2  =    2.0054443506614068 +/-   6.0238019468712668E-03
  ! para # 3  =    2.8306538403277148 +/-   0.1384711293432960
  write(*,*) ''
  write(*,*) '-----------------------------------'
  dtmp1 = stddev(mcmc_paras(:,1), ddof=1)
  dtmp2 = stddev(mcmc_paras(:,2), ddof=1)
  dtmp3 = stddev(mcmc_paras(:,3), ddof=1)
  if ( (nint(dtmp1*10000000,i4) .eq. 583_i4)   .and. &
       (nint(dtmp2*10000000,i4) .eq. 60238_i4) .and. &
       (nint(dtmp3*10000000,i4) .eq. 1384711_i4) ) then
     write(*,*) 'mo_mcmc: mcmc ', color('o.k.', c_green)
  else
     write(*,*) 'mo_mcmc: mcmc ', color('failed', c_red)
  end if

    write(*,*) ''
  write(*,*) '---------------------------------------------------------------------------------------------'
  write(*,*) ' (A2) "real" likelihood  (sigma is an error model or given) --> e.g. loglikelihood of mo_likelihood'
  write(*,*) '      RESTART '
  write(*,*) '---------------------------------------------------------------------------------------------'
  ! starting MCMC:
  !     (1) starting from restart file
  call mcmc(loglikelihood_dp, parabest, rangePar, mcmc_paras, burnin_paras, &
       ParaSelectMode_in=2_i4,tmp_file='A_make_check_test_file',              &
       restart=.true., restart_file='restart_make_check_test_file', &
       seed_in=seed, loglike_in=.true., maskpara_in=maskpara, printflag_in=.true.)

  write(*,*)''
  write(*,*) '--------------------------------------------------'
  write(*,*) 'Results '
  write(*,*) '--------------------------------------------------'
  write(*,*) ''
  write(*,*) 'Number of parameter sets sampled'
  ! number of samples per chain x number of chains
  samples = size(mcmc_paras,1)
  write(*,*) samples

  write(*,*) ''
  write(*,*) 'Mean parameter value +/- standard deviation'
  do i=1,size(parabest)
     if (maskpara(i)) then
        write(*,*) 'para #',i, ' = ',mean(mcmc_paras(:,i)), '+/-',stddev(mcmc_paras(:,i), ddof=1)
     end if
  end do

  ! Does it run properly?
  ! para # 1  =    0.9999637290554980 +/-   5.8265791202762377E-05
  ! para # 2  =    2.0054443506614068 +/-   6.0238019468712668E-03
  ! para # 3  =    2.8306538403277148 +/-   0.1384711293432960
  write(*,*) ''
  write(*,*) '-----------------------------------'
  dtmp1 = stddev(mcmc_paras(:,1), ddof=1)
  dtmp2 = stddev(mcmc_paras(:,2), ddof=1)
  dtmp3 = stddev(mcmc_paras(:,3), ddof=1)
  if ( (nint(dtmp1*10000000,i4) .eq. 583_i4)   .and. &
       (nint(dtmp2*10000000,i4) .eq. 60238_i4) .and. &
       (nint(dtmp3*10000000,i4) .eq. 1384711_i4) ) then
     write(*,*) 'mo_mcmc: mcmc with restart ', color('o.k.', c_green)
  else
     write(*,*) 'mo_mcmc: mcmc with restart ', color('failed', c_red)
  end if

  write(*,*) ''
  write(*,*) '---------------------------------------------------------------------------------------------'
  write(*,*) ' (B) "faked" likelihood (sigma is computed by obs vs model) --> e.g. loglikelihood_stddev of mo_likelihood'
  write(*,*) '---------------------------------------------------------------------------------------------'

  ! your best parameter set should have the largest likelihood
  ! sigma of errors is unknown --> initial guess e.g. 1.0_dp
  !    stddev_new = standard deviation of errors using paraset
  !    likeli_new = likelihood using stddev_new
  p = loglikelihood_stddev_dp(parabest,1.0_dp,likeli_new=likeli_new,stddev_new=stddev_new)
  write(*,*) 'guessed log-likelihood = ',p
  write(*,*) 'best log-likelihood    = ',likeli_new

  ! initializing the ranges of parameters
  rangePar(:,1) = -10.0_dp
  rangePar(:,2) =  10.0_dp

  ! which parameter will be sampled
  maskpara = (/ .true., .true., .true. /)

  ! fixing seeds for test case:
  seed = 62001519_i8

  ! starting MCMC:
  !     (1) Burn-in will be performed to optimize settings for MCMC
  !     (2) posterior distribution of the parameters at the minimum (best parameterset)
  !         will be sampled by MCMC
  call mcmc_stddev(loglikelihood_stddev_dp, parabest, rangePar, mcmc_paras, burnin_paras, &
       ParaSelectMode_in=2_i4,tmp_file='B_make_check_test_file',              &
       seed_in=seed, loglike_in=.true., maskpara_in=maskpara, printflag_in=.true.)

  write(*,*)''
  write(*,*) '--------------------------------------------------'
  write(*,*) 'Results '
  write(*,*) '--------------------------------------------------'
  write(*,*) ''
  write(*,*) 'Number of parameter sets sampled'
  ! number of samples per chain x number of chains
  samples = size(mcmc_paras,1)
  write(*,*) samples

  write(*,*) ''
  write(*,*) 'Mean parameter value +/- standard deviation'
  do i=1,size(parabest)
     if (maskpara(i)) then
        write(*,*) 'para #',i, ' = ',mean(mcmc_paras(:,i)), '+/-',stddev(mcmc_paras(:,i), ddof=1)
     end if
  end do

  ! Does it run properly?
  ! para # 1  =    0.9999781778559107 +/-   6.3209436919767900E-05
  ! para # 2  =    2.0038078713380076 +/-   6.6280930370445947E-03
  ! para # 3  =    2.8654065450088502 +/-   0.1498533843248984
  write(*,*) ''
  write(*,*) '-----------------------------------'
  dtmp1 = stddev(mcmc_paras(:,1), ddof=1)
  dtmp2 = stddev(mcmc_paras(:,2), ddof=1)
  dtmp3 = stddev(mcmc_paras(:,3), ddof=1)
  if ( (nint(dtmp1*10000000,i4) .eq. 632_i4)   .and. &
       (nint(dtmp2*10000000,i4) .eq. 66281_i4) .and. &
       (nint(dtmp3*10000000,i4) .eq. 1498534_i4) ) then
     write(*,*) 'mo_mcmc: mcmc_stddev ', color('o.k.', c_green)
  else
     write(*,*) 'mo_mcmc: mcmc_stddev ', color('failed', c_red)
  end if

  ! clean-up
  deallocate(mcmc_paras)
  deallocate(burnin_paras)

end program test_mcmc


!--------------------------------------------------------------------------------------------------
! MODULE: orthog_mod
!> @author Marin Sapunar, Ruđer Bošković Institute
!> @date October, 2017
!
!> @brief Simple orthogonalization subroutines.
!--------------------------------------------------------------------------------------------------
module orthog_mod
    use global_defs
    implicit none

    private
    public :: orthog_gs
    public :: orthog_lowdin

contains

    !----------------------------------------------------------------------------------------------
    ! SUBROUTINE: orthog_gs
    !
    !> @brief Modified Gram-Schmidt orthogonalization of a set of column vectors.
    !> @details
    !! Input is a real matrix with dimensions n x m matrix containing m vectors of dimension n.
    !! The vectors are orthogonalized (in place) using the Gram-Schmidt algorithm.
    !----------------------------------------------------------------------------------------------
    subroutine orthog_gs(u)
        real(dp), intent(inout) :: u(:, :) !< Column vectors.
        integer :: j
        integer :: i

        u(:, 1) = u(:, 1) / norm(u(:, 1))
        do i = 2, size(u, 2)
            do j = 1, i - 1
                u(:, i) = u(:, i) - proj(u(:, i), u(:, j)) * u(:, j)
            end do
            u(:, i) = u(:, i) / norm(u(:, i))
        end do
    end subroutine orthog_gs

    pure function proj(v1, v2) result(p)
        real(dp), intent(in) :: v1(:)
        real(dp), intent(in) :: v2(:)
        real(dp) :: p
        p = dot_product(v1, v2) / dot_product(v2, v2)
    end function proj

    pure function norm(v) result(n)
        real(dp), intent(in) :: v(:)
        real(dp) :: n
        n = sqrt(dot_product(v, v))
    end function norm

    !----------------------------------------------------------------------------------------------
    ! SUBROUTINE: orthog_lowdin
    !
    !> @brief Lowdin (symmetric) orthogonalization of a set of column vectors.
    !> @details
    !! Input is a real matrix with dimensions n x m matrix containing m vectors of dimension n.
    !! The vectors are orthogonalized (in place) using singular value decomposition.
    !----------------------------------------------------------------------------------------------
    subroutine orthog_lowdin(mat)
        use linalg_wrapper_mod, only : gesvd, gemm
        real(dp), intent(inout) :: mat(:, :) !< Column vectors.
        real(dp), allocatable :: s(:) !< Singular values (not used.)
        real(dp), allocatable :: vt(:, :) !< Transposed V matrix.
        real(dp), allocatable :: u(:, :) !< U matrix.
        integer :: m, n !< Array size.
        integer :: info !< LAPACK return code.

        m = size(mat, 1)
        n = size(mat, 2)
        allocate(s(min(m, n)))
        allocate(u(m, min(m, n)))
        allocate(vt(n, min(m, n)))

        ! Compute SVD
        call gesvd(mat, s, u=u, vt=vt, info=info)
        if (info /= 0) call errstop('ORTH_LOWDIN', 'GESVD call failed.', info)

        ! Overwrite input matrix with orthogonized matrix U.Vt.
        call gemm(u, vt, mat)
    end subroutine orthog_lowdin

end module orthog_mod


double precision function heat_fort_eval_bc(iface,t,x,y)
    implicit none

    integer iface
    double precision x,y,t

    integer bctype(0:3)
    common /comm_bc/ bctype

    integer normals(0:3,2)
    double precision q,grad(2), qn, a,b

    data normals /-1,1,0,0,0,0,-1,1/

    call heat_qexact_gradient(x,y,q,grad)

    !! q_n = grad q \cdot n
    qn = normals(iface,1)*grad(1) + normals(iface,2)*grad(2)

    !! bc_type is set in options .ini file as [multigrid] boundary_conditions
    if (bctype(iface) .eq. 1) then
        a = 1
        b = 0
    elseif (bctype(iface) .eq. 2) then
        a = 0
        b = 1
    endif

    heat_fort_eval_bc = a*q + b*qn
    !!heat_fort_eval_bc = 0

    return

end function heat_fort_eval_bc

double precision function heat_neumann(iface,t,x,y)
    implicit none

    integer iface
    double precision t,x,y

    if (iface .eq. 0) then
        heat_neumann = 0
    elseif (iface .eq. 1) then
        heat_neumann = 0
    else
        heat_neumann = 0
    endif

end function heat_neumann


      SUBROUTINE TUTEST(DATA1,N1,DATA2,N2,T,PROB)
      DIMENSION DATA1(N1),DATA2(N2)
      CALL AVEVAR(DATA1,N1,AVE1,VAR1)
      CALL AVEVAR(DATA2,N2,AVE2,VAR2)
      T=(AVE1-AVE2)/SQRT(VAR1/N1+VAR2/N2)
      DF=(VAR1/N1+VAR2/N2)**2/((VAR1/N1)**2/(N1-1)+(VAR2/N2)**2/(N2-1))
      PROB=BETAI(0.5*DF,0.5,DF/(DF+T**2))
      RETURN
      END


c----67--1---------2---------3---------4---------5---------6---------712
c
c    File: plate5.f   latest modification July 11, 1997
c    "write(*,...)" changed to "write(iws,...)", 7/29/97
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function xaxel(r,fi,alfa)
      double precision r,fi,alfa
      xaxel=(r+1.0/r)*alfa*cosd(fi)
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function yaxel(r,fi,alfa)
      double precision r,fi,alfa
      yaxel=(r-1.0/r)*alfa*sind(fi)
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision  function rhox(x,fi,alfa)
      double precision  x,fi,alfa
      rhox=(x+sqrt(x**2.0 - 4.0*(alfa*cosd(fi))**2.0))/
     &     (2.0*alfa*cosd(fi))
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function rhoy(y,fi,alfa)
      double precision   y,fi,alfa
      rhoy=(y+sqrt(y**2.0 + 4.0*(alfa*sind(fi))**2.0))/
     &     (2.0*alfa*sind(fi))
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function rhoxy(x,y,fi,alfa)
      double precision   x,y,fi,alfa,fic
      double precision   rhox,rhoy
      parameter (fic = 45.0)
      if (fi.le.fic) then
         rhoxy = rhox(x,fi,alfa)
      else
         rhoxy = rhoy(y,fi,alfa)
      endif
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function rho_ellips(a,b,fi,alfa)
      double precision fi,a,b,alfa,s1,c1,k
      c1=cosd(fi)
      s1=sind(fi)
      k = ( 2.0*( (b*c1)**2.0 - (a*s1)**2.0 ) - (a*b/alfa)**2.0 )/
     &        ( (b*c1)**2.0 + (a*s1)**2.0 )
      rho_ellips = sqrt( (-k + sqrt(k**2.0 - 4.0) ) / 2.0 )
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function calc_lambda(l1,ln,n)
c
c This routine computes the bias parameter which is used in
c mesh refinement/coarsening.
c
      implicit none
      double precision eps
      parameter (eps=1.e-06)
      integer  n,i
      double precision l1,ln,lambda,xtot,dlambda,sum,sumold,xi
      xtot    = ln / l1
      lambda  = 0.0150
      dlambda = 0.0050
      sum=0
 10   continue
        lambda = lambda + dlambda
        sumold = sum
        sum = 0.0
        do i=1, n
           xi = i-1
           sum = sum + exp( lambda*xi )
        enddo
      if ( abs( sum/xtot - 1.0) .gt. eps) then
         if ( ((xtot-sum)*(xtot-sumold) ) .lt. 0.0) dlambda=-dlambda/2.
         goto 10
      endif
      calc_lambda=lambda
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function hyperbel_length(r1,r2,fi,alfa)
c
c Computes the arc-length of a segment of an hyperbolic curve
c
c   Hyperble Eq.   (x/a)**2 - (z/b)**2 = 1
c
      double precision r1,r2,fi,alfa,a,b,dr,xi
      a = alfa*cosd(fi)
      b = alfa*sind(fi)
      dr = r2-r1
      sum = sqrt( (a*(1.0-1.0/r1**2.0))**2.0 +
     &            (b*(1.0+1.0/r1**2.0))**2.0 )
c
c Divide the interval in 50 equidistant segments, use Simpson's rule
c
      do i=2, 50, 2
         xi = i-1
         r  = r1 + dr*xi / 50.0
         sum = sum + 4.0*sqrt( (a*(1.0-1.0/r**2.0))**2.0 +
     &                         (b*(1.0+1.0/r**2.0))**2.0 )
      enddo
      do i=3, 49, 2
         xi  = i-1
         r   = r1 + dr*xi / 50.0
         sum = sum + 2.0*sqrt( (a*(1.0-1.0/r**2.0))**2.0 +
     &                         (b*(1.0+1.0/r**2.0))**2.0 )
      enddo
      sum = sum + sqrt( (a*(1.0-1.0/r2**2.0))**2.0 +
     &                  (b*(1.0+1.0/r2**2.0))**2.0 )
      hyperbel_length = sum*dr / 150.0
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      subroutine angular_distribution(phi,a,c,alfa,jms,tcr,
     &           iws,errorFlag)
c
      implicit none
c
      double precision  ellipse_l
      external          ellipse_l
c
      double precision pi,eps,unit
      parameter ( eps=1.e-7, unit=1.0 )
c
      integer jms,tcr,i,j,ns,n1,n2,k1,k2,dk,dk1,iws,errorFlag
c
      double precision eta
c      parameter (eta=0.40)
      double precision beta1,beta2
      parameter (beta1=1.8, beta2=0.5)
      double precision phi(200),a,c,alfa,rho0,tmp(200),rel,rel_old,psi,
     &                 dphi,phi1,phi2,s,ds(100),dstar,xi,bias1,bias2
c
      double precision a1,b1
      common /ellipse/ a1,b1
c
      pi = 4.*atan(unit)
      rho0 = sqrt( (c+a) / (c-a) )
      a1 = alfa*( rho0 + 1.0/rho0 )
      b1 = alfa*( rho0 - 1.0/rho0 )
c
c Determine the length of the segments along the crack front.
c
      phi1 = 0.0
      phi2 = pi/2.
      call qromb(ellipse_l,phi1,phi2,s,eps, iws)
c
      ns = (jms-1)/2
c
c Embeddied crack or Semi elliptical crack - major axis free surface
c
      if ((tcr .eq. 1) .or. (tcr.eq.2)) then
         eta = 0.50
         k1 = int(eta*ns+0.5)
         n1 = k1
         psi = 0.5**(a/c)
         ds(1)  = psi*(pi/2.)*(a/real(ns)) * (a/c)
         call calc_bias_int1(ds(1),s,ns,k1,bias1)
         do i=1, n1
            xi = i - 1
            ds(i) = ds(1)*bias1**xi
         enddo
         do i=n1+1, ns
            ds(i) = ds(n1)
         enddo
      else
c
c Semi elliptical crack (minor axis free surface) or
c  1/4 elliptical crack (two free surfaces)
c
         dk  = ns/4
         dk1 = dk*0.5*(a/c)**0.4
         k2  = ns/2 - (dk-dk1)
         k1  = ns/2 -  dk1
         n1 = k1
         n2 = ns - k2 + 1
         psi = 0.5**(a/c)
         ds(1)  = psi*(pi/2.)*(a/real(ns)) * (a/c)
         ds(ns) = 0.5*s/real(ns)
         call calc_bias_int2(ds(1),ds(ns),s,ns,k1,k2,bias1,bias2)
         do i=1, n1
            xi = i - 1
            ds(i) = ds(1)*bias1**xi
         enddo
         do i=ns, n2, -1
            xi = ns-i
            ds(i) = ds(ns)*bias2**xi
         enddo
         do i=n1+1, n2-1
            ds(i) = ds(n1)
         enddo
c
      endif
c
c Determine the anglular function
c
      phi1 = 0.0
      do i=1, ns
         rel_old = 1.
         dphi = 0.002*pi
         phi2 = phi1
 10      phi2 = phi2 + dphi
         call qromb(ellipse_l,phi1,phi2,dstar,eps, iws)
         rel = (ds(i)-dstar)/ds(i)
         if (abs(rel).gt.eps) then
            if ( (rel_old*rel).lt.0.) dphi = -dphi/2.
            rel_old = rel
            goto 10
         endif
         tmp(i) = (phi2/pi)*180.
         phi1=phi2
      enddo
c
      j = 1
      phi(j) = 90.0
      do i = ns-1, 1, -1
         j = j + 2
         phi(j) = tmp(i)
         phi(j-1) = ( phi(j)+phi(j-2) ) / 2.
      enddo
      j = j + 2
      phi(j) = 0.0
      phi(j-1) = ( phi(j)+phi(j-2) ) / 2.
      if (j.ne.jms) then
         write(iws,*) '>> Angular distribution erroneous!'
         errorFlag = 1
         return
      endif
c
      return
      end
c

c----67--1---------2---------3---------4---------5---------6---------712
c
      subroutine calc_bias_int1(ds,s,n,k,bias)
c
c Computes bias to be used for k out of n elements
c
      implicit none
      double precision eps
      parameter (eps=1.e-06)
      integer  n,k,i
      double precision ds,s,bias,dbias,sratio,sum,sumold,xi
      sratio = s/ds
      bias  = 0.8
      dbias = 0.1
      sum   = 0.0
 10   continue
        bias = bias + dbias
        sumold = sum
        sum = 1.0
        do i=2, k
           xi = i-1
           sum = sum + bias**xi
        enddo
        xi = k-1
        sum = sum + (n-k)*bias**xi
      if ( abs( sum/sratio - 1.0) .gt. eps) then
         if ( ((sratio-sum)*(sratio-sumold)) .lt. 0.0) dbias=-dbias/2.
         goto 10
      endif
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      subroutine calc_bias_int2(ds1,ds2,s,n,k1,k2,bias1,bias2)
c
c Computes bias to be used for k1, k2, elements respectivly.
c The total number of elements is n ( n > k1 + k2)
c
      implicit none
      double precision eps
      parameter (eps=1.e-06)
      integer  n,k1,k2,i
      double precision ds1,ds2,s,bias1,bias2,b,db,sum,sumold,xi,xk1,xk2
      b   = 0.8
      db  = 0.1
      sum = 0.0
 10   continue
        b = b + db
        sumold = sum
        sum = ds1
        bias1 = b
        do i=2, k1
           xi = i-1
           sum = sum + ds1*bias1**xi
        enddo
        sum = sum + ds2
        xk1 = k1 - 1
        xk2 = k2 - 1
        bias2 = ((ds1/ds2)*bias1**xk1)**(1.0/xk2)
        do i=2, k2
           xi = i-1
           sum = sum + ds2*bias2**xi
        enddo
        xi = k1-1
        sum = sum + (n-k1-k2)*ds1*bias1**xi
      if ( abs( sum/s - 1.0) .gt. eps) then
         if ( ((s-sum)*(s-sumold)) .lt. 0.0) db = - db / 2.
         goto 10
      endif
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      subroutine mapping_s(alfa,phi,icase,r,r1,r2,psi,sl,eps,iws)
c
c Coordinates originally located on a plane are mapped on a single
c curved surface.
c The curvature of the surface are defined by a hyperble
c
      implicit none
c
      double precision  hyperbel_l
      external          hyperbel_l
c
      integer     icase,it,itmax,iws
      parameter ( itmax = 1000 )
      double precision alfa,phi,r,r1,r2,psi,sl,eps
      double precision s,ds,dstar,drho,rho2,rel,rel_old,eps1
c
      double precision  a1,b1
      common /hyperbel/ a1,b1
c
      a1 = alfa*cosd(phi)
      b1 = alfa*sind(phi)
      it = 0
      eps1 = eps/10.0
c
      if (icase.eq.1) then
c
c Keep the physical length ahead of the crack
         ds = sl
         drho = (r2-r1)*(psi-1.)
         rho2 = r2 + drho
         drho = 0.80*drho
         rel_old = 1.0
 10      continue
            it = it + 1
            rho2 = rho2 + drho
            call qromb(hyperbel_l,r2,rho2,dstar,eps1, iws)
            rel = (ds-dstar)/ds
         if (it.gt.itmax) then
ccc            write(*,110) '* it >',itmax,'(1) in subr. mapping_s'
            write(iws,110) '* it >',itmax,'(1) in subr. mapping_s'
            goto 30
         elseif (abs(rel).gt.eps) then
            if ( (rel_old*rel).lt.0.d0) drho = -drho/2.
            rel_old = rel
            goto 10
         endif
         r = rho2
      else
c
c Conformal mapping in combined with linear scaling
c
         call qromb(hyperbel_l,r1,r2,s,eps1/10., iws)
         ds = (1.0-psi)*s
c
         drho = (1.-psi)*(r2-r1)
         rho2 = r2 - drho
         drho = -0.10*drho
         rel_old = 1.0
  20     continue
            it = it + 1
            rho2 = rho2 + drho
            call qromb(hyperbel_l,rho2,r2,dstar,eps1, iws)
            rel = (ds-dstar)/ds
         if (it.gt.itmax) then
ccc            write(*,110) '* it >',itmax,'(2) in subr. mapping_s'
            write(iws,110) '* it >',itmax,'(2) in subr. mapping_s'
            goto 30
         elseif (abs(rel).gt.eps) then
            if ( (rel_old*rel).lt.0.d0) drho = -drho/2.0
            rel_old = rel
            goto 20
         endif
         r = rho2
c
      endif
c
  30  continue
c
 110  format(a,i3,a)
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine mapping_a1(alfa,phi,r,r1,r2,psi,eps,iws)
c
c Coordinates originally located on a plane are mapped on
c a single curved surface. The curvature of the surface are
c defined by a hyperble
c
      implicit none
c
      double precision  hyperbel_l
      external          hyperbel_l
c
      integer     it,itmax,iws
      parameter ( itmax = 40 )
      double precision alfa,phi,r,r1,r2,psi,eps,eps1
      double precision s,ds,dstar,drho,rho2,rel,rel_old
c
      double precision  a1,b1
      common /hyperbel/ a1,b1
c
      a1 = alfa*cosd(phi)
      b1 = alfa*sind(phi)
      it = 0
c
      eps1 = 0.1*eps
      call qromb(hyperbel_l,r1,r2,s,eps1, iws)
      ds = psi*s
c
      drho = psi*(r2-r1)
      rho2 = r1 + drho
      drho = 0.10*drho
      rel_old = 1.0
c
  10  continue
         it = it + 1
         rho2 = rho2 + drho
         call qromb(hyperbel_l,r1,rho2,dstar,eps1, iws)
         rel = (ds-dstar)/ds
      if (it.gt.itmax) then
ccc         write(*,110) '* it >',itmax,' in subr. mapping_a(...)'
         write(iws,110) '* it >',itmax,' in subr. mapping_a(...)'
         goto 20
      elseif (abs(rel).gt.eps) then
         if ( (rel_old*rel).lt.0.) drho = -drho/2.
         rel_old = rel
         goto 10
      endif
  20  continue
      r = rho2
c
 110  format(a,i3,a)
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine mapping_a2(alfa,p1,p2,r1,r2,r,phi,psi,eps,iws)
c
c Coordinates originally located on a plane are mapped on
c a single curved surface. The curvature of the surface are
c defined in a elliptic-hyperbolic coordinate system.
c
      implicit none
c
      double precision  hyper_l
      external          hyper_l
c
      integer     it,itmax,iws
      parameter ( itmax = 100 )
      double precision alfa,p1,p2,r1,r2,r,phi,psi,eps,eps1
      double precision s,ds,dstar,drho,rho2,rel,rel_old
      double precision pi,phip,phi_fcn4
c
      double precision alpha,rr1,rr2,pp1,pp2
      common   /hyper/ alpha,rr1,rr2,pp1,pp2
c
      pi = 4.*atan(1.0)
      alpha = alfa
      rr1 = r1
      rr2 = r2
      pp1 = p1*pi/180.0
      pp2 = p2*pi/180.0
c
      eps1 = 0.1*eps
      call qromb(hyper_l,r1,r2,s,eps1, iws)
      ds = psi*s
c
      drho = psi*(r2-r1)
      rho2 = r1 + drho
      drho = 0.10*drho
      rel_old = 1.0
      it = 0
c
  10  continue
         it = it + 1
         rho2 = rho2 + drho
         call qromb(hyper_l,r1,rho2,dstar,eps1, iws)
         rel = (ds-dstar)/ds
      if (it.gt.itmax) then
ccc         write(*,110) '* it >',itmax,' in subr. mapping_a2(...)'
         write(iws,110) '* it >',itmax,' in subr. mapping_a2(...)'
         goto 20
      elseif (abs(rel).gt.eps) then
         if ( (rel_old*rel).lt.0.) drho = -drho/2.
         rel_old = rel
         goto 10
      endif
  20  continue
      r = rho2
      phi = phi_fcn4(phip,r,r1,r2,p1,p2)
c
 110  format(a,i3,a)
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function calc_amap_bias(eta,u1,um,v1,vm,m,
     &                          alfa,p1,p2,r1,r2,eps, iws)
      implicit none
      double precision hyper_l
      external         hyper_l
      integer          m, iws
      double precision eta,u1,um,v1,vm,alfa,p1,p2,r1,r2,eps,
     &                 t1,tn,s1,sn,beta,calc_lambda
c
      double precision alpha,rr1,rr2,pp1,pp2
      common   /hyper/ alpha,rr1,rr2,pp1,pp2
c
      alpha = alfa
      rr1 = r1
      rr2 = r2
      pp1 = p1
      pp2 = p2
      call qromb( hyper_l, r1, r2, sn, 0.1*eps, iws)
      t1 = (1.0-eta)*u1 + eta*v1
      tn = (1.0-eta)*um + eta*vm
      beta = 0.4
      s1 = t1*(sn/tn)**beta
      calc_amap_bias = exp( calc_lambda(s1,sn,m) )
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function calc_amap_length(alfa,p1,p2,r1,r2,
     &                          eps,iws)
      implicit none
      double precision hyper_l
      external         hyper_l
      double precision alfa,p1,p2,r1,r2,eps,sn,pi
      integer iws
c
      double precision alpha,rr1,rr2,pp1,pp2
      common   /hyper/ alpha,rr1,rr2,pp1,pp2
c
      pi = 4.*atan(1.0)
      alpha = alfa
      rr1 = r1
      rr2 = r2
      pp1 = p1*pi/180.0
      pp2 = p2*pi/180.0
      call qromb( hyper_l, r1, r2, sn, 0.1*eps, iws)
      calc_amap_length = sn
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function calc_amap_yp(a,b,y,psi,eps,iws)
      implicit none
      double precision ellipse_l
      external         ellipse_l
      integer     it,itmax,iws
      parameter ( itmax = 100 )
      double precision a,b,y,psi,eps,
     &                 pi,eps1,p1,p2,sn,ds,dstar,dp,rel,rel_old
c
      double precision a1,b1
      common /ellipse/ a1,b1
c
      pi = 4.*atan(1.0)
      a1 = a
      b1 = b
      p1 = 0.0
      p2 = asin(y/b)
      eps1 = 0.1*eps
      call qromb( ellipse_l, p1, p2, sn, eps1, iws)
c
      ds = psi*sn
c
      dp = psi*(p2-p1)
      p2 = p1 + dp
      dp = 0.20*dp
      rel_old = 1.0
      it = 0
c
  10  continue
         it = it + 1
         p2 = p2 + dp
         call qromb( ellipse_l, p1, p2, dstar, eps1, iws)
         rel = ( ds - dstar ) / ds
      if ( it .gt. itmax ) then
ccc         write(*,110) '* it >',itmax,' in fcn. calc_amap_yp( )'
         write(iws,110) '* it >',itmax,' in fcn. calc_amap_yp( )'
         goto 20
      elseif( abs(rel) .gt. eps ) then
         if ( (rel_old*rel) .lt. 0.0 ) dp = - dp / 2.0
         rel_old = rel
         goto 10
      endif
  20  continue
      calc_amap_yp = b*sin(p2)
c
 110  format(a,i3,a)
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine div_hyperbel(r1,r2,f1,f2,n,l1,alfa,xy,rf)
c
      implicit none
      integer  n,i,j,m
      double precision r1,r2,f1,f2,l1,alfa,xy(2,*),rf(2,*),
     &           psi,r,f,dx,x1,x2,dy,y1,y2,ds,ln,lambda,xl(100),s
      double precision xaxel,yaxel,calc_lambda
c
c Compute the total length of the line
c
      m=200
      ln = 0.
      x1 = xaxel(r1,f1,alfa)
      y1 = yaxel(r1,f1,alfa)
      do i=1, m
         psi=real(i-1)/real(m-1)
         r = (1.-psi)*r1 + psi*r2
         f = (1.-psi)*f1 + psi*f2
         x2 = xaxel(r,f,alfa)
         y2 = yaxel(r,f,alfa)
         dx = x2-x1
         dy = y2-y1
         ds = sqrt(dx*dx+dy*dy)
         ln = ln + ds
         x1 = x2
         y1 = y2
      enddo
c
      lambda = calc_lambda(l1,ln,n)
c         write(31,*) '>> Ln = ',real(ln),' l1=',real(l1),
c     &      ' lambda=',real(lambda),' exp(lambda)=',real(exp(lambda))
      if (exp(lambda).le.1.) then
         do i=1, n
            xl(i) = ln*real(i)/real(n)
         enddo
      else
         xl(1) = l1
         do i=2, n
            xl(i) = xl(i-1) + l1*exp(lambda*real(i-1))
         enddo
      endif
c
      m=501
      s = 0
      j = 1
      x1 = xaxel(r1,f1,alfa)
      y1 = yaxel(r1,f1,alfa)
      do i=1, m
         psi=real(i-1)/real(m-1)
         r = (1.-psi)*r1 + psi*r2
         f = (1.-psi)*f1 + psi*f2
         x2 = xaxel(r,f,alfa)
         y2 = yaxel(r,f,alfa)
         dx = x2-x1
         dy = y2-y1
         ds = sqrt(dx*dx+dy*dy)
         s = s + ds
         x1 = x2
         y1 = y2
         if ( s.ge.xl(j) ) then
            xy(1,j) = xaxel(r,f,alfa)
            xy(2,j) = yaxel(r,f,alfa)
            rf(1,j) = r
            rf(2,j) = f
            j = j + 1
         endif
      enddo
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine div_linear(r1,r2,f1,f2,n,l1,alfa,xy)
c
      implicit none
      integer  n,i
      double precision r1,r2,f1,f2,l1,alfa,xy(2,*),
     &                 x1,x2,y1,y2,ln,lambda,xl(100),norm_x,norm_y
      double precision xaxel,yaxel,calc_lambda
c
c Compute the total length of the line
c
      x1 = xaxel(r1,f1,alfa)
      y1 = yaxel(r1,f1,alfa)
      x2 = xaxel(r2,f2,alfa)
      y2 = yaxel(r2,f2,alfa)
      ln = sqrt( (x2-x1)**2. + (y2-y1)**2. )
c
c calc. norm vector
c
      norm_x = (x2-x1)/ln
      norm_y = (y2-y1)/ln
c
      lambda = calc_lambda(l1,ln,n)
      if (exp(lambda).le.1.) then
         do i=1, n
            xl(i) = ln*real(i)/real(n)
         enddo
      else
         xl(1) = l1
         do i=2, n
            xl(i) = xl(i-1) + l1*exp(lambda*real(i-1))
         enddo
      endif
      do i=1, n
         xy(1,i) = x1 + xl(i)*norm_x
         xy(2,i) = y1 + xl(i)*norm_y
      enddo
c
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine calc_rf(x,y,alfa,rho,phi)
c
c Coordinate transformation - calculates rho and phi
c for a given set of x and y
c
      implicit none
      double precision  x,y,alfa,rho,phi,xn,yn,xy_p,xy_m,t1,t2,
     &                  f,fprim,eps,one,four
      parameter (one=1.0, four=4.0)
      eps = 1.e-10
c Compute rho
      xn = x/alfa
      yn = y/alfa
      xy_p = xn*xn + yn*yn
      xy_m = xn*xn - yn*yn
c Find the root (Newton Raphson's method):
      t1 =  xy_p + 2.0
 10   continue
         f = one + t1*( -xy_p + t1*( 2.0*(xy_m - one) + t1*(t1-xy_p)))
         fprim=(-xy_p + t1*(four*(xy_m-one)+t1*(four*t1-3.0*xy_p) ))
         t2 = t1 - f / fprim
         if ( abs( (t2-t1) / t2 ) .lt. eps) then
            goto 20
         else
           t1 = t2
           goto 10
         endif
20    continue
      if (t2.lt.one) then
         t1=1.d0/t2
         goto 10
      endif
      rho=dsqrt(t2)
c phi = 0.5 * acosd((xyn-t2-1.d0/t2)*0.5)
      phi = acosd( xn / (rho+1.0/rho) )
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function ellipse_l(phi)
c
      implicit none
      double precision phi,a,b,f
      common /ellipse/ a,b
c
      f = sqrt( (a*sin(phi))**2.0 + (b*cos(phi))**2.0 )
      ellipse_l = f
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function hyperbel_l(rho)
c
      implicit none
      double precision rho,a,b,f
      common /hyperbel/ a,b
c
      f = sqrt((a*(1.-1./(rho*rho)))**2.0+(b*(1.+1./(rho*rho)))**2.0)
      hyperbel_l = f
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      double precision function hyper_l(r)
c
      implicit none
      double precision r,psi,phi,phip,c,s,f
      double precision phi_fcn4
c
      double precision alpha,r1,r2,p1,p2
      common   /hyper/ alpha,r1,r2,p1,p2
c
      psi = (r-r1) / (r2-r1)
      phi = phi_fcn4(phip,r,r1,r2,p1,p2)
      c = cos(phi)
      s = sin(phi)
c
      f =   ( (1.0 - 1.0/(r*r))*c - (r + 1.0/r )*phip*s )**2.0
     &    + ( (1.0 + 1.0/(r*r))*s + (r - 1.0/r )*phip*c )**2.0
      f = alpha*sqrt(f)
      hyper_l = f
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn0(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi
      psi  = sqrt( (r-r1) / (r2-r1) )
      phi  =  p1*(1.0-psi) + p2*psi
c the derivative is singular at psi=0
      phip = (p2-p1) / (2.*sqrt(psi)*(r2-r1))
      phi_fcn0 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn1(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi
      psi  = (r-r1) / (r2-r1)
      phi  =  p1*(1.0-psi) + p2*psi
      phip = (p2-p1) / (r2-r1)
      phi_fcn1 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn2(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi
      psi  = (r-r1) / (r2-r1)
      phi  = p1*(1.0-psi*psi) + p2*psi*psi
      phip = ( 2.0*(p2-p1) / (r2-r1) )*psi
      phi_fcn2 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn3(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi,eta
      psi  = (r-r1) / (r2-r1)
      eta  = 0.5
      phi  = p1*( eta*(1.0-psi) + (1.0-eta)*(1.0-psi*psi) )
     &     + p2*( eta*(1.0-psi) + (1.0-eta)*(1.0-psi*psi) )
      phip = eta * (p2-p1)/(r2-r1)
     &    + (1.0-eta) * (2.*(p2-p1)/(r2-r1))*psi
      phi_fcn3 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn4(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi,pi2
      pi2 = 2.0*atan(1.0)
      psi  = sin( pi2 * (r-r1)/(r2-r1) )
      phi  = p1*(1.0-psi) + p2*psi
      phip = ( pi2*(p2-p1) / (r2-r1) )*cos(pi2*psi)
      phi_fcn4 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      double precision function phi_fcn5(phip,r,r1,r2,p1,p2)
      implicit none
      double precision phi,phip,r,r1,r2,p1,p2,psi,pi2
      pi2 = 2.0*atan(1.0)
      psi  = cos( pi2 * (r-r1)/(r2-r1) )
      phi  = p1*(1.0-psi) + p2*psi
      phip = ( pi2*(p2-p1) / (r2-r1) )*sin(pi2*psi)
      phi_fcn5 = phi
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712
c
c Below, routines "inspired" from Numerical Recipies
c
c----67--1---------2---------3---------4---------5---------6---------712
c
      subroutine qromb(func,a,b,ss,eps,iws)
      implicit none
c
      double precision a,b,ss,func
      external func
c
      integer  jmax,jmaxp,k,km, iws
      double precision eps
c      parameter (eps=1.d-5, jmax=20, jmaxp=jmax+1, k=5, km=k-1)
      parameter (jmax=20, jmaxp=jmax+1, k=5, km=k-1)
c
      integer  j
      double precision dss, h(jmaxp), s(jmaxp)
c
      h(1) = 1.d0
      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.d0, ss, dss, iws)
            if (abs(dss).le.eps*abs(ss)) return
         endif
         s(j+1)=s(j)
         h(j+1)=0.25d0*h(j)
      enddo
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine trapzd(func, a, b, s, n)
      implicit none
c
      integer  n
      double precision a,b,s,func
      external func
c
      integer it,j
      double precision del, sum, tnm, x
c
      if (n.eq.1) then
         s = 0.5d0*(b-a)*(func(a)+func(b))
      else
         it = 2**(n-2)
         tnm=dble(it)
         del=(b-a)/tnm
         x = a + 0.5d0*del
         sum = 0.d0
         do j=1, it
            sum = sum + func(x)
            x = x + del
         enddo
         s = 0.5d0*(s+(b-a)*sum/tnm)
      endif
      return
      end
c
c----67--1----*----2----*----3----*----4----*----5----*----6----*----712
c
      subroutine polint(xa, ya, n, x, y, dy, iws)
      implicit none
c
      integer  n,nmax,iws
      double precision dy,x,y,xa(n),ya(n)
      parameter  (nmax=10)
c
      integer  i,m,ns
      double precision den,dif,dift,ho,hp,w,c(nmax),d(nmax)
c
      ns = 1
      dif = abs(x-xa(1))
      do i=1, n
         dift=abs(x-xa(i))
         if (dift.lt.dif) then
            ns=i
            dif = dift
         endif
         c(i) = ya(i)
         d(i) = ya(i)
      enddo
      y = ya(ns)
      ns = ns - 1
      do m=1, n-1
         do i=1, n-m
            ho = xa(i)-x
            hp = xa(i+m)-x
            w  = c(i+1)-d(i)
            den= ho - hp
ccc            if (den.eq.0.d0) write(*,*) '>> Failure in subr. polint!'
            if (den.eq.0.d0) write(iws,*) '>> Failure in subr. polint!'
            den = w/den
            d(i) = hp*den
            c(i) = ho*den
         enddo
         if (2*ns.lt.(n-m)) then
            dy = c(ns+1)
         else
            dy = d(ns)
            ns = ns-1
         endif
         y = y+dy
      enddo
      return
      end
c
c----67--1---------2---------3---------4---------5---------6---------712


module type_oscillator_model
    use mod_kinds,          only: rk, ik
    use mod_constants,      only: ZERO, ONE, TWO, PI
    use mod_rigid_body_motion, only: rigid_body_motion_disp_old, rigid_body_motion_disp_new, &
                                    rigid_body_motion_vel, rigid_body_t0, rigid_body_t1
    use mod_chidg_mpi,      only: IRANK, GLOBAL_MASTER
    implicit none

    type    :: oscillator_model_t
        ! Linearly damped oscillator ODE model
        ! mass*x'' + damping_coeff*x' + stiffness_coeff*x = external_force
        real(rk)    :: mass = ONE
        real(rk)    :: damping_coeff(3) = ONE
        real(rk)    :: stiffness_coeff(3) = ONE
        real(rk)    :: external_forces(3) = ZERO

        real(rk)    :: undamped_angular_frequency(3)
        real(rk)    :: undamped_natural_frequency(3)
        real(rk)    :: damping_factor(3)
        real(rk)    :: minimum_stable_timestep

        character(:), allocatable :: damping_type

        real(rk)    :: eq_pos(3)                        ! Equilibrium position
        real(rk)    :: pos(3)                         ! Displaced position

        real(rk)    :: disp(2,3)                        ! Displacement
        real(rk)    :: vel(2,3)                         ! Velocity

        real(rk), allocatable, dimension(:,:)   :: history_pos(:,:), history_vel(:,:), history_force(:,:)

    contains

        procedure :: init

        procedure :: set_external_forces
        procedure :: update_disp
        procedure :: update_vel
        procedure :: update_oscillator_step
        procedure :: update_oscillator_subcycle_step

    end type oscillator_model_t

contains

    subroutine init(self, mass_in, damping_coeff_in, stiffness_coeff_in, initial_displacement_in, initial_velocity_in)
        class(oscillator_model_t), intent(inout)    :: self
        real(rk),intent(in),optional         :: mass_in
        real(rk),intent(in),optional         :: damping_coeff_in(3)
        real(rk),intent(in),optional         :: stiffness_coeff_in(3)
        real(rk),intent(in),optional         :: initial_displacement_in(3)
        real(rk),intent(in),optional         :: initial_velocity_in(3)

        real(rk) :: tol
        real(rk) :: mass, damping_coeff(3), stiffness_coeff(3), initial_displacement(3), initial_velocity(3), external_forces(3),t0
        integer(ik)             :: unit, msg, myunit
        logical             :: file_exists, exists

        namelist /viv_cylinder/    mass,&
                                    damping_coeff, &
                                    stiffness_coeff, &
                                    initial_displacement, &
                                    initial_velocity

        tol = 1.0e-14

        self%disp = ZERO
        self%pos = ZERO
        self%vel = ZERO
        self%external_forces = ZERO

        if (present(mass_in)) then
            self%mass = mass_in
        end if

        if (present(damping_coeff_in)) then
            self%damping_coeff = damping_coeff_in
        end if

        if (present(stiffness_coeff_in)) then
            self%stiffness_coeff = stiffness_coeff_in
        end if
        if (present(initial_displacement_in)) then
            self%disp(1,:) = initial_displacement_in
        end if
        if (present(initial_velocity_in)) then
            self%vel(1,:) = initial_velocity_in
        end if
        !
        ! Check if input from 'models.nml' is available.
        !   1: if available, read and set self%mu
        !   2: if not available, do nothing and mu retains default value
        !
        inquire(file='structural_models.nml', exist=file_exists)
        if (file_exists) then
            open(newunit=unit,form='formatted',file='structural_models.nml')
            read(unit,nml=viv_cylinder,iostat=msg)
            if (msg == 0) self%mass = mass
            if (msg == 0) self%damping_coeff = damping_coeff
            if (msg == 0) self%stiffness_coeff = stiffness_coeff
            if (msg == 0) self%disp(1,:) = initial_displacement
            if (msg == 0) self%vel(1,:) = initial_velocity
            close(unit)
        end if

        self%undamped_angular_frequency = sqrt(self%stiffness_coeff/self%mass)
        self%undamped_natural_frequency = self%undamped_angular_frequency/(TWO*PI)
        self%damping_factor = self%damping_coeff/(TWO*sqrt(self%mass*self%stiffness_coeff))

        !
        ! Compute the minimum stable timestep size for the lepfrog algorithm.
        ! dt < 2/ang_freq
        !

        self%minimum_stable_timestep = TWO/(maxval(self%undamped_angular_frequency))

        if (maxval(self%damping_factor) > ONE + tol) then
            self%damping_type = 'overdamped'
        else if (maxval(self%damping_factor) < ONE-tol) then
            self%damping_type = 'underdamped'
        else
            self%damping_type = 'critically damped'
        end if
!        print *, 'Oscillator mass'
!        print *, self%mass
!        print *, 'Oscillator damping coefficients'
!        print *, self%damping_coeff
!        print *, 'Oscillator stiffness coefficients'
!        print *, self%stiffness_coeff
!        print *, 'Oscillaing cylinder damping type:'
!        print *, self%damping_type
!
!        print *, 'Oscillating cylinder minimum stable time step size:'
!        print *, self%minimum_stable_timestep

        rigid_body_motion_disp_new = self%disp(1,:)
        rigid_body_motion_vel = self%vel(1,:)

        external_forces = ZERO
        t0 = ZERO

!        ! Write initial state to file to files
!        !
!        if (IRANK == GLOBAL_MASTER) then
!        inquire(file="viv_output.txt", exist=exists)
!        if (exists) then
!            open(newunit=myunit, file="viv_output.txt", status="old", position="append",action="write")
!        else
!            open(newunit=myunit, file="viv_output.txt", status="new",action="write")
!        end if
!        write(myunit,*) t0, rigid_body_motion_disp_new(1), rigid_body_motion_disp_new(2),  &
!                                 rigid_body_motion_vel(1), rigid_body_motion_vel(2), &
!                                 external_forces(1),external_forces(2)
!        close(myunit)
!        end if

    end subroutine init

    subroutine set_external_forces(self, external_forces)
        class(oscillator_model_t)    :: self
        real(rk)                :: external_forces(3)

        self%external_forces = external_forces

    end subroutine set_external_forces

    subroutine update_disp(self,dt_struct)
        class(oscillator_model_t) :: self
        real(rk)                :: dt_struct

        self%disp(2,:) = self%disp(1,:) + dt_struct*self%vel(1,:)
        self%disp(1,:) = self%disp(2,:)

    end subroutine update_disp

    subroutine update_vel(self,dt_struct)
        class(oscillator_model_t) :: self
        real(rk)                :: dt_struct

        real(rk) :: gam(3), mass
        !
        ! Special version of the Leapfrog algorithm for linear damping
        !

        gam = self%damping_coeff
        mass = self%mass

        self%vel(2,:) = ((ONE - gam*dt_struct/(TWO*mass))*self%vel(1,:) + &
            dt_struct*(self%external_forces-self%stiffness_coeff*self%disp(1,:))/mass)/ &
            (ONE + gam*dt_struct/(TWO*mass))

        self%vel(1,:) = self%vel(2,:)

    end subroutine update_vel

    subroutine update_oscillator_subcycle_step(self, dt_struct, external_forces)
        class(oscillator_model_t) :: self
        real(rk)                :: dt_struct
        real(rk)                :: external_forces(3)

        call self%set_external_forces(external_forces)
        call self%update_vel(dt_struct)
        call self%update_disp(dt_struct)

    end subroutine update_oscillator_subcycle_step

    subroutine update_oscillator_step(self, dt_fluid, t0_in, external_forces)
        class(oscillator_model_t) :: self
        real(rk)                :: dt_fluid
        real(rk)                :: t0_in
        real(rk)                :: external_forces(3)

        real(rk)                :: dt_struct

        integer(ik)             :: nsteps, istep, max_steps

        integer(ik) :: myunit
        logical :: exists

        integer(ik)             :: unit, msg
        logical             :: file_exists

        real(rk)                :: mass
        real(rk), dimension(3)  :: damping_coeff, stiffness_coeff, initial_displacement, initial_velocity
        namelist /viv_cylinder/    mass,&
                                    damping_coeff, &
                                    stiffness_coeff, &
                                    initial_displacement, &
                                    initial_velocity

        !
        ! Perform update
        !

        rigid_body_t0 = t0_in
        rigid_body_t1 = t0_in+dt_fluid
        rigid_body_motion_disp_old = rigid_body_motion_disp_new
        ! Check stability of the initial time step and decrease it until it becomes stable
        dt_struct = dt_fluid
        nsteps = 1
        max_steps = 1000

        do while ((dt_struct> 0.1_rk*self%minimum_stable_timestep) .and. (nsteps < max_steps))
            dt_struct = dt_struct/TWO
            nsteps = nsteps*2
        end do

        do istep = 1, nsteps

            call self%update_oscillator_subcycle_step(dt_struct, external_forces)

        end do

        !Force one DOF motion
        self%disp(1,1) = ZERO
        self%disp(1,3) = ZERO
        self%vel(1,1) = ZERO
        self%vel(1,3) = ZERO

        rigid_body_motion_disp_new = self%disp(1,:)
        rigid_body_motion_vel = self%vel(1,:)

        !
        ! Write to files
        !
        if (IRANK == GLOBAL_MASTER) then
        inquire(file="viv_output.txt", exist=exists)
        if (exists) then
            open(newunit=myunit, file="viv_output.txt", status="old", position="append",action="write")
        else
            open(newunit=myunit, file="viv_output.txt", status="new",action="write")
        end if
        write(myunit,*) t0_in, rigid_body_motion_disp_new(1), rigid_body_motion_disp_new(2),  &
                                 rigid_body_motion_vel(1), rigid_body_motion_vel(2), &
                                 external_forces(1),external_forces(2)
        close(myunit)
        end if

        !
        ! Write the new position and velocity to models.nml for restart purposes
        !

        mass                    = self%mass
        damping_coeff           = self%damping_coeff(1:3)
        stiffness_coeff         = self%stiffness_coeff(1:3)
        initial_displacement    = self%disp(1,:)
        initial_velocity        = self%vel(1,:)

        if (IRANK == GLOBAL_MASTER) then
        inquire(file='structural_models.nml', exist=file_exists)
        if (file_exists) then
            open(newunit=unit,form='formatted',file='structural_models.nml')
            write(unit,nml=viv_cylinder,iostat=msg)
            close(unit)
        end if
        end if

    end subroutine update_oscillator_step
end module type_oscillator_model