module mod_psycho
    use kind_m
    use mod_mpg
    use mod_fft
    implicit none
    private
    public :: psycho, calc_mask  ! subroutine
    real (kind = kd), save :: pi, pi2
    real (kind = kd), save :: ath_l(576), ath_s(192, 3), ath0_l(100), ath0_s(100), dbath_l(100), dbath_s(100)
    real (kind = kd), save :: sf_l(100, 100),  sf_s(100, 100)
    real (kind = kd), save :: afft_l(576, 2, 2), afft_s(192, 3, 2, 2)
    real (kind = kd), save :: dbfft_l(100, 2, 2), dbfft_s(100, 3, 2, 2)
    real (kind = kd), save :: freq_l(576), freq_s(192), bark_l(576), bark_s(192), bw_l(576), bw_s(192)
    real (kind = kd), save :: weight_l(576), weight_s(192)
    complex (kind = kd), save :: cfft_l(1152, 2, 2), cfft_s(384, 3, 2, 2)
    integer, save          :: ibark_l(576), ibark_s(192), ifb_l(100, 0:2), ifb_s(100, 0:2), bw0_l(100), bw0_s(192)
contains
    !----------------------------------------------------------------
    subroutine init_absolute_threshold(isample_rate)
        integer, intent(in) :: isample_rate
        real(kind = kd):: freq, temp !, ath(576)
        integer :: i
        pi  = 4.0_kd * atan(1.0_kd)
        pi2 = 2.0_kd * pi
        !
        do i = 1, 576
            freq = real(isample_rate, kind = kd) / 2.0_kd / 1000.0_kd * (real(i - 1, kind = kd) + 0.0_kd) / 576.0_kd
        !    temp =  3.64_kd  * freq ** (-0.8_kd) &
        !         -  6.50_kd  * exp(-0.6_kd * (freq -  3.3_kd)**2.0_kd) &
        !         +  0.001_kd * freq ** 4.0_kd
        !         + ath_min
            temp = 3.64_kd * freq ** (-0.8_kd) &                      ! alternative ATH function
                 - 6.50_kd * exp(-0.6_kd * (freq - 3.3_kd)**2.0_kd) & ! reference: lame ath-type 3
            !      + 5.1_kd
            !     + 0.001_kd * freq ** 3.80_kd + 4.92_kd  ! 12.61
            !  temp = 0.001_kd * freq ** 3.81_kd + 4.60_kd  ! 12.93
            !  temp = 0.001_kd * freq ** 3.82_kd + 4.27_kd  ! 13.26
            !  temp = 0.001_kd * freq ** 3.83_kd + 3.94_kd  ! 13.59
            !  temp = 0.001_kd * freq ** 3.84_kd + 3.61_kd  ! 13.93
                  + 0.001_kd * freq ** 3.85_kd + 3.27_kd  ! 14.28
            !      + 0.001_kd * freq ** 4.00_kd - 2.86_kd  ! 20.70
            temp = min(temp + ath_min, ath_max)
            ath_l(i) = 10.0_kd**(temp / 20.0_kd)
        end do
        !
        do i = 1, 192
            freq = real(isample_rate, kind = kd) / 2.0_kd / 1000.0_kd * (real(i - 1, kind = kd) + 0.0_kd) / 192.0_kd
            temp = 3.64_kd * freq ** (-0.8_kd) &                      ! alternative ATH function
                 - 6.50_kd * exp(-0.6_kd * (freq - 3.3_kd)**2.0_kd) & ! reference: lame ath-type 3
            !     + 0.001_kd * freq ** 3.80_kd + 4.92_kd  ! 12.61
                 + 0.001_kd * freq ** 3.85_kd + 3.27_kd  ! 14.28
            !     + 0.001_kd * freq ** 4.00_kd - 2.86_kd  ! 20.70
            temp = min(temp + ath_min, ath_max)
            ath_s(i, :) = 10.0_kd**(temp / 20.0_kd)
        end do
    end subroutine init_absolute_threshold
!--------------------------------------------------------------------------------------------------------------------
    function switch_q(wx, igranule, ichannel) result(ires) ! attack detection
        real (kind = kd), intent(in) :: wx(:, :, :)
        integer, intent(in) :: igranule, ichannel
        integer :: ires
        real (kind = kd), save :: sum0a(2, 2), sum1a(2, 2) = 0.0_kd, sum0b(2, 2), sum1b(2, 2) = 0.0_kd
        sum0a(igranule, ichannel) = sum1a(igranule, ichannel)
        sum1a(igranule, ichannel) = sum( abs(wx(1:36, igranule, ichannel)) )
        sum0b(igranule, ichannel) = sum1b(igranule, ichannel)
        sum1b(igranule, ichannel) = sum( abs(wx(37:, igranule, ichannel)) )
        !
        if ( sum0a(igranule, ichannel) < 1.25_kd .or. sum0b(igranule, ichannel) < 0.70_kd ) then  ! when silent, use long
            ires = 0
        else if (sum1a(igranule, ichannel) > xsm * switch * sum0a(igranule, ichannel) ) then  ! band 1:2 attack
            ires = mblock_type_param ! 20
            nn1 = nn1 + 1 ! debug info
        else if ( sum1b(igranule, ichannel) >   switch * sum0b(igranule, ichannel) ) then    ! band 3:32 attack
            ires = mblock_type_param ! 20
            if ( q_sm ) ires = 21 ! mixed
            nn2 = nn2 + 1 ! debug info
        else    ! long block
            ires = 0
        end if
        if (mblock_type_param == 0) ires = 0 ! force long-only mode
    end function switch_q
!--------------------------------------------------------------------------------------------------------------------
    subroutine attack(wx, mblock_type) !..... ! iso figure c.7 (p.95) window switching state diagram
        real (kind = kd), intent(in ) :: wx(:, :, :)
        integer         , intent(out) :: mblock_type(:, :)
        integer :: igranule, ichannel, iattack
        integer, save :: mblock_prev(2, 2) = 0
        do ichannel = 1, size(wx, dim = 3)
            do igranule = 1, 2
                iattack = switch_q(wx, igranule, ichannel)
                select case (iattack)
                case (0) ! no-attack
                    select case (mblock_prev(igranule, ichannel))
                    case ( 0, 30, 31) ! long
                        mblock_type(igranule, ichannel) =  0
                    case (10)
                        mblock_type(igranule, ichannel) = 20
                    case (11)
                        mblock_type(igranule, ichannel) = 21
                    case (20) ! short
                        mblock_type(igranule, ichannel) = 30
                    case (21) ! mixed
                        mblock_type(igranule, ichannel) = 31
                    case default
                        write(*, *) 'error: psycho : unexpected block type: case 0-x',  igranule, &
                                     mblock_prev(igranule, ichannel), mblock_type(igranule, ichannel)
                        stop
                    end select
                case (20) ! attack-short
                    select case (mblock_prev(igranule, ichannel))
                    case ( 0, 30, 31)
                        mblock_type(igranule, ichannel) = 10
                    case (10)
                        mblock_type(igranule, ichannel) = 20
                    case (11)
                        mblock_type(igranule, ichannel) = 21
                    case (20)
                        mblock_type(igranule, ichannel) = 20
                    case (21)
                        mblock_type(igranule, ichannel) = 21
                    case default
                       write(*, *) 'error: psycho : unexpected block type: case 20-x',  igranule, &
                                    mblock_prev(igranule, ichannel), mblock_type(igranule, ichannel)
                       stop
                    end select
                case (21) ! attack-mixed
                    select case (mblock_prev(igranule, ichannel))
                    case ( 0, 30, 31)
                        mblock_type(igranule, ichannel) = 11
                    case (10)
                        mblock_type(igranule, ichannel) = 20
                    case (11)
                        mblock_type(igranule, ichannel) = 21
                    case (20)
                        mblock_type(igranule, ichannel) = 20
                    case (21)
                        mblock_type(igranule, ichannel) = 21
                    case default
                        write(*, *) 'error: psycho : unexpected block type: case 21-x', igranule, &
                                     mblock_prev(igranule, ichannel), mblock_type(igranule, ichannel)
                        stop
                    end select
                case default
                    write(*, *) 'error: psycho : unexpected block type'
                    stop
                end select
                mblock_prev(igranule, ichannel) = mblock_type(igranule, ichannel)
                !....... debug info ...............................................
                select case ( mblock_type(igranule, ichannel) )
                case ( 0)
                    long = long + 1
                case (20)
                    nshort = nshort + 1
                case (21)
                    mix = mix + 1
                case (10, 11)
                    m1 = m1 + 1
                case (30, 31)
                    m3 = m3 + 1
                end select
            end do
        end do
    end subroutine attack
!-----------------------------------------------------------------------------------------------------------------------
    subroutine fft_long(nchannel, pcm, afft_l, fft576)
        integer            , intent(in ) :: nchannel
        real (kind = kd)   , intent(in ) :: pcm(:, :)
        real (kind = kd)   , intent(out) :: afft_l(:, :, :)
        complex (kind = kd), intent(out) :: fft576(1152, 2, 2)
        integer :: ichannel, igranule, m1, m2
        do igranule = 1, 2
            do ichannel = 1, nchannel
                m1 = 1 + 480 * (igranule - 1)
                m2 = m1 + 1152 - 1
                fft576(:, igranule, ichannel) = cmplx(pcm(m1:m2, ichannel), 0.0_kd, kind = kd) ! put 1152 real data -> get 576 complex fft
                call fft23han(7, 2, indx576, omega576, fft576(:, igranule, ichannel), han576 ) ! 2^7 * 3^2 = 1152
            !    call fft23(7, 2, indx576, omega576, fft576(:, igranule, ichannel) )
                afft_l(:, igranule, ichannel) = abs(fft576(1:576, igranule, ichannel))
            end do
        end do
    end subroutine fft_long
!------------------------------------------------------------------------------------------------
    subroutine fft_short(nchannel, pcm, afft_s, fft192)
        integer            , intent(in ) :: nchannel
        real (kind = kd)   , intent(in ) :: pcm(:, :)
        real (kind = kd)   , intent(out) :: afft_s(:, :, :, :)
        complex (kind = kd), intent(out) :: fft192(384, 3, 2, 2)
        integer :: ichannel, igranule, iwin, m1, m2
        do igranule = 1, 2
            do ichannel = 1, nchannel
               do iwin = 1, 3
                    m1 = 1 + 480 * (igranule - 1) + 384 * (iwin - 1)
                    m2 = m1 + 384 - 1
                    fft192(:, iwin, igranule, ichannel) = cmplx(pcm(m1:m2, ichannel), 0.0_kd, kind = kd) ! put 384 real data -> get 192 complex fft
                    call fft23han(7, 1, indx192, omega192, fft192(:, iwin, igranule, ichannel), han192 ) ! 2^7 * 3^1 = 384
                !    call fft23(7, 1, indx192, omega192, fft192(:, iwin, igranule, ichannel) )
                    afft_s(:, iwin, igranule, ichannel) = abs(fft192(1:192, iwin, igranule, ichannel))
                end do
            end do
        end do
    end subroutine fft_short
!------------------------------------------------------------------------------------------------
    subroutine calc_wx(nchannel, wx)
        integer        , intent(in ) :: nchannel
        real(kind = kd), intent(out) :: wx(:, :, :)
        integer :: igranule, ichannel
        forall (igranule = 1:2, ichannel = 1:nchannel) wx(:, igranule, ichannel) = afft_l(:, igranule, ichannel) * weight_l
    end subroutine calc_wx
!------------------------------------------------------------------------------------------------
    subroutine psycho(pcm, mpg, mblock_type)
        type (mpeg_parameters), intent(in out) :: mpg
        integer               , intent(   out) :: mblock_type(:, :)
        real (kind = kd)      , intent(in    ) :: pcm(:, :)
        logical, save :: qfirst = .true.
        integer       :: nchannel
        real (kind = kd) :: wx(576, 2, 2)
        !..... initialization .........................................................................................
        if (qfirst) then
            qfirst = .false.
            call init_absolute_threshold( mpeg_sample_rates(mpg%isample_rate) )
            call init_mask( mpeg_sample_rates(mpg%isample_rate) )
            call init_fft()
        end if
        !..... fft 576 / 192 ................................................................................................
        nchannel = size(mblock_type, 2)
        call fft_long (nchannel, pcm, afft_l, cfft_l)
        call fft_short(nchannel, pcm, afft_s, cfft_s)
        !..... weighted intensity .....................................................................................
        call calc_wx(nchannel, wx)
        !..... attack detection .......................................................................................
        call attack(wx, mblock_type)
        !  psycho calaulates fft & decides long/short/mixed block.
        !  mid-side/normal stereo decision (layer3.f90)
        !
        ! debug info
        nbits(mpg%ibit_rate) = nbits(mpg%ibit_rate) + 1
    end subroutine psycho
!------------------------------------------------------------------------------------------------
    subroutine init_mask(nsample_rate)
        integer, intent(in) :: nsample_rate
        integer :: i
        real (kind  = 8) :: f0, f1

        do i = 1, 576
            freq_l(i) = real(nsample_rate, kind = kd) / 2.0_kd * (real(i - 1, kind = kd) + 0.0_kd) / 576.0_kd ! khz
            bark_l(i) = bark(freq_l(i) / 1000.0_kd)
            ibark_l(i) = int(bark_l(i) + 0.1_kd) + 1
            f0 = real(nsample_rate, kind = kd) / 2000.0_kd * real(i - 1, kind = kd) / 576.0_kd
            f1 = real(nsample_rate, kind = kd) / 2000.0_kd * real(i    , kind = kd) / 576.0_kd
            bw_l(i) = bark(f1) - bark(f0)
            weight_l(i) = bw_l(i) / (f1 - f0)
        end do
        do i = 1, 192
            freq_s(i) = real(nsample_rate, kind = kd) / 2.0_kd * (real(i - 1, kind = kd) + 0.0_kd) / 192.0_kd ! khz
            bark_s(i) = bark(freq_s(i) / 1000.0_kd)
            ibark_s(i) = int(bark_s(i) + 0.1_kd) + 1
            f0 = real(nsample_rate, kind = kd) / 2000.0_kd * real(i - 1, kind = kd) / 192.0_kd
            f1 = real(nsample_rate, kind = kd) / 2000.0_kd * real(i    , kind = kd) / 192.0_kd
            bw_s(i) = bark(f1) - bark(f0)
            weight_s(i) = bw_s(i) / (f1 - f0)
        end do
!
        do i = 576, 1, -1
            ifb_l(max(1, ceiling(4 * bark_l(i))), 1) = i
        end do
        do i = 1, 576
            ifb_l(max(1, ceiling(4 * bark_l(i))), 2) = i
        end do
        bw_l = -huge(0.0_kd)
        do i = 1, 100
            ifb_l(i, 0) = ifb_l(i, 2) - ifb_l(i, 1) + 1
            if (ifb_l(i, 1) /= 0) bw0_l(i) = ifb_l(i, 0) * (freq_l(2) - freq_l(1))
            if (ifb_l(i, 1) /= 0) ath0_l(i) = ath_l( (ifb_l(i, 2) + ifb_l(i, 1)) / 2)
            if (ifb_l(i, 1) /= 0) dbath_l(i) = 20.0_kd * log10(ath0_l(i))
        end do
!
        do i = 192, 1, -1
            ifb_s(max(1, ceiling(4 * bark_s(i))), 1) = i
        end do
        do i = 1, 192
            ifb_s(max(1, ceiling(4 * bark_s(i))), 2) = i
        end do
        bw_s = -huge(0.0_kd)
        do i = 1, 100
            ifb_s(i, 0) = ifb_s(i, 2) - ifb_s(i, 1) + 1
            if (ifb_s(i, 1) /= 0) bw0_s(i) = ifb_s(i, 0) * (freq_s(2) - freq_s(1))
            if (ifb_s(i, 1) /= 0) ath0_s(i) = ath_s( (ifb_s(i, 2) + ifb_s(i, 1)) / 2, 1)
            if (ifb_s(i, 1) /= 0) dbath_s(i) = 20.0_kd * log10(ath0_s(i))
        end do
    end subroutine init_mask
!------------------------------------------------------------------------------------------------
    subroutine calc_mask(igranule, ichannel, mblock_type, xmask)
        integer         , intent(in ) :: igranule, ichannel, mblock_type
        real (kind = kd), intent(out) :: xmask(:, :)
        real (kind = kd) :: x0_l(100), x0_s(100, 3)
        real (kind = kd) :: y_l(576), y_s(192, 3)
        real (kind = kd) :: yn_l(576), yn_s(192, 3)
        real (kind = kd) :: tmp_l(100), tmp_s(100, 3)
        real (kind = kd) :: v_l(100), v_s(100, 3)
        integer :: i, j, iwin, iband
        integer, parameter :: il = 1, iu = 1 ! average over bark: i-il:i+iu
        != reference =======================================================================================================
        ! F. Baumgarte, C. Ferekidis and H. Fuchs, "A Nonlinear Psychoacoustic Model Applied to the ISO MPEG Layer 3 Coder"
        !===================================================================================================================
        ! sum up to 1/4 bark
        v_l = 1.0e-32_kd
        forall(i = 1:100, ifb_l(i, 1) /= 0) v_l(i) = sum(afft_l(ifb_l(i, 1):ifb_l(i, 2), igranule, ichannel)**2) / ifb_l(i, 0) !* 4
        ! non-linear sum
        dbfft_l(:, igranule, ichannel) = 10.0_kd * log10(2.0_kd * 1152.0_kd * v_l / 2.0_kd) ! 1/N FFT.f90 ! AFFT 1152 -> 576
        forall (i = 1:100, j = 1:100) sf_l(i, j) = spreading_function( 0.25_kd * (i - j), dbfft_l(j, igranule, ichannel) )
        do i = 1, 100   !  pcm^2 : MDCT^2 ~ 300 : 1 ; pcm^2 : FFT^2 ~ sqrt(1152/384) : 1
            tmp_l = sf_l(i, :) + dbfft_l(:, igranule, ichannel) - dbath_l + ath_min + offset_l &![dB]  49.2dB
                                                                                + 20.0_kd * log10( 576.0_kd / 2.0_kd)
            x0_l(i) = sum( 10.0_kd**(tmp_l * pow / 10.0_kd) )**( 0.5_kd / pow) * ath0_l(i)
        end do
        forall (i = 2:99, ifb_l(i, 1) /= 0) & ! average over nearest neighbors ~ 1bark
               tmp_l(i) = minval(x0_l(i - il:i + iu), mask = ifb_l(i - il:i + iu, 1) /= 0)
        forall (i = 1:100, ifb_l(i, 1) /= 0) yn_l(ifb_l(i, 1):ifb_l(i, 2)) = tmp_l(i)
        y_l = max( ath_l, yn_l )
        ! help for parameter decision
        if (q_help .and. mod(iframe, 200) == 0) print *, count(ath_l < yn_l), ' ath_l < yn_l'
        ! short block : uses psychoacoustic analysis for long blocks
        forall (i = 1:192) yn_s(i, :) = minval( yn_l(3 * (i - 1) + 1:3 * (i - 1) + 3) )
        y_s = max( ath_s, yn_s )
        ! short block (if q_short_fft == .true.) : independent psychoacoustic analysis for short blocks
        if (q_short_fft .and. (mblock_type == 20 .or. mblock_type == 21)) then
            v_s = 1.0e-32_kd
            forall (i = 1:100, iwin = 1:3, ifb_s(i, 1) /= 0) &
                  v_s(i, iwin) = sum(afft_s(ifb_s(i, 1):ifb_s(i, 2), iwin, igranule, ichannel)**2) / ifb_s(i, 0)
            dbfft_s(:, :, igranule, ichannel) = 10.0_kd * log10(2.0_kd * 384.0_kd * v_s * 3 * 3 / 2.0_kd) !normalization factor 9 ?? ! 1/N FFT.f90
            do iwin = 1, 3
                forall (i = 1:100, j = 1:100) sf_s(i, j) = &
                       spreading_function( 0.25_kd * (i - j), dbfft_s(j, iwin, igranule, ichannel))
                do i = 1, 100
                    tmp_s(:, iwin) = sf_s(i, :) + dbfft_s(:, iwin, igranule, ichannel) &
                                   - dbath_s + ath_min  + offset_l + 20.0_kd * log10( 576.0_kd / 2.0_kd) ! 49.2dB shift polyphase+MDCT : FFT
                    x0_s(i, iwin) = sum( 10.0_kd**(tmp_s(:, iwin) * pow / 10.0_kd) )**( 0.5_kd / pow) * ath0_s(i)
                end do
            end do
            forall (i = 2:99,  iwin = 1:3, ifb_s(i, 1) /= 0) &  ! average over nearest neighbors ~ 1bark
                    tmp_s(i, iwin) = minval(x0_s(i - il:i + iu, iwin), mask = ifb_s(i - il:i + iu, 1) /= 0)
            forall (i = 1:100, iwin = 1:3, ifb_s(i, 1) /= 0) yn_s(ifb_s(i, 1):ifb_s(i, 2), iwin) = tmp_s(i, iwin)
            y_s = max( ath_s, yn_s )
        end if
        ! reorder 32x18 --> 576 (long), 32x3x6 --> 576 (short), 2x18 + 30x3x6 -> 576 (mixed)
        select case (mblock_type)
        case (0, 10, 11, 30, 31) ! long block  ! transition block
            forall (iband = 1:32, i = 1:18) xmask(iband, i) = y_l(18 * (iband - 1) + i)
        case (20) ! short block
            forall (iband = 1:32, iwin = 1:3, i = 1:6) xmask(iband, 6 * (iwin - 1) + i) = y_s(6 * (iband - 1) + i, iwin)
        case (21) ! mixed block
            forall (iband = 1:2, i = 1:18) xmask (iband, i) = y_l(18 * (iband - 1) + i)
            forall (iband = 3:32, iwin = 1:3, i = 1:6) xmask(iband, 6 * (iwin - 1) + i) = y_s(6 * (iband - 1) + i, iwin)
        case default
            print *, 'mblock_type', mblock_type
            stop 'calc_mask: should not come here'
        end select
    end subroutine calc_mask
 !------------------------------------------------------------------------------------------------
    pure elemental function bark(f) result(res)
        real (kind = kd), intent(in) :: f
        real (kind = kd) :: res
        res = 13.0_kd * atan(0.76_kd * f) + 3.5_kd * atan( (f / 7.5_kd)**2.0_kd )
    end function bark
 !------------------------------------------------------------------------------------------------
    pure elemental function spreading_function(z, x) result(res)
        real (kind = kd), intent(in) :: z, x
        real (kind = kd) :: res
        if ( z > 0.0_kd ) then
            res = -(22.0_kd - 0.2_kd * (x + ath_min)) !93.3_kd)) ! 10.0_kd * log10( 1.0_kd / 2.0_kd**(15*2 + 1) ))) ! -93.31929866
        else
            res =  30.0_kd * z
        end if
    end function spreading_function
 !------------------------------------------------------------------------------------------------
end module mod_psycho


!
!     -------------------------------------------------------------
!      C 0 T 5 S
!     -------------------------------------------------------------
!
!                                                                  *
!     THIS PACKAGE DETERMINES THE VALUES OF FOLLOWING              *
!                                                ---          ---  *
!                                                I  Q  1/2  C   I  *
!     CLEBSCH - GORDAN COEFFICIENT:              I              I  *
!                                                I  QM  SM  CM  I  *
!                                                ---          ---  *
!                                                                  *
!     Written by G. Gaigalas,                                      *
!     Vilnius,  Lithuania                          December 1993   *
!
      SUBROUTINE C0T5S(Q, QM, SM, C, CM, A)
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY:  DOUBLE
      USE CONSTS_C
!...Translated by Pacific-Sierra Research 77to90  4.3E  09:56:00  11/16/01
!...Switches:
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE ittk_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      REAL(DOUBLE) , INTENT(IN) :: Q
      REAL(DOUBLE) , INTENT(IN) :: QM
      REAL(DOUBLE) , INTENT(IN) :: SM
      REAL(DOUBLE) , INTENT(IN) :: C
      REAL(DOUBLE) , INTENT(IN) :: CM
      REAL(DOUBLE) , INTENT(OUT) :: A
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: IIQ, IIC, IE
      REAL(DOUBLE), DIMENSION(2) :: GC
!-----------------------------------------------
      GC(1) = ONE
      GC(2) = -ONE
      A = ZERO
      IIQ = TWO*Q + TENTH
      IIC = TWO*C + TENTH
      IF (ITTK(IIQ,IIC,1) == 0) RETURN
      IF (DABS(QM + SM - CM) > EPS) RETURN
      IF (HALF + TENTH < DABS(SM)) RETURN
      IF (Q + TENTH < DABS(QM)) RETURN
      IF (C + TENTH < DABS(CM)) RETURN
      IE = DABS(HALF - SM) + ONE + TENTH
      IF (DABS(Q + HALF - C) < EPS) THEN
         A = DSQRT((C + GC(IE)*CM)/(TWO*C))
      ELSE
         IF (DABS(Q - HALF - C) > EPS) RETURN
         A = -GC(IE)*DSQRT((C - GC(IE)*CM+ONE)/(TWO*C+TWO))
      ENDIF
      RETURN
      END SUBROUTINE C0T5S


!*************************************************
! module mod_sabicm
!	SABICM
!
!*************************************************

module mod_sabicm

	implicit none

contains
!=========
subroutine SABICM

	use prm_inv
	use mod_subabic

	implicit none

	real(8), parameter :: pi = 3.14159265358979324d0
	real(8), parameter :: base = 10.0d0

	real(8) :: detc, detf, tn, abic(20), alphalog(20), step, abicmin, offset
	integer :: i, j, imin

	open(89,file="alpha.dat")
	write(89,*) "log(alpha) alpha abic rsq2 sigma" ! detc jct detf jmt jdata"

	step    = 0.5d0
	offset  = -4.d0
	abicmin = 100000.d0
	imin = 0

	do j = 1, 2

		if(Setalpha < 90.d0) then
			imin = 1
			alphalog(imin) = Setalpha
			exit
		end if

		do i = 1, 20
			alphalog(i) = dble(i-10)*step + offset
			Alpha = base ** alphalog(i) ! 2019.10.10 ynakamura log is natural, not base 10

			CALL RESET (Alpha, detc, Jdata) ! Jdata: E_prior/=0 の数 detc = Detca
			CALL FREEMIN(detf, Rsq2, Jdata) !

			tn = dble(Jdata + Jcp - Jsp)

			abic(i) = tn + tn * dlog(2.d0*pi*Rsq2/tn) - 2.d0 * (dble(Jcp) * dlog(Alpha) + detc - detf)
			Sigma = dsqrt(Rsq2/tn)

			write(89,'(F6.2,4X,F10.5,4X,F10.3,4X,F10.5,4X,F10.5)') alphalog(i), Alpha, abic(i), Rsq2, Sigma

			if(abic(i) < abicmin) then
				abicmin = abic(i)
				imin = i
			end if

		end do

		write(89,*)

		if(imin == 1 .or. imin == 20) stop 'OUT OF ALPHA REGION: LOG ALPHA = (-9,+1)'
		if(j == 2) exit

		step = 0.1d0
		offset = alphalog(imin)

	end do

	alphalog(1) = alphalog(imin)
	Alpha = base ** alphalog(1) ! 2019.10.10 ynakamura dlog() is a natural log, not base 10

	CALL RESET (Alpha, detc, Jdata)
	CALL FREEMIN(detf, Rsq2, Jdata)

	tn = dble(Jdata + Jcp - Jsp)

	abic(1) = tn + tn * dlog(2.d0*pi*Rsq2/tn) - 2.d0 * (dble(Jcp) * dlog(Alpha) + detc - detf)
	Sigma = dsqrt(Rsq2/tn)

	print 600, alphalog(1), abic(1), Rsq2, Sigma, detc, Jcp, detf, Jsp, Jdata
		  600 FORMAT(/' ','LOG_ALPHA=',F6.3,' ABIC=',F10.3,' RSQ2=',F10.3, &
             ' SIGMA=',F10.3//' ','DETC= ',E10.4,' JCT= ',I4/' DETF= ',E10.4, &
             ' JMT= ',I4,12X,' JDATA= ',I4)
	write(89,'(F6.2,4X,F10.5,4X,F10.3,4X,F10.5,4X,F10.5)') alphalog(1), Alpha, abic(1), Rsq2, Sigma

	close(89)

return
end subroutine SABICM
!=========

end module mod_sabicm


      subroutine mch_kerk(w_in,w_out)
******************************************
cc     Written by Lin-Wang Wang, March 30, 2001.
*************************************************************************
**  copyright (c) 2003, The Regents of the University of California,
**  through Lawrence Berkeley National Laboratory (subject to receipt of any
**  required approvals from the U.S. Dept. of Energy).  All rights reserved.
*************************************************************************

******************************************
ccc   now, in n1L,n2L,n3L

      use fft_data
      use load_data
      use data

      implicit double precision (a-h,o-z)
      include 'param.escan_real'

      real*8 w_in(mr_nL),w_out(mr_nL)

      real*8, allocatable,dimension (:)  :: workr_n,vi

      allocate(workr_n(mr_nL))
      allocate(vi(mr_nL))

      ng2_nL=ngtotnod2L(inode)
********************************************************
*** Kerker mixing, this is like the precoditioning,
*** and it adds the new component w_out-w_in into w_in
********************************************************
      do i=1,nr_nL
      w_out(i)=w_out(i)-w_in(i)
      vi(i)=0.d0
      enddo

      workr_n = w_out

      call d3fft_real2L(vi,workr_n,1,0)

      do i=1,ng2_nL

********** in SLDA, v(q=0) is not zero, but has the information of
********** relative highs between up and down, so this point should be
********** treated carefully.

       if(inode.eq.iorg2L(1).and.i.eq.iorg2L(2)) then
       vi(i*2)=(0.3d0-0.8d0)*vi(i*2)
       vi(i*2-1)=(0.3d0-0.8d0)*vi(i*2-1)
        else
        vi(i*2)=(0.8d0*gkk2_nL(i)/(gkk2_nL(i)+0.5d0)-0.8d0)*vi(i*2)
        vi(i*2-1)=(0.8d0*gkk2_nL(i)/(gkk2_nL(i)+0.5d0)-0.8d0)*vi(i*2-1)
       endif

cccccccc   the -0.8d0 is used to cancel the 0.8*w_out in real space, but left with
cccccccc   the gkk components outside Ecut2L
      enddo

      call d3fft_real2L(vi,workr_n,-1,0)

cccccc  The 0.8*w_out cancels the 0.8*vi(i) except for gkk output Ecut2L

      do i=1,nr_nL
      w_in(i)=w_in(i)+workr_n(i)+0.8d0*w_out(i)
      enddo
**************************************************
      deallocate(workr_n)
      deallocate(vi)
      return
      end


C      ALGORITHM 680, COLLECTED ALGORITHMS FROM ACM.
C      THIS WORK PUBLISHED IN TRANSACTIONS ON MATHEMATICAL SOFTWARE,
C      VOL. 16, NO. 1, PP. 47.
      SUBROUTINE WOFZ (XI, YI, U, V, FLAG)
C
C  GIVEN A COMPLEX NUMBER Z = (XI,YI), THIS SUBROUTINE COMPUTES
C  THE VALUE OF THE FADDEEVA-FUNCTION W(Z) = EXP(-Z**2)*ERFC(-I*Z),
C  WHERE ERFC IS THE COMPLEX COMPLEMENTARY ERROR-FUNCTION AND I
C  MEANS SQRT(-1).
C  THE ACCURACY OF THE ALGORITHM FOR Z IN THE 1ST AND 2ND QUADRANT
C  IS 14 SIGNIFICANT DIGITS; IN THE 3RD AND 4TH IT IS 13 SIGNIFICANT
C  DIGITS OUTSIDE A CIRCULAR REGION WITH RADIUS 0.126 AROUND A ZERO
C  OF THE FUNCTION.
C  ALL REAL VARIABLES IN THE PROGRAM ARE DOUBLE PRECISION.
C
C
C  THE CODE CONTAINS A FEW COMPILER-DEPENDENT PARAMETERS :
C     RMAXREAL = THE MAXIMUM VALUE OF RMAXREAL EQUALS THE ROOT OF
C                RMAX = THE LARGEST NUMBER WHICH CAN STILL BE
C                IMPLEMENTED ON THE COMPUTER IN DOUBLE PRECISION
C                FLOATING-POINT ARITHMETIC
C     RMAXEXP  = LN(RMAX) - LN(2)
C     RMAXGONI = THE LARGEST POSSIBLE ARGUMENT OF A DOUBLE PRECISION
C                GONIOMETRIC FUNCTION (DCOS, DSIN, ...)
C  THE REASON WHY THESE PARAMETERS ARE NEEDED AS THEY ARE DEFINED WILL
C  BE EXPLAINED IN THE CODE BY MEANS OF COMMENTS
C
C
C  PARAMETER LIST
C     XI     = REAL      PART OF Z
C     YI     = IMAGINARY PART OF Z
C     U      = REAL      PART OF W(Z)
C     V      = IMAGINARY PART OF W(Z)
C     FLAG   = AN ERROR FLAG INDICATING WHETHER OVERFLOW WILL
C              OCCUR OR NOT; TYPE LOGICAL;
C              THE VALUES OF THIS VARIABLE HAVE THE FOLLOWING
C              MEANING :
C              FLAG=.FALSE. : NO ERROR CONDITION
C              FLAG=.TRUE.  : OVERFLOW WILL OCCUR, THE ROUTINE
C                             BECOMES INACTIVE
C  XI, YI      ARE THE INPUT-PARAMETERS
C  U, V, FLAG  ARE THE OUTPUT-PARAMETERS
C
C  FURTHERMORE THE PARAMETER FACTOR EQUALS 2/SQRT(PI)
C
C  THE ROUTINE IS NOT UNDERFLOW-PROTECTED BUT ANY VARIABLE CAN BE
C  PUT TO 0 UPON UNDERFLOW;
C
C  REFERENCE - GPM POPPE, CMJ WIJERS; MORE EFFICIENT COMPUTATION OF
C  THE COMPLEX ERROR-FUNCTION, ACM TRANS. MATH. SOFTWARE.
C
*
*
*
*
      IMPLICIT DOUBLE PRECISION (A-H, O-Z)
c      DOUBLE PRECISION D1MACH, FACTOR, RMAX, RMAXREAL, RMAXEXP
*
      LOGICAL A, B, FLAG
c      PARAMETER (FACTOR   = 1.12837916709551257389615890312154517D0,
c     *           RMAXREAL = 1.340780792994259D+154,
c     *           RMAXEXP  = 709.0895657128241D0,
c     *           RMAXGONI = 0.6746518850690209D10)
      PARAMETER (FACTOR   = 1.12837916709551257389615890312154517D0,
     *           RMAXREAL = 0.5D+154,
     *           RMAXEXP  = 708.503061461606D0,
     *           RMAXGONI = 3.53711887601422D+15)
c      RMAX = D1MACH(2)
c      RMAXREAL = DSQRT(RMAX)
c      RMAXEXP = DLOG(RMAX)-DLOG(2D0)
*
      FLAG = .FALSE.
*
      XABS = DABS(XI)
      YABS = DABS(YI)
      X    = XABS/6.3
      Y    = YABS/4.4
*
C
C     THE FOLLOWING IF-STATEMENT PROTECTS
C     QRHO = (X**2 + Y**2) AGAINST OVERFLOW
C
      IF ((XABS.GT.RMAXREAL).OR.(YABS.GT.RMAXREAL)) GOTO 100
*
      QRHO = X**2 + Y**2
*
      XABSQ = XABS**2
      XQUAD = XABSQ - YABS**2
      YQUAD = 2*XABS*YABS
*
      A     = QRHO.LT.0.085264D0
*
      IF (A) THEN
C
C  IF (QRHO.LT.0.085264D0) THEN THE FADDEEVA-FUNCTION IS EVALUATED
C  USING A POWER-SERIES (ABRAMOWITZ/STEGUN, EQUATION (7.1.5), P.297)
C  N IS THE MINIMUM NUMBER OF TERMS NEEDED TO OBTAIN THE REQUIRED
C  ACCURACY
C
        QRHO  = (1-0.85*Y)*DSQRT(QRHO)
        N     = IDNINT(6 + 72*QRHO)
        J     = 2*N+1
        XSUM  = 1.0/J
        YSUM  = 0.0D0
        DO 10 I=N, 1, -1
          J    = J - 2
          XAUX = (XSUM*XQUAD - YSUM*YQUAD)/I
          YSUM = (XSUM*YQUAD + YSUM*XQUAD)/I
          XSUM = XAUX + 1.0/J
 10     CONTINUE
        U1   = -FACTOR*(XSUM*YABS + YSUM*XABS) + 1.0
        V1   =  FACTOR*(XSUM*XABS - YSUM*YABS)
        DAUX =  DEXP(-XQUAD)
        U2   =  DAUX*DCOS(YQUAD)
        V2   = -DAUX*DSIN(YQUAD)
*
        U    = U1*U2 - V1*V2
        V    = U1*V2 + V1*U2
*
      ELSE
C
C  IF (QRHO.GT.1.O) THEN W(Z) IS EVALUATED USING THE LAPLACE
C  CONTINUED FRACTION
C  NU IS THE MINIMUM NUMBER OF TERMS NEEDED TO OBTAIN THE REQUIRED
C  ACCURACY
C
C  IF ((QRHO.GT.0.085264D0).AND.(QRHO.LT.1.0)) THEN W(Z) IS EVALUATED
C  BY A TRUNCATED TAYLOR EXPANSION, WHERE THE LAPLACE CONTINUED FRACTION
C  IS USED TO CALCULATE THE DERIVATIVES OF W(Z)
C  KAPN IS THE MINIMUM NUMBER OF TERMS IN THE TAYLOR EXPANSION NEEDED
C  TO OBTAIN THE REQUIRED ACCURACY
C  NU IS THE MINIMUM NUMBER OF TERMS OF THE CONTINUED FRACTION NEEDED
C  TO CALCULATE THE DERIVATIVES WITH THE REQUIRED ACCURACY
C
*
        IF (QRHO.GT.1.0) THEN
          H    = 0.0D0
          KAPN = 0
          QRHO = DSQRT(QRHO)
          NU   = IDINT(3 + (1442/(26*QRHO+77)))
        ELSE
          QRHO = (1-Y)*DSQRT(1-QRHO)
          H    = 1.88*QRHO
          H2   = 2*H
          KAPN = IDNINT(7  + 34*QRHO)
          NU   = IDNINT(16 + 26*QRHO)
        ENDIF
*
        B = (H.GT.0.0)
*
        IF (B) QLAMBDA = H2**KAPN
*
        RX = 0.0
        RY = 0.0
        SX = 0.0
        SY = 0.0
*
        DO 11 N=NU, 0, -1
          NP1 = N + 1
          TX  = YABS + H + NP1*RX
          TY  = XABS - NP1*RY
          C   = 0.5/(TX**2 + TY**2)
          RX  = C*TX
          RY  = C*TY
          IF ((B).AND.(N.LE.KAPN)) THEN
            TX = QLAMBDA + SX
            SX = RX*TX - RY*SY
            SY = RY*TX + RX*SY
            QLAMBDA = QLAMBDA/H2
          ENDIF
 11     CONTINUE
*
        IF (H.EQ.0.0) THEN
          U = FACTOR*RX
          V = FACTOR*RY
        ELSE
          U = FACTOR*SX
          V = FACTOR*SY
        END IF
*
        IF (YABS.EQ.0.0) U = DEXP(-XABS**2)
*
      END IF
*
*
C
C  EVALUATION OF W(Z) IN THE OTHER QUADRANTS
C
*
      IF (YI.LT.0.0) THEN
*
        IF (A) THEN
          U2    = 2*U2
          V2    = 2*V2
        ELSE
          XQUAD =  -XQUAD
*
C
C         THE FOLLOWING IF-STATEMENT PROTECTS 2*EXP(-Z**2)
C         AGAINST OVERFLOW
C
          IF ((YQUAD.GT.RMAXGONI).OR.
     *        (XQUAD.GT.RMAXEXP)) GOTO 100
*
          W1 =  2*DEXP(XQUAD)
          U2  =  W1*DCOS(YQUAD)
          V2  = -W1*DSIN(YQUAD)
        END IF
*
        U = U2 - U
        V = V2 - V
        IF (XI.GT.0.0) V = -V
      ELSE
        IF (XI.LT.0.0) V = -V
      END IF
*
      RETURN
*
  100 FLAG = .TRUE.
      RETURN
*
      END


      subroutine VKlSt(q,m,aux,i,f)
c     subroutine for massive V spinor alla Kleiss and Stirling
C     q complex vector q(4),q(1),q(2),q(3)=E_q,qx,qy,qz
C     'aux' massless auxiliary complex vector
C     i polarization 1 or -1
C     f output spinor
      implicit none
      integer i,j
      double precision m
      double complex q(4),qt(4),aux(4),f(4),fqt(4),eta(4),den,qDaux,qsq

      if (m .eq. 0d0) then
        call uspinor0(q,-i,f)
        return
      else
        qDaux=q(4)*aux(4)-q(1)*aux(1)-q(2)*aux(2)-q(3)*aux(3)
        qsq=q(4)**2-q(1)**2-q(2)**2-q(3)**2
        qt(:)=q(:)-0.5d0*qsq/qDaux*aux(:)
        call uspinor0(aux,i,eta)
        call ubarspinor0(qt,-i,fqt)
        den=eta(1)*fqt(1)+eta(2)*fqt(2)+eta(3)*fqt(3)+eta(4)*fqt(4)
      endif

      call kslashU(q,eta,f)
      do j=1,4
      f(j)=(f(j)-eta(j)*m)/den
      enddo
      return
      end


c.....Function BK(n,z)
c.....BK(n,z) is the n-derivative of BesselK[nu,z]
c.....with respect to nu in nu=1

c     NOW b0->b0p INCLUDED

      function IK(m)
      implicit none
      external BK
      real *8 BK,IK,argum,dloqt,a_param,b0p
      integer m
      include 'scales.F'
      include 'const.F'
      common/a_param/a_param,b0p

      argum=b0p*qt/q
      dloqt=DLOG(a_param*qt/q)

      if (m.eq.1) then
         IK=-(2*b0p/q/qt)*BK(0,argum)
      elseif (m.eq.2) then
         IK=-(4*b0p/q/qt)*(BK(1,argum)-BK(0,argum)*DLOqt)
      elseif (m.eq.3) then
         IK=(b0p/q/qt)*(-6*BK(2,argum)+12*BK(1,argum)*DLOqt+
     /                 BK(0,argum)*(pi**2-6*(DLOqt**2)))
      elseif (m.eq.4) then
         IK=-(4*b0p/q/qt)*(2*BK(3,argum)-6*DLOqt*BK(2,argum)+
     /                    BK(1,argum)*(6*(DLOqt)**2-pi**2)+
     /                    BK(0,argum)*(pi**2*DLOqt-
     /                                 2*(DLOqt)**3-4*Z3))
      endif

      return
      end

      function BK(n,z)
      implicit none
      external fb
      real *8 bk,fb,errest,z,zz,max,adpint
      integer n,nn,ifail
      common/nuorder/nn
      common/zz/zz
      nn=n
      zz=z
      max=10d0
      bk=adpint(fb,0d0,max,1d-10,1d-5,errest,ifail)
      return
      end

      function fb(t)
      implicit none
      integer nn,nu
      real *8 fb,t,zz
      common/nuorder/nn
      common/zz/zz
      nu=1
      if(nn.eq.0) then
         fb=dexp(-zz*dcosh(t))*dcosh(nu*t)
      elseif(nn.eq.1) then
         fb=dexp(-zz*dcosh(t))*t*dsinh(nu*t)
      elseif(nn.eq.2) then
         fb=dexp(-zz*dcosh(t))*t*t*dcosh(nu*t)
      elseif(nn.eq.3) then
         fb=dexp(-zz*dcosh(t))*t*t*t*dsinh(nu*t)
      endif
      return
      end


      double precision function bmount(xc,yc)
      implicit none

      double precision xc,yc
      double precision Rsphere, Omega, Px, Py, Pz
      double precision theta,thetam
      double precision pi, xp, yp, zp

      common /comsphere/ Rsphere, Omega
      common /comic/ Px,Py,Pz
      common /compi/ pi

      bmount = -4.d4

c     # compute latitude:
      call mapc2m(xc,yc,xp,yp,zp)
      theta = asin((xp*Px + yp*Py + zp*Pz) / Rsphere)

      thetam = -pi/6.d0 !position of ridge
      bmount = bmount + 3.d4*exp(-1000.d0*(theta-thetam)**2)
      return
      end


      FUNCTION ABLIQ(WATER,FREQ,TEMP)
C     COMPUTES ABSORPTION IN NEPERS/KM BY SUSPENDED WATER DROPLETS
c     ARGUMENTS (INPUT):
C     WATER IN G/M**3
C     FREQ IN GHZ     (VALID FROM 0 TO 1000 GHZ)
C     TEMP IN KELVIN
C
C     REFERENCES:
C     LIEBE, HUFFORD AND MANABE, INT. J. IR & MM WAVES V.12, pp.659-675
C      (1991);  Liebe et al, AGARD Conf. Proc. 542, May 1993.
c
C     REVISION HISTORY:
C        PWR 8/3/92   original version
c        PWR 12/14/98 temp. dependence of EPS2 eliminated to agree
c                     with MPM93
c        pwr 2/27/02  use exponential dep. on T, eq. 2b instead of eq. 4a
C
      COMPLEX EPS,RE
      IF(WATER.LE.0.) THEN
       ABLIQ = 0.
       RETURN
      ENDIF
      THETA1 = 1.-300./TEMP
      EPS0 = 77.66 - 103.3*THETA1
      EPS1 = .0671*EPS0
      EPS2 = 3.52                 ! from MPM93
      FP = 20.1*EXP(7.88*THETA1)  ! from eq. 2b
      FS = 39.8*FP
      EPS = (EPS0-EPS1)/CMPLX(1.,FREQ/FP) +
     & (EPS1-EPS2)/CMPLX(1.,FREQ/FS) +EPS2
      RE = (EPS-1.)/(EPS+2.)
      ABLIQ = -.06286*AIMAG(RE)*FREQ*WATER
      RETURN
      END


      subroutine nextls(igrow,igmax,sdedy,sunstp,anome,eccn,vinc,xls)

!     Legacy Mars GCM v24
!     Mars Climate Modeling Center
!     NASA Ames Research Center

      use constants_h, only: PI

      implicit none

      real*8 anome(1000)
      integer dy, sdedy

      real*8  :: quad, si, co, x, arsin, arcos

      quad(si,co) = PI-SIGN(PI-CO,si)
      arsin(X)    = ASIN(X)
      arcos(X)    = ACOS(X)

      integer :: ig, igmax, igrow
      real*8  :: esq, xls, raddeg, vinc, anomtp, anomt, v2, v1
      real*8  :: cose, sine, sd, eccn, sunstp

C======================================================================C

      esq = SQRT(1.0-ECCN**2)

      ig = igrow+1
      sd = sdedy

      if(ig.gt.1) then
        if(ig.le.igmax) then
          sd = sdedy+1
        end if

        if(ig.gt.igmax) then
          sd = sdedy+sunstp
        end if
      end if

      dy = sd+1

      cose   = cos(anome(dy))
      sine   = sin(anome(dy))
      v1     = arcos((cose-eccn)/(1.0-eccn*cose))
      v2     = arsin(esq*sine/(1.0-eccn*cose))
      anomt  = quad(v2,v1)
      anomtp = anomt-vinc+PI*0.5

      if(anomtp.lt.0.0) then
        anomtp = anomtp+2.0*PI
      end if

      raddeg = 180.0/PI
      xls    = anomtp*raddeg

      if(xls.lt.0.0) then
        xls = xls+360.0
      end if
      xls = mod(xls,360.0)

      return
      end


*                              D   I   S   C   L   A   I   M   E   R
*
*       You are using a BETA version of the program fourier_lib_module.f, which is currently
*       under development by D. A. Spong of the Fusion Energy Division,
*       Oak Ridge National Laboratory.  Please report any problems or comments
*       to him.  As a BETA version, this program is subject to periodic change
*       and improvement without notice.
*
      module kind_spec
c
c     Double precision
c
      integer, parameter :: rprec = selected_real_kind(12,100)
      integer, parameter :: iprec = selected_int_kind(8)
      end module kind_spec
c
c
c Variables that need to be set for each time the configuration is changed:
c   ith, izt, nfp, mpol, ntor, mp_col, nt_col, mode_family, tokamak
c
      module fourier_lib
      use kind_spec
      implicit none
      integer :: ith, izt, nfp, mode_family
      integer :: mpol, ntor
      integer :: nznt, mnmx
      integer :: i, j, lg, nu, nl, m, n, mn, istat
c   Equilibrium coefficient arrays
      real(kind=rprec), allocatable :: thtgrd(:),ztgrd(:),rn(:),rm(:)
      real(kind=rprec), allocatable :: fnm(:), f(:), anm(:)
      real(kind=rprec), allocatable :: cos_ar(:,:),sin_ar(:,:)
      real(kind=rprec), allocatable :: cos_toF(:,:),sin_toF(:,:)
      integer, allocatable :: in(:), im(:)
      real(kind=rprec) twopi, arg
c   Eigenfunction arrays,variables
      integer :: count, mn_col, ith_col, izt_col, ntors
      real(kind=rprec), allocatable :: rn_col(:),rm_col(:),
     >     rho(:), y_phi(:)
      real(kind=rprec), allocatable :: rn2_col(:),rm2_col(:),rnm_col(:)
      integer, allocatable :: in_col(:), im_col(:)
      integer, allocatable :: ntora(:), mpol_lwr(:), mpol_upr(:)
c   Fourier mode block sub-matrices
      real(kind=rprec), allocatable :: a(:,:,:), b(:,:,:), d(:,:,:),
     1  aa(:,:,:), bb(:,:,:), dd(:,:,:), dd1(:,:,:), dd2(:,:,:),
     2  cc(:,:,:)

      contains

      subroutine readin
       open(unit=20,file="fourier.dat",status="old")
       read(20,*) nfp, ith, izt, mode_family
c       mpol = ith/5; ntor = izt/5
       mpol = ith*2/5; ntor = izt*2/5
       nznt=ith*izt; mnmx=(2*ntor+1)*mpol-ntor
       read(20,*) ntors
       allocate(ntora(ntors), stat=istat)
       allocate(mpol_lwr(ntors), stat=istat)
       allocate(mpol_upr(ntors), stat=istat)
       do i=1,ntors
        read(20,*) ntora(i), mpol_lwr(i), mpol_upr(i)
       end do
       close(unit=20)
      end subroutine readin
c
      subroutine trig_array
      use kind_spec
c      use fourier_internal_vars
      implicit none
      real(kind=rprec) :: dum,dnorm
      allocate(thtgrd(nznt), stat=istat)
      allocate(ztgrd(nznt), stat=istat)
      allocate(im(mnmx), stat=istat)
      allocate(in(mnmx), stat=istat)
      allocate(rm(mnmx), stat=istat)
      allocate(rn(mnmx), stat=istat)
      allocate(fnm(mnmx), stat=istat)
      allocate(f(nznt), stat=istat)
      allocate(anm(mnmx), stat=istat)
      allocate(cos_ar(nznt,mnmx), stat=istat)
      allocate(sin_ar(nznt,mnmx), stat=istat)
      allocate(cos_toF(nznt,mnmx), stat=istat)
      allocate(sin_toF(nznt,mnmx), stat=istat)
      twopi = 8.d0*atan(1.d0)
c    Generate theta, zeta grid
      lg = 0
      do i=1,izt
      do j=1,ith
       lg = lg + 1
       ztgrd(lg) = twopi*dble(i-1)/(dble(nfp*izt))
       thtgrd(lg) = twopi*dble(j-1)/dble(ith)
      end do
      end do
c    Generate Fourier mode distribution
      mn = 0
      nu = ntor
      do m=0,mpol-1
       nl = -ntor
       if(m .eq. 0) nl = 0
        do n = nl,nu
         mn = mn + 1
         rm(mn) = dble(m)
         rn(mn) = dble(n*nfp)
         im(mn) = m
         in(mn) = n*nfp
        enddo
      enddo
      do i=1,nznt
       do mn=1,mnmx
        arg = -rn(mn)*ztgrd(i) + rm(mn)*thtgrd(i)
        cos_ar(i,mn) = cos(arg)
        sin_ar(i,mn) = sin(arg)
        dnorm = 2.d0/dble(nznt)
        dum = abs(rn(mn)) + abs(rm(mn))
        if(nint(dum) .eq. 0) dnorm = .5d0*dnorm
        cos_toF(i,mn) = cos_ar(i,mn)*dnorm
        sin_toF(i,mn) = sin_ar(i,mn)*dnorm
       end do
      end do
      end subroutine trig_array

      subroutine convolution_array
      use kind_spec
      implicit none
c     First, count the number of modes to be used
      count = 0
      do i=1,ntors
       count = count + mpol_upr(i) - mpol_lwr(i) + 1
      end do
      mn_col = count
      write(*,'("mn_col = ",i5)') mn_col
c
c     Allocate memory for arrays needed in eigenfunction calculations
      allocate(rm_col(mn_col), stat=istat)
      allocate(rn_col(mn_col), stat=istat)
      allocate(im_col(mn_col), stat=istat)
      allocate(in_col(mn_col), stat=istat)
      allocate(rm2_col(mn_col), stat=istat)
      allocate(rn2_col(mn_col), stat=istat)
      allocate(rnm_col(mn_col), stat=istat)
c     Create mode number arrays needed for eigenfunction
       count = 0
       do n = 1,ntors
        do m = mpol_lwr(n), mpol_upr(n)
         count = count + 1
         rm_col(count) = real(m)
         rn_col(count) = real(ntora(n))
         im_col(count) = m
         in_col(count) = ntora(n)
	 write(*,*) count, im_col(count),in_col(count)
         rm2_col(count) = rm_col(count)*rm_col(count)
         rn2_col(count) = rn_col(count)*rn_col(count)
         rnm_col(count) = rm_col(count)*rn_col(count)
        end do
      end do
      mn_col = count
c      write(*,*) mn_col, count
      end subroutine convolution_array
c
c
      subroutine scs_convolve(ans,m1,n1,m2,n2,meq,neq)
      use kind_spec
      implicit none
      real(kind=rprec) :: tht_int1, tht_int2, tht_int3, tht_int4,
     1  zeta_int1, zeta_int2, zeta_int3, zeta_int4, ans
      integer :: m1, m2, n1, n2, meq, neq
      integer :: sm1, sm2, sn1, sn2, smeq, sneq
c
c     This subroutine calculates the 2D integral for tht and zeta running
c     from 0 to 2*PI of:
c     sin(m1*tht - n1*zeta)*cos(meq*tht - neq*zeta)*sin(m2*tht - n2*zeta)
c     The value returned is actually (2/PI) times this integral.
c
      sm1=1;sm2=1;sn1=1;sn2=1;smeq=1;sneq=1
      if(m1 .ne. 0) sm1 = m1/abs(m1)
      if(m2 .ne. 0) sm2 = m2/abs(m2)
      if(n1 .ne. 0) sn1 = n1/abs(n1)
      if(n2 .ne. 0) sn2 = n2/abs(n2)
      if(meq .ne. 0) smeq = meq/abs(meq)
      if(neq .ne. 0) sneq = neq/abs(neq)
      m1 = sm1*m1; n1 = sn1*n1
      m2 = sm2*m2; n2 = sn2*n2
      meq = smeq*meq; neq = sneq*neq
      call css(tht_int1, meq, m1, m2)
      call ccc(zeta_int1, n1, n2, neq)
      call ccc(tht_int2, m1, m2, meq)
      call css(zeta_int2, neq, n1, n2)
      call css(tht_int3, m2, m1, meq)
      call css(zeta_int3, n1, n2, neq)
      call css(tht_int4, m1, m2, meq)
      call css(zeta_int4, n2, n1, neq)
      ans = tht_int1*zeta_int1*sm1*sm2
     1    + tht_int2*zeta_int2*sn1*sn2
     2    - tht_int3*zeta_int3*sm1*smeq*sn2*sneq
     3    - tht_int4*zeta_int4*sm2*smeq*sn1*sneq
      m1 = sm1*m1; n1 = sn1*n1
      m2 = sm2*m2; n2 = sn2*n2
      meq = smeq*meq; neq = sneq*neq
      end subroutine scs_convolve
c
c
c
      subroutine ccc_convolve(ans,m1,n1,m2,n2,meq,neq)
      use kind_spec
      implicit none
      real(kind=rprec) :: tht_int1, tht_int2, tht_int3, tht_int4,
     1  zeta_int1, zeta_int2, zeta_int3, zeta_int4, ans
      integer :: m1, m2, n1, n2, meq, neq
      integer :: sm1, sm2, sn1, sn2, smeq, sneq
c
c     This subroutine calculates the 2D integral for tht and zeta running
c     from 0 to 2*PI of:
c     cos(m1*tht - n1*zeta)*cos(meq*tht - neq*zeta)*cos(m2*tht - n2*zeta)
c     The value returned is actually (2/PI) times this integral.
c
      sm1=1;sm2=1;sn1=1;sn2=1;smeq=1;sneq=1
      if(m1 .ne. 0) sm1 = m1/abs(m1)
      if(m2 .ne. 0) sm2 = m2/abs(m2)
      if(n1 .ne. 0) sn1 = n1/abs(n1)
      if(n2 .ne. 0) sn2 = n2/abs(n2)
      if(meq .ne. 0) smeq = meq/abs(meq)
      if(neq .ne. 0) sneq = neq/abs(neq)
      m1 = sm1*m1; n1 = sn1*n1
      m2 = sm2*m2; n2 = sn2*n2
      meq = smeq*meq; neq = sneq*neq
      call ccc(tht_int1, m1, m2, meq)
      call ccc(zeta_int1, n1, n2, neq)
      call css(tht_int2, meq, m1, m2)
      call css(zeta_int2, neq, n1, n2)
      call css(tht_int3, m1, m2, meq)
      call css(zeta_int3, n1, n2, neq)
      call css(tht_int4, m2, m1, meq)
      call css(zeta_int4, n2, n1, neq)
      ans = tht_int1*zeta_int1
     1    + tht_int2*zeta_int2*sm1*sm2*sn1*sn2
     2    + tht_int3*zeta_int3*sm2*smeq*sn2*sneq
     3    + tht_int4*zeta_int4*sm1*smeq*sn1*sneq
      m1 = sm1*m1; n1 = sn1*n1
      m2 = sm2*m2; n2 = sn2*n2
      meq = smeq*meq; neq = sneq*neq
      end subroutine ccc_convolve
c
c
c
      subroutine ccc(result, i, j, k)
      use kind_spec
      real(kind=rprec), parameter :: zero = 0, one = 1,
     1    two = 2, four = 4
      real(kind=rprec) :: result
      integer :: i, j, k, izeros
c
c     This subroutine calculates the 1D integral of
c     cos(i*x)*cos(j*x)*cos(k*x)
c     for x running from 0 to 2*PI.
c     The value returned is actually (2/PI) times this integral.
c
      result = zero
      izeros = 0
      if(i .eq. 0) izeros = izeros + 1
      if(j .eq. 0) izeros = izeros + 1
      if(k .eq. 0) izeros = izeros + 1
      if(izeros .ne. 0) then
       if(izeros .eq. 3) then
        result = four
        return
       else if(izeros .eq. 2) then
        result = zero
        return
       else if(izeros .eq. 1) then
        if(i .eq. 0) then
         result = zero
         if(j*j .eq. k*k) then
          result = two
         endif
         return
        else if(j .eq. 0) then
          result = zero
          if(i*i .eq. k*k) then
          result = two
         endif
         return
        else if(k .eq. 0) then
          result = zero
          if(i*i .eq. j*j) then
          result = two
         endif
         return
        endif
       endif
      else if (izeros .eq. 0) then
       if(k .eq. (i-j)) result = one
       if(k .eq. (i+j)) result = one
       if(k .eq. (j-i)) result = one
       if(k .eq. -(i+j)) result = one
      endif
      end subroutine ccc
c
c
c
      subroutine css(result, k, i, j)
      use kind_spec
      real(kind=rprec), parameter :: one = 1.d0, neg_one = -1.d0,
     1   zero = 0.d0, two = 2.d0, neg_two = -2.d0
      integer :: i, j, k
      real(kind=rprec) :: result
c
c     This subroutine calculates the 1D integral of
c     cos(k*x)*sin(i*x)*sin(j*x)
c     for x running from 0 to 2*PI.
c     The value returned is actually (2/PI) times this integral.
c
      result = zero
      if(i .eq. 0 .or. j .eq. 0) return
      if(k .eq. 0) then
        if(i*i .ne. j*j) then
         return
        else if(i*i .eq. j*j) then
         result = two
         return
        endif
      endif
      if(k .eq. (i-j)) result = one
      if(k .eq. (i+j)) result = neg_one
      if(k .eq. (j-i)) result = one
      if(k .eq. -(i+j)) result = neg_one
      end subroutine css
c
c
      subroutine trig_deallocate
      integer :: istat
c      deallocate(thtgrd,ztgrd,rm,rn,cos_ar,sin_ar,
c     1   rm_col,rn_col,im_col,in_col,
c     2   rm2_col,rn2_col,rnm_col,stat=istat)
      deallocate(thtgrd,ztgrd,cos_ar,sin_ar,stat=istat)
      end subroutine trig_deallocate

      subroutine toFourier(ftype)
      use kind_spec
      implicit none
      character*1 ftype
c
c   Do Fourier transform integrations needed to convert data on a
c   theta, zeta grid [stored in array f(i=1,nznt)] to a set
c   of Fourier amplitudes [stored in array fnm(mn=1,mnmx)].
c   Typically, the number of grid points in each direction needs
c   to be > 3*number of modes used in each direction to avoid
c   aliasing errors(implies nznt > 9*mnmx).
c
c       if(ftype .eq. 's') then
c         fnm = matmul(f,sin_toF)
c       else if(ftype .eq. 'c') then
c         fnm = matmul(f,cos_toF)
c       endif
      do mn=1,mnmx     !loop over Fourier modes
       fnm(mn) = 0.d0
      do i=1,nznt      !loop over theta,zeta grid
       if(ftype .eq. 's') then
         fnm(mn) = fnm(mn) + f(i)*sin_toF(i,mn)
       else if(ftype .eq. 'c') then
         fnm(mn) = fnm(mn) + f(i)*cos_toF(i,mn)
       endif
      end do
      end do
c
      return
      end subroutine toFourier
c
c
      subroutine toReal(ftype)
c
c    Convert Fourier mode representation [stored in array anm(mn=1,mnmx)]
c    to values of function on a regularly spaced 2D grid
c    [stored in array f(i=1,nznt)].
c
      use kind_spec
      implicit none
      character*1 ftype
      do i=1,nznt
      f(i) = 0.d0
        do mn=1,mnmx
         if(ftype .eq. 's') then
          f(i) = f(i) + anm(mn)*sin_ar(i,mn)
         else if(ftype .eq. 'c') then
          f(i) = f(i) + anm(mn)*cos_ar(i,mn)
         endif
        end do
       end do
      return
      end subroutine toReal
c
c
c
      subroutine dbydth(ftype)
c
c    Take the theta derivative of the input Fourier amplitude array, fnm
c    and place the result in the output Fourier amplitude array, anm.
c    Changes to the sin/cos parity are reflected through the sin_type and
c    cos_type variables.
c
      use kind_spec
      implicit none
      character*1 ftype
      do i=1,mnmx
         if(ftype .eq. 's') then
          anm(i) = rm(i)*fnm(i)
         else if(ftype .eq. 'c') then
          anm(i) = -rm(i)*fnm(i)
         endif
        end do
c
c   following lines for resetting ftype cannot be used if
c    dbydth is called with constant arg (i.e., dbydth('c')
c    as it is in ae_solve.f
c
c        if(ftype .eq. 's') then
c          ftype = 'c'
c         else if(ftype .eq. 'c') then
c          ftype = 's'
c        endif
       return
       end subroutine dbydth
c
c
c
      subroutine dbydzt(ftype)
c
c    Take the zeta derivative of the input Fourier amplitude array, fnm
c    and place the result in the output Fourier amplitude array, anm.
c    Changes to the sin/cos parity are reflected through the sin_type and
c    cos_type variables.
c
      use kind_spec
      implicit none
      character*1 ftype
      do i=1,mnmx
         if(ftype .eq. 's') then
          anm(i) = -rn(i)*fnm(i)
          else if(ftype .eq. 'c') then
          anm(i) = rn(i)*fnm(i)
         endif
        end do
c
c   following lines for resetting ftype cannot be used if
c    dbydzt is called with constant arg (i.e., dbydzt('c')
c    as it is in ae_solve.f
c
c        if(ftype .eq. 's') then
c          ftype = 'c'
c         else if(ftype .eq. 'c') then
c          ftype = 's'
c        endif
       return
       end subroutine dbydzt
c
c
      end module fourier_lib


!	Find means and covariance matrices corresponding to a predictor list for the multinomial case.

!	beta is the parameter vector.
!	predlist is the predictor list.
!	quadpoint is the array of quadrature points.
!	quadweight is the array of quadrature weights.

!	covs are the covariance matrices.
!	means are the means.

subroutine getmeancovm(beta,predlist,quadpoint,quadweight,covs,means)
implicit none
interface
!
!  Find statistics of mean and covariance matrix of underlying latent vector.
!
!   beta is the estimated parameter vector.
!	quadpoint is the array of quadrature points.
!	quadweight is the array of quadrature weights.
!   thpr is a predictor vector.
!   covmatrix is the estimated covariance matrix of the underlying latent vector corresponding to beta.
!   meanvec is the estimated mean of the underlying latent vector.
!   Note that this routine is for the case of an underlying multinomial latent vector.
	subroutine meancovstatm(beta,quadpoint,quadweight,thpr,covmatrix,meanvec)
		implicit none
		real(kind=8),intent(in)::beta(:),quadpoint(:,:),quadweight(:),thpr(:)
		real(kind=8),intent(out)::covmatrix(:,:),meanvec(:)
	end subroutine meancovstatm

end interface

real(kind=8),intent(in)::beta(:),predlist(:,:),quadpoint(:,:),quadweight(:)
real(kind=8),intent(out)::covs(:,:,:),means(:,:)
!	row counts entries in predlist.
integer::row
do row=1,size(predlist,2)
	call meancovstatm(beta,quadpoint,quadweight,predlist(:,row),covs(:,:,row),means(:,row))
end do
return
end subroutine getmeancovm


program test_free_energy_fft

! nuclear charge: 1 Gaussian
! electronic charge: 1 Gaussian
! calculation: single free energy evaluation

! This test uses FFT and produces the same result as test_free_energy3

use types, only: dp
use constants, only: i_
use ofdft, only: read_pseudo
use ofdft_fft, only: free_energy, radial_potential_fourier, &
    reciprocal_space_vectors, free_energy_min, real_space_vectors
use constants, only: Ha2eV, pi
use utils, only: loadtxt, stop_error, assert, linspace
use splines, only: spline3pars, iixmin, poly3, spline3ders
use interp3d, only: trilinear
use converged_energies, only: one_gaussian
implicit none
real(dp) :: Eee, Een, Ts, Exc, Etot, Etot_conv
integer :: Ng
real(dp) :: Z
real(dp), allocatable :: R(:), G(:, :, :, :), G2(:, :, :), Xn(:, :, :, :)
real(dp), allocatable :: ne(:, :, :), dFdn(:, :, :)
real(dp), allocatable :: VenG0(:, :, :)
real(dp) :: V0
complex(dp), allocatable :: VenG(:, :, :)
real(dp) :: L, T_eV, T_au
integer :: i, j, k
real(dp) :: x, y, z_, r_, alpha_ne, alpha_nen

Ng = 80

L = 2
T_eV = 0.0862_dp
T_au = T_ev / Ha2eV

alpha_nen = 6
alpha_ne = 5

Z = 1

allocate(VenG0(Ng, Ng, Ng), VenG(Ng, Ng, Ng), ne(Ng, Ng, Ng), dFdn(Ng, Ng, Ng))
allocate(G(Ng, Ng, Ng, 3), G2(Ng, Ng, Ng), Xn(Ng, Ng, Ng, 3))
allocate(R(10000))
R = linspace(1._dp/10000, 0.9_dp, 10000)
call radial_potential_fourier(R, Z*erf(alpha_nen*R)/R, L, Z, VenG0, V0)

call real_space_vectors([L, L, L], Xn)
call reciprocal_space_vectors([L, L, L], G, G2)
ne = 1
do i = 1, Ng
do j = 1, Ng
do k = 1, Ng
    x = Xn(i, j, k, 1) - L/2
    y = Xn(i, j, k, 2) - L/2
    z_ = Xn(i, j, k, 3) - L/2
    r_ = sqrt(x**2+y**2+z_**2)
    ne(i, j, k) = alpha_ne**3/pi**(3._dp/2) * exp(-alpha_ne**2*r_**2)
end do
end do
end do
! Ion position:
x = L/2 + L/64
y = L/2
z_ = L/2
VenG = -VenG0 * exp(-i_*(G(:,:,:,1)*x+G(:,:,:,2)*y+G(:,:,:,3)*z_))
call free_energy(L, G2, T_au, VenG, ne, Eee, Een, Ts, Exc, Etot, dFdn, &
    calc_value=.true., calc_derivative=.false.)
Etot_conv = sum(one_gaussian)
print *, "Summary of energies [a.u.]:"
print "('    Ts   = ', f14.8)", Ts
print "('    Een  = ', f14.8)", Een
print "('    Eee  = ', f14.8)", Eee
print "('    Exc  = ', f14.8)", Exc
print *, "   ---------------------"
print "('    Etot = ', f14.8, ' a.u. = ', f14.8, ' eV')", Etot, Etot*Ha2eV
print *, "Errors:"
print *, abs(Ts - one_gaussian(1))
print *, abs(Een - one_gaussian(2))
print *, abs(Eee - one_gaussian(3))
print *, abs(Exc - one_gaussian(4))
print *, abs(Etot - Etot_conv)
call assert(abs(Ts - one_gaussian(1)) < 1e-8_dp)
call assert(abs(Een - one_gaussian(2)) < 1e-8_dp)
call assert(abs(Eee - one_gaussian(3)) < 1e-8_dp)
call assert(abs(Exc - one_gaussian(4)) < 1e-8_dp)
call assert(abs(Etot - Etot_conv) < 1e-8_dp)
end program


c  Numerical Recipes routines but change NMAX in svdfit to be bigger
c  (was 1000, try 10000).  Renamed to "bigsvdfit" to avoid collisions.
c  I don't think I need to change any of the subroutines.

      SUBROUTINE bigsvdfit(x,y,sig,ndata,a,ma,u,v,w,mp,np,chisq,funcs)
      INTEGER ma,mp,ndata,np,NMAX,MMAX
      REAL chisq,a(ma),sig(ndata),u(mp,np),v(np,np),w(np),x(ndata),
     *y(ndata),TOL
      EXTERNAL funcs
      PARAMETER (NMAX=10000,MMAX=50,TOL=1.e-5)
CU    USES svbksb,svdcmp
      INTEGER i,j
      REAL sum,thresh,tmp,wmax,afunc(MMAX),b(NMAX)
      do 12 i=1,ndata
        call funcs(x(i),afunc,ma)
        tmp=1./sig(i)
        do 11 j=1,ma
          u(i,j)=afunc(j)*tmp
11      continue
        b(i)=y(i)*tmp
12    continue
      call svdcmp(u,ndata,ma,mp,np,w,v)
      wmax=0.
      do 13 j=1,ma
        if(w(j).gt.wmax)wmax=w(j)
13    continue
      thresh=TOL*wmax
      do 14 j=1,ma
        if(w(j).lt.thresh)w(j)=0.
14    continue
      call svbksb(u,w,v,ndata,ma,mp,np,b,a)
      chisq=0.
      do 16 i=1,ndata
        call funcs(x(i),afunc,ma)
        sum=0.
        do 15 j=1,ma
          sum=sum+a(j)*afunc(j)
15      continue
        chisq=chisq+((y(i)-sum)/sig(i))**2
16    continue
      return
      END


      subroutine runCY_0000lli(k,l,i1,Xtwiddle,Gtwiddle,Shat7zz,N0)
      implicit none
C---  Expression for Eq. 5.60b
C---  Calculates C0000lli, requires C0000lll
C---  Small terms of order Xtwiddle(0,k)*C00iiii,Xtwiddle(0,0)*C00iiiii
C---  Denominator Gtwiddle(k,l)
      include 'pvCnames.f'
      include 'pvCv.f'
      include 'Carraydef.f'
      include 'Carrays.f'
      integer ep,N0,k,l,i1,np
      parameter(np=2)
      double precision Xtwiddle(0:np,0:np),Gtwiddle(np,np)
      double complex Shat7zz(np,z4max,-2:0)

      if ((i1 .eq. l) .or. (i1 .eq. 0)) then
      return
      endif

      do ep=-2,0

      Cv(czzzziii(z3(l,l,i1))+N0,ep)=
     . (-2d0*Gtwiddle(k,i1)*Cv(czzzziii(z3(l,l,l))+N0,ep)
     . +Gtwiddle(k,1)*Shat7zz(1,z4(l,l,l,i1),ep)
     . +Gtwiddle(k,2)*Shat7zz(2,z4(l,l,l,i1),ep)
     . +Xtwiddle(k,0)*Cv(czziiii(z4(l,l,l,i1))+N0,ep)
     . -Xtwiddle(0,0)*Cv(czziiiii(z5(k,l,l,l,i1))+N0,ep))
     . /(6d0*Gtwiddle(k,l))

      enddo

      return
      end


module wdt_module
    implicit none
    public :: exists, propagate_dist
    public :: nx,ny
    public :: LEFT, DOWN, RIGHT, UP, NUM_DIRS, KNOWN, UNKNOWN, CANDIDATE, NEW_CANDIDATE
    integer (kind=4) :: nx,ny
    real (kind=8) :: obstacle_value, unknown_value

    integer (kind=4), parameter :: LEFT = 0
    integer (kind=4), parameter :: DOWN = 1
    integer (kind=4), parameter :: RIGHT = 2
    integer (kind=4), parameter :: UP = 3
    integer (kind=4), parameter :: NUM_DIRS = 4

    integer (KIND=4), parameter :: KNOWN = 0
    integer (KIND=4), parameter :: UNKNOWN = 1
    integer (KIND=4), parameter :: CANDIDATE = 2
    integer (KIND=4), parameter :: NEW_CANDIDATE = 3

contains
    subroutine new_candidate_cells(cell_x,cell_y,wdt_field,n_x,n_y,cand_cells)
        implicit none
        ! Find new candidate cells
        integer (kind=4), intent(in) :: cell_x,cell_y,n_x,n_y
        real (kind=8), dimension(0:n_x-1,0:n_y-1), intent(in) :: wdt_field
        integer (kind=4), dimension(0:3,0:1), intent(out) :: cand_cells
        integer (kind=4) :: direction,nb_x,nb_y
        cand_cells = -1
        do direction=0,3
            nb_x = sign(mod(direction+1,2),direction-1) + cell_x
            nb_y = sign(mod(direction,2),direction-2) + cell_y
            if (exists(nb_x,nb_y)==1)  then
                if (wdt_field(nb_x,nb_y) > obstacle_value) then
                    cand_cells(direction,0) = nb_x
                    cand_cells(direction,1) = nb_y
                end if
            end if
        end do
    end subroutine

    subroutine propagate_dist(cell_x,cell_y,wdt_field,costs_x,costs_y,nx,ny,out_pot)
    !   Compute the potential in a single cell with a first order upwind method
    implicit none
    integer (kind=4),intent(in) :: cell_x,cell_y,nx,ny
    real (kind=8) :: a,b,c,D ! parameters for upwind approximation
    integer (kind=4) :: normal_x,normal_y,face_index_x,face_index_y! Administration
    integer (kind=4) :: nb_cell_x,nb_cell_y! neighbour indices
    real (kind=8):: hor_potential,ver_potential,hor_cost,ver_cost

    integer (kind=4) :: direction

    real (kind=8), dimension(0:nx-1,0:ny-1), intent(in) :: wdt_field
    real (kind=8), dimension(0:nx,0:ny-1), intent(in) :: costs_x
    real (kind=8), dimension(0:nx-1,0:ny), intent(in) :: costs_y
    real (kind=8), dimension(0:3) :: neighbour_pots
    real (kind=8) :: pot,cost
    real (kind=8), intent(out) :: out_pot

    neighbour_pots=  (/ obstacle_value, obstacle_value, obstacle_value, obstacle_value /)
    hor_cost = obstacle_value
    ver_cost = obstacle_value
        ! Find the minimal directions along a grid cell.
        ! Assume left and below are best, then overwrite with right and up if they are better
    do direction=0,3
            normal_x = sign(mod(direction+1,2),direction-1)
            normal_y = sign(mod(direction,2),direction-2)
            ! numerical direction
            nb_cell_x = cell_x + normal_x
            nb_cell_y = cell_y + normal_y
            if (exists(nb_cell_x,nb_cell_y) == 0) then
                cycle
            endif
            pot = wdt_field(nb_cell_x,nb_cell_y)
            ! potential in that neighbour field
            if (direction == RIGHT) then
                face_index_x = nb_cell_x
                face_index_y = nb_cell_y
                ! Unit cost values are defined w.r.t faces, not cells! So the indexing is different with right and up.
                cost = costs_x(face_index_x,face_index_y)
                ! Cost to go from there to here
            elseif (direction == UP) then
                face_index_x = nb_cell_x
                face_index_y = nb_cell_y
                ! Unit cost values are defined w.r.t faces, not cells! So the indexing is different with right and up.
                cost = costs_y(face_index_x,face_index_y)
            elseif (direction == LEFT) then
                face_index_x = nb_cell_x+1
                face_index_y = nb_cell_y
                cost = costs_x(face_index_x,face_index_y)
            elseif (direction == DOWN) then
                face_index_x = nb_cell_x
                face_index_y = nb_cell_y+1
                cost = costs_y(face_index_x,face_index_y)
            else
                !write(*,*) "Exception in compute_potential"
            endif

            neighbour_pots(direction) = pot + cost
            ! total potential
            if (neighbour_pots(direction) < neighbour_pots(mod(direction+2,4))) then
                if (mod(direction,2) == 0) then
                    hor_potential = pot
                    hor_cost = cost
                    !write(*,*) "Horizontal changed"
                    ! lowest in horizontal direction
                else
                    ver_potential = pot
                    ver_cost = cost
                    !write(*,*) "Vertical changed"
                    ! lowest in vertical direction
                endif
            endif
    end do

    if (hor_potential >= obstacle_value) then
        a = 1. / (ver_cost * ver_cost)
        b = -2. * ver_potential / (ver_cost * ver_cost)
        c = (ver_potential / ver_cost) * (ver_potential / ver_cost) -1
    elseif (ver_potential >=obstacle_value) then
        a = 1. / (hor_cost * hor_cost)
        b = -2. * hor_potential / (hor_cost * hor_cost)
        c = (hor_potential / hor_cost) * (hor_potential / hor_cost) -1
    else
        a = 1. / (hor_cost * hor_cost) + 1. / (ver_cost * ver_cost)
        b = -2. * (hor_potential / (hor_cost * hor_cost) + ver_potential / (ver_cost * ver_cost))
        c = (hor_potential / hor_cost) * (hor_potential / hor_cost) + (ver_potential / ver_cost) * (ver_potential / ver_cost) - 1
    endif
    D = b*b-4.*a*c
    out_pot = (-b + sqrt(D)) / (2.*a)
    end subroutine

    function exists(cell_x,cell_y) result(ex)
        implicit none
        ! Find out whether the cell exists given the dimensions of the scene
        integer (kind=4), intent(in) :: cell_x,cell_y
        integer (kind=4) :: ex
        if (cell_x < 0 .or. cell_y < 0 .or. cell_x>=nx .or. cell_y>=ny) then
            ex = 0
        else
            ex = 1
        end if
    end function exists
end module wdt_module

subroutine weighted_distance_transform(cost_field,wdt_field,n_x,n_y,obs_val)
    ! Todo: Somehow make sure I don't have to input the n_x/n_y in Python
use wdt_module
implicit none
!   Compute the potential in a single cell with a first order upwind method
!f2py intent(in) cost_field,obs_val,n_x,n_y
!f2py depend(n_x,n_y) cost_field,wdt_field
!f2py intent(out) wdt_field
integer (kind=4) :: n_x,n_y
real (kind=8):: obs_val

integer (kind=4), dimension(0:3,0:1) :: new_cand_cells
integer (kind=4), dimension(0:1) :: best_cell, new_cell

integer (kind=4), allocatable, dimension(:,:) :: cand_heap
integer (kind=4), allocatable, dimension(:) :: indx
integer (kind=4) :: heap_capacity, heap_length, tree_length

integer (kind=4) :: i,j,l
real (kind=8), dimension(0:n_x-1,0:n_y-1) :: cost_field,wdt_field
real (kind=8), dimension(0:n_x,0:n_y-1) :: costs_x
real (kind=8), dimension(0:n_x-1,0:n_y) :: costs_y
integer (kind=4), dimension(0:n_x-1,0:n_y-1) :: cell_indicators
real (kind=8) :: dist

nx = n_x
ny = n_y
obstacle_value = obs_val
unknown_value = obs_val + 1
! Cost for moving along horizontal lines
costs_x = obstacle_value
costs_x(1:n_x-1,:) = (cost_field(1:n_x-1,:) + cost_field(0:n_x-2,:))/2
! Cost for moving along vertical lines
costs_y = obstacle_value
costs_y(:,1:n_y-1) = (cost_field(:,1:n_y-1) + cost_field(:,0:n_y-2))/2

!Initialize locations (known(exit/obstacles)/unknown)
wdt_field = unknown_value
cell_indicators = UNKNOWN

heap_length = 0
tree_length = 0
heap_capacity = (n_x+n_y)*1000 ! Todo: This number is pretty arbitrary. I think it should be hc = a*(nx+ny) + b*(#exits)
allocate(cand_heap(0:1,0:heap_capacity-1))
allocate(indx(0:heap_capacity-1))
call heap_init(indx,heap_capacity)

do j=0,n_y-1
    do i=0,n_x-1
        if (cost_field(i,j)==0) then
            ! No cost, so this is an exit
            wdt_field(i,j) = 0
            cell_indicators(i,j) = KNOWN
            new_cell(0) = i
            new_cell(1) = j
            call heap_insert(cand_heap,indx,heap_capacity,heap_length,tree_length,new_cell,wdt_field,n_x,n_y)

        elseif (cost_field(i,j)>=obstacle_value) then
            ! 'infinite cost', so this is an obstacle
            wdt_field(i,j) = obstacle_value
            cell_indicators(i,j) = KNOWN
        endif
    enddo
enddo
! Iteration of level set
do while (.true.)
    if (heap_length==0) then
        exit
    end if
    call heap_pop(cand_heap, indx, heap_capacity, heap_length, wdt_field, n_x, n_y, best_cell)
    call new_candidate_cells(best_cell(0),best_cell(1),wdt_field,n_x,n_y,new_cand_cells)
    do l=0,3
        i = new_cand_cells(l,0)
        if (i >=0) then
            j = new_cand_cells(l,1)
            call propagate_dist(i,j,wdt_field,costs_x,costs_y,n_x,n_y,dist)
            if (wdt_field(i,j) >= dist) then
                wdt_field(i,j) = dist
                new_cell(0) = i
                new_cell(1) = j
                call heap_insert(cand_heap,indx,heap_capacity,heap_length,tree_length,new_cell,wdt_field,n_x,n_y)
            end if
        end if
    end do
end do
deallocate(cand_heap)
deallocate(indx)

end subroutine

program test_wdt
    implicit none
    integer (kind=4), parameter :: n_x=400
    integer (kind=4), parameter :: n_y=500
    integer (kind=4) :: k
    real (kind=8), dimension(0:n_x-1,0:n_y-1) :: cost_field,wdt_field
    real (kind=8), parameter :: obstacle_value = 2000

    ! Example:
    !- - - -
    !* * * -
    !- * - -
    !- - - -

!    cost_field = 0.01
!    cost_field(1,1) = 0
!    cost_field(0:3,2) = 0
!    cost_field(3,1:3)= obstacle_value
!    cost_field(50,:) = obstacle_value
!    cost_field(50,250) = 0.01
    call weighted_distance_transform(cost_field,wdt_field,n_x,n_y,obstacle_value)
    k = 3
    open(k, file="output.dat", access="stream")
    write(k) wdt_field
    close(k)
end program


        SUBROUTINE ZCLPPF(VALUE,Q0,Q1,F,TAU1)
*
*  Module number:
*
*  Module name: ZCLPPF
*
*  Keyphrase:
*  ----------
*       Perform HRS paired pulse correction
*  Description:
*  ------------
*       This routine performs the paired pulse correction
*       of GHRS data.
*       if q0 is not equal to zero then the following equation
*       is used:
*                      y
*               x = --------
*                   (1 - yt)
*
*       where:
*               x is the true count rate
*               y is the observed count rate
*               t = q0 for y less than or equal to F
*               t = q0 + q1*(y-F) for y greater than F
*               q0, q1, and F are coefficients in CCG2
*
*       IF q0 is equal to zero then the following equation is used:
*
*               x = log(1-ty)/(-t)
*
*       where:
*               t = tau1 ( a coefficient in CCG2)
*               the log is a natural logarithm
*
*
*  FORTRAN name: ZCLPPF.FOR
*
*  Subroutines Called:
*  -------------------
*       NONE
*  History:
*  --------
*  Version      Date        Author          Description
*       1       May 88  D. Lindler      Designed and coded
*-------------------------------------------------------------------------------
*
* Input parameters
*       Q0,Q1,F,TAU1 - all real*4 (paired pulse coefficients from
*                       relation CCG2.
*
* Input/Output
*       VALUE - data value (on output it is paired pulse corrected)
*
* Output
*
*       ISTAT - error status
*
*------------------------------------------------------------------------------
        REAL VALUE,Q0,Q1,F,TAU1
        REAL X,T
C
C CASE 1  Q0 is not zero
C
        IF (Q0 .NE. 0.0) THEN
                T=Q0
                IF(VALUE.GT.F)T=T+(VALUE-F)*Q1
                X=(1.0-VALUE*T)
                IF(X .LT. 1.0E-12) X=1.0E-12
                VALUE=VALUE/X
            ELSE
C
C CASE 2  Q0 is zero
C
                X=(1.0-VALUE*TAU1)
                IF(X .LT. 1.0E-12) X=1.0E-12
                VALUE=LOG(X)/(-TAU1)
        ENDIF
        RETURN
        END


      subroutine gdmax(istart,iend,sumgd,ityp)
c
c     + + + PURPOSE + + +
c     If the user does not input the number of growing degree days
c     in the growing season (gddmax), this subroutine computes the
c     growing degree days between two julian dates (istart and iend).
c     Monthly ave. max and min temperatures (obmint & obmaxt), and
c     threshold temperatures for crop growth (btemp) are read from
c     common blocks.  All years are handled as if they are leap
c     years.
c
c     Called from YOPTP and YLDOPT
c     Author(s): Arnold, Meyer
c     Reference in User Guide:
c
c     Changes:
c            1) Completely re-coded this routine.  Original one
c               set up a do-loop which was stepped through for
c               EACH DAY from ISTART to IEND.  Each time through
c               the loop, XMONTH was called.  I recoded it such
c               we dealt with the degree days in the initial
c               partial month, then those from the complete
c               months, and finally those from the final incomplete
c               month.  The call to XMONTH was eliminated.
c               CRM -- 7/07/92.
c
c
c     Version: This module recoded from Arnold's WEPP Post-version 92.25.
c     Date recoded: 07/07/92.
c     Recoded by: Charles R. Meyer.
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
      include 'pmxcrp.inc'
      include 'pmxpln.inc'
      include 'pmxres.inc'
      include 'pmxtil.inc'
      include 'pmxtls.inc'
      include 'pntype.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
      integer istart, iend, ityp
      real sumgd
c
c     + + + ARGUMENT DEFINITIONS + + +
c     istart - Julian date for start of growing season.
c     iend   - Julian date for end of growing season.
c     ityp   - index of crop type.
c     sumgd  - sum of the growing degree days in the season.
c
c     + + + COMMON BLOCKS + + +
      include 'cobclim.inc'
c       read: obmint(12), obmaxt(12)
      include 'ccrpprm.inc'
c       read: btemp(10)
c
c     + + + LOCAL VARIABLES + + +
      integer juldat(13), iendaz, ienmon, istdaz, istmon, month,
     1    monlen(12)
      real gdd, tave
c     + + + LOCAL DEFINITIONS + + +
c     juldat - cumulative days to beginning of each month (through the
c              "13th" month; ie, the end of the 12th month).
c     monlen - length of each month (days).
c
c     + + + FUNCTION DECLARATIONS + + +
c
c     + + + DATA INITIALIZATIONS + + +
      data juldat /0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305,
     1    335, 366/
      data monlen /31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31/
c
c     + + + END SPECIFICATIONS + + +
c
c
c
c     write (6,*) " ISTART:", istart, " IEND:", iend, "-->", iend-istart
c
c ---- Find starting month.
      istmon = 13
c     *** BEGIN L0 LOOP ***
   10 continue
      istmon = istmon - 1
      if (istart.le.juldat(istmon)) go to 10
c     *** END L0 LOOP ***
c     ---- Determine number of days covered in starting month; ie, the number
c     from (ISTART - 1) to the end of the month.
      istdaz = juldat(istmon+1) - istart + 1
c
c     ---- Find ending month.
      ienmon = 13
c     *** BEGIN M0 LOOP ***
   20 continue
      ienmon = ienmon - 1
c     *** END M0 LOOP ***
      if (iend.le.juldat(ienmon)) go to 20
c
c     ---- Determine number of days covered in ending month; ie, the number
c     from the end of the preceeding month to IEND.
      iendaz = iend - juldat(ienmon)
c
      sumgd = 0.0
c
c     --- Compute growing degree days (gdd) contributed by starting month.
      tave = (obmaxt(istmon)+obmint(istmon)) / 2.0
      if (tave.gt.btemp(ityp)) then
        gdd = tave - btemp(ityp)
        sumgd = sumgd + gdd * istdaz
      end if
c
c     --- Compute growing degree days (gdd) contributed by months between
c     starting and ending months.
c
      do 30 month = istmon + 1, ienmon - 1
        tave = (obmaxt(month)+obmint(month)) / 2.0
        if (tave.gt.btemp(ityp)) then
          gdd = tave - btemp(ityp)
          sumgd = sumgd + gdd * monlen(month)
        end if
   30 continue
c
c     --- Compute growing degree days (gdd) contributed by ending month.
      tave = (obmaxt(ienmon)+obmint(ienmon)) / 2.0
      if (tave.gt.btemp(ityp)) then
        gdd = tave - btemp(ityp)
        sumgd = sumgd + gdd * iendaz
      end if
c
      return
      end


      SUBROUTINE COSQB1 (N,X,W,XH)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION       X(1)       ,W(1)       ,XH(1)
      NS2 = (N+1)/2
      NP2 = N+2
      DO 101 I=3,N,2
         XIM1 = X(I-1)+X(I)
         X(I) = X(I)-X(I-1)
         X(I-1) = XIM1
  101 CONTINUE
      X(1) = X(1)+X(1)
      MODN = MOD(N,2)
      IF (MODN .EQ. 0) X(N) = X(N)+X(N)
      CALL DFFTB (N,X,XH)
      DO 102 K=2,NS2
         KC = NP2-K
         XH(K) = W(K-1)*X(KC)+W(KC-1)*X(K)
         XH(KC) = W(K-1)*X(K)-W(KC-1)*X(KC)
  102 CONTINUE
      IF (MODN .EQ. 0) X(NS2+1) = W(NS2)*(X(NS2+1)+X(NS2+1))
      DO 103 K=2,NS2
         KC = NP2-K
         X(K) = XH(K)+XH(KC)
         X(KC) = XH(K)-XH(KC)
  103 CONTINUE
      X(1) = X(1)+X(1)
      RETURN
      END