!*******************************************************************************
!>
!  Kepler propagation routines.

    module kepler_module

    use numbers_module
    use kind_module,     only: wp
    use iso_fortran_env, only: error_unit

    implicit none

    private

    public :: kepler_shepperd
    public :: kepler_goodyear_stienon_klumpp
    public :: kepler_classical

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

!*******************************************************************************
!>
!  Classical Kepler propagator for elliptical and hyperbolic orbits.
!  Uses Lagrange formulations from Battin & Newton's method.
!
!### See also
!  * Dario Izzo: pykep/src/core_functions/propagate_lagrangian.h

    subroutine kepler_classical(x0, dt, mu, xf)

    use newton_module, only: newton

    implicit none

    real(wp),dimension(6),intent(in)  :: x0  !! initial position,velocity vector
    real(wp),intent(in)               :: dt  !! propagation time
    real(wp),intent(in)               :: mu  !! central body gravitational parameter
    real(wp),dimension(6),intent(out) :: xf  !! final position,velocity vector

    real(wp),dimension(3) :: r0  !! initial position vector
    real(wp),dimension(3) :: v0  !! initial velocity vector
    real(wp) :: de     !! eccentric anomaly difference
    real(wp) :: dh     !! hyperbolic anomaly difference
    integer  :: iflag  !! newton status flag
    real(wp) :: r,rmag,vmag,energy,a,sqrta,f,&
                g,ft,gt,sigma0,dm,dn,xs,fx,b,p,x,z,term

    real(wp),parameter :: parabolic_tol = 1.0e-12_wp !! zero tol for parabolic orbits (energy)
    real(wp),parameter :: ftol = 1.0e-12_wp          !! function tol for root finding
    real(wp),parameter :: xtol = 1.0e-12_wp          !! indep variable tol for root finding
    integer,parameter  :: max_iter = 1000            !! maximum number of iterations in newton

    ! check trivial case:
    if (dt==zero) then
        xf = x0
        return
    end if

    r0     = x0(1:3)
    v0     = x0(4:6)
    rmag   = norm2(r0)
    vmag   = norm2(v0)
    energy = (vmag * vmag / two - mu / rmag)
    sigma0 = dot_product(r0,v0) / sqrt(mu)

    ! if not parabolic, then compute semimajor axis
    if (abs(energy) > parabolic_tol) then
        a = -mu / two / energy
    end if

    if (energy < -parabolic_tol) then ! elliptical case

        sqrta = sqrt(a)
        dm = sqrt(mu / a**3) * dt
        de = dm

        call newton(de,kepde_,d_kepde_,ftol,xtol,max_iter,xs,fx,iflag)
        if (iflag<0) then
            write(error_unit,'(A)') 'Error in kepler_classical [elliptical]: newton did not converge'
            write(*,*) xs,fx,iflag
        end if

        de = xs
        r = a + (rmag - a) * cos(de) + sigma0 * sqrta * sin(de) ! eqn 4.42

        ! lagrange coefficients (Battin eqn 4.41)
        f  = one - a / rmag * (one - cos(de))
        g  = a * sigma0 / sqrt(mu) * (one - cos(de)) + rmag * sqrt(a / mu) * sin(de)
        ft = -sqrt(mu * a) / (r * rmag) * sin(de)
        gt = one - a / r * (one - cos(de))

    else if (energy > parabolic_tol) then ! hyperbolic case

        sqrta = sqrt(-a)
        dn = sqrt(-mu / a**3) * dt
        dh = sign(one,dt)      ! todo: need a better initial guess

        call newton(dh,kepdh_,d_kepdh_,ftol,xtol,max_iter,xs,fx,iflag)
        if (iflag<0) then
            write(error_unit,'(A)') 'Error in kepler_classical [hyperbola]: newton did not converge'
            write(*,*) xs,fx,iflag
        end if

        dh = xs
        r = a + (rmag - a) * cosh(dh) + sigma0 * sqrta * sinh(dh)

        ! lagrange coefficients (Battin eqn 4.62)
        f  = one - a / rmag * (one - cosh(dh))
        g  = a * sigma0 / sqrt(mu) * (one - cosh(dh)) + rmag * sqrt(-a / mu) * sinh(dh)
        ft = -sqrt(-mu * a) / (r * rmag) * sinh(dh)
        gt = one - a / r * (one - cosh(dh))

    else  ! parbolic case

        ! See Battin Section 4.2

        p    = two * rmag - sigma0**2
        B    = one / p**1.5_wp * ( sigma0 * (rmag + p) + three * sqrt(mu) * dt )
        term = B + sqrt(one+B*B)
        z    = (term)**(one/three) - (term)**(-one/three) ! Battin eqn 4.12 where z = tan(f/2)
        x    = sqrt(p) * z - sigma0
        r    = rmag + sigma0 * x + one/two * x**2

        f  = one - x**2 / two / rmag
        g  = x / two / sqrt(mu) * ( two * rmag + sigma0 * x )
        ft = - sqrt(mu) * x / rmag / r
        gt = one - x**2 / two / r

    end if

    ! results:
    xf(1:3) = f  * r0 + g  * v0
    xf(4:6) = ft * r0 + gt * v0

    contains

        ! function wrappers for newtons method:

        subroutine kepde_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! de
        real(wp),intent(out) :: f
        f = kepde(x, dm, sigma0, sqrta, a, rmag)
        end subroutine kepde_

        subroutine d_kepde_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! de
        real(wp),intent(out) :: f
        f = d_kepde(x, sigma0, sqrta, a, rmag)
        end subroutine d_kepde_

        subroutine kepdh_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! dh
        real(wp),intent(out) :: f
        f = kepdh(x, dn, sigma0, sqrta, a, rmag)
        end subroutine kepdh_

        subroutine d_kepdh_(x,f)
        implicit none
        real(wp),intent(in)  :: x  !! dh
        real(wp),intent(out) :: f
        f = d_kepdh(x, sigma0, sqrta, a, rmag)
        end subroutine d_kepdh_

    end subroutine kepler_classical
!*******************************************************************************

!*******************************************************************************
!>
!  Elliptic Kepler's equation

    pure function kepe(e, m, ecc)

    implicit none

    real(wp),intent(in) :: e   !! eccentric anomaly
    real(wp),intent(in) :: m   !! mean anomaly
    real(wp),intent(in) :: ecc !! eccentricity
    real(wp) :: kepe

    kepe = e - ecc * sin(e) - m

    end function kepe
!*******************************************************************************

!*******************************************************************************
!>
!  Derivative of [[kepe]] w.r.t. `e`

    pure function d_kepe(e, ecc)

    implicit none

    real(wp),intent(in) :: e   !! eccentric anomaly
    real(wp),intent(in) :: ecc !! eccentricity
    real(wp) :: d_kepe

    d_kepe = one - ecc * cos(e)

    end function d_kepe
!*******************************************************************************

!*******************************************************************************
!>
!  Elliptic Kepler's equation written in terms of the
!  eccentric anomaly difference.  See Battin, eqn 4.43.

    pure function kepde(de, dm, sigma0, sqrta, a, r)

    implicit none

    real(wp),intent(in) :: de      !! eccentric anomaly difference
    real(wp),intent(in) :: dm      !! mean anomaly difference
    real(wp),intent(in) :: sigma0
    real(wp),intent(in) :: sqrta
    real(wp),intent(in) :: a
    real(wp),intent(in) :: r
    real(wp) :: kepde

    kepde = -dm + de + sigma0 / sqrta * (one - cos(de)) - &
            (one - r / a) * sin(de)

    end function kepde
!*******************************************************************************

!*******************************************************************************
!>
!  Derivative of [[kepde]] w.r.t `de`.

    pure function d_kepde(de, sigma0, sqrta, a, r)

    implicit none

    real(wp),intent(in) :: de      !! eccentric anomaly difference
    real(wp),intent(in) :: sigma0
    real(wp),intent(in) :: sqrta
    real(wp),intent(in) :: a
    real(wp),intent(in) :: r
    real(wp) :: d_kepde

    d_kepde = one + sigma0 / sqrta * sin(de) - (one - r / a) * cos(de)

    end function d_kepde
!*******************************************************************************

!*******************************************************************************
!>
!  Battin, eqn. 4.64.

    pure function kepdh(dh, dn, sigma0, sqrta, a, r)

    implicit none

    real(wp),intent(in) :: dh      !! hyperbolic anomaly difference
    real(wp),intent(in) :: dn
    real(wp),intent(in) :: sigma0
    real(wp),intent(in) :: sqrta
    real(wp),intent(in) :: a
    real(wp),intent(in) :: r
    real(wp) :: kepdh

    kepdh = -dn - dh + sigma0 / sqrta * (cosh(dh) - one) + &
            (one - r / a) * sinh(dh)

    end function kepdh
!*******************************************************************************

!*******************************************************************************
!>
!  Derivative of [[kepdh]] w.r.t `dh`.

    pure function d_kepdh(dh, sigma0, sqrta, a, r)

    implicit none

    real(wp),intent(in) :: dh      !! hyperbolic anomaly difference
    real(wp),intent(in) :: sigma0
    real(wp),intent(in) :: sqrta
    real(wp),intent(in) :: a
    real(wp),intent(in) :: r
    real(wp) :: d_kepdh

    d_kepdh = -one + sigma0 / sqrta * sinh(dh) + (one - r / a) * cosh(dh)

    end function d_kepdh
!*******************************************************************************

!*******************************************************************************
!>
!  Barker time of flight equation

    pure function barker(r1, r2, mu)

    implicit none

    real(wp),dimension(3),intent(in) :: r1
    real(wp),dimension(3),intent(in) :: r2
    real(wp),intent(in) :: mu
    real(wp) :: barker

    real(wp) :: x, r1mag, r2mag, r21mag, sigma, r1pr2mag
    real(wp),dimension(3) :: r21

    r1mag    = norm2(r1)
    r2mag    = norm2(r2)
    r21      = r2 - r1
    r21mag   = norm2(r21)
    x        = r1(1) * r2(2) - r1(2) * r2(1)
    sigma    = sign(one,x)
    r1pr2mag = r1mag + r2mag

    barker = (r1pr2mag + r21mag)**1.5_wp - &
             sigma * (r1pr2mag - r21mag)**1.5_wp / (six * sqrt(mu))

    end function barker
!*******************************************************************************

!*******************************************************************************
!>
!
    pure function kepds(ds, dt, r0, vr0, alpha, mu)

    implicit none

    real(wp),intent(in) :: ds  !! universal anomaly difference
    real(wp),intent(in) :: dt
    real(wp),intent(in) :: r0
    real(wp),intent(in) :: vr0
    real(wp),intent(in) :: alpha
    real(wp),intent(in) :: mu
    real(wp) :: kepds

    real(wp) :: c,s,ads2,ds2

    ds2  = ds * ds
    ads2 = alpha * ds2
    s    = stumpff_s(ads2)
    c    = stumpff_c(ads2)

    kepds = -sqrt(mu) * dt + r0 * vr0 * ds2 * c / sqrt(mu) + &
            (one - alpha * r0) * ds2 * ds * s + r0 * ds

    end function kepds
!*******************************************************************************

!*******************************************************************************
!>
!
    pure function d_kepds(ds, r0, vr0, alpha, mu)

    implicit none

    real(wp),intent(in) :: ds
    real(wp),intent(in) :: r0
    real(wp),intent(in) :: vr0
    real(wp),intent(in) :: alpha
    real(wp),intent(in) :: mu
    real(wp) :: d_kepds

    real(wp) :: c,s,ads2,ds2

    ds2  = ds * ds
    ads2 = alpha * ds2
    s    = stumpff_s(ads2)
    c    = stumpff_c(ads2)

    d_kepds = r0 * vr0 / sqrt(mu) * ds * (one - ads2 * s) + &
              (one - alpha * r0) * ds2 * c + r0

    end function d_kepds
!*******************************************************************************

!*******************************************************************************
!>
!  Stumpff function S(z)

    pure function stumpff_s(z) result(s)

    implicit none

    real(wp),intent(in) :: z
    real(wp) :: s

    if (z > zero) then
        s = (sqrt(z) - sin(sqrt(z))) / sqrt(z)**3
    else if (z < zero) then
        s = (sinh(sqrt(-z)) - sqrt(-z)) / (-z)**(three/two)
    else
        s = one/six
    end if

    end function stumpff_s
!*******************************************************************************

!*******************************************************************************
!>
!  Stumpff function C(z)

    pure function stumpff_c(z) result(c)

    implicit none

    real(wp),intent(in) :: z
    real(wp) :: c

    if (z > zero) then
        c = (one - cos(sqrt(z))) / z
    else if (z < zero) then
        c = (cosh(sqrt(-z)) - one) / (-z)
    else
        c = 0.5_wp
    end if

    end function stumpff_c
!*******************************************************************************

!*******************************************************************************
!>
!  Kepler propagation using Shepperd's method.
!
!### Reference
!  * S.W. Shepperd, "Universal Keplerian State Transition Matrix".
!    Celestial Mechanics 35(1985) p. 129-144.
!  * Rody P.S. Oldenhuis, `progress_orbitM` Matlab function (BSD license).
!    http://www.mathworks.com/matlabcentral/fileexchange/26349-kepler-state-transition-matrix-mex
!  * C.D. Eagle, Orbital Mechanics with MATLAB, `twobody2.m` Matlab function (BSD license).
!    http://www.mathworks.com/matlabcentral/fileexchange/48723-matlab-functions-for-two-body-orbit-propagation

    subroutine kepler_shepperd(mu,rv1,dt,rv2,istat)

    implicit none

    real(wp),intent(in)               :: mu    !! gravitational parameter
    real(wp),dimension(6),intent(in)  :: rv1   !! initial position,velocity vector
    real(wp),intent(in)               :: dt    !! time step
    real(wp),dimension(6),intent(out) :: rv2   !! final position,velocity vector
    integer,intent(out),optional      :: istat !! status flag (if not present, warnings are printed):
                                               !! Linear combination of :
                                               !!
                                               !! `0` : all is well
                                               !! `-10` : failed to converge in time loop
                                               !! `-100` : failed to converge in `g` loop

    ! note: some of these could also be (optional) input:
    real(wp),parameter :: min_dt     = epsilon(one) !! time step considered as zero (sec)
    integer,parameter  :: max_iter   = 1000         !! max iterations for time
    integer,parameter  :: max_iter_g = 1000         !! max iterations for g
    real(wp),parameter :: ttol       = 1.0e-8_wp    !! tolerance for time (sec)
    real(wp),parameter :: gtol       = 1.0e-12_wp   !! tolerance for g
    real(wp),parameter :: zero_tol   = epsilon(one) !! tolerance for beta=0 (parabola)
    logical,parameter  :: use_halley = .true.       !! use the Halley update
                                                    !! rather than the original Newton

    real(wp),dimension(3) :: r1  !! initial position vector
    real(wp),dimension(3) :: v1  !! initial velocity vector
    real(wp) :: r1mag,nu0,beta,p,deltau,u,t,deltat,bu,q,&
                a,b,gprev,u0w2,u1w2,uu,u0,u1,u2,u3,r,&
                ff,f,gg,g,umin,umax,du,abs_beta
    integer :: n,iter,k,d,l,iterg

    if (present(istat)) istat = 0

    if (abs(dt) <= min_dt) then

        rv2 = rv1

    else

        r1 = rv1(1:3)
        v1 = rv1(4:6)
        r1mag = norm2(r1)
        nu0 = dot_product(r1,v1)
        beta = two*mu/r1mag - dot_product(v1,v1)
        abs_beta = abs(beta)

        if (abs_beta>zero_tol) then
            umax = one/sqrt(abs_beta)
        else
            umax = huge(one)
        end if
        umin = -umax

        if (beta > zero) then
            p = twopi*mu*beta**(-three/two)
            n = floor((dt + p/two - two*nu0/beta)/p)
            deltau = twopi*n*beta**(-five/two)
        else
            deltau = zero
        end if

        u = zero
        t = zero
        iter = 0
        deltat = dt-t
        do while (abs(deltat) > ttol)
            iter = iter + 1
            bu = beta*u*u
            q = min(0.5_wp,bu/(one+bu))  ! avoid q > 0.5 due to numerical issues
            a = one
            b = one
            g = one
            n = 0
            k = -9
            d = 15
            l = 3
            gprev = huge(one)
            iterg = 0
            do while (abs(g-gprev) > gtol)
                iterg = iterg + 1
                k = -k
                l = l + 2
                d = d + 4*l
                n = n + (1+k)*l
                a = d/(d - n*a*q)
                b = (a-one)*b
                gprev = g
                g = g + b
                if (iterg==max_iter_g) then
                    if (present(istat)) then
                        istat = istat - 100
                    else
                        write(*,*) 'Warning: kepler_shepperd failed to converge in g iteration'
                    end if
                    exit
                end if
            end do

            u0w2 = one - two*q
            u1w2 = two*(one-q)*u
            uu = 16.0_wp/15.0_wp*u1w2**5*g + deltau
            u0 = two*u0w2**2-one
            u1 = two*u0w2*u1w2
            u2 = two*u1w2**2
            u3 = beta*uu + u1*u2/three
            r = r1mag*u0 + nu0*u1 + mu*u2
            t = r1mag*u1 + nu0*u2 + mu*u3
            deltat = dt - t

            if (use_halley) then
                ! Halley version from Oldenhuis routine
                du = deltat/((one-q)*(four*r + deltat*beta*u))
            else
                ! original Newton
                du = deltat/four/(one-q)/r
            end if

            ! this logic is from the Eagle routine
            if (du<zero) then
                umax = u
                u = u + du
                if (u<umin) u = (umin + umax)/two
            else
                umin = u
                u = u + du
                if (u>umax) u = (umin + umax)/two
            end if

            if (iter==max_iter) then
                if (abs(deltat) > ttol) then
                    ! it still hasn't converged
                    if (present(istat)) then
                        istat = istat - 10
                    else
                        write(*,*) 'Warning: kepler_shepperd failed to converge in time iteration'
                    end if
                    exit
                end if
            end if

        end do

        ff = one - mu/r1mag*u2
        f  = -mu*u1/r/r1mag
        gg = r1mag*u1 + nu0*u2
        g  = one - mu/r*u2

        rv2 = [r1*ff + v1*gg, r1*f + v1*g]

    end if

    end subroutine kepler_shepperd
!*******************************************************************************

!*******************************************************************************
!>
!  Kepler propagator based on the Goodyear code with
!  modifications by Stienon and Klumpp.
!
!### See also
!  * W. H. Goodyear, "Completely General Closed-Form Solution for Coordinates
!    and Partial Derivatives of the Two-Body Problem", Astronomical Journal,
!    Vol. 70, No. 3, April 1965.
!    [pdf](http://adsabs.harvard.edu/full/1965AJ.....70..189G)
!  * W. H. Goodyear, "A General Method for the Computation of Cartesian
!    Coordinates and Partial Derivatives of the Two-Body Problem",
!    NASA CR-522, Sep 1, 1966.
!    [pdf](https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19660027556.pdf)
!  * A. Klumpp, "Performance Comparision of Lambert and Kepler Algorithms",
!    JPL Interoffice Memorandum, 314.1-0426-ARK, Jan 2, 1991.
!   (See the `KEPGSK` Fortran 77 routine)

    subroutine kepler_goodyear_stienon_klumpp(rv0,tau,mu,accy,rvf)

    implicit none

    real(wp),dimension(6),intent(in)  :: rv0   !! state vector at reference
                                               !! time T0 [km,km/s]
    real(wp),intent(in)               :: tau   !! Time interval T-T0 [sec]
    real(wp),intent(in)               :: mu    !! Central body gravitational
                                               !! constant [km^3/s^2]
    real(wp),intent(in)               :: accy  !! Fractional accuracy required
                                               !! [0->0.1]
    real(wp),dimension(6),intent(out) :: rvf   !! state vector at solution time
                                               !! T [km,km/s]

    real(wp),parameter :: onethird  = 1.0_wp / 3.0_wp !! 0.333333 in original code

    integer  :: iloop             !! loop counter defining number of iterations
    real(wp) :: r                 !! radius at solution time `T`
    real(wp) :: r0                !! radius at solution time `T0`
    integer  :: i                 !! index
    integer  :: j                 !! index
    integer  :: m                 !! counter for normalizing lambda
    real(wp) :: psi               !! independent variable in Kepler's equation
    real(wp) :: psin              !! lower limit to `psi`
    real(wp) :: psip              !! upper limit to `psi`
    real(wp) :: dlim              !! tau/periapsis distance = abs(limit to `psi`)
    real(wp) :: dtau              !! Kepler's equation residual
    real(wp) :: dtaun             !! lower limit to `dtau`
    real(wp) :: dtaup             !! upper limit to `dtau`
    real(wp) :: accrcy            !! accuracy = midval(zero, `accy`, 0.1)
    real(wp) :: sig0              !! dot product: `r` with `v`
    real(wp) :: alpha             !! twice the energy per unit mass
    real(wp) :: h2                !! square of angular momentum per unit mass
    real(wp) :: a                 !! reduced argument of `s_0`, `s_1`, etc.
    real(wp) :: ap                !! actual argument of `s_0`, `s_1`, etc.
    real(wp) :: c0                !! c(n) is the nth Goodyear polynomial `s_n`
                                  !! divided by its leading term, so that
                                  !!
                                  !! * `c0 = s_0 = cosh(z)`
                                  !! * `c1 = s_1/psi = sinh(z)/z`
                                  !! * `c(n+2) = (n+1)(n+2)[c(n)-1]/z^2`
                                  !!
                                  !! where `z = psi*sqrt(alpha)`. `ap=norm2(z)`
    real(wp) :: c1
    real(wp) :: c2
    real(wp) :: c3
    real(wp) :: c4
    real(wp) :: c5x3
    real(wp) :: s1                !! `s_n` is the nth Goodyear
                                  !! polynomial, given by
                                  !! `s_n = psi^n sim{z^(2k)/(2k)!} k >=0 `
    real(wp) :: s2
    real(wp) :: s3
    real(wp) :: g                 !! the G-function
    real(wp) :: gdm1              !! time derivative of the G-function minus one
    real(wp) :: fm1               !! the F-function minus one
    real(wp) :: fd                !! time derivative of the F-function
    real(wp),dimension(4) :: c    !! terms in F(psi)

    accrcy = min(max(zero, accy), 0.1_wp)

    r0    = dot_product(rv0(1:3), rv0(1:3)) ! r dot r
    sig0  = dot_product(rv0(1:3), rv0(4:6)) ! r dot v
    alpha = dot_product(rv0(4:6), rv0(4:6)) ! v dot v
    h2    = max(r0*alpha - sig0*sig0, zero)
    r0    = sqrt(r0)
    alpha = alpha - two*mu/r0

    !compute initial limits for phi and dtau:
    c0 = sqrt(max(mu*mu + h2*alpha, zero))    ! gm * eccentricity
    dlim = div(1.1_wp * tau * (c0 + mu), h2)  ! tau/periapsis distance = max(a).
                                              ! Arbitrary factor 1.1 ensures
                                              ! psin <= psi <= psip
                                              ! when converged, despite
                                              ! numerical imprecision.

    if (tau < zero) then
        psin  = dlim
        psip  = zero
        dtaun = psin
        dtaup = -tau
    else
        psin  = zero
        psip  = dlim
        dtaun = -tau
        dtaup = psip
    end if

    !compute initial value of psi:
    psi = tau/r0

    if (alpha >= zero) then
        c2 = one
        if (tau < zero) c2 = -one
        c3 = abs(tau)
        if (alpha > zero) then
            c0 = h2*sqrt(h2)/(mu + c0)**2
            if (c3 > c0) then
                c1 = sqrt(alpha)
                psi = log(two*alpha*c1*c3/(r0*alpha + c2*c1*sig0 + mu))
                psi = max(psi,one)*c2/c1
            end if
        else    !parabola
            c0 = r0*sqrt(six*r0/mu)
            if (c3 > c0) psi = c2*(six*c3/mu)**onethird
        end if
    end if

    ! begin loop for solving Kepler's equation:

    m = 0
    iloop = 0

    do
        iloop = iloop + 1

        ! begin series summation:

        !compute argument a in reduced series obtained by factoring out psi's:
        a = alpha*psi*psi
        if (abs(a)>one) then
            !save a in ap and mod a if a exceeds unity in magnitude
            ap = a
            do
                m = m + 1
                a = a*0.25_wp
                if (abs(a)<=one) exit
            end do
        end if

        !sum series c5x3=3*s5/psi**5 and c4=s4/psi**4
        c4 = one
        c5x3 = one
        do i=9,3,-1
            j = 2*i
            c5x3 = one + a*c5x3/(j*(j+1))
            c4   = one + a*c4  /(j*(j-1))
        end do

        c5x3 = c5x3/40.0_wp
        c4   = c4  /24.0_wp

        !compute series c3=s3/psi**3,c2=s2/psi**2,c1=s1/psi,c0=rv0
        c3 = (0.5_wp + a*c5x3)/ three
        c2 = 0.5_wp + a*c4
        c1 = one + a*c3
        c0 = one + a*c2
        if (m>0) then

            !demod series c0 and c1 if necessary with double angle formulas:
            do
                c1 = c1*c0
                c0 = two*c0*c0 - one
                m = m - 1
                if (m<=0) exit
            end do

            !compute c2,c3,c4,c5x3 from c0,c1,ap if demod required:
            c2 = (c0 - one)/ap
            c3 = (c1 - one)/ap
            c4 = (c2 - 0.5_wp)/ap
            c5x3 = (three*c3 - 0.5_wp)/ap

        end if

        !compute series s1,s2,s3 from c1,c2,c3:
        s1 = c1*psi
        s2 = c2*psi*psi
        s3 = c3*psi*psi*psi

        ! compute slope r and residuals for kepler's equation:

        c(1) = r0*s1
        c(2) = sig0*s2
        c(3) = mu*s3
        c(4) = tau
        g    = c(4) - c(3)
        dtau = c(1) + c(2) - g
        r    = abs(r0*c0 + (sig0*s1 + mu*s2))

        ! compute next psi:
        do

            !method = 0
            if (dtau < zero) then
                psin  = psi
                dtaun = dtau
                psi   = psi - dtau/r
                if (psi < psip) exit    ! < (not <=) to avoid false convergence
            else
                psip  = psi
                dtaup = dtau
                psi   = psi - dtau/r
                if (psi > psin) exit    ! > (not >=) to avoid false convergence
            end if

            !reset psi within bounds psin and psip:

            !try incrementing bound with dtau nearest zero by the ratio 4*dtau/tau
            !--method = 1
            if (abs(dtaun) < abs(dtaup)) psi = psin*(one-div(four*dtaun,tau))
            if (abs(dtaup) < abs(dtaun)) psi = psip*(one-div(four*dtaup,tau))
            if (psi_in(psi)) exit

            !try doubling bound closest to zero:
            !--method = 2
            if (tau > zero) psi = psin + psin
            if (tau < zero) psi = psip + psip
            if (psi_in(psi)) exit

            !try interpolating between bounds:
            !--method = 3
            psi = psin + (psip - psin) * div(-dtaun, dtaup - dtaun)
            if (psi_in(psi)) exit

            !try halving between bounds:
            !--method = 4
            psi = psin + (psip - psin) * 0.5_wp
            exit

        end do

        ! test for convergence
        i = 1
        do j=2,4
            if (abs(c(j)) > abs(c(i))) i = j
        end do

        if (abs(dtau)>abs(c(i))*accrcy .and. &
            psip-psin>abs(psi)*accrcy .and. &
            psi/=psin .and. psi/=psip ) then
            cycle
        else
            exit
        end if

    end do

    !compute remaining three of four functions: g, gdm1, fm1, fd
    gdm1 = -mu*s2/r
    fm1  = -mu*s2/r0
    fd   = -mu*s1/r0/r

    ! compute state at time T = T0 + TAU
    rvf(1:3) = rv0(1:3) + fm1*rv0(1:3) + g*rv0(4:6)   ! terminal positions
    rvf(4:6) = rv0(4:6) + fd*rv0(1:3) + gdm1*rv0(4:6) ! terminal velocities

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

    !***************************************************************************
    !>
    !  Returns a nonoverflowing quotient

        pure function div(num,den)

        implicit none

        real(wp)            :: div
        real(wp),intent(in) :: num
        real(wp),intent(in) :: den

        real(wp),parameter :: fsmall  = 1.0e-38_wp  !! small number above underflow limit
        real(wp),parameter :: flarge  = 1.0e+38_wp  !! large number below underflow limit

        div = num / ( sign(one,den) * max(abs(den),abs(num)/flarge, fsmall) )

        end function div
    !***************************************************************************

    !***************************************************************************
    !>
    !  Returns true if `phi` is within and not on bounds

        pure function psi_in(psi)

        implicit none

        logical :: psi_in
        real(wp),intent(in) :: psi

        ! > & < (not >= & <=) to avoid false convergence
        psi_in = (psi > psin .and. psi < psip)

        end function psi_in
    !***************************************************************************

    end subroutine kepler_goodyear_stienon_klumpp
!*******************************************************************************

!*******************************************************************************
    end module kepler_module
!*******************************************************************************


* Eigen2x2.f
* diagonalization of a Hermitian 2-by-2 matrix
* this file is part of FormCalc
* last modified 15 Jun 04 th

	subroutine Eigen2x2(m, msq, U, M11, M22, M12, *)
	implicit none
	double precision m(*), msq(*)
	double complex U(2,*)
	double precision M11, M22
	double complex M12

	double precision m1, m2, delta, tau
	double complex s

	call Jacobi(delta, s, tau, M11 - M22, M12, abs(M12))

	m1 = M11 + delta
	m2 = M22 - delta

	if( m1 .gt. m2 ) then
	  msq(1) = m2
	  msq(2) = m1
	  U(1,1) = -dconjg(s)
	  U(2,2) = s
	  U(1,2) = 1 - tau
	  U(2,1) = U(1,2)
	else
	  msq(1) = m1
	  msq(2) = m2
	  U(1,1) = 1 - tau
	  U(2,2) = U(1,1)
	  U(1,2) = s
	  U(2,1) = -dconjg(s)
	endif

	if( msq(1) .lt. 0 ) then
	  print *, "Negative mass-squares."
	  return 1
	endif
	m(1) = sqrt(msq(1))
	m(2) = sqrt(msq(2))
	end


!********************************************************************************
!* This file is adapted from part of python_finite_volume_solver
!* (https://github.com/bwvdnbro/python_finite_volume_solver), and is part of
!* BAFFLE (https://github.com/ATHannington/BAFFLE)
!*
!* BAFFLE:
!* Copyright (C) 2017 Andrew Hannington (ath4@st-andrews.ac.uk)
!*
!* python_finite_volume_solver:
!* Copyright (C) 2017 Kenneth Wood (kw25@st-andrews.ac.uk)
!*                    Bert Vandenbroucke (bert.vandenbroucke@gmail.com)
!*
!* python_finite_volume_solver and BAFFLE are free software: you can redistribute
!* them and/or modify them under the terms of the GNU Affero General Public License
!* as published by the Free Software Foundation, either version 3 of the License, or
!* (at your option) any later version.
!*
!* python_finite_volume_solver and BAFFLE are distributed in the hope that they will
!* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
!* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!* GNU Affero General Public License for more details.
!*
!* You should have received a copy of the GNU Affero General Public License
!* along with python_finite_volume_solver and BAFFLE. If not, see
!* <http://www.gnu.org/licenses/>.
!********************************************************************************

!********************************************************************************
!* @file riemann.f90
!*
!* @brief Standalone Riemann solver library: Fortran 90 version.
!*
!* This Riemann solver is the original exact Riemann solver as it is presented in
!* Toro, E., Riemann Solvers and Numerical Methods for Fluid Dynamics, 3rd
!* edition (Springer, 2009), chapter 4.
!*
!* It was carefully copied (by hand) from that book to actual Fortran 77 code by
!* Kenneth Wood, and slightly modified to be used as a Riemann solver routine in
!* a 1-3D finite volume code. It was then adapted to Fortran 90, and combined with
!* Bert Vandenbroucke's python riemann solver, by Andrew Hannington.
!*
!* Additional changes (including this documentation) were made by Bert
!* Vandenbroucke to make the Python, C++ and Fortran version of the Riemann
!* solver more homogeneous. Subsequent changes (including edits to this documentation)
!* were made by Andrew Hannington, including adding vacuum capability from Bert
!* Vandenbroucke's own python Riemann solver.
!*
!* @author Andrew Hannington (ath4@st-andrews.ac.uk)
!* @author Kenneth Wood (kw25@st-andrews.ac.uk)
!* @author Bert Vandenbroucke (bv7@st-andrews.ac.uk)
!********************************************************************************

!********************************************************************************
!* @brief Solve the Riemann problem with the given left and right state.
!*
!* @pCRam d1 Left state density.
!* @pCRam u1 Left state fluid velocity.
!* @pCRam p1 Left state pressure.
!* @pCRam d2 Right state density.
!* @pCRam u2 Right state fluid velocity.
!* @pCRam p2 Right state pressure.
!* @pCRam gg Adiabatic index of the gas.
!* @pCRam ds VCRiable to store the resulting density in.
!* @pCRam us VCRiable to store the resulting fluid velocity in.
!* @pCRam ps VCRiable to store the resulting pressure in.
!* @pCRam uflag Flag vCRiable used to signal if the solution was sampled from the
!* left state (-1) or the right state (1). A vacuum state (0) is currently not
!* supported by this version of the Riemann solver.
!* @pCRam x_over_t Point (in velocity space) where we want to sample the
!* solution. In a finite volume scheme you usually want to set this value to 0.
!********************************************************************************
module gamma_module
	REAL*8 :: G1, G2, G3, G4, G5, G6, G7, G8
end module gamma_module

subroutine initialise_gammas
	USE CONSTANTS
	use gamma_module
     	implicit none

      G1 = (gam - 1.0d0)/(2.0d0*gam)
      G2 = (gam + 1.0d0)/(2.0d0*gam)
      G3 = 2.0d0*gam/(gam - 1.0d0)
      G4 = 2.0d0/(gam - 1.0d0)
      G5 = 2.0d0/(gam + 1.0d0)
      G6 = (gam - 1.0d0)/(gam + 1.0d0)
      G7 = (gam - 1.0d0)/2.0d0
      G8 = gam - 1.0d0

end subroutine initialise_gammas
!********************************************************************************
!********************************************************************************
!********************************************************************************
!********************************************************************************

subroutine riemann(d1, u1, p1, d2, u2, p2, gg, ds,us, ps, uflag, x_over_t,i,j,k,xyz,time)

	USE CONSTANTS
	use gamma_module
     	implicit none

	Integer,intent(in) :: i,j,k
	Real*8, intent(in) :: time
	Character(len=1),intent(in) :: xyz					!ChCRacter passed in from cells to Riemann that tells this subroutine whether we CRe looking at x, y, or z direction.

!     DeclCRation of vCRiables:
      integer uflag
      Real*8 ::  AGAMMA, DL, UL, PL, CL, DR, UR, PR, CR, &
          DIAPH, DOMLEN, DS, DX, PM, MPA, PS, S, &
          TIMEOUT, UM, US, XPOS

      real*8 gg, d1, u1, p1, d2, u2, p2
      real*8 x_over_t

      COMMON /STATES/ DL, UL, PL, CL, DR, UR, PR, CR

!     Compute gamma related constants

      AGAMMA = gg
      DL = d1
      UL = u1
      PL = p1
      DR = d2
      UR = u2
      PR = p2

      MPA=1.0d0  ! Not sure about this????

!     Compute sound speeds

	If (IsoBool .eqv. .FALSE.) then
		CL = DSQRT(AGAMMA*PL/DL)
		CR = DSQRT(AGAMMA*PR/DR)
	else if (IsoBool .eqv. .TRUE.) then
		CL = DSQRT(PL/DL)
		CR = DSQRT(PR/DR)
	ENDIF

!     The pressure positivity condition is tested for

      IF(G4*(CL+CR).LE.(UR-UL))THEN
!		Print*,
!		Print*,"***!*!*!*!*!***"
!		Print*, "Vacuum generated by data [@RIEMANN SOLVER]."
!		Print*,"Whilst solving in ",xyz," direction."
!		Print*,"i=",i,"j=",j,"k=",k
!		Print*,"Time=",time
!		Print*,
!		Print*,"Attempting to Solve!"
		call solve_vacuum(dL, uL, PL, CL, dR, uR, PR, CR, x_over_t, ds, us, Ps, uflag)
	!	     solve_vacuum(rhoL, uL, PL, CL, rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)

	else

!     Exact solution for pressure and velocity in star region is found
		CALL STCRPU(PM, UM, MPA)

	      if(pm.ne.pm) then
		Print*,"NAN DETECTED! [@RIEMANN SOLVER]. PROGRAM WILL ABORT!"
		Print*,"Whilst solving in ",xyz," direction."
		Print*,"i=",i,"j=",j,"k=",k
		Print*,"Time=",time
		print*,"PM=",pm,"PL=",pl,"DL=",dl,"uL=",ul,"PR=",pr,"DR=",dr,"uR=",ur
		stop
	      endif
	      if(UM .gt.x_over_t) then
		uflag=-1 ! "left state"
	      else
		uflag=1 ! "right state
	      endif

	      CALL SAMPLE(PM, UM, x_over_t, DS, US, PS)

	endif

      RETURN
      END

!********************************************************************************
!* @brief Get the pressure and velocity in the middle state.
!*
!* @pCRam p VCRiable to store the resulting pressure in.
!* @pCRam u VCRiable to store the resulting fluid velocity in.
!* @pCRam mpa Mach number? Not used.
!********************************************************************************
      SUBROUTINE STCRPU(P, U, MPA)

	USE CONSTANTS
	use gamma_module
     	implicit none

!     Purpose: to compute the solution for pressure and velocity in stCR region

      INTEGER I, NRITER

      REAL*8 DL, UL, PL, CL, DR, UR, PR, CR, &
         CHANGE, FL, FLD, FR, FRD, P, POLD, PSTCRT, &
         TOLPRE, U, UDIFF, MPA

      COMMON/STATES/ DL, UL, PL, CL, DR, UR, PR, CR
      DATA TOLPRE, NRITER/1.0d-09, 20/

!     Guessed value PSTCRT is computed

      CALL GUESSP(PSTCRT)

      POLD = PSTCRT
      UDIFF = UR - UL
!      WRITE(6,*)'Iteration number Change '

      DO 10 I = 1, NRITER

         CALL PREFUN(FL, FLD, POLD, DL, PL, CL)
         CALL PREFUN(FR, FRD, POLD, DR, PR, CR)
         P = POLD - (FL + FR + UDIFF)/(FLD + FRD)

         CHANGE = 2.0d0*DABS((P - POLD)/(P + POLD))
!         WRITE(6, 30)I, CHANGE
         IF(CHANGE.LE.TOLPRE)GOTO 20
         IF(P.LT.0.0)P = TOLPRE
         POLD = P

 10   CONTINUE

!      WRITE(6,*)'Divergence in Newton-Raphson iteration'

 20   CONTINUE

!     Compute velocity in stCR region

      U = 0.5d0*(UL + UR + FR - FL)

!      WRITE(6,*)'Pressure	Velocity'
!      WRITE(6,40)P/MPA, U

 30   FORMAT(5X, I5,15X, F12.7)
 40   FORMAT(2(F14.6, 5X))

      RETURN
      END

!********************************************************************************
!* @brief Get an initial guess for the pressure in the middle state, as a
!* stCRting point for the Newton-Raphson iteration.
!*
!* @pCRam pm VCRiable to store the resulting pressure guess in.
!********************************************************************************
      SUBROUTINE GUESSP(PM)

!     Purpose: to provide a guess value for pressure PM in the StCR Region.
!     The choice is made according to adaptive Riemann solver using the PVRS,
!     TRRS, and TSRS approximate Riemann solvers

	USE CONSTANTS
	use gamma_module
     	implicit none

      REAL*8 DL, UL, PL, CL, DR, UR, PR, CR, &
          AGAMMA, &
          CUP, GEL, GER, PM, PMAX, PMIN, PPV, PQ, &
          PTL, PTR, QMAX, QUSER, UM

      COMMON /STATES/ DL, UL, PL, CL, DR, UR, PR, CR

      QUSER = 2.0d0

!     Compute guess pressure for PVRS Riemann solver

      CUP = 0.25d0*(DL + DR)*(CL + CR)
      PPV = 0.5d0*(PL + PR) + 0.5d0*(UL - UR)*CUP
      PPV = AMAX1(0.0d0, PPV)
      PMIN = AMIN1(PL, PR)
      PMAX = AMAX1(PL, PR)
      QMAX = PMAX/PMIN

      IF(QMAX.LE.QUSER.AND. &
       (PMIN.LE.PPV.AND.PPV.LE.PMAX))THEN

!        Select PVRS Riemann solver

         PM = PPV

      ELSE
         IF(PPV.LT.PMIN)THEN

!           Select Two-RCRefaction Riemann solver

            PQ = (PL/PR)**G1
            UM = (PQ*UL/CL + UR/CR + &
                 G4*(PQ - 1.0d0))/(PQ/CL + 1.0d0/CR)
            PTL = 1.0d0 + G7*(UL - UM)/CL
            PTR = 1.0d0 + G7*(UM - UR)/CR
            PM = 0.5d0*(PL*PTL**G3 + PR*PTR**G3)
         ELSE

!           Select Two=Shock Riemann solver with PVRS as estimate

            GEL = DSQRT((G5/DL)/(G6*PL + PPV))
            GER = DSQRT((G5/DR)/(G6*PR + PPV))
            PM = (GEL*PL + GER*PR - (UR - UL))/(GEL + GER)
         ENDIF
      ENDIF

      RETURN
      END

!********************************************************************************
!* @brief Evaluate the pressure functions for the given pressure guess and the
!* given left or right state vCRiables.
!*
!* @pCRam f VCRiable to store the pressure function value in.
!* @pCRam fd VCRiable to store the value of the derivative of the presure
!* function in.
!* @pCRam p Current pressure guess.
!* @pCRam dk Left or right state density.
!* @pCRam pk Left or right state pressure.
!* @pCRam ck Left or right state sound speed.
!********************************************************************************
      SUBROUTINE PREFUN(F, FD, P, DK, PK, CK)

!     Purpose: to evaluate the pressure functions FL and FR in the exact Riemann
!     solverPM

	USE CONSTANTS
	use gamma_module
     	implicit none

      REAL*8 AK, BK, CK, DK, F, FD, P, PK, PRAT, QRT, &
          AGAMMA

      COMMON /GAMMAS/ AGAMMA

      IF(P.LE.PK) THEN

!        RCRefaction wave

         PRAT = P/PK
         F = G4*CK*(PRAT**G1 - 1.0d0)
         FD = (1.0d0/(DK*CK))*PRAT**(-G2)

      ELSE

!        Shock wave
         AK = G5/DK
         BK = G6*PK
         QRT = DSQRT(AK/(BK + P))
         F = (P - PK)*QRT
         FD = (1.0d0 - 0.5d0*(P - PK)/(BK + P))*QRT

      ENDIF

      RETURN
      END

!********************************************************************************
!* @brief Sample the Riemann solution with the given middle state pressure and
!* fluid velocity at the given point in velocity space.
!*
!* @pCRam pm Middle state pressure.
!* @pCRam um Middle state fluid velocity.
!* @pCRam s Sampling point in velocity space.
!* @pCRam d VCRiable to store the resulting density in.
!* @pCRam u VCRiable to store the resulting fluid velocity in.
!* @pCRam p VCRiable to store the resulting pressure in.
!********************************************************************************
      SUBROUTINE SAMPLE(PM, UM, S, D, U, P)

!     Purpose to sample the solution throughout the wave pattern. Pressure PM
!     and velocity UM in the StCR Region CRe known. Sampling is performed in
!     terms of the 'speed' S = X/T. Sampled values CRe D, U, P

!     Input vCRiables: PM, UM, S, /GAMMAS. /STATES/
!     Output vCRiables: D, U, P
	USE CONSTANTS
	use gamma_module
     	implicit none

      REAL*8 DL, UL, PL, CL, DR, UR, PR, CR, &
          AGAMMA, &
          C, CML, CMR, D, P, PM, PML, PMR, S, &
          SHL, SHR, SL, SR, STL, STR, U, UM

      COMMON /GAMMAS/ AGAMMA
      COMMON /STATES/ DL, UL, PL, CL, DR, UR, PR, CR

      IF(S.LE.UM)THEN

!     Sampling point lies to the left of the contact discontinuity

         IF(PM.LE.PL)THEN

!          Left rCRefaction

            SHL = UL - CL

            IF(S.LE.SHL)THEN

!              Sampled point is left data state

               D = DL
               U = UL
               P = PL
            ELSE
               CML = CL*(PM/PL)**G1
               STL = UM - CML

               IF(S.GT.STL)THEN

!                 Sampled point is StCR Left state

                  D = DL*(PM/PL)**(1.0d0/AGAMMA)
                  U = UM
                  P = PM
               ELSE

!                 Sampled point is inside left fan

                  U = G5*(CL + G7*UL + S)
                  C = G5*(CL + G7*(UL - S))
                  D = DL*(C/CL)**G4
                  P = PL*(C/CL)**G3
               ENDIF
            ENDIF
!         ENDIF
         ELSE

!            Left shock

            PML = PM/PL
            SL = UL - CL*DSQRT(G2*PML + G1)

            IF(S.LE.SL)THEN

!              Sampled point is left data state

               D = DL
               U = UL
               P = PL

             ELSE

!              Sampled point is StCR Left state

               D = DL*(PML+G6)/(PML*G6 + 1.0d0)
               U = UM
               P = PM
             ENDIF
           ENDIF
          ELSE

!    Sampling point lies to the right of the contact discontinuity

         IF(PM.GT.PR)THEN

!           Right shock

            PMR = PM/PR
            SR = UR + CR*DSQRT(G2*PMR + G1)

            IF(S.GE.SR)THEN

!              Sampled point is right data state

               D = DR
               U = UR
               P = PR
            ELSE

!              Sampled point is StCR Right state

               D = DR*(PMR + G6)/(PMR*G6 + 1.0d0)
               U = UM
               P = PM
            ENDIF
         ELSE

!           Right rCRefaction

            SHR = UR + CR

            IF(S.GE.SHR)THEN

!              Sampled point is right data state

               D = DR
               U = UR
               P = PR
            ELSE
               CMR = CR*(PM/PR)**G1
               STR = UM + CMR

               IF(S.LE.STR)THEN

!                 Sampled point is StCR Right state

                  D = DR*(PM/PR)**(1.0d0/AGAMMA)
                  U = UM
                  P = PM
               ELSE

!                 Sampled point is inside left fan

                  U=G5*(-CR + G7*UR + S)
                  C = G5*(CR - G7*(UR-S))
                  D = DR*(C/CR)**G4
                  P = PR*(C/CR)**G3
               ENDIF
            ENDIF
         ENDIF
      ENDIF

      RETURN
END

!  ##############################################################################
  ! @brief Vacuum Riemann solver.
!
  ! This solver is called when one or both states have a zero density, or when
  ! the vacuum generation condition is satisfied (meaning vacuum is generated
  ! in the middle state, although strictly speaking there is no "middle"
  ! state if vacuum is involved).
!
  ! @pCRam rhoL Density of the left state.
  ! @pCRam uL Velocity of the left state.
  ! @pCRam PL Pressure of the left state.
  ! @pCRam CL Soundspeed of the left state.
  ! @pCRam rhoR Density of the right state.
  ! @pCRam uR Velocity of the right state.
  ! @pCRam PR Pressure of the right state.
  ! @pCRam CR Soundspeed of the right state.
  ! @pCRam x_over_t Point in velocity space where we want to sample the solution.
  ! @return Density, velocity and pressure solution, and a flag indicating
  ! wether the left state (-1), the right state (1), or a vacuum state (0) was
  ! sampled.
!  ##############################################################################
subroutine solve_vacuum(rhoL, uL, PL, CL, rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)

	USE CONSTANTS
	use gamma_module
     	implicit none

		Real*8,intent(in) :: rhoL,uL,PL,CL
		Real*8,intent(in) :: rhoR,uR,PR,CR
		Real*8,intent(in) :: x_over_t
	   	Real*8,intent(out) :: rhosol, usol, Psol
		Integer,intent(out) :: flag
		Real*8 :: SR,SL,Base

! if both states are vacuum, the solution is also vacuum
	If ((rhoL == 0.d0) .and. (rhoR == 0.d0)) then
     		RhoSol= 0.d0
		uSol= 0.d0
		PSol= 0.d0
		flag=0

	else if (rhoR == 0.d0) then
		! vacuum right state
		call sample_right_vacuum(rhoL, uL, PL, CL, x_over_t, rhosol, usol, Psol, flag)
	else if (rhoL == 0.d0) then
		! vacuum left state
		call sample_left_vacuum(rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)
	else
		! vacuum "middle" state
		call sample_vacuum_generation(rhoL, uL, PL, CL, rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)
	endif

end subroutine solve_vacuum

!  ##############################################################################
!  ! @brief Sample the vacuum Riemann problem if the right state is a vacuum.
  ! @pCRam rhoL Density of the left state.
  ! @pCRam uL Velocity of the left state.
  ! @pCRam PL Pressure of the left state.
  ! @pCRam CL Soundspeed of the left state.
  ! @pCRam x_over_t Point in velocity space where we want to sample the solution.
  ! @return Density, velocity and pressure solution, and a flag indicating
  ! wether the left state (-1), the right state (1), or a vacuum state (0) was
  ! sampled.
 ! ##############################################################################
subroutine sample_right_vacuum(rhoL, uL, PL, CL, x_over_t, rhosol, usol, Psol, flag)
	USE CONSTANTS
	use gamma_module
     	implicit none

		Real*8,intent(in) :: rhoL,uL,PL,CL,x_over_t
	   	Real*8,intent(out) :: rhosol, usol, Psol
		Integer,intent(out) :: flag
		Real*8 :: SL,Base

	If ((uL - CL).lt.x_over_t) then

	! vacuum regime
	!get the vacuum rCRefaction wave speed
		SL = uL + G4 * CL

      		If (SL .gt. x_over_t) then
	! rCRefaction wave regime
	! vCRiable used twice below
			base = G5 + G6* (uL - x_over_t) / CL

			rhosol = rhoL * base**G4
			usol = G5 * (CL + (G7 * uL) + x_over_t)
			Psol = PL * base**G3
			flag = -1
      		else
			! vacuum
			rhosol = 0.d0
			usol = 0.d0
			Psol = 0.d0
			flag = 0
		endif
	else
		! left state regime
     		rhosol = rhoL
      		usol = uL
     		Psol = PL
      		flag = -1
	endif

end subroutine sample_right_vacuum
!##############################################################################
  ! @brief Sample the vacuum Riemann problem if the left state is a vacuum.
!
  ! @pCRam rhoR Density of the right state.
  ! @pCRam uR Velocity of the right state.
  ! @pCRam PR Pressure of the right state.
  ! @pCRam CR Soundspeed of the right state.
  ! @pCRam x_over_t Point in velocity space where we want to sample the solution.
  ! @return Density, velocity and pressure solution, and a flag indicating
  ! wether the left state (-1), the right state (1), or a vacuum state (0) was
  ! sampled.
!  ##############################################################################
subroutine sample_left_vacuum(rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)
	USE CONSTANTS
	use gamma_module
     	implicit none

		Real*8,intent(in) :: rhoR,uR,PR,CR,x_over_t
	   	Real*8,intent(out) :: rhosol, usol, Psol
		Integer,intent(out) :: flag
		Real*8 :: SR,Base

	If (x_over_t .lt. (uR + CR)) then
	! vacuum regime
	! get the vacuum rarefaction wave speed
		SR = uR - G4 * CR

		if (SR .lt. x_over_t) then
		! rarefaction wave regime
			! vCRiable used twice below
			base = G5 - G6* (uR - x_over_t) / CR
			rhosol = rhoR * base**G4
			usol = G5 * (-1.d0*CR + G4 * uR + x_over_t)
			Psol = PR * base**G3
			flag = 1
		else
		! vacuum
			rhosol = 0.d0
			usol = 0.d0
			Psol = 0.d0
			flag = 0
		endif

	else
	! right state regime
	      rhosol = rhoR
	      usol = uR
	      Psol = PR
	      flag = 1
	endif

end subroutine sample_left_vacuum

!  ##############################################################################
  ! @brief Sample the vacuum Riemann problem in the case vacuum is generated in
  ! between the left and right state.
!
  ! @pCRam rhoL Density of the left state.
  ! @pCRam uL Velocity of the left state.
  ! @pCRam PL Pressure of the left state.
  ! @pCRam CL Soundspeed of the left state.
  ! @pCRam rhoR Density of the right state.
  ! @pCRam uR Velocity of the right state.
  ! @pCRam PR Pressure of the right state.
  ! @pCRam CR Soundspeed of the right state.
  ! @pCRam x_over_t Point in velocity space where we want to sample the solution.
  ! @return Density, velocity and pressure solution, and a flag indicating
  ! wether the left state (-1), the right state (1), or a vacuum state (0) was
  ! sampled.
!  ##############################################################################
subroutine sample_vacuum_generation(rhoL, uL, PL, CL, rhoR, uR, PR, CR, x_over_t, rhosol, usol, Psol, flag)

	USE CONSTANTS
	use gamma_module
     	implicit none

		Real*8,intent(in) :: rhoL,uL,PL,CL
		Real*8,intent(in) :: rhoR,uR,PR,CR
		Real*8,intent(in) :: x_over_t
	   	Real*8,intent(out) :: rhosol, usol, Psol
		Integer,intent(out) :: flag
		Real*8 :: SR,SL,Base

! get the speeds of the left and right rCRefaction waves
    SR = uR - G4 * CR
    SL = uL + G4 * CL

	If ((SR .gt. x_over_t) .and. (SL .lt. x_over_t)) then
		! vacuum
	      rhosol = 0.d0
	      usol = 0.d0
	      Psol = 0.d0
	      flag = 0

	else if (SL .lt. x_over_t) then
	! right state
		If (x_over_t .lt. (uR + CR)) then
		! right rCRefaction wave regime
		! vCRiable used twice below
			base = G5 - G6* (uR - x_over_t) / CR
			rhosol = rhoR * base**G4
			usol = G5 * (-1.d0*CR + G4 * uR + x_over_t)
			Psol = PR * base**G3
   		else
		! right state regime
			rhosol = rhoR
			usol = uR
			Psol = PR
			flag = 1
		endif
	else
	! left state
		If (x_over_t .gt. (uL - CL)) then
		! left rCRefaction wave regime
		! vCRiable used twice below
			base = G5 + G6* (uL - x_over_t) / CL
			rhosol = rhoL * base**G4
			usol = G5 * (CL + G4 * uL + x_over_t)
			Psol = PL * base**G3
        	else
			! left state regime
			rhosol = rhoL
			usol = uL
			Psol = PL
			flag = -1
		endif
	endif

END subroutine sample_vacuum_generation


!*********************************************************
!>
! Sorting Routines

module sorting
  implicit none

  private
  integer, parameter :: dp = kind(0.d0)

  public :: swap
  public :: qsort

  interface swap
    module procedure swap_i, swap_r, swap_d, swap_Cr, swap_Cd
  end interface

  interface qsort
    module procedure qsort_i, qsort_r, qsort_d
  end interface

contains

  subroutine swap_i(a,b,condition)
    implicit none
    integer :: a, b, aDum, bDum
    logical :: ifSwap
    logical, optional :: condition
    if(present(condition)) then
      ifSwap = condition
    else
      ifSwap = .true.
    end if
    if(ifSwap) then
      aDum = a; bDum = b
      a = bDum; b = aDum
    end if
  end subroutine

  subroutine swap_r(a,b,condition)
    implicit none
    real :: a, b, aDum, bDum
    logical :: ifSwap
    logical, optional :: condition
    if(present(condition)) then
      ifSwap = condition
    else
      ifSwap = .true.
    end if
    if(ifSwap) then
      aDum = a; bDum = b
      a = bDum; b = aDum
    end if
  end subroutine

  subroutine swap_d(a,b,condition)
    implicit none
    real(dp) :: a, b, aDum, bDum
    logical :: ifSwap
    logical, optional :: condition
    if(present(condition)) then
      ifSwap = condition
    else
      ifSwap = .true.
    end if
    if(ifSwap) then
      aDum = a; bDum = b
      a = bDum; b = aDum
    end if
  end subroutine

  subroutine swap_Cr(a,b,condition)
    implicit none
    complex :: a, b, aDum, bDum
    logical :: ifSwap
    logical, optional :: condition
    if(present(condition)) then
      ifSwap = condition
    else
      ifSwap = .true.
    end if
    if(ifSwap) then
      aDum = a; bDum = b
      a = bDum; b = aDum
    end if
  end subroutine

  subroutine swap_Cd(a,b,condition)
    implicit none
    complex(dp) :: a, b, aDum, bDum
    logical :: ifSwap
    logical, optional :: condition
    if(present(condition)) then
      ifSwap = condition
    else
      ifSwap = .true.
    end if
    if(ifSwap) then
      aDum = a; bDum = b
      a = bDum; b = aDum
    end if
  end subroutine

  subroutine qsort_i(array)
    implicit none
    integer :: k, n, left, right, marker
    integer :: pivot, array(:)
    n = size(array,1)
    if(n>1) then
      k = (n/2)
      pivot = array(k)
      left = 0
      right = n+1
      do while(left<right)
        right = right-1
        do while(array(right).gt.pivot)
          right = right-1
        end do
        left = left+1
        do while(array(left).lt.pivot)
          left = left+1
        end do
        if(left<right) then
          call swap(array(left),array(right))
        end if
      end do
      if(left==right) then
        marker = left +1
      else
        marker = left
      end if
      call qsort(array(:marker-1))
      call qsort(array(marker:))
    end if
  end subroutine

  subroutine qsort_r(array)
    implicit none
    integer :: k, n, left, right, marker
    real :: pivot, array(:)
    n = size(array,1)
    if(n>1) then
      k = (n/2)
      pivot = array(k)
      left = 0
      right = n+1
      do while(left<right)
        right = right-1
        do while(array(right).gt.pivot)
          right = right-1
        end do
        left = left+1
        do while(array(left).lt.pivot)
          left = left+1
        end do
        if(left<right) then
          call swap(array(left),array(right))
        end if
      end do
      if(left==right) then
        marker = left +1
      else
        marker = left
      end if
      call qsort(array(:marker-1))
      call qsort(array(marker:))
    end if
  end subroutine

  subroutine qsort_d(array)
    implicit none
    integer :: k, n, left, right, marker
    real(dp) :: pivot, array(:)
    n = size(array,1)
    if(n>1) then
      k = (n/2)
      pivot = array(k)
      left = 0
      right = n+1
      do while(left<right)
        right = right-1
        do while(array(right).gt.pivot)
          right = right-1
        end do
        left = left+1
        do while(array(left).lt.pivot)
          left = left+1
        end do
        if(left<right) then
          call swap(array(left),array(right))
        end if
      end do
      if(left==right) then
        marker = left +1
      else
        marker = left
      end if
      call qsort(array(:marker-1))
      call qsort(array(marker:))
    end if
  end subroutine

end module


!-------------------------------------------------------------------------------
! Copyright (c) 2021, Whitman T. Dailey
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without
! modification, are permitted provided that the following conditions are met:
! 1. Redistributions of source code must retain the above copyright notice, this
!    list of conditions and the following disclaimer.
! 2. Redistributions in binary form must reproduce the above copyright notice,
!    this list of conditions and the following disclaimer in the documentation
!    and/or other materials provided with the distribution.
! 3. Neither the name of the copyright holder nor the names of its
!    contributors may be used to endorse or promote products derived from
!    this software without specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
! DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
! FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
! OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
!-------------------------------------------------------------------------------
Module Tridiagonal

    Implicit None
    Private
    Public :: Solve_TriDiag

Contains

Function Solve_TriDiag(n,a,b,c,rhs) Result(x)
    Use Kinds, Only: dp
    Implicit None
    Integer, Intent(In) :: n !number of elements in a,b,c,rhs, and x
    Real(dp) :: x(0:n+1) !solution vector
    !a,b,c define tridiagonal matrix with:
    Real(dp), Intent(In) :: a(1:n+1) !below diagonal
    Real(dp), Intent(In) :: b(0:n+1) !on diagonal
    Real(dp), Intent(In) :: c(0:n) !above diagonal
    Real(dp), Intent(In) :: rhs(0:n+1) !right hand side of system
    Real(dp) :: g(0:n) !decomposed upper diagonal
    Real(dp) :: r(0:n+1) !decomposed rhs
    Integer :: i

    !STAGE 1:  LU decomposition
    !first row, special case
    g(0) = c(0) / b(0)
    r(0) = rhs(0) / b(0)
    !rows 2 to n-1 are recursive
    Do i = 1,n
        g(i) = c(i) / ( b(i) - a(i) * g(i-1) )
        r(i) = ( rhs(i) - a(i) * r(i-1) ) / ( b(i) - a(i) * g(i-1) )
    End Do
    !row n+1 is only in rhs,r
    r(n+1) = ( rhs(n+1) - a(n+1) * r(n) ) / ( b(n+1) - a(n+1) * g(n) )
    !STAGE 2:  Solve back up the matrix
    x = 0._dp
    !last row, special case
    x(n+1) = r(n+1)
    !rows n-1 to 1 are recursive
    Do i = n,0,-1
        x(i) = r(i) - g(i) * x(i+1)
    End Do
End Function Solve_TriDiag

End Module Tridiagonal


!!## MODULE: FUNCTION  xyCONVEXHULL
MODULE FUN_xyCONVEXHULL
!!### PURPOSE
!! Determine the counter-clockwise convex hull
!! ordering from points in a list.
!
!             order = xyCONVEXHULL( N , Px )
!
!!   where N is the number of points to use in the point list,
!
!!                 Px = [ P_1 , P_2 , P_3 , ... , P_N ],
!
!!   where the jth point,
!
!!                P_j = [ x_j , y_j ],
!
!!   is given by the Cartesian x-y coordinates.  The return
!!   value is the counter-clockwise ordered points of
!!   the corresponding convex hull,
!
!!            order = [ k_1 , k_2 , k_3 , ... , k_M ],
!
!!   where k_j corresponds to an index of Px UNLESS k_j==0, then
!!   then the jth point P_j is excluded in order to make
!!   the hull convex.
!!

!!### EXTERNAL KINDS
USE KND_IntrinsicTypes      ,ONLY: KIND_Rsp , KIND_Rdp      !!((01-A-KND_IntrinsicTypes.f90))

!!### EXTERNAL PARAMETERS
USE PAR_Constants_Rsp,ONLY: c_1_Rsp => c_1 , c_0_Rsp => c_0 !!((02-A-PAR_Constants_Rsp.f90))
USE PAR_Constants_Rdp,ONLY: c_1_Rdp => c_1 , c_0_Rdp => c_0 !!((02-A-PAR_Constants_Rdp.f90))

!!### EXTERNAL PROCEDURES
USE FUN_xyDIRECTION       ,ONLY: xyDIRECTION_PP             !!((04-B-FUN_xyDIRECTION.f90))
USE FUN_xyANGLE           ,ONLY: xyANGLE_UU                 !!((05-B-FUN_xyANGLE.f90))
USE FUN_xyROTATE          ,ONLY: xyROTATE_Px                !!((05-B-FUN_xyROTATE.f90))
USE FUN_xyTRANSLATE       ,ONLY: xyTRANSLATE_Px             !!((05-B-FUN_xyTRANSLATE.f90))
USE FUN_xyCOUNTERCLOCKWISE,ONLY: xyCOUNTERCLOCKWISE_Cn      !!((06-B-FUN_xyCOUNTERCLOCKWISE.f90))

!!### DEFAULT IMPLICIT
IMPLICIT NONE

!!### DEFAULT ACCESS
PRIVATE

!!### PROCEDURE OVERLOADING
INTERFACE xyCONVEXHULL_Px
 MODULE PROCEDURE xyCONVEXHULL_Px_Rsp
 MODULE PROCEDURE xyCONVEXHULL_Px_Rdp
ENDINTERFACE

!!### PUBLIC ACCESS LIST
PUBLIC :: xyCONVEXHULL_Px

!!### MODULE PROCEDURES
CONTAINS

!!### PURE FUNCTION: xyCONVEXHULL_Px_Rsp
PURE FUNCTION xyCONVEXHULL_Px_Rsp( N , Px ) RESULT(order_cvh)
!!#### LOCAL KINDS
INTEGER    ,PARAMETER :: KIND_R = KIND_Rsp
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rsp
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rsp
INCLUDE "07-B-FUN_xyCONVEXHULL_Px.f90.hdr"
!!--begin--
INCLUDE "07-B-FUN_xyCONVEXHULL_Px.f90.bdy"
!!--end--
END FUNCTION

!!### PURE FUNCTION: xyCONVEXHULL_Px_Rdp
PURE FUNCTION xyCONVEXHULL_Px_Rdp( N , Px ) RESULT(order_cvh)
!!#### LOCAL KINDS
INTEGER    ,PARAMETER :: KIND_R = KIND_Rdp
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rdp
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rdp
INCLUDE "07-B-FUN_xyCONVEXHULL_Px.f90.hdr"
!!--begin--
INCLUDE "07-B-FUN_xyCONVEXHULL_Px.f90.bdy"
!!--end--
END FUNCTION

END MODULE


program main
    use forlab, only: empty, disp, operator(.x.)
    implicit none
    real, dimension(:, :), allocatable :: x_sp, y_sp
    real(8), dimension(:, :), allocatable :: x_dp, y_dp
    real(16), dimension(:, :), allocatable :: x_qp, y_qp

    x_sp = empty(2, 2); x_dp = empty(2, 2); x_qp = empty(2, 2)
    y_sp = empty(2, 2); y_dp = empty(2, 2); y_qp = empty(2, 2)

    x_sp = 1.; x_dp = 1.; x_qp = 1.
    y_sp = 2.; y_dp = 2.; y_qp = 2.

    call disp('----------------------------')
    call disp(x_sp .x. y_sp, 'x .x. y SP✨:')
    call disp(x_dp .x. y_dp, 'x .x. y DP✨:')
    call disp(x_qp .x. y_qp, 'x .x. y QP✨:')

end program


MODULE m_eigen

    ! Dependencies =============================================================
    USE mpi                     ! Message passing interface (MPI) module
    ! ==========================================================================

    IMPLICIT NONE

    CONTAINS

        SUBROUTINE s_eigs(Amat,Eigs,EVec,N)

            REAL(KIND(0d0)), TARGET, DIMENSION(:,:), INTENT(IN) :: Amat
            REAL(KIND(0d0)), TARGET, DIMENSION(:), INTENT(OUT)  :: EVec, Eigs
            INTEGER, INTENT(IN) :: N

            REAL(KIND(0d0)), ALLOCATABLE, DIMENSION(:,:) ::  VR, A
            REAL(KIND(0d0)), ALLOCATABLE, DIMENSION(:)   :: WR, WI, Work
            REAL(KIND(0d0)) ::  DUMMY(1,1), Eigmax

            INTEGER ::  NB2, NMAX, LDA, LDVR, LWORK, INFO,LWKOPT
            INTEGER :: i, j
            INTEGER, PARAMETER :: LWMAX = 1000

            EXTERNAL         DGEEV
            INTRINSIC        DCMPLX

            Nb2=64
            LDA=N
            LDVR=N
            LWORK=LWMAX

            ALLOCATE( VR(LDVR,N), A(N,N) )
            ALLOCATE( WORK(LWORK), WR(N), WI(N) )

            A = Amat

            PRINT*, 'N', N

            CALL DGEEV('N','V',N,A,LDA,WR,WI,DUMMY,1,VR,LDVR,WORK,LWORK,INFO)
            LWKOPT = WORK(1)

            IF (INFO/=0) THEN
                PRINT*, 'Failure in DGEEV.  INFO = ', INFO
            END IF

            EVec(1:N) = VR(1,N:1:-1)
            Eigs = WR(N:1:-1)

            ! ELSE
                ! EigMax = 0d0
                ! DO j = 1,N
                !     IF( WR(j) > EigMax ) THEN
                !         EigMax = WR(j)
                !         ! EVec(:) = VR(j,:)
                !         EVec(:) = VR(:,j)
                !     END IF
                ! END DO
            ! END IF

            ! PRINT*, 'eigenvectors'
            ! PRINT*, VR(1,:)
            ! PRINT*, VR(2,:)
            ! PRINT*, VR(3,:)

            ! print*, 'in eigs'
            ! print*, 'max eig', Eigmax
            ! print*, 'max loc', MAXLOC(WR(:),DIM=1)
            ! print*, 'real eigs: ', WR(:)
            ! print*, 'eigvec associated with largeest eigenvalue', EVec(1:N)

        END SUBROUTINE s_eigs

END MODULE m_eigen


module compute
  use misc, only : meanval

  implicit none

  private

  integer :: opt_lwork = -1

  public :: compute_inertia_tensor, correct_positions

contains
  ! This routine shall be used with positions between 0 and 1. If an halo is close
  ! to the border of the box, it may have positions ~0 and others ~1. This problem
  ! is solved by the routine and the position are changed
  subroutine compute_inertia_tensor(mass, pos, I_t)
    real(kind=8), intent(in), dimension(:)       :: mass
    real(kind=8), intent(inout), dimension(:, :) :: pos

    real(kind=8), intent(out), dimension(3, 3) :: I_t

    real(kind=8), dimension(size(pos, 1)) :: tmp_mean
    real(kind=8), dimension(size(pos, 1), size(pos, 2)) :: corr_pos
    real(kind=8) :: mtot, tmp

    integer :: i, j
    integer :: ndim, nparts

    ndim = size(pos, 1)
    nparts = size(pos, 2)

    mtot = sum(mass)

    call meanval(pos, tmp_mean, ndim, nparts)
    do i = 1, ndim
       corr_pos(i, :) = pos(i, :) - tmp_mean(i)
    end do

    I_t = 0d0

    !-------------------------------------
    ! Compute the tensor
    !-------------------------------------
    do i = 1, ndim
       do j = i, ndim
          tmp = sum(mass(:)*corr_pos(i, :)*corr_pos(j, :)) / mtot
          I_t(i, j) = tmp + I_t(i, j)
          I_t(j, i) = tmp + I_t(i, j)
       end do
    end do

    ! call dsytrd('U', 3, I_t, 3, I_t_diag, E, tau, worpart, opt_lwork, info)
    ! if (info == 0) then
    !    opt_lwork = int(work(1))
    ! end if

  end subroutine compute_inertia_tensor

  subroutine correct_positions (pos)
    real(kind=8), intent(inout), dimension(:, :) :: pos

    real(kind=8), dimension(size(pos, 1)) :: span
    integer :: ndim, nparts, part, dim

    ndim = size(pos, 1)
    nparts = size(pos, 2)

    span = maxval(pos, 2) - minval(pos, 2)

    !-------------------------------------
    ! If the particles have a separation > 0.5
    ! then shift 'em
    !-------------------------------------
    do dim = 1, 3
       if (span(dim) > 0.5) then
          do part = 1, nparts
             if (pos(dim, part) > 0.5) then
                pos(dim, part) = pos(dim, part) - 1d0
             end if
          end do
       end if
    end do
  end subroutine correct_positions

end module compute


 !***************************************************************************************************
 	module evolve_routines
 !***************************************************************************************************
 	implicit none

 	!the target string:
 	character(len=*),parameter :: targ = 'METHINKS IT IS LIKE A WEASEL'

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

 !********************************************************************
 	pure elemental function fitness(member) result(n)
 !********************************************************************
 ! The fitness function.  The lower the value, the better the match.
 ! It is zero if they are identical.
 !********************************************************************

 	implicit none
 	integer :: n
 	character(len=*),intent(in) :: member

 	integer :: i

 	n=0
 	do i=1,len(targ)
 		n = n + abs( ichar(targ(i:i)) - ichar(member(i:i))  )
 	end do

 !********************************************************************
 	end function fitness
 !********************************************************************

 !********************************************************************
 	pure elemental subroutine mutate(member,factor)
 !********************************************************************
 ! mutate a member of the population.
 !********************************************************************

 	implicit none
 	character(len=*),intent(inout) :: member   !population member
 	real,intent(in) :: factor                  !mutation factor

 	integer,parameter :: n_chars = 27	!number of characters in set
 	character(len=n_chars),parameter :: chars = 'ABCDEFGHIJKLMNOPQRSTUVWXYZ '

 	real    :: rnd_val
 	integer :: i,j,n

 	n = len(member)

 	do i=1,n
 		rnd_val = rand()
 		if (rnd_val<=factor) then   !mutate this element
 			rnd_val = rand()
 			j = int(rnd_val*n_chars)+1   !an integer between 1 and n_chars
 			member(i:i) = chars(j:j)
 		end if
 	end do

 !********************************************************************
	end subroutine mutate
 !********************************************************************

 !***************************************************************************************************
 	end module evolve_routines
 !***************************************************************************************************

 !***************************************************************************************************
 	program evolve
 !***************************************************************************************************
 ! The main program
 !***************************************************************************************************
 	use evolve_routines

 	implicit none

 	!Tuning parameters:
 	integer,parameter :: seed = 12345             !random number generator seed
 	integer,parameter :: max_iter = 10000         !maximum number of iterations
 	integer,parameter :: population_size = 200    !size of the population
 	real,parameter    :: factor = 0.04            ![0,1] mutation factor
 	integer,parameter :: iprint = 5               !print every iprint iterations

 	!local variables:
 	integer :: i,iter
 	integer,dimension(1) :: i_best
 	character(len=len(targ)),dimension(population_size) :: population

 	!initialize random number generator:
 	call srand(seed)

 	!create initial population:
 	! [the first element of the population will hold the best member]
 	population(1) = 'PACQXJB CQPWEYKSVDCIOUPKUOJY'  !initial guess
 	iter=0

 	write(*,'(A10,A30,A10)') 'iter','best','fitness'
 	write(*,'(I10,A30,I10)') iter,population(1),fitness(population(1))

 	do

 		iter = iter + 1 !iteration counter

  		!write the iteration:
 		if (mod(iter,iprint)==0) write(*,'(I10,A30,I10)') iter,population(1),fitness(population(1))

 		!check exit conditions:
 		if ( iter>max_iter .or. fitness(population(1))==0 ) exit

 		!copy best member and mutate:
 		population = population(1)
 		do i=2,population_size
 			call mutate(population(i),factor)
 		end do

 		!select the new best population member:
 		! [the best has the lowest value]
 		i_best = minloc(fitness(population))
 		population(1) = population(i_best(1))

 	end do

 	!write the last iteration:
 	if (mod(iter,iprint)/=0) write(*,'(I10,A30,I10)') iter,population(1),fitness(population(1))

 	if (iter>max_iter) then
 		write(*,*) 'No solution found.'
 	else
 		write(*,*) 'Solution found.'
 	end if

 !***************************************************************************************************
 	end program evolve
 !***************************************************************************************************


c function pbinom.f
c
c source
c   Bevington, page 31.
c
c purpose
c   evaluate binomial probability coefficient
c
c usage
c   result = pbinom (nobs, ntotal, prob)
c
c description of parameters
c   nobs   - number of items observed
c   ntotal - total number of items
c   prob   - probability of observing each item
c
c subroutines and function subprograms required
c   factor (n)
c      calculates n factorial for integers
c
      function pbinom (nobs,ntotal,prob)
1     notobs=ntotal-nobs
2     pbinom=factor(ntotal)/(factor(nobs)*factor(notobs))*
     *       (prob**nobs)*(1.-prob)**notobs
      return
      end


submodule (interpolation) interpolation2d

implicit none (type, external)

contains

module procedure interp2

!! A 2D BILINEAR INTERPOLATION FUNCTION.  THIS VERSION ASSUMES
!! THAT THE LIST OF OUTPUT POINTS IS A 'FLAT LIST' RATHER THAN
!! DESCRIPTIVE OF A 2D MESHGRID.

real(wp) :: fx1ix2prev, fx1ix2next    !function estimates at x1i point  vs. at x2 interfaces
real(wp) :: slope

integer :: lx1,lx2,lxi,ix1,ix2,ixi
integer :: ix10,ix1fin,ix20,ix2fin

lx1=size(x1,1)
lx2=size(x2,1)
lxi=size(x1i,1)    !only one size since this a flat list of grid points

do ixi=1,lxi
  !find the x1 'bin' for this point; i.e. find ix1 s.t. xi(ix1i) is between x(ix1-1) and x(ix1)
  ix10=1
  ix1=lx1/2
  ix1fin=lx1
  if (x1i(ixi)>=x1(1) .and. x1i(ixi)<=x1(lx1)) then    !in bounds
    do while(.not.(x1i(ixi)>=x1(ix1-1) .and. x1i(ixi)<=x1(ix1)))    !keep going until we are in the interval we want
      if (x1i(ixi)>=x1(ix10) .and. x1i(ixi)<=x1(ix1)) then    !left half (correct guess)
        ix1fin=ix1
      else    !wrong take the "right" half (har har)
        ix10=ix1
      end if
      ix1=(ix1fin+ix10)/2
      if (ix10==ix1) then
        ix1=lx1
      end if
    end do
  else if (x1i(ixi)<x1(1)) then
    ix1=1
  else
    ix1=lx1
  end if

  !find the x2 'bin' for this point; i.e. find ix2 s.t. x2i(ix2i) is between x2(ix2-1) and x2(ix2)
  ix20=1
  ix2=lx2/2
  ix2fin=lx2
  if (x2i(ixi)>=x2(1) .and. x2i(ixi)<=x2(lx2)) then    !in bounds
    do while(.not.(x2i(ixi)>=x2(ix2-1) .and. x2i(ixi)<=x2(ix2)))    !keep going until we are in the interval we want
      if (x2i(ixi)>=x2(ix20) .and. x2i(ixi)<=x2(ix2)) then    !left half (correct guess)
        ix2fin=ix2
      else    !wrong take the "right" half (har har)
        ix20=ix2
      end if
      ix2=(ix2fin+ix20)/2
      if (ix20==ix2) then
        ix2=lx2
      end if
    end do
  else if (x2i(ixi)<x2(1)) then
    ix2=1
  else
    ix2=lx2
  end if

  !execute interpolations in x1 for fixed values of x2 at this point
  if (ix1>1 .and. ix1<=lx1 .and. ix2>1 .and. ix2<=lx2) then   !interpolation
    !first the "prev" x2 value
    slope=(f(ix1,ix2-1)-f(ix1-1,ix2-1))/(x1(ix1)-x1(ix1-1))
    fx1ix2prev=f(ix1-1,ix2-1)+slope*(x1i(ixi)-x1(ix1-1))

    !now the "next" x2 value
    slope=(f(ix1,ix2)-f(ix1-1,ix2))/(x1(ix1)-x1(ix1-1))
    fx1ix2next=f(ix1-1,ix2)+slope*(x1i(ixi)-x1(ix1-1))

    !finally an interpolation in x2 to finish things off
    slope=(fx1ix2next-fx1ix2prev)/(x2(ix2)-x2(ix2-1))
    interp2(ixi)=fx1ix2prev+slope*(x2i(ixi)-x2(ix2-1))
  else
    interp2(ixi)=0
  end if
end do

!THERE IS SOME ISSUE WITH POINTS OUTSIDE INTERPOLANT DOMAIN - THIS IS A WORKAROUND UNTIL I CAN PIN DOWN THE EXACT PROBLEM
do ixi=1,lxi
if(x1i(ixi)<x1(1) .or. x1i(ixi)>x1(lx1) .or. x2i(ixi)<x2(1) .or. x2i(ixi)>x2(lx2)) then
  interp2(ixi)=0
end if
end do

end procedure interp2

module procedure interp2_plaid

!! A 2D BILINEAR INTERPOLATION FUNCTION.  THIS VERSION ASSUMES
!! A PLAID INTERPRETATION OF THE OUTPUT POINTS (I.E. THAT THEY
!! FORM A 2D MESHGRID RATHER THAN A FLAT LIST OF POINTS.
!!
!! MZ - this may not be used at all anymore, but kept
!! for potential future use???

real(wp) :: fx1ix2prev, fx1ix2next    !function estimates at x1i point  vs. at x2 interfaces
real(wp) :: slope

integer :: lx1,lx2,lx1i,lx2i,ix1,ix2,ix1i,ix2i
real(wp), dimension(1:size(x1i)) :: slicex1i
real(wp), dimension(1:size(x2i)) :: slicex2i

lx1=size(x1,1)
lx2=size(x2,1)
lx1i=size(x1i,1)
lx2i=size(x2i,1)

do ix2i=1,lx2i
  do ix1i=1,lx1i
    !find the x1 'bin' for this point; i.e. find ix1 s.t. xi(ix1i) is between x(ix1-1) and x(ix1)
    ix1=1
    do while(x1i(ix1i)>x1(ix1) .and. ix1<=lx1)
      ix1=ix1+1
    end do

    !find the x2 'bin' for this point; i.e. find ix2 s.t. x2i(ix2i) is between x2(ix2-1) and x2(ix2)
    ix2=1
    do while(x2i(ix2i)>x2(ix2) .and. ix2<=lx2)
      ix2=ix2+1
    end do

    !execute interpolations in x1 for fixed values of x2 at this point
    if (ix1>1 .and. ix1<=lx1 .and. ix2>1 .and. ix2<=lx2) then   !interpolation
      !first the "prev" x2 value
      slope=(f(ix1,ix2-1)-f(ix1-1,ix2-1))/(x1(ix1)-x1(ix1-1))
      fx1ix2prev=f(ix1-1,ix2-1)+slope*(x1i(ix1i)-x1(ix1-1))

      !now the "next" x2 value
      slope=(f(ix1,ix2)-f(ix1-1,ix2))/(x1(ix1)-x1(ix1-1))
      fx1ix2next=f(ix1-1,ix2)+slope*(x1i(ix1i)-x1(ix1-1))

      !finally an interpolation in x2 to finish things off
      slope=(fx1ix2next-fx1ix2prev)/(x2(ix2)-x2(ix2-1))
      interp2_plaid(ix1i,ix2i)=fx1ix2prev+slope*(x2i(ix2i)-x2(ix2-1))
    else
      interp2_plaid(ix1i,ix2i)=0
    end if
  end do
end do

end procedure interp2_plaid

end submodule interpolation2d


module advection_schemes

  implicit none

  contains

  ! ---------------------------------------------------------------------------
  !> Use a first-order centered advection scheme to calculate the advctive
  !! thickness tendency

  subroutine h_advec_1_centered(dhdt_advec, h, u, v, dx, dy, &
                                xlow, xhigh, ylow, yhigh, layers, OL)
    implicit none

    ! dhdt is evaluated at the centre of the grid box
    double precision, intent(out) :: dhdt_advec(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: h(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: u(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: v(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: dx, dy
    integer, intent(in) :: xlow, xhigh, ylow, yhigh, layers, OL

    integer i, j, k

    dhdt_advec = 0d0

    do k = 1, layers
      do j = ylow-OL+1, yhigh+OL-1
        do i = xlow-OL+1, xhigh+OL-1
          dhdt_advec(i,j,k) = &
              - ((h(i,j,k)+h(i+1,j,k))*u(i+1,j,k) &
               - (h(i-1,j,k)+h(i,j,k))*u(i,j,k))/(dx*2d0) & ! d(hu)/dx
              - ((h(i,j,k)+h(i,j+1,k))*v(i,j+1,k) &
               - (h(i,j-1,k)+h(i,j,k))*v(i,j,k))/(dy*2d0)   ! d(hv)/dy
        end do
      end do
    end do

    return
  end subroutine h_advec_1_centered

  ! ---------------------------------------------------------------------------
  !> Use a first-order upwind advection scheme to calculate the advctive
  !! thickness tendency

  subroutine h_advec_1_upwind(dhdt_advec, h, u, v, dx, dy, &
                              xlow, xhigh, ylow, yhigh, layers, OL)
    implicit none

    ! dhdt is evaluated at the centre of the grid box
    double precision, intent(out) :: dhdt_advec(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: h(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: u(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: v(xlow-OL:xhigh+OL, ylow-OL:yhigh+OL, layers)
    double precision, intent(in)  :: dx, dy
    integer, intent(in) :: xlow, xhigh, ylow, yhigh, layers, OL

    integer i, j, k

    dhdt_advec = 0d0

    do k = 1, layers
      do j = ylow-OL+1, yhigh+OL-1
        do i = xlow-OL+1, xhigh+OL-1
          dhdt_advec(i,j,k) = &
              ! d(hu)/dx
              ! u(i+1,j,k) point
              - ( h(i+1,j,k)*(u(i+1,j,k) - abs(u(i+1,j,k)))/2d0 & ! u < 0
                + h(i,j,k)*(u(i+1,j,k)   + abs(u(i+1,j,k)))/2d0 & ! u > 0
                ! u(i,j,k) point
                - (h(i,j,k)*(u(i,j,k)   - abs(u(i,j,k)))/2d0 & ! u < 0
                  + h(i-1,j,k)*(u(i,j,k) + abs(u(i,j,k)))/2d0) & ! u > 0
                )/dx &

              ! d(hv)/dx
              ! v(i,j+1,k) point
              - ( h(i,j+1,k)*(v(i,j+1,k) - abs(v(i,j+1,k)))/2d0 & ! v < 0
                + h(i,j,k)*(v(i,j+1,k)   + abs(v(i,j+1,k)))/2d0 & ! v > 0
                ! v(i,j,k) point
                - ( h(i,j,k)*(v(i,j,k)   - abs(v(i,j,k)))/2d0 & ! v < 0
                  + h(i,j-1,k)*(v(i,j,k) + abs(v(i,j,k)))/2d0) & ! v > 0
                )/dy
        end do
      end do
    end do

    return
  end subroutine h_advec_1_upwind

end module advection_schemes


!> @brief       A test for the calculation of the cumulative distribution function
program test_cdf
    use fplus_statistic
    use fplus_strings
    use fplus_array_tools
    implicit none

    ! some example data array
    real (kind=8), dimension(:), allocatable :: a
    integer :: i
    real (kind=8) :: b, c
    a = (/9,8,7,6,5,4,3,1,2,1,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,2,2,2,4,4,5,7,8,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,7,7,7,7,7,7,7,7,7,8,8,8,8/)

    do i = 1, 9
        b = cumulative_distribution_function(a, real(i,8))
        c = invers_cumulative_distribution_function(a, b)
        write(*, "(A,I1,A,F4.2,A,F4.2,A,I1)") "cdf(", i, ") = ", b, "  =>  cdf^-1(", b, ") = ", int(c)
    end do

end program