!> \file mo_sampling.f90

!> \brief Random and Latin Hypercube Sampling.

!> \details Random and Latin Hypercube Sampling for a set of parameters with Uniform(0,1) or Gaussian(0,1) Distribution.

!> \authors Giovanni Dalmasso
!> \date Apr 2013

module mo_sampling

  ! This module provides random and latin hypercube sampling routines.

  ! Written  Giovanni Dalmasso, Apr 2013

  ! License
  ! -------
  ! This file is part of the JAMS Fortran package, distributed under the MIT License.
  !
  ! Copyright (c) 2012-2013 Giovanni Dalmasso
  !
  ! 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.

  implicit none

  public :: random        ! Random Sampling
  public :: lhs           ! Latin Hypercube Sampling
  public :: setupxor      ! Initialize xor4096.f90 (uniform distribution)
  public :: setupxor_g    ! Initialize xor4096.f90 (gaussian distribution)

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

  !     NAME
  !         random

  !     PURPOSE
  !         Calculate the Random Sampling for a set of parameters with Uniform(0,1) or Gaussian(0,1) Distribution.
  !
  !>        \brief Random Sampling.
  !
  !>        \details Calculate the Random Sampling for a set of $N$ parameters with Uniform(0,1) or Gaussian(0,1) Distribution.
  !
  !     INTENT(IN)
  !>        \param[in] "integer(i4)                                      :: Nsample"             number of samples
  !>        \param[in] "integer(i4)                                      :: Npar"                number of parameters
  !
  !     INTENT(INOUT)
  !>        \param[in] "integer(i4/i8),  dimension(Npar, n_save_state)   :: save_the_setup(:,:)"  xor4096 variable
  !
  !     INTENT(OUT)
  !         None
  !
  !     INTENT(IN), OPTIONAL
  !>        \param[in] "logical,      dimension(Npar),    optional       :: dist(:)"             distributions
  !>                                                                                if true = Uniform
  !>                                                                                if false = Gaussian
  !>                                                                                DEFAULT: true
  !
  !     INTENT(INOUT), OPTIONAL
  !         None
  !
  !     INTENT(OUT), OPTIONAL
  !         None
  !
  !     RETURN
  !>       \return     real(sp/dp),       dimension(Nsample, Npar)       :: randomSamp(:,:)       Random Sampling
  !
  !     RESTRICTIONS
  !>       \note The random number generator has to be initialized before and therefore is called without a seed.
  !
  !     EXAMPLE
  !         -> see also example in test directory
  !
  !     LITERATURE
  !         none
  !
  !     HISTORY
  !>        \author Giovanni Dalmasso
  !>        \date Apr 2013
  !         Modified,
  !
  interface random
     module procedure random_sp, random_dp
  end interface random

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

  !     NAME
  !         lhs

  !     PURPOSE
  !         Calculate the Latin Hypercube Sampling for a set of parameters with Uniform(0,1) or Gaussian(0,1) Distribution.
  !
  !>        \brief Random Sampling.
  !
  !>        \details Calculate the Latin Hypercube Sampling for a set of $N$ parameters with Uniform(0,1)
  !>                  or Gaussian(0,1) Distribution.
  !
  !     INTENT(IN)
  !>        \param[in] "integer(i4)                                      :: Nsample"             number of samples
  !>        \param[in] "integer(i4)                                      :: Npar"                number of parameters
  !
  !     INTENT(INOUT)
  !>        \param[in] "integer(i4/i8),  dimension(Npar, n_save_state)   :: save_the_setup(:,:)"  xor4096 variable
  !
  !     INTENT(OUT)
  !         None
  !
  !     INTENT(IN), OPTIONAL
  !>        \param[in] "logical,      dimension(Npar),    optional       :: dist(:)"             distributions
  !>                                                                                if true = Uniform
  !>                                                                                if false = Gaussian
  !>                                                                                DEFAULT: true
  !
  !     INTENT(INOUT), OPTIONAL
  !         None
  !
  !     INTENT(OUT), OPTIONAL
  !         None
  !
  !     RETURN
  !>       \return     real(sp/dp),       dimension(Nsample, Npar)       :: lhsSamp(:,:)          Latin Hypercube Sampling
  !
  !     RESTRICTIONS
  !>       \note The random number generator has to be initialized before and therefore is called without a seed.
  !>             There is an i4 stream initialized inside the method.
  !>             Fortran90 implementation adapted from Chad Sprouse's C version
  !>             (http://home.online.no/~pjacklam/notes/invnorm/impl/sprouse/ltqnorm.c)
  !
  !     EXAMPLE
  !         -> see also example in test directory
  !
  !     LITERATURE
  !         Stein, M. 1987. Large Sample Properties of Simulations Using Latin Hypercube Sampling. Technometrics 29:143-151
  !
  !     HISTORY
  !>        \author Giovanni Dalmasso
  !>        \date Apr 2013
  !         Modified,
  !
  interface lhs
     module procedure lhs_sp, lhs_dp
  end interface lhs

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

  !     NAME
  !         setupxor

  !     PURPOSE
  !         Set up the module mo_xor4096.f90 with uniform distribution.
  !
  !>        \brief Random Sampling.
  !
  !>        \details The random number generator has to be initialized before using.
  !
  !     INTENT(IN)
  !>         \param[in] "integer(i4)                                              ::    sizeArray"            number of samples
  !>         \param[in] "logical,      dimension(sizeArray)                       ::    distribution"         distributions
  !>                                                                                if true = Uniform
  !>                                                                                if false = Gaussian
  !
  !     INTENT(INOUT)
  !         None
  !
  !     INTENT(OUT)
  !!>        \param[out] "integer(i4/i8),    dimension(sizeArray, n_save_state)    ::    save_the_setup(:,:)
  !
  !     INTENT(IN), OPTIONAL
  !>          \param[in] "integer(i4/i8),    dimension(sizeArray),   optional      ::    seed(:)"              seed
  !
  !     INTENT(INOUT), OPTIONAL
  !         None
  !
  !     INTENT(OUT), OPTIONAL
  !         None
  !
  !
  !     RESTRICTIONS
  !>       \note The intent in variable sizeArray could be omitted if the problem is scalar (dim=0).
  !>              Therefore save_the_setup will have only one dimension equal to sizeArray and the seed will be a scalar.
  !>              If distribution is omitted the default is uniform.
  !
  !     EXAMPLE
  !         -> see also example in test directory
  !
  !     LITERATURE
  !         None
  !
  !     HISTORY
  !>        \author Giovanni Dalmasso
  !>        \date Apr 2013
  !         Modified,
  !
  interface setupxor
     module procedure setupxor4096_i4_0d, &
          setupxor4096_sp_1d, setupxor4096_dp_1d, &
          setupxor4096_sp_1d_dist, setupxor4096_dp_1d_dist
  end interface setupxor

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

  !     NAME
  !         setupxor_g

  !     PURPOSE
  !         Set up the module mo_xor4096.f90 with gaussian distribution.
  !
  !>        \brief Random Sampling.
  !
  !>        \details The random number generator has to be initialized before using.
  !
  !     INTENT(IN)
  !>         \param[in] "integer(i4)                                              ::    sizeArray"            number of samples
  !>         \param[in] "logical,      dimension(sizeArray)                       ::    distribution"         distributions
  !>                                                                                if true = Uniform
  !>                                                                                if false = Gaussian
  !
  !     INTENT(INOUT)
  !         None
  !
  !     INTENT(OUT)
  !!>        \param[out] "integer(i4/i8),    dimension(sizeArray, n_save_state)    ::    save_the_setup(:,:)
  !
  !     INTENT(IN), OPTIONAL
  !>          \param[in] "integer(i4/i8),    dimension(sizeArray),   optional      ::    seed(:)"              seed
  !
  !     INTENT(INOUT), OPTIONAL
  !         None
  !
  !     INTENT(OUT), OPTIONAL
  !         None
  !
  !
  !     RESTRICTIONS
  !>       \note The intent in variable sizeArray could be omitted if the problem is scalar (dim=0).
  !>              Therefore save_the_setup will have only one dimension equal to sizeArray and the seed will be a scalar.
  !>              If distribution is omitted the default is uniform.
  !
  !     EXAMPLE
  !         -> see also example in test directory
  !
  !     LITERATURE
  !         None
  !
  !     HISTORY
  !>        \author Giovanni Dalmasso
  !>        \date Apr 2013
  !         Modified,
  !
  interface setupxor_g
     module procedure setupxor4096g_sp_1d, setupxor4096g_dp_1d
  end interface setupxor_g

  private

  ! ------------------------------------------------------------------
  ! Private Method
  ! Lower tail quantile for standard normal distribution function
  interface ltqnorm
     module procedure &
          ltqnorm_sp_d1, ltqnorm_dp_d1
          ! ltqnorm_sp_d0, ltqnorm_dp_d0,
  end interface ltqnorm

contains

  ! Random Sampling SINGLE PRECISION
  function random_sp( Nsample, Npar, save_the_setup, dist )    result(randomSamp)

    use mo_kind,            only    :   i4, sp
    use mo_xor4096,         only    :   xor4096, xor4096g, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                    intent(in)      ::  Nsample             ! number of samples
    integer(i4),                                    intent(in)      ::  Npar                ! number of parameters
    logical,        dimension(Npar),    optional,   intent(in)      ::  dist                ! distribution (unif, norm)

    ! Intent InOut
    integer(i4),    dimension(Npar, n_save_state),  intent(inout)   ::  save_the_setup      ! xor4096 variable

    ! Results
    real(sp),       dimension(Nsample, Npar)                        ::  randomSamp          ! random sampling

    ! Internal variables
    integer(i4)                         ::  j, jj   ! counters
    logical,        dimension(Npar)     ::  local_dist

    if ( Npar .eq. 0_i4 )   stop 'random_sp --> number of parameters could not be zero!!!'

    if( present(dist) ) then
       local_dist = dist
    else
       local_dist = .true.
    end if

    do j=1, Nsample
       do jj=1, Npar
          if ( local_dist(jj) )   then
             call xor4096( 0_i4, randomSamp(j,jj), save_state=save_the_setup(jj,:) )
          else
             call xor4096g( 0_i4, randomSamp(j,jj), save_state=save_the_setup(jj,:) )
          end if
       end do
    end do

  end function random_sp

  ! Random Sampling DOUBLE PRECISION
  function random_dp( Nsample, Npar, save_the_setup, dist )    result(randomSamp)

    use mo_kind,            only    :   i4, i8, dp
    use mo_xor4096,         only    :   xor4096, xor4096g, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                    intent(in)      ::  Nsample             ! number of samples
    integer(i4),                                    intent(in)      ::  Npar                ! number of parameters
    logical,        dimension(Npar),    optional,   intent(in)      ::  dist                ! distribution (unif, norm)

    ! Intent InOut
    integer(i8),    dimension(Npar, n_save_state),  intent(inout)   ::  save_the_setup      ! xor4096 variable

    ! Results
    real(dp),       dimension(Nsample, Npar)                        ::  randomSamp          ! random sampling

    ! Internal variables
    integer(i4)                         ::  j, jj   ! counters
    logical,        dimension(Npar)     ::  local_dist

    if ( Npar .eq. 0_i4 )   stop 'random_dp --> number of parameters could not be zero!!!'

    if( present(dist) ) then
       local_dist = dist
    else
       local_dist = .true.
    end if

    do j=1, Nsample
       do jj=1, Npar
          if ( local_dist(jj) )   then
             call xor4096( 0_i8, randomSamp(j,jj), save_state=save_the_setup(jj,:) )
          else
             call xor4096g( 0_i8, randomSamp(j,jj), save_state=save_the_setup(jj,:) )
          end if
       end do
    end do

  end function random_dp

  ! Latin Hypercube Sampling SINGLE PRECISION
  function lhs_sp( Nsample, Npar, save_the_setup, dist )    result(lhsSamp)

    use mo_kind,            only    :   i4, sp
    use mo_xor4096,         only    :   n_save_state
    use mo_xor4096_apps,    only    :   xor4096_array
    use mo_combinatorics,   only    :   random_index_permut

    implicit none

    ! Intent IN
    integer(i4),                                    intent(in)      ::  Nsample         ! number of samples
    integer(i4),                                    intent(in)      ::  Npar            ! number of parameters
    logical,        dimension(Npar),    optional,   intent(in)      ::  dist            ! distribution (unif, norm)

    ! Intent InOut
    integer(i4),    dimension(Npar, n_save_state),  intent(inout)   ::  save_the_setup  ! xor4096 variable

    ! Results
    real(sp),       dimension(Nsample, Npar)                        ::  lhsSamp         ! random sampling

    ! Internal variables
    integer(i4)                                 ::  j                   ! counter
    integer(i4)                                 ::  seed                ! seed
    integer(i4),    dimension(n_save_state)     ::  save_the_setup_i4   ! setup xor4096 i4 stream
    integer(i4),    dimension(Nsample)          ::  idx
    real(sp),       dimension(Nsample, Npar)    ::  ranSam              ! random sampling
    real(sp),       dimension(Nsample)          ::  p
    logical,        dimension(Npar)             ::  local_dist

    if ( Npar .eq. 0_i4 )   stop 'lhs_sp --> number of parameters could not be zero!!!'

    if( present(dist) ) then
       local_dist = dist
    else
       local_dist = .true.
    end if

    ! random sampling matrix
    call xor4096_array( ranSam, save_state=save_the_setup )

    ! seed for setup xor4096 i4 stream
    if ( save_the_setup(1,1) .gt. huge(1_i4)/save_the_setup(1,2) )  then
       seed = save_the_setup(3,1)
    else
       seed = save_the_setup(1,1)*save_the_setup(1,2) + save_the_setup(1,3)
    end if
    ! setup xor4096 i4 stream
    call setupxor( save_the_setup_i4, seed=seed )

    ! Latin Hypercube Sampling
    idx = random_index_permut( Nsample, save_the_setup_i4 )
    do j=1, Npar
       p = ( real(idx,sp) - ranSam(:,j) )/real(Nsample,sp)
       if( local_dist(j) )    then
          lhsSamp(:,j) = p                ! --> unif
       else
          lhsSamp(:,j) = ltqnorm(p(:))    ! --> gauss
       end if
    end do

  end function lhs_sp

  ! Latin Hypercube Sampling DOUBLE PRECISION
  function lhs_dp( Nsample, Npar, save_the_setup, dist )    result(lhsSamp)

    use mo_kind,            only    :   i4, i8, dp
    use mo_xor4096,         only    :   n_save_state
    use mo_xor4096_apps,    only    :   xor4096_array
    use mo_combinatorics,   only    :   random_index_permut

    implicit none

    ! Intent IN
    integer(i4),                                    intent(in)      ::  Nsample         ! number of samples
    integer(i4),                                    intent(in)      ::  Npar            ! number of parameters
    logical,        dimension(Npar),    optional,   intent(in)      ::  dist            ! distribution (unif, norm)

    ! Intent InOut
    integer(i8),    dimension(Npar, n_save_state),  intent(inout)   ::  save_the_setup  ! xor4096 variable

    ! Results
    real(dp),       dimension(Nsample, Npar)                        ::  lhsSamp         ! random sampling

    ! Internal variables
    integer(i4)                                 ::  j                   ! counter
    integer(i4)                                 ::  seed                ! seed
    integer(i4),    dimension(n_save_state)     ::  save_the_setup_i4   ! setup xor4096 i4 stream
    integer(i4),    dimension(Nsample)          ::  idx
    real(dp),       dimension(Nsample, Npar)    ::  ranSam              ! random sampling
    real(dp),       dimension(Nsample)          ::  p
    logical,        dimension(Npar)             ::  local_dist

    if ( Npar .eq. 0_i4 )   stop 'lhs_dp --> number of parameters could not be zero!!!'

    if ( present(dist) ) then
       local_dist = dist
    else
       local_dist = .true.
    end if

    ! random sampling matrix
    call xor4096_array( ranSam, save_state=save_the_setup )

    ! seed for setup xor4096 i4 stream
    if ( save_the_setup(1,1) .gt. huge(1_i4)/save_the_setup(1,2) )  then
       seed = int( save_the_setup(1,3),i4 )
    else
       seed = int( save_the_setup(1,1)*save_the_setup(1,2) + save_the_setup(1,3), i4 )
    end if
    ! setup xor4096 i4 stream
    call setupxor( save_the_setup_i4, seed=seed )

    ! Latin Hypercube Sampling
    idx = random_index_permut( Nsample, save_the_setup_i4 )
    do j=1, Npar
       p = ( real(idx,dp) - ranSam(:,j) )/real(Nsample,dp)
       if( local_dist(j) )    then
          lhsSamp(:,j) = p                ! --> unif
       else
          lhsSamp(:,j) = ltqnorm(p(:))    ! --> gauss
       end if
    end do

  end function lhs_dp

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

  ! SETUP XOR

  subroutine setupxor4096_i4_0d( save_the_setup, seed )

    use mo_kind,    only : i4
    use mo_xor4096, only : get_timeseed, xor4096, n_save_state

    implicit none

    ! Intent OUT
    integer(i4),    dimension(n_save_state),    intent(out) ::  save_the_setup

    ! Intent IN optional
    integer(i4),            optional,           intent(in)  ::  seed

    ! Internal variables
    integer(i4)             :: seedin
    integer(i4)             :: dummyRn

    if ( present(seed) ) then
       seedin = seed
    else
       call get_timeseed( seedin )
    end if

    call xor4096( seedin, dummyRn, save_state=save_the_setup )
    seedin = 0_i4

  end subroutine setupxor4096_i4_0d

  subroutine setupxor4096_sp_1d( sizeArray, save_the_setup, seed )

    use mo_kind,    only : i4, sp
    use mo_xor4096, only : get_timeseed, xor4096, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  ::  sizeArray

    ! Intent OUT
    integer(i4),    dimension(sizeArray, n_save_state), intent(out) ::  save_the_setup

    ! Intent IN optional
    integer(i4),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i4),    dimension(sizeArray)  :: seedin
    real(sp),       dimension(sizeArray)  :: dummyRn

    if ( present(seed) ) then
       seedin(:) = seed(:)
    else
       call get_timeseed( seedin )
    end if

    call xor4096( seedin, dummyRn, save_state=save_the_setup )
    seedin(:) = 0_i4

  end subroutine setupxor4096_sp_1d

  subroutine setupxor4096_dp_1d( sizeArray, save_the_setup, seed )

    use mo_kind,    only : i4, i8, dp
    use mo_xor4096, only : get_timeseed, xor4096, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  ::  sizeArray

    ! Intent OUT
    integer(i8),    dimension(sizeArray, n_save_state), intent(out) ::  save_the_setup

    ! Intent IN optional
    integer(i8),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i8),    dimension(sizeArray)  :: seedin
    real(dp),       dimension(sizeArray)  :: dummyRn

    if ( present(seed) ) then
       seedin(:) = seed(:)
    else
       call get_timeseed( seedin )
    end if

    call xor4096( seedin, dummyRn, save_state=save_the_setup )
    seedin(:) = 0_i8

  end subroutine setupxor4096_dp_1d

  subroutine setupxor4096g_sp_1d( sizeArray, save_the_setup_g, seed )

    use mo_kind,    only : i4, sp
    use mo_xor4096, only : get_timeseed, xor4096g, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  :: sizeArray

    ! Intent OUT
    integer(i4),    dimension(sizeArray, n_save_state), intent(out) :: save_the_setup_g

    ! Intent IN optional
    integer(i4),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i4),    dimension(sizeArray)  :: seedin
    real(sp),       dimension(sizeArray)  :: dummyRn

    if ( present(seed) ) then
       seedin(:) = seed(:)
    else
       call get_timeseed( seedin )
    end if

    call xor4096g( seedin, dummyRn, save_state=save_the_setup_g )
    seedin(:) = 0_i4

  end subroutine setupxor4096g_sp_1d

  subroutine setupxor4096g_dp_1d( sizeArray, save_the_setup_g, seed )

    use mo_kind,    only : i4, i8, dp
    use mo_xor4096, only : get_timeseed, xor4096g, n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  :: sizeArray

    ! Intent OUT
    integer(i8),    dimension(sizeArray, n_save_state), intent(out) :: save_the_setup_g

    ! Intent IN optional
    integer(i8),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i8),    dimension(sizeArray)  :: seedin
    real(dp),       dimension(sizeArray)  :: dummyRn

    if ( present(seed) ) then
       seedin(:) = seed(:)
    else
       call get_timeseed( seedin )
    end if

    call xor4096g( seedin, dummyRn, save_state=save_the_setup_g )
    seedin(:) = 0_i8

  end subroutine setupxor4096g_dp_1d

  subroutine setupxor4096_sp_1d_dist( sizeArray, distribution, save_the_setup, seed )

    use mo_kind,    only : i4
    use mo_xor4096, only : n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  :: sizeArray
    logical,        dimension(sizeArray),               intent(in)  :: distribution

    ! Intent OUT
    integer(i4),    dimension(sizeArray, n_save_state), intent(out) :: save_the_setup

    ! Intent IN optional
    integer(i4),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i4)                                 ::  j                   ! counter
    integer(i4)                                 ::  Nunif, Ngauss       ! number of uniform and gaussian
    integer(i4),    dimension(:,:), allocatable ::  save_the_setup_u    ! xor variable unif
    integer(i4),    dimension(:,:), allocatable ::  save_the_setup_g    ! xor variable gauss

    Nunif = count(distribution)
    Ngauss = sizeArray - Nunif

    if ( allocated(save_the_setup_u) ) deallocate(save_the_setup_u)
    if ( allocated(save_the_setup_g) ) deallocate(save_the_setup_g)
    if ( Nunif .ne. 0_i4 )  allocate( save_the_setup_u(Nunif, n_save_state) )
    if ( Ngauss .ne. 0_i4 ) allocate( save_the_setup_g(Ngauss, n_save_state) )
    save_the_setup_u = 0_i4
    save_the_setup_g = 0_i4

    ! setup xor4096 - xor4096g
    if ( present(seed) ) then
       if ( Nunif .ne. 0_i4 )  call setupxor( Nunif, save_the_setup_u, seed=seed )
       if ( Ngauss .ne. 0_i4 ) call setupxor_g( Ngauss, save_the_setup_g, seed=seed )
    else
       if ( Nunif .ne. 0_i4 )  call setupxor( Nunif, save_the_setup_u )
       if ( Ngauss .ne. 0_i4 ) call setupxor_g( Ngauss, save_the_setup_g )
    end if

    do j=1,sizeArray
       if ( distribution(j) ) then
          save_the_setup(j,:) = save_the_setup_u(count(distribution(1:j)),:)
       else
          save_the_setup(j,:) = save_the_setup_g(count(.not.distribution(1:j)),:)
       end if
    end do

  end subroutine setupxor4096_sp_1d_dist

  subroutine setupxor4096_dp_1d_dist( sizeArray, distribution, save_the_setup, seed )

    use mo_kind,    only : i4, i8
    use mo_xor4096, only : n_save_state

    implicit none

    ! Intent IN
    integer(i4),                                        intent(in)  :: sizeArray
    logical,        dimension(sizeArray),               intent(in)  :: distribution

    ! Intent OUT
    integer(i8),    dimension(sizeArray, n_save_state), intent(out) :: save_the_setup

    ! Intent IN optional
    integer(i8),    dimension(sizeArray),   optional,   intent(in)  ::  seed

    ! Internal variables
    integer(i4)                                 ::  j, counter          ! counter
    integer(i4)                                 ::  Nunif, Ngauss       ! number of uniform and gaussian
    integer(i8),    dimension(:,:), allocatable ::  save_the_setup_u    ! xor variable unif
    integer(i8),    dimension(:,:), allocatable ::  save_the_setup_g    ! xor variable gauss
    integer(i8),    dimension(:),   allocatable ::  seedU
    integer(i8),    dimension(:),   allocatable ::  seedG

    Nunif = count(distribution)
    Ngauss = sizeArray - Nunif

    if ( allocated(save_the_setup_u) )  deallocate(save_the_setup_u)
    if ( allocated(save_the_setup_g) )  deallocate(save_the_setup_g)
    if ( allocated(seedU) )             deallocate(seedU)
    if ( allocated(seedG) )             deallocate(seedG)
    if ( Nunif .ne. 0_i4 )  then
       allocate( save_the_setup_u(Nunif, n_save_state) )
       allocate( seedU(Nunif) )
    end if
    if ( Ngauss .ne. 0_i4 ) then
       allocate( save_the_setup_g(Ngauss, n_save_state) )
       allocate( seedG(Ngauss) )
    end if

    ! setup xor4096 - xor4096g
    if ( present(seed) ) then
       ! seed for uniform
       if ( Nunif .ne. 0_i4 )  then
          counter = 1_i4
          do j=1, sizeArray
             if (distribution(j) )   then
                seedU(counter) = seed(j)
                counter = counter + 1_i4
             end if
          end do
          call setupxor( Nunif, save_the_setup_u, seed=seedU )
       end if
       ! seed for gaussian
       if ( Ngauss .ne. 0_i4 ) then
          counter = 1_i4
          do j=1, sizeArray
             if ( .not. distribution(j) )   then
                seedG(counter) = seed(j)
                counter = counter + 1_i4
             end if
          end do
          call setupxor_g( Ngauss, save_the_setup_g, seed=seedG )
       end if
    else
       if ( Nunif .ne. 0_i4 )  call setupxor( Nunif, save_the_setup_u )
       if ( Ngauss .ne. 0_i4 ) call setupxor_g( Ngauss, save_the_setup_g )
    end if

    do j=1,sizeArray
       if ( distribution(j) ) then
          save_the_setup(j,:) = save_the_setup_u(count(distribution(1:j)),:)
       else
          save_the_setup(j,:) = save_the_setup_g(count(.not.distribution(1:j)),:)
       end if
    end do

  end subroutine setupxor4096_dp_1d_dist

  ! ============================================================================
  !   PRIVATE METHODS
  ! ============================================================================

  ! ! Lower tail quantile for standard normal distribution function SINGLE PRECISION D0
  ! function ltqnorm_sp_d0( p )  result(ltqnorm)

  !   use mo_kind,            only    :   sp

  !   implicit none

  !   ! Intent IN
  !   real(sp),       intent(in)  ::  p

  !   ! Results
  !   real(sp)                    ::  ltqnorm

  !   ! Internal variables
  !   real(sp)                    ::  z,q,r

  !   ! Parameters
  !   real(sp),   parameter                   ::  p_low = 0.02425_sp
  !   real(sp),   parameter                   ::  p_high = 1.0_sp - p_low
  !   real(sp),   dimension(6),   parameter   ::  a = (/ -39.6968302866538_sp, 220.946098424521_sp, -275.928510446969_sp, &
  !        138.357751867269_sp, -30.6647980661472_sp, 2.50662827745924_sp /)
  !   real(sp),   dimension(5),   parameter   ::  b = (/ -54.4760987982241_sp, 161.585836858041_sp, -155.698979859887_sp, &
  !        66.8013118877197_sp, -13.2806815528857_sp /)
  !   real(sp),   dimension(6),   parameter   ::  c = (/ -0.00778489400243029_sp, -0.322396458041136_sp, -2.40075827716184_sp, &
  !        -2.54973253934373_sp, 4.37466414146497_sp, 2.93816398269878_sp /)
  !   real(sp),   dimension(4),   parameter   ::  d = (/ 0.00778469570904146_sp, 0.32246712907004_sp, &
  !        2.445134137143_sp, 3.75440866190742_sp /)

  !   if ( p .lt. 0.0_sp .or. p .gt. 1.0_sp )   then
  !      z = 0.0_sp          ! values out of domain
  !   else if ( abs(p) .lt. epsilon(1.0_sp) )  then
  !      z = -huge(1.0_sp)   ! out of range, minus "infinity"
  !   else if ( abs(p-1.0_sp) .lt. epsilon(1.0_sp) )  then
  !      z = huge(1.0_sp)    ! out of range, "infinity"
  !   else if ( p .lt. p_low ) then
  !      q = sqrt(-2.0_sp*log(p))
  !      z = (((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_sp)
  !   else if ( p .gt. p_high ) then
  !      q = sqrt(-2.0_sp*log(1.0_sp-p))
  !      z = -(((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_sp)
  !   else
  !      q = p - 0.5_sp
  !      r = q*q
  !      z = (((((a(1)*r+a(2))*r+a(3))*r+a(4))*r+a(5))*r+a(6))*q/(((((b(1)*r+b(2))*r+b(3))*r+b(4))*r+b(5))*r+1.0_sp)
  !   end if

  !   ltqnorm = z

  ! end function ltqnorm_sp_d0

  ! Lower tail quantile for standard normal distribution function SINGLE PRECISION D1
  function ltqnorm_sp_d1( p )  result(ltqnorm)

    use mo_kind,            only    :   i4, sp

    implicit none

    ! Intent IN
    real(sp),   dimension(:),   intent(in)  ::  p

    ! Results
    real(sp),   dimension(size(p))          ::  ltqnorm

    ! Internal variables
    integer(i4)                             ::  j       ! counter
    real(sp)                                ::  q,r
    real(sp),   dimension(size(p))          ::  z

    ! Parameters
    real(sp),   parameter                   ::  p_low = 0.02425_sp
    real(sp),   parameter                   ::  p_high = 1.0_sp - p_low
    real(sp),   dimension(6),   parameter   ::  a = (/ -39.6968302866538_sp, 220.946098424521_sp, -275.928510446969_sp, &
         138.357751867269_sp, -30.6647980661472_sp, 2.50662827745924_sp /)
    real(sp),   dimension(5),   parameter   ::  b = (/ -54.4760987982241_sp, 161.585836858041_sp, -155.698979859887_sp, &
         66.8013118877197_sp, -13.2806815528857_sp /)
    real(sp),   dimension(6),   parameter   ::  c = (/ -0.00778489400243029_sp, -0.322396458041136_sp, -2.40075827716184_sp, &
         -2.54973253934373_sp, 4.37466414146497_sp, 2.93816398269878_sp /)
    real(sp),   dimension(4),   parameter   ::  d = (/ 0.00778469570904146_sp, 0.32246712907004_sp, &
         2.445134137143_sp, 3.75440866190742_sp /)

    do j=1, size(p)
       if ( p(j) .lt. 0.0_sp .or. p(j) .gt. 1.0_sp )   then
          z(j) = 0.0_sp           ! values out of domain
       else if ( abs(p(j)) .lt. epsilon(1.0_sp) )  then
          z(j) = -huge(1.0_sp)    ! out of range, minus "infinity"
       else if ( abs(p(j)-1.0_sp) .lt. epsilon(1.0_sp) )  then
          z(j) = huge(1.0_sp)     ! out of range, "infinity"
       else if ( p(j) .lt. p_low ) then
          q = sqrt(-2.0_sp*log(p(j)))
          z(j) = (((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_sp)
       else if ( p(j) .gt. p_high ) then
          q = sqrt(-2.0_sp*log(1.0_sp-p(j)))
          z(j) = -(((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_sp)
       else
          q = p(j) - 0.5_sp
          r = q*q
          z(j) = (((((a(1)*r+a(2))*r+a(3))*r+a(4))*r+a(5))*r+a(6))*q/(((((b(1)*r+b(2))*r+b(3))*r+b(4))*r+b(5))*r+1.0_sp)
       end if
    end do

    ltqnorm(:) = z(:)

  end function ltqnorm_sp_d1

  ! ! Lower tail quantile for standard normal distribution function DOUBLE PRECISION D0
  ! function ltqnorm_dp_d0( p )  result(ltqnorm)

  !   use mo_kind,            only    :   dp

  !   implicit none

  !   ! Intent IN
  !   real(dp),       intent(in)  ::  p

  !   ! Results
  !   real(dp)                    ::  ltqnorm

  !   ! Internal variables
  !   real(dp)                    ::  z,q,r

  !   ! Parameters
  !   real(dp),   parameter                   ::  p_low = 0.02425_dp
  !   real(dp),   parameter                   ::  p_high = 1.0_dp - p_low
  !   real(dp),   dimension(6),   parameter   ::  a = (/ -39.6968302866538_dp, 220.946098424521_dp, -275.928510446969_dp, &
  !        138.357751867269_dp, -30.6647980661472_dp, 2.50662827745924_dp /)
  !   real(dp),   dimension(5),   parameter   ::  b = (/ -54.4760987982241_dp, 161.585836858041_dp, -155.698979859887_dp, &
  !        66.8013118877197_dp, -13.2806815528857_dp /)
  !   real(dp),   dimension(6),   parameter   ::  c = (/ -0.00778489400243029_dp, -0.322396458041136_dp, -2.40075827716184_dp, &
  !        -2.54973253934373_dp, 4.37466414146497_dp, 2.93816398269878_dp /)
  !   real(dp),   dimension(4),   parameter   ::  d = (/ 0.00778469570904146_dp, 0.32246712907004_dp, &
  !        2.445134137143_dp, 3.75440866190742_dp /)

  !   if ( p .lt. 0.0_dp .or. p .gt. 1.0_dp )   then
  !      z = 0.0_dp          ! values out of domain
  !   else if ( abs(p) .lt. epsilon(1.0_dp) )  then
  !      z = -huge(1.0_dp)   ! out of range, minus "infinity"
  !   else if ( abs(p-1.0_dp) .lt. epsilon(1.0_dp) )  then
  !      z = huge(1.0_dp)    ! out of range, "infinity"
  !   else if ( p .lt. p_low ) then
  !      q = sqrt(-2.0_dp*log(p))
  !      z = (((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_dp)
  !   else if ( p .gt. p_high ) then
  !      q = sqrt(-2.0_dp*log(1.0_dp-p))
  !      z = -(((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_dp)
  !   else
  !      q = p - 0.5_dp
  !      r = q*q
  !      z = (((((a(1)*r+a(2))*r+a(3))*r+a(4))*r+a(5))*r+a(6))*q/(((((b(1)*r+b(2))*r+b(3))*r+b(4))*r+b(5))*r+1.0_dp)
  !   end if

  !   ltqnorm = z

  ! end function ltqnorm_dp_d0

  ! Lower tail quantile for standard normal distribution function DOUBLE PRECISION D1
  function ltqnorm_dp_d1( p )  result(ltqnorm)

    use mo_kind,            only    :   i4, dp

    implicit none

    ! Intent IN
    real(dp),   dimension(:),   intent(in)  ::  p

    ! Results
    real(dp),   dimension(size(p))          ::  ltqnorm

    ! Internal variables
    integer(i4)                             ::  j       ! counter
    real(dp)                                ::  q,r
    real(dp),   dimension(size(p))          ::  z

    ! Parameters
    real(dp),   parameter                   ::  p_low = 0.02425_dp
    real(dp),   parameter                   ::  p_high = 1.0_dp - p_low
    real(dp),   dimension(6),   parameter   ::  a = (/ -39.6968302866538_dp, 220.946098424521_dp, -275.928510446969_dp, &
         138.357751867269_dp, -30.6647980661472_dp, 2.50662827745924_dp /)
    real(dp),   dimension(5),   parameter   ::  b = (/ -54.4760987982241_dp, 161.585836858041_dp, -155.698979859887_dp, &
         66.8013118877197_dp, -13.2806815528857_dp /)
    real(dp),   dimension(6),   parameter   ::  c = (/ -0.00778489400243029_dp, -0.322396458041136_dp, -2.40075827716184_dp, &
         -2.54973253934373_dp, 4.37466414146497_dp, 2.93816398269878_dp /)
    real(dp),   dimension(4),   parameter   ::  d = (/ 0.00778469570904146_dp, 0.32246712907004_dp, &
         2.445134137143_dp, 3.75440866190742_dp /)

    do j=1, size(p)
       if ( p(j) .lt. 0.0_dp .or. p(j) .gt. 1.0_dp )   then
          z(j) = 0.0_dp           ! values out of domain
       else if ( abs(p(j)) .lt. epsilon(1.0_dp) )  then
          z(j) = -huge(1.0_dp)    ! out of range, minus "infinity"
       else if ( abs(p(j)-1.0_dp) .lt. epsilon(1.0_dp) )  then
          z(j) = huge(1.0_dp)     ! out of range, "infinity"
       else if ( p(j) .lt. p_low ) then
          q = sqrt(-2.0_dp*log(p(j)))
          z(j) = (((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_dp)
       else if ( p(j) .gt. p_high ) then
          q = sqrt(-2.0_dp*log(1.0_dp-p(j)))
          z(j) = -(((((c(1)*q+c(2))*q+c(3))*q+c(4))*q+c(5))*q+c(6))/((((d(1)*q+d(2))*q+d(3))*q+d(4))*q+1.0_dp)
       else
          q = p(j) - 0.5_dp
          r = q*q
          z(j) = (((((a(1)*r+a(2))*r+a(3))*r+a(4))*r+a(5))*r+a(6))*q/(((((b(1)*r+b(2))*r+b(3))*r+b(4))*r+b(5))*r+1.0_dp)
       end if
    end do

    ltqnorm(:) = z(:)

  end function ltqnorm_dp_d1

end module mo_sampling


c     nilsson model
      implicit real*8(a-h,o-z)
      parameter (km=100)
      dimension ham(km,km), eval(km), evec(km,km)
      dimension ial(km),ialam(km),iasig(km)
      dimension delta_list(60), Epc_list(60, 2)
      dimension pes_list(60), pgp_list(60), pgn_list(60)
      dimension spe(100,364),vk2(364)
      dimension rkappa(0:11),rmu(0:11)
      data (rkappa(i),i=0,7) /8*0.05D0/
      data (rmu(i),i=0,5) /0.0D0,0.0D0,0.0D0,0.35D0,0.625D0,0.630D0 /
      data (rmu(i),i=6,7) /0.448D0,0.434D0 /
      open (4,file='bcs_data.txt',status='unknown')

      ip=40
      in=44
      ra=dfloat(ip+in)
      icol=0
      do n=0,14,2  !all quantum numbers multiplied by 2
        do iom=1,n+1,2
          nbas=0
          do l=n,0,-4
            do lam=-l,l,2
              isig=iom-lam
              if(abs(isig).ne.1)cycle
              nbas=nbas+1
              ial(nbas)=l
              ialam(nbas)=lam
              iasig(nbas)=isig
            enddo
          enddo
          idef = 0
          do delta=-0.3d0, 0.301d0, 0.01d0

            fdel=((1.d0+(2.d0/3.d0)*delta)**2.d0
     &            *(1.d0-(4.d0/3.d0)*delta))**(-1.d0/6.d0)
            hw00=41.d0*ra**(-1.d0/3.d0)
            hw0=hw00*fdel
            c=-2.0D0*hw00*rkappa(n/2)
            d=rmu(n/2)*c/2.0D0
            do i=1,nbas
              l=ial(i)
              lam=ialam(i)
              isig=iasig(i)
              do j=i,nbas
                l1=ial(j)
                lam1=ialam(j)
                isig1=iasig(j)

                h00=0.d0
                hl2=0.d0
                hls=0.d0
                hr2=0.d0
                hy20=0.d0
                if(i.eq.j)then  !Diagonal matrix elements
                  h00=dfloat(n+3)/2.d0*hw0
                  hl2=dfloat(l*(l+2))/4.d0
                endif

                if(l1.eq.l)then
                  if((lam1.eq.lam+2).and.(isig1.eq.isig-2))then
                    hls=dsqrt(dfloat((l-lam)*(l+lam+2)))/4.d0
                  elseif((lam1.eq.lam).and.(isig1.eq.isig))then
                    hls=lam*isig/4.d0
                  elseif((lam1.eq.lam-2).and.(isig1.eq.isig+2))then
                    hls=dsqrt(dfloat((l+lam)*(l-lam+2)))/4.d0
                  endif
                  hr2=dfloat(n+3)/2.d0
                elseif(l1.eq.l-4)then
                  hr2=dsqrt(dfloat((n-l+4)*(n+l+2)))/2.d0
                elseif(l1.eq.l+4)then
                  hr2=dsqrt(dfloat((n-l)*(n+l+6)))/2.d0
                endif
                if((abs(hr2).gt.1e-5).and.(lam1.eq.lam))then
                  hy20=dsqrt(dfloat(l+1)/dfloat(l1+1))
     &                 *cgc(l,4,l1,lam,0)*cgc(l,4,l1,0,0)
                endif
                hdel=-delta*hw0*dsqrt(4.d0)/3.d0*hr2*hy20

                ham(i,j)=h00+hdel+c*hls+d*hl2
                ham(j,i)=ham(i,j)
              enddo
            enddo

            call diag(ham,km,nbas,eval,evec)
            idef=idef+1
            delta_list(idef) = delta
            do i=1,nbas
              spe(idef,icol+i)=eval(i)
            enddo
          enddo
          icol=icol+nbas
        enddo
      enddo
      open(unit=45, file='bcs_spe.txt')
      do idef=1,60
        call sort(spe(idef,:),1000,icol)
        sum=sumene(iz,spe(idef,:),1000)
        sum=sum+sumene(in,spe(idef,:),1000)
        sum=sum-sum0
        pes_list(idef)=sum
        if (idef .eq. 51)
     &   write(45, *) delta_list(idef), spe(idef,:)
      enddo
      close(45)
      idef=1
c      write(*,'(2x,1000f12.6)')delta,(spe(idef,j),j=1,icol)
      g0=19.2d0
      g1= 7.4d0
      gp=(g0+g1*dfloat(in-ip)/ra)/ra
      gn=(g0-g1*dfloat(in-ip)/ra)/ra
      write(*,*) gn, gp
c$$$      Delta vs G
      open(unit=25, file="delta_vs_g.txt")
      do g_mul=1.5d0, 0.0d0, -0.001d0
         g_mod = g_mul*gn
         call bcs(in,g_mod,spe(idef,:),bcsd,bcsl,epc,vk2)
         if (bcsd .eq. 0.0d0) exit
         write(25,*) g_mul, g_mod, bcsd
      end do
      close(25)
      open(unit=99, file="bcs_delta.txt")
      do idef=1,60
         call bcs(in,gn,spe(idef,:),bcsd,bcsl,epc,vk2)
         pgn_list(idef) = bcsd
         Epc_list(idef, 1) = epc
         call bcs(ip,gp,spe(idef,:),bcsd,bcsl,epc,vk2)
         pgp_list(idef) = bcsd
         Epc_list(idef, 2) = epc
      enddo
      do idef=1,60
         write(99, '(6f12.6)') delta_list(idef),
     &      pes_list(idef)-pes_list(31),
     &      Epc_list(idef,1)-Epc_list(31, 1),
     &      Epc_list(idef,2)-Epc_list(31, 2),
     &    pgp_list(idef), pgn_list(idef)
      enddo
      close(99)
c$$$      call bcs(in,gn,spe(idef,:),bcsd,bcsl,epc,vk2)
c$$$      write(*,'(2x,4f12.6)')gn,bcsd,bcsl,epc
c$$$      do i=1,icol
c$$$        esp=spe(idef,i)
c$$$        eqp=dsqrt((esp-bcsl)**2+bcsd**2)
c$$$        v2=vk2(i)
c$$$        uv=dsqrt(1-v2)*dsqrt(v2)
c$$$        write(4,'(2x,4f12.6)')esp,eqp,v2,uv
c$$$      enddo
      close(4)
      end

      function sumene(ip,spe,ndim)
      implicit real*8(a-h,o-z)
      dimension spe(ndim)
      sumene=0.d0
      do i=1,ip/2
        sumene=sumene+spe(i)
      end do
      if(mod(ip,2).ne.0)sumene=sumene+spe(ip/2+1)
      end


!----------------------------------------------------------------------------- best with 100 columns

!> impact of particle on facets
module modImpact
  private

  integer,parameter::DIMS=3 !< three dimensions

  public::testImpact

contains

  !> see if particle k impacts with any neighbor facets if moved to location newX
  pure subroutine testImpact(grid,p,k,newX,f,h)
    use modPICGrid
    use modPolyGrid
    use modParticle
    class(PICGrid),intent(in)::grid !< the grid
    class(ptcls),intent(in)::p !< the particle collection
    integer,intent(in)::k !< the particle to be tested
    double precision,intent(inout)::newX(DIMS) !< new location of particle
    integer,intent(out)::f !< facet index
    double precision,intent(out)::h !< fraction of step at impact
    double precision::s(DIMS,2),t(DIMS,3),tmp,tmpX(DIMS)
    logical::isPenetrate

    f=0
    h=2d0 ! any value greater than 1
    s(:,1)=p%x(:,k)
    s(:,2)=newX(:)
    do i=1,grid%nNeibF(p%iC(k))
      n=grid%iNeibF(i,p%iC(k))
      select case(grid%sE(n))
      case(TRI)
        t(:,:)=grid%pN(:,grid%iNE(1:N_TRI,n))
        call testPenetration(s,t,isPenetrate,tmp,tmpX)
        if(isPenetrate)then
          if(tmp<h)then
            f=n
            h=tmp
            newX=tmpX
          end if
        end if
      case(TRI6)
      case default
      end select
    end do
  end subroutine

  !> see if line segment s penetrates triangle t, return the point of penetration
  pure subroutine testPenetration(s,t,isPenetrate,h,p)
    double precision,intent(in)::s(DIMS,2) !< two points defining the line segment
    double precision,intent(in)::t(DIMS,3) !< three points defining the line segment
    logical,intent(out)::isPenetrate !< whether s penetrates t
    double precision,intent(inout)::h !< location of the penetration point on the segment (0 to 1)
    double precision,intent(inout)::p(DIMS) !< location of the penetration point
    double precision::A(DIMS,DIMS),invA(DIMS,DIMS),rhs(DIMS),sol(DIMS)
    logical::isSingular

    isPenetrate=.false.
    A(:,1)=s(:,2)-s(:,1)
    A(:,2)=t(:,1)-t(:,2)
    A(:,3)=t(:,1)-t(:,3)
    rhs(:)=t(:,1)-s(:,1)
    call findInv3by3(A,invA,isSingular)
    if(isSingular)then
      isPenetrate=.false.
    else
      sol=matmul(invA,rhs)
      if(all(sol>=0d0).and.sol(1)<=1d0.and.sol(2)+sol(3)<=1d0)then
        isPenetrate=.true.
        h=sol(1)
        p=s(:,1)+h*(s(:,2)-s(:,1))
      else
        isPenetrate=.false.
      end if
    end if
  end subroutine

  !> find inverse of 3 by 3 matrix
  pure subroutine findInv3by3(A,invA,isSingular)
    double precision,intent(in)::A(3,3) !< 3 by 3 matrix
    double precision,intent(inout)::invA(3,3) !< inverse of A
    logical,intent(out)::isSingular !< whether detA is 0
    double precision::detA

    detA=A(1,1)*A(2,2)*A(3,3)-A(1,1)*A(2,3)*A(3,2)-A(1,2)*A(2,1)*A(3,3)&
    &    +A(1,2)*A(2,3)*A(3,1)+A(1,3)*A(2,1)*A(3,2)-A(1,3)*A(2,2)*A(3,1)
    if(abs(detA)>tiny(1d0))then
      invA(1,1)=(A(2,2)*A(3,3)-A(2,3)*A(3,2))
      invA(2,1)=-(A(2,1)*A(3,3)-A(2,3)*A(3,1))
      invA(3,1)=(A(2,1)*A(3,2)-A(2,2)*A(3,1))
      invA(1,2)=-(A(1,2)*A(3,3)-A(1,3)*A(3,2))
      invA(2,2)=(A(1,1)*A(3,3)-A(1,3)*A(3,1))
      invA(3,2)=-(A(1,1)*A(3,2)-A(1,2)*A(3,1))
      invA(1,3)=(A(1,2)*A(2,3)-A(1,3)*A(2,2))
      invA(2,3)=-(A(1,1)*A(2,3)-A(1,3)*A(2,1))
      invA(3,3)=(A(1,1)*A(2,2)-A(1,2)*A(2,1))
      invA(:,:)=invA(:,:)/detA
      isSingular=.false.
    else
      isSingular=.true.
    end if
  end subroutine

end module


subroutine upperbound

use vars
use params
use microphysics, only: micro_field, index_water_vapor, is_water_vapor, nmicro_fields, mk0, mklsadv
implicit none
real coef, coefxy
integer i,j,k,n
real, parameter ::  tau_nudging = 3600.
real a,b,c,d,e

call t_startf ('upperbound')

coefxy = 1./float(nx*ny)/dtn

if(dolargescale.AND.(.NOT.doExtrapolate_UpperBound)) then

!
! if there is an "observed" sounding - nudge two highest levels to it
! to avoid problems with the upper boundary.
!

  coef = dtn / tau_nudging
  do k=nzm-1,nzm
    do j=1,ny
      do i=1,nx
         !bloss: add tendency to *NUDGE so that it is included in the
         !  vertically-integrated energy and moisture budgets.
         tnudge(k) = tnudge(k) - (t(i,j,k)-tg0(k)-gamaz(k))*coef*coefxy
         qnudge(k) = qnudge(k) - (micro_field(i,j,k,index_water_vapor)-qg0(k))*coef*coefxy

         t(i,j,k)=t(i,j,k)-(t(i,j,k)-tg0(k)-gamaz(k))*coef
         micro_field(i,j,k,index_water_vapor)=micro_field(i,j,k,index_water_vapor)- &
                              (micro_field(i,j,k,index_water_vapor)-qg0(k))*coef
      end do
    end do
  end do

else

!  otherwise, preserve the vertical gradients:
! MK: change starting from v. 6.8.3: limit gradients
  coef = dz*adz(nzm)
  gamt0=max(0.1e-3,(t0(nzm-1)-t0(nzm-2))/(z(nzm-1)-z(nzm-2)))
!bloss  gamq0=min(0.,(q0(nzm-1)-q0(nzm-2))/(z(nzm-1)-z(nzm-2)))
  do j=1,ny
   do i=1,nx
     !bloss: add tendency to *NUDGE so that it is included in the
     !  vertically-integrated energy and moisture budgets.
     tnudge(nzm) = tnudge(nzm) + (t(i,j,nzm-1)+gamt0*coef-t(i,j,nzm))*coefxy
!bloss     qnudge(nzm) = qnudge(nzm) + (max(0.,micro_field(i,j,nzm-1,index_water_vapor)+gamq0*coef) &
!bloss                                     - micro_field(i,j,nzm,index_water_vapor))*coefxy

     t(i,j,nzm)=t(i,j,nzm-1)+gamt0*coef
!bloss     micro_field(i,j,nzm,index_water_vapor)=max(0.,micro_field(i,j,nzm-1,index_water_vapor)+gamq0*coef)
   end do
  end do

!bloss: Vertical gradients of water vapor at the model top can lead to trouble
!   in long simulations.  A long-time RCE simulation with large-scale vertical
!   motion at the top of the model had cloud appearing in the top model level.
!   To avoid this, relax the water vapor in the top model level to the avgerage
!   of the four model levels below.
  coef = dtn / tau_nudging
  gamq0=0.25*SUM(q0(nzm-4:nzm-1))
  do j=1,ny
   do i=1,nx
     !bloss: add tendency to *NUDGE so that it is included in the
     !  vertically-integrated energy and moisture budgets.
     qnudge(nzm) = qnudge(nzm) + coef*( gamq0 - micro_field(i,j,nzm,index_water_vapor) )*coefxy

     micro_field(i,j,nzm,index_water_vapor)= micro_field(i,j,nzm,index_water_vapor) &
          + ( gamq0 - micro_field(i,j,nzm,index_water_vapor) )*coef
   end do
  end do

  !bloss: add extrapolation for other water vapor variables (e.g., water isotopologues)
  !  We'd like to make the treatment of isotopic water as close to that of standard water
  !  as we can to prevent spurious fractionation.
  coef = dtn / tau_nudging
  do n = 1,nmicro_fields
    if( is_water_vapor(n) .AND. (n.ne.index_water_vapor) ) then
      gamq0=0.25*SUM(mk0(nzm-4:nzm-1,n))
      do j=1,ny
        do i=1,nx
          !bloss: add tendency to *NUDGE so that it is included in the
          !  vertically-integrated energy and moisture budgets.
          mklsadv(nzm,n) = mklsadv(nzm,n) + coef*( gamq0 - micro_field(i,j,nzm,n) )*coefxy

          micro_field(i,j,nzm,n)= micro_field(i,j,nzm,n) &
               + ( gamq0 - micro_field(i,j,nzm,n) )*coef
        end do
      end do
    end if
  end do

!
! make gradient accross two highest scalar lavels the same preserving the mass-weighted integral
! (experimental, not proven to work yet)
!
!   a = adz(nzm)/adz(nzm-1)
!   d = adzw(nzm-1)*rho(nzm-1)/(adzw(nzm-1)*rhow(nzm-1)+adzw(nzm)*rhow(nzm))
!   e = adzw(nzm)*rhow(nzm)/(adzw(nzm-1)*rhow(nzm-1)+adzw(nzm)*rhow(nzm))
!   b = 1./(a + d)
!   a = a*b
!   c = b*e
!   do j=1,ny
!    do i=1,nx
!      t(i,j,nzm)=a*t(i,j,nzm)+b*t(i,j,nzm-1)-c*t(i,j,nzm-2)
!      t(i,j,nzm-1)=d*t(i,j,nzm)+e*t(i,j,nzm-2)
!      micro_field(i,j,nzm,index_water_vapor)=max(0., &
!              a*micro_field(i,j,nzm,index_water_vapor)+b*micro_field(i,j,nzm-1,index_water_vapor) &
!              -c*micro_field(i,j,nzm-2,index_water_vapor))
!      micro_field(i,j,nzm-1,index_water_vapor)=d*micro_field(i,j,nzm,index_water_vapor)+ &
!               e*micro_field(i,j,nzm-2,index_water_vapor)
!    end do
!   end do

end if

call t_stopf('upperbound')

end


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

program RamseyGov

    use globals
    use toolbox

    implicit none

    ! start timer
    call tic()

    ! initialize grid and policy function
    call grid_Cons_Equi(k, k_l, k_u)
    c(:) = k(:)**alpha - delta*k(:)

    ! initialize government consumption
    gbar = gy*(alpha/(delta+1d0/beta-1d0))**(alpha/(1d0-alpha))

    ! iterate until policy function converges
    do iter = 1, itermax

        ! interpolate coefficients
        call spline_interp(c, coeff_c)

        ! calculate decisions for every gridpoint
        do ik = 0, NK

            ! set starting value and communicate resource level
            x_in = c(ik)
            k_com = k(ik)

            ! find the optimal consumption level
            call fzero(x_in, foc, check)
            if(check)write(*,*)'ERROR IN ROOTFINDING PROCESS'

            ! get optimal consumption and value function
            c_new(ik) = x_in

        enddo

        ! get convergence level
        con_lev = maxval(abs(c_new(:) - c(:))/max(abs(c(:)), 1d-10))
        write(*,'(i5,2x,f20.7)')iter, con_lev

        ! check for convergence
        if(con_lev < sig)then
            call output()
        endif

        c = c_new
    enddo

    write(*,*)'No Convergence'

contains

    ! For creating output plots
    subroutine output()

        use toolbox

        implicit none
        integer, parameter :: TTplot = 800
        real*8 :: theta, UU, UU_0
        integer, parameter :: n_err = 10000
        real*8 :: err, c_err, err_temp

        ! end timer
        call toc()

        ! calculate the time path of consumption and capital numerically
        theta = 1d0/beta-1d0
        k_t(0) = (alpha/(theta+delta))**(1d0/(1d0-alpha))
        c_t(0) = spline_eval(k_t(0), coeff_c, k_l, k_u)
        y_t(0) = k_t(0)**alpha
        i_t(0) = y_t(0)-c_t(0)-gbar
        w_t(0) = (1d0-alpha)*k_t(0)**alpha
        r_t(0) = alpha*k_t(0)**(alpha-1d0)-delta
        b_t(0) = 0d0
        do it = 1, TT
            k_t(it) = (1d0-delta)*k_t(it-1) + i_t(it-1)
            c_t(it) = spline_eval(k_t(it), coeff_c, k_l, k_u)
            y_t(it) = k_t(it)**alpha
            i_t(it) = y_t(it)-c_t(it)-gbar
            w_t(it) = (1d0-alpha)*k_t(it)**alpha
            r_t(it) = alpha*k_t(it)**(alpha-1d0)-delta
            b_t(it) = by*k_t(it)**alpha
            tauw_t(it-1) = (gbar+(1d0+r_t(it-1))*b_t(it-1)-b_t(it)-tau_r*r_t(it-1)*(k_t(it-1)+b_t(it-1)))/w_t(it-1)
        enddo
        tauw_t(TT) = (gbar+r_t(TT)*b_t(TT)-tau_r*r_t(TT)*(k_t(TT)+b_t(TT)))/w_t(it-1)

        ! calculate the steady state utility level
        UU = c_t(TT)**egam/egam/(1d0-beta)

        ! calculate the utility level
        UU_0 = 0d0
        do it = 0, TT-1
            UU_0 = UU_0 + beta**(it)*c_t(it)**egam/egam
        enddo
        UU_0 = UU_0 + beta**TT*UU

        ! write output
        write(*,'(/a/)')'STEADY STATE VALUES AND WELFARE'
        write(*,'(a)')'   tau_w     K/Y     C/Y     I/Y     G/Y       C         U       U_0'
        write(*,'(6f8.2,2f10.3/)')(/tauw_t(TT), k_t(TT)/y_t(TT)/4d0, c_t(TT)/y_t(TT), &
            i_t(TT)/y_t(TT), gbar/y_t(TT)/)*100d0, c_t(TT), UU, UU_0

        call plot((/(dble(it),it=0,TTplot)/), k_t(0:TTplot))
        call execplot(xlabel='Time t', ylabel='Capital Stock k_t')

        call plot((/(dble(it),it=0,TTplot)/), c_t(0:TTplot))
        call execplot(xlabel='Time t', ylabel='Consumption c_t')

        call plot((/(dble(it),it=0,TTplot)/), r_t(0:TTplot))
        call execplot(xlabel='Time t', ylabel='Interest rate r_t')

        call plot((/(dble(it),it=0,TTplot)/), tauw_t(0:TTplot))
        call execplot(xlabel='Time t', ylabel='Wage tax tauw')

        ! calculate euler equation error
        err = 0d0
        do ik = 0, n_err
            k_com = k_l + (k_u-k_l)*dble(ik)/dble(n_err)
            c_err = spline_eval(k_com, coeff_c, k_l, k_u)
            err_temp = abs(foc(c_err)/c_err)
            if(err_temp > err)err = err_temp
        enddo
        write(*,'(/a, es15.7)')'Euler equation error:',err

        ! quit program
        stop

    end subroutine

end program


      real*8 function tsat(p,t,q)
      character*100 id,revision,revdate
      data id/'$RCSfile: tsat.f,v $'/
      data revision/'$Revision: 1.3 $'/
      data revdate/'$Date: 1994/03/12 17:59:04 $'/
c
c
c     this function calculates the temperature at which a
c     parcel becomes saturated using the formula from bolton
c     (1980),mwr, vol.108,no.7, 1046-1053.
c
      include "const.h"
      real*8 q,p,t,e
c
      e=(q*p/(eps+q))
      tsat=(2840./(3.5*dlog(t)-dlog(e)-4.805))+55.
      return
      end