#####################
# Utility Functions #
#####################

"""
Kronecker Delta Function.

Returns 1 if inputs are equal returns 0 otherwise.

Arguments:
- `a::Real`: First input argument
- `b::Real`: Second input argument

Returns:
- `delta:Integer`: Kronecker delta result.
"""
function kron(a::Real, b::Real)
    return a == b ? 1 : 0
end

"""
Internal helper function to aid in denormalization of gravity field coefficients.

Provides method co compute the factorial ratio: (n-m)!/(n+m)!

Arguments:
- `n::Real`: Gravity model degree, n.
- `m::Real`: Gravity model order, m.

Returns:
- `p::Real`: Factorial product
"""
function facprod(n::Int, m::Int)
    p = 1.0

    for i in (n-m+1):(n+m+1) # Is this correct? Should it be (n-m+1):(n+m)
        p = p/i
    end

    return p
end

###########
# Gravity #
###########

export accel_point_mass
"""
Computes the acceleration of a satellite caused by a point-mass approximation
of the central body. Returns the acceleration vector of the satellite.

Assumes the satellite is much, much less massive than the central body.

Arguments:
- `r_sat::Array{<:Real, 1}`: satellite position in a commonn inertial frame [m]
- `r_body::Array{<:Real, 1}`: position of body in a commonn inertial frame [m]
- `GM::Array{<:Real, 1}`: gravitational coeffient of attracting body [m^3/s^2] Default: SatelliteDynamics.GM_EARTH)
(Default: SatelliteDynamics.GM_EARTH

Return:
- `a::Array{<:Real, 1}`: Acceleration in X, Y, and Z inertial directions [m/s^2]
"""
function accel_point_mass(r_sat::Array{<:Real, 1}, r_body::Array{<:Real, 1}, gm_body::Real=GM_EARTH)
    # Restrict inputs to position only
    r_sat  = r_sat[1:3]
    r_body = r_body[1:3]

    # Relative position vector of satellite w.r.t. the attraching body
    d = r_sat - r_body

    # Acceleration
    a = -gm_body * (d/norm(d)^3 + r_body/norm(r_body)^3)

    return a
end

"""
Computes the acceleration on a satellite caused by a point-mass approximation
of a massive body. Returns the acceleration vector of the satellite.

Arguments:
- `r_sat::Array{<:Real, 1}`: satellite position in the inertial frame [m]
- `GM::Array{<:Real, 1}`: gravitational coeffient of attracting body [m^3/s^2] Default: SatelliteDynamics.GM_EARTH)
(Default: SatelliteDynamics.GM_EARTH

Return:
- `a::Array{<:Real, 1}`: Acceleration in X, Y, and Z inertial directions [m/s^2]
"""
function accel_point_mass(x::Array{<:Real, 1}, gm_body::Real=GM_EARTH)
    # Restrict inputs to position only. Considered in body frame
    r  = x[1:3]

    # Acceleration
    a = -gm_body * r/norm(r)^3

    return a
end

"""
Compute the gravitational acceleration at a model given a spherical harmonic
gravity field model.

Arguments:
- `r::Array{<:Real, 1}`: Position of the point in the body (field) fixed frame. [m]
- `coef::Array{<:Real, 2}`: Gravity coefficients stored in dense matrix form. C_nm terns are stored along rows indexed from C_00 in coef[1, 1] to C_nm in coef[n+1, m+1]. S_nm terms are stored along matrix columns with S_n0 stored in coef[0, n+1]
- `n_max::Integer`: Maximum degree coefficient to use in expansion
- `m_max::Integer`: Maximum order coefficient to use in the expansion. Must be less than the degree.
- `r_ref::Real`: Reference distance of the gravity field.
- `GM::Real`: Gravitational constant of central body
- `normralized::Bool`: Whether the input gravity field coefficients are normalized coefficients (Default: true)

Returns:
- `a::Array{<:Real, 1}`: Acceleration in X, Y, and Z inertial directions [m/s^2]
"""
function spherical_harmonic_gravity(r::Array{<:Real, 1}, coef::Array{<:Real, 2}, n_max::Integer, m_max::Integer, r_ref::Real, GM::Real; normalized::Bool=true)
    # Intermediate computations
    r_sqr = dot(r, r)
    rho   = r_ref^2/r_sqr
    x0    = r_ref * r[1] / r_sqr
    y0    = r_ref * r[2] / r_sqr
    z0    = r_ref * r[3] / r_sqr

    # Initialize Intermediary Matrices
    V = zeros(eltype(r), n_max+2, n_max+2)
    W = zeros(eltype(r), n_max+2, n_max+2)

    # Calculate zonal terms V(n, 0). Set W(n,0)=0.0
    V[0+1, 0+1] = r_ref /sqrt(r_sqr)
    W[0+1, 0+1] = 0.0

    V[1+1, 0+1] = z0 * V[0+1, 0+1]
    W[1+1, 0+1] = 0.0

    for n in 2:(n_max+1)
        V[n+1, 0+1] = ((2*n-1)*z0*V[n+1-1, 0+1] - (n-1)*rho*V[n+1-2,0+1])/n
        W[n+1, 0+1] = 0.0
    end

    # Calculate tesseral and sectoral terms
    for m in 1:m_max+1
        # Calculate V(m,m) to V(n_max+1,m)
        V[m+1, m+1] = (2*m-1)*(x0*V[m+1-1, m+1-1] - y0*W[m+1-1, m+1-1])
        W[m+1, m+1] = (2*m-1)*(x0*W[m+1-1, m+1-1] + y0*V[m+1-1, m+1-1])

        if m <= m_max
            V[m+1+1, m+1] = (2*m+1)*z0*V[m+1,m+1]
            W[m+1+1, m+1] = (2*m+1)*z0*W[m+1,m+1]
        end

        for n in (m+2):(n_max+1)
            V[n+1,m+1] = ((2*n-1)*z0*V[n+1-1,m+1]-(n+m-1)*rho*V[n+1-2,m+1])/(n-m)
            W[n+1,m+1] = ((2*n-1)*z0*W[n+1-1,m+1]-(n+m-1)*rho*W[n+1-2,m+1])/(n-m)
        end
    end

    # Calculate accelerations
    ax = 0.0
    ay = 0.0
    az = 0.0

    for m in 0:m_max
        for n in m:n_max
            C = 0.0
            S = 0.0
            if m == 0
                # Denormalize Coeeficients
                if normalized
                    N = sqrt(2*n+1)
                    C = N*coef[n+1, 0+1]
                else
                    C = coef[n+1, 0+1]
                end

                ax -= C*V[n+1+1, 1+1]
                ay -= C*W[n+1+1, 1+1]
                az -= (n+1)*C*V[n+1+1, 0+1]

            else
                if normalized
                    N = sqrt((2 - kron(0,m))*(2*n+1)*facprod(n,m))
                    C = N*coef[n+1,   m+1]
                    S = N*coef[m+1-1, n+1]
                else
                    C = coef[n+1,   m+1]
                    S = coef[m+1-1, n+1]
                end

                fac =  0.5 * (n-m+1)*(n-m+2)
                ax += +0.5 * (-C * V[n+1+1, m+1+1] - S * W[n+1+1, m+1+1])
                      +fac * (+C * V[n+1+1, m+1-1] + S * W[n+1+1, m+1-1])
                ay += +0.5 * (-C * W[n+1+1, m+1+1] + S * V[n+1+1, m+1+1])
                      +fac * (-C * W[n+1+1, m+1-1] + S * V[n+1+1, m+1-1])
                az += (n-m+1)*(-C*V[n+1+1, m+1] - S*W[n+1+1, m+1])
            end
        end
    end

    a = (GM / (r_ref^2)) * [ax, ay, az]

    return a
end

export accel_gravity
"""
Computes the accleration caused by Earth gravity as modeled by a spherical
harmonic gravity field.

Arguments:
- `r_sat::Array{<:Real, 1}`: Satellite position in the inertial frame [m]
- `R_eci_ecef::Array{<:Real, 2}`: Rotation matrix transforming a vector from the inertial to body-fixed reference frames.
- `n_max::Integer`: Maximum degree coefficient to use in expansion
- `m_max::Integer`: Maximum order coefficient to use in the expansion. Must be less than the degree.

Return:
- `a::Array{<:Real, 1}`: Gravitational acceleration in X, Y, and Z inertial directions [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.56-68.
"""
function accel_gravity(x::Array{<:Real, 1}, R_eci_ecef::Array{<:Real, 2}, n_max::Int=20, m_max::Int=20)

    # Check Limits of Gravity Field
    if n_max > GRAVITY_MODEL.n_max
        error("Requested gravity model order $n_max is larger than the maximum order of the model maximum: ", GRAVITY_MODEL.n_max)
    end

    if m_max > GRAVITY_MODEL.m_max
        error("Requested gravity model degree $m_max is larger than the maximum degree of the model maximum: ", GRAVITY_MODEL.m_max)
    end

    # Gravitational parameters of primary body
    GM    = GRAVITY_MODEL.GM
    R_ref = GRAVITY_MODEL.R

    # Body-fixed position
    r_bf = R_eci_ecef * x[1:3]

    # Compute spherical harmonic acceleration
    a_ecef = spherical_harmonic_gravity(r_bf, GRAVITY_MODEL.data, n_max, m_max, R_ref, GM, normalized=GRAVITY_MODEL.normalized)

    # Inertial acceleration
    a_eci = R_eci_ecef' * a_ecef

    # Finished
    return a_eci
end

#########################
# Planetary Ephemerides #
#########################

export sun_position
"""
Compute the Sun's position in the EME2000 inertial frame through the use
of low-precision analytical functions.

Argument:
- `epc::Epoch`: Epoch

Returns:
- `r_sun::Array{<:Real, 1}`: Position vector of the Sun in the Earth-centered inertial fame.

Notes:
1. The EME2000 inertial frame is for most purposes equivalent to the GCRF frame.

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.70-73.
"""
function sun_position(epc::Epoch)
    # Constants
    mjd_tt  = mjd(epc, tsys="TT")      # MJD of epoch in TT
    epsilon = 23.43929111*pi/180.0     # Obliquity of J2000 ecliptic
    T       = (mjd_tt-MJD2000)/36525.0 # Julian cent. since J2000

    # Variables

    # Mean anomaly, ecliptic longitude and radius
    M = 2.0*pi * modf(0.9931267 + 99.9973583*T)[1]                 # [rad]
    L = 2.0*pi * modf(0.7859444 + M/(2.0*pi) + (6892.0*sin(M)+72.0*sin(2.0*M)) / 1296.0e3)[1] # [rad]
    r = 149.619e9 - 2.499e9*cos(M) - 0.021e9*cos(2*M)           # [m]

    # Equatorial position vector
    p_sun = Rx(-epsilon) * [r*cos(L), r*sin(L), 0.0]

    return p_sun
end

export moon_position
"""
Compute the Moon's position in the EME2000 inertial frame through the use
of low-precision analytical functions.

Argument:
- `epc::Epoch`: Epoch

Returns:
- `r_moon::Array{<:Real, 1}`: Position vector of the Moon in the Earth-centered inertial fame.

Notes:
1. The EME2000 inertial frame is for most purposes equivalent to the GCRF frame.

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.70-73.
"""
function moon_position(epc::Epoch)
    # Constants
    mjd_tt  = mjd(epc, tsys="TT")      # MJD of epoch in TT
    epsilon = 23.43929111*pi/180.0     # Obliquity of J2000 ecliptic
    T       = (mjd_tt-MJD2000)/36525.0 # Julian cent. since J2000

    # Mean elements of lunar orbit
    L_0 =     modf(0.606433 + 1336.851344*T)[1] # Mean longitude [rev] w.r.t. J2000 equinox
    l   = 2.0*pi*modf(0.374897 + 1325.552410*T)[1] # Moon's mean anomaly [rad]
    lp  = 2.0*pi*modf(0.993133 +   99.997361*T)[1] # Sun's mean anomaly [rad]
    D   = 2.0*pi*modf(0.827361 + 1236.853086*T)[1] # Diff. long. Moon-Sun [rad]
    F   = 2.0*pi*modf(0.259086 + 1342.227825*T)[1] # Argument of latitude

    # Ecliptic longitude (w.r.t. equinox of J2000)
    dL = + 22640*sin(l) - 4586*sin(l-2*D) + 2370*sin(2*D) +  769*sin(2*l)
         - 668*sin(lp) - 412*sin(2*F) - 212*sin(2*l-2*D) - 206*sin(l+lp-2*D)
         + 192*sin(l+2*D) - 165*sin(lp-2*D) - 125*sin(D) - 110*sin(l+lp)
         + 148*sin(l-lp) - 55*sin(2*F-2*D)

    L = 2.0*pi * modf(L_0 + dL/1296.0e3)[1]  # [rad]

    # Ecliptic latitude
    S  = F + (dL+412*sin(2*F)+541*sin(lp)) * AS2RAD
    h  = F-2*D
    N  = - 526*sin(h) + 44*sin(l+h) - 31*sin(-l+h) - 23*sin(lp+h)
         + 11*sin(-lp+h) - 25*sin(-2*l+F) + 21*sin(-l+F)
    B  = (18520.0*sin(S) + N) * AS2RAD   # [rad]

    # Distance [m]
    r = + 385000e3 - 20905e3*cos(l) - 3699e3*cos(2*D-l) - 2956e3*cos(2*D)
        - 570e3*cos(2*l) + 246e3*cos(2*l-2*D) - 205e3*cos(lp-2*D)
        - 171e3*cos(l+2*D) - 152e3*cos(l+lp-2*D)

    # Equatorial coordinates
    p_moon = Rx(-epsilon) * [r*cos(L)*cos(B), r*sin(L)*cos(B), r*sin(B)]

    return p_moon
end

######################
# Third-Body Gravity #
######################

export accel_thirdbody_sun
"""
Computes the acceleration of a satellite in the inertial frame due to the
gravitational attraction of the Sun.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `r_sun::Array{<:Real, 1}`: Position of sun in inertial frame.

Return:
- `a::Array{<:Real, 1}`: Acceleration due to the Sun's gravity in the inertial frame [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.69-70.
"""
function accel_thirdbody_sun(x::Array{<:Real, 1}, r_sun::Array{<:Real, 1})
    # Acceleration due to sun point mass
    a_sun = accel_point_mass(x[1:3], r_sun, GM_SUN)

    return a_sun
end

function accel_thirdbody_sun(epc::Epoch, x::Array{<:Real, 1})
    # Compute solar position
    r_sun = sun_position(epc)

    # Acceleration due to sun point mass
    a_sun = accel_point_mass(x[1:3], r_sun, GM_SUN)

    return a_sun
end

export accel_thirdbody_moon
"""
Computes the acceleration of a satellite in the inertial frame due to the
gravitational attraction of the Moon.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `r_moon::Array{<:Real, 1}`: Position of moon in inertial frame.

Returns:
- `a::Array{<:Real, 1}`: Acceleration due to the Moon's gravity in the inertial frame [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.69-70.
"""
function accel_thirdbody_moon(x::Array{<:Real, 1}, r_moon::Array{<:Real, 1})
    # Acceleration due to moon point mass
    a_moon = accel_point_mass(x[1:3], r_moon, GM_MOON)

    return a_moon
end

function accel_thirdbody_moon(epc::Epoch, x::Array{<:Real, 1})
    # Compute solar position
    r_moon = moon_position(epc)

    # Acceleration due to moon point mass
    a_moon = accel_point_mass(x[1:3], r_moon, GM_MOON)

    return a_moon
end

####################
# Atmospheric Drag #
####################

export density_harris_priester
"""
Computes the local density using the Harris-Priester density model.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `r_sun::Array{<:Real, 1}`: Position of sun in inertial frame.

Returns:
- `rho:Float64`: Local atmospheric density [kg/m^3]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.89-91.
"""
function density_harris_priester(x::Array{<:Real, 1}, r_sun::Array{<:Real, 1})
    # Harris-Priester Constants
    hp_upper_limit =   1000.0          # Upper height limit [km]
    hp_lower_limit =    100.0          # Lower height limit [km]
    hp_ra_lag      = 0.523599          # Right ascension lag [rad]
    hp_n_prm       =        3          # Harris-Priester parameter
                                        # 2(6) low(high) inclination
    hp_N           = 50                # Number of coefficients

    # Height [km]
    hp_h = [100.0, 120.0, 130.0, 140.0, 150.0, 160.0, 170.0, 180.0, 190.0, 200.0,
            210.0, 220.0, 230.0, 240.0, 250.0, 260.0, 270.0, 280.0, 290.0, 300.0,
            320.0, 340.0, 360.0, 380.0, 400.0, 420.0, 440.0, 460.0, 480.0, 500.0,
            520.0, 540.0, 560.0, 580.0, 600.0, 620.0, 640.0, 660.0, 680.0, 700.0,
            720.0, 740.0, 760.0, 780.0, 800.0, 840.0, 880.0, 920.0, 960.0,1000.0]

    # Minimum density [g/km^3]
    hp_c_min = [4.974e+05, 2.490e+04, 8.377e+03, 3.899e+03, 2.122e+03, 1.263e+03,
                8.008e+02, 5.283e+02, 3.617e+02, 2.557e+02, 1.839e+02, 1.341e+02,
                9.949e+01, 7.488e+01, 5.709e+01, 4.403e+01, 3.430e+01, 2.697e+01,
                2.139e+01, 1.708e+01, 1.099e+01, 7.214e+00, 4.824e+00, 3.274e+00,
                2.249e+00, 1.558e+00, 1.091e+00, 7.701e-01, 5.474e-01, 3.916e-01,
                2.819e-01, 2.042e-01, 1.488e-01, 1.092e-01, 8.070e-02, 6.012e-02,
                4.519e-02, 3.430e-02, 2.632e-02, 2.043e-02, 1.607e-02, 1.281e-02,
                1.036e-02, 8.496e-03, 7.069e-03, 4.680e-03, 3.200e-03, 2.210e-03,
                1.560e-03, 1.150e-03]

    # Maximum density [g/km^3]
    hp_c_max = [4.974e+05, 2.490e+04, 8.710e+03, 4.059e+03, 2.215e+03, 1.344e+03,
                8.758e+02, 6.010e+02, 4.297e+02, 3.162e+02, 2.396e+02, 1.853e+02,
                1.455e+02, 1.157e+02, 9.308e+01, 7.555e+01, 6.182e+01, 5.095e+01,
                4.226e+01, 3.526e+01, 2.511e+01, 1.819e+01, 1.337e+01, 9.955e+00,
                7.492e+00, 5.684e+00, 4.355e+00, 3.362e+00, 2.612e+00, 2.042e+00,
                1.605e+00, 1.267e+00, 1.005e+00, 7.997e-01, 6.390e-01, 5.123e-01,
                4.121e-01, 3.325e-01, 2.691e-01, 2.185e-01, 1.779e-01, 1.452e-01,
                1.190e-01, 9.776e-02, 8.059e-02, 5.741e-02, 4.210e-02, 3.130e-02,
                2.360e-02, 1.810e-02]

    # Satellite height
    geod   = sECEFtoGEOD(x[1:3], use_degrees=true)
    height = geod[3]/1.0e3 # height in [km]

    # Exit with zero density outside height model limits
    if height > hp_upper_limit || height < hp_lower_limit
        return 0.0
    end

    # Sun right ascension, declination
    ra_sun  = atan( r_sun[2], r_sun[1] )
    dec_sun = atan( r_sun[3], sqrt( r_sun[1]^2 + r_sun[2]^2 ) )

    # Unit vector u towards the apex of the diurnal bulge
    # in inertial geocentric coordinates
    c_dec = cos(dec_sun)
    u     = [c_dec * cos(ra_sun + hp_ra_lag),
             c_dec * sin(ra_sun + hp_ra_lag),
             sin(dec_sun)]

    # Cosine of half angle between satellite position vector and
    # apex of diurnal bulge
    c_psi2 = 0.5 + 0.5 * dot(x[1:3], u)/norm(x[1:3])

    # Height index search and exponential density interpolation
    ih = 0                            # section index reset
    for i in 1:hp_N                   # loop over N_Coef height regimes
        if height >= hp_h[i] && height < hp_h[i+1]
            ih = i                    # ih identifies height section
            break
        end
    end

    h_min = ( hp_h[ih] - hp_h[ih+1] )/log( hp_c_min[ih+1]/hp_c_min[ih] )
    h_max = ( hp_h[ih] - hp_h[ih+1] )/log( hp_c_max[ih+1]/hp_c_max[ih] )

    d_min = hp_c_min[ih] * exp( (hp_h[ih]-height)/h_min )
    d_max = hp_c_max[ih] * exp( (hp_h[ih]-height)/h_max )

    # Density computation
    density = d_min + (d_max-d_min) * c_psi2^hp_n_prm

    # Convert from g/km^3 to kg/m^3
    density *= 1.0e-12

    # Finished
    return density
end

function density_harris_priester(epc::Epoch, x::Array{<:Real, 1})
    r_sun = sun_position(epc)
    return density_harris_priester(x, r_sun)
end

export accel_drag
"""
Computes the perturbing, non-conservative acceleration caused by atmospheric
drag assuming that the ballistic properties of the spacecraft are captured by
the coefficient of drag.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `rho::Real`: atmospheric density [kg/m^3]
- `mass::Real`: Spacecraft mass [kg]
- `area::Real`: Wind-facing cross-sectional area [m^2]
- `Cd::Real`: coefficient of drag [dimensionless]
- `T::Array{<:Real, 2}`: Rotation matrix from the inertial to the true-of-date frame

Return:
- `a::Array{<:Real, 1}`: Acceleration due to drag in the X, Y, and Z inertial directions. [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.83-86.
"""
function accel_drag(x::Array{<:Real, 1}, rho::Real, mass::Real, area::Real, Cd::Real, T::Array{<:Real, 2})

    # Constants
    omega = [0, 0, OMEGA_EARTH]

    # Position and velocity in true-of-date system
    r_tod = T * x[1:3]
    v_tod = T * x[4:6]

    # Velocity relative to the Earth's atmosphere
    v_rel = v_tod - cross(omega, r_tod)
    v_abs = norm(v_rel)

    # Acceleration
    a_tod  = -0.5*Cd*(area/mass)*rho*v_abs*v_rel
    a_drag = T' * a_tod

    return a_drag
end

############################
# Solar Radiation Pressure #
############################

export eclipse_cylindrical
"""
Computes the illumination fraction of a satellite in Earth orbit using a
cylindrical Earth shadow model.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `r_sun::Array{<:Real, 1}`: Position of sun in inertial frame.

Return:
- `nu::Float64`: Illumination fraction (0 <= nu <= 1). nu = 0 means spacecraft in complete shadow, nu = 1 mean spacecraft fully illuminated by sun.

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.80-83.
"""
function eclipse_cylindrical(x::Array{<:Real, 1}, r_sun::Array{<:Real, 1})
    # Satellite inertial position
    r = x[1:3]

    # Sun-direction unit-vector
    e_sun = r_sun / norm(r_sun)

    # Projection of spacecraft position
    s = dot(r, e_sun)

    # Compute illumination
    nu = 0.0
    if s/norm(s) >= 1.0 || norm(r - s*e_sun) > R_EARTH
        nu = 1.0
    end

    return nu
end

function eclipse_cylindrical(epc::Epoch, x::Array{<:Real, 1})
    r_sun = sun_position(epc)
    return eclipse_cylindrical(x, r_sun)
end

export eclipse_conical
"""
Computes the illumination fraction of a satellite in Earth orbit using a
conical Earth shadow model.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]
- `r_sun::Array{<:Real, 1}`: Position of sun in inertial frame.

Return:
- `nu::Float64`: Illumination fraction (0 <= nu <= 1). nu = 0 means spacecraft in complete shadow, nu = 1 mean spacecraft fully illuminated by sun.

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.80-83.
"""
function eclipse_conical(x::Array{<:Real, 1}, r_sun::Array{<:Real, 1})
    # Satellite inertial position
    r = x[1:3]

    # Occultation Geometry
    a = asin(R_SUN/norm(r_sun - r))
    b = asin(R_EARTH/norm(r))
    c = acos(dot(r, r_sun-r)/(norm(r)*norm(r_sun-r)) )

    e_sun = r_sun / norm(r_sun)

    # Test Occulation Conditions
    nu = 0.0
    if abs(a - b) < c && c < (a + b)
        # Partial occultation

        xx = (c^2 + a^2 - b^2)/(2*c)
        yy = sqrt(a^2 - xx^2)
        A  = a^2 * acos(xx/a) + b^2 * acos((c-xx)/b) - c * yy

        nu = 1 - A/(pi*a^2)
    elseif (a + b) <= c
        # No occultation
        nu = 1.0
    else
        # Full occultation
        nu = 0.0
    end

    return nu
end

function eclipse_conical(epc::Epoch, x::Array{<:Real, 1})
    r_sun = sun_position(epc)
    return eclipse_conical(x, r_sun)
end

export accel_srp
"""Computes the perturbing acceleration due to direct solar radiation
pressure assuming the reflecting surface is a flat plate pointed directly at
the Sun.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]

Returns:
- `a::Array{<:Real, 1}`: Satellite acceleration due to solar radiation pressure [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.77-79.
"""
function accel_srp(x::Array{<:Real, 1}, r_sun::Array{<:Real, 1}, mass::Real=0, area::Real=0, CR::Real=1.8, p0::Real=P_SUN, au::Real=AU)
    # Spacecraft position vector
    r = x[1:3]

    # Relative position vector of spacecraft w.r.t. Sun
    d = r - r_sun

    # Acceleration due to moon point mass
    a_srp = d * (CR*(area/mass)*p0*AU^2 / norm(d)^3)

    # Return
    return a_srp
end

##############
# Relativity #
##############

export accel_relativity
"""Computes perturbation accleration of a satellite in the Inertial frame
due to the combined effects of special and general relativity.

Arguments:
- `x::Array{<:Real, 1}`: Satellite Cartesean state in the inertial reference frame [m; m/s]

Returns:
- `a::Array{<:Real, 1}`: Satellite acceleration due to relativity. [m/s^2]

References:
1. O. Montenbruck, and E. Gill, _Satellite Orbits: Models, Methods and Applications_, 2012, p.110-112.
"""
function accel_relativity(x::Array{<:Real, 1})
    # Extract state variables
    r = x[1:3]
    v = x[4:6]

    # Intermediate computations
    norm_r = norm(r)
    r2     = norm_r^2

    norm_v = norm(v)
    v2     = norm_v^2

    c  = C_LIGHT
    c2 = c^2

    # Compute unit vectors
    er = r/norm_r
    ev = v/norm_v

    # Compute perturbation and return
    a_rel = GM_EARTH/r2 * ( (4*GM_EARTH/(c2*norm_r) - v2/c2)*er + 4*v2/c2*dot(er, ev)*ev)

    return a_rel
end


### A Pluto.jl notebook ###
# v0.14.1

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ 37b018c4-34b0-11eb-234d-4fbeaa683873
using LinearAlgebra, SparseArrays, Plots, DifferentialEquations

# ╔═╡ baed3ace-34ad-11eb-0cf6-19357238b1de
md"## Heat equation with Dirichlet boundary conditions

The domain is given by the one-dimensional rod $\Omega = (0,L)$ and the time by $t \in (0,T).$ The rod has the physical properties
- thermal conductivity $\lambda$,
- specific heat capacity $c$,
- mass density $\rho$.

### Heat Equation

The diffusivity constant is given by $\alpha = \frac{\lambda}{c \rho}$. The heat equation is noted as

$\dot{\vartheta}(t,x) = \alpha \frac{\partial^2}{\partial x^2} \vartheta(t,x)$

for $(t,x) \in (0,T) \times \Omega$ with initial condition

$\vartheta(0,x) = \vartheta_{0}(x)$

for $x \in \overline{\Omega}$ and boundary conditions  $\qquad \vartheta(\cdot,0) = \vartheta(\cdot,L) = 0$.
"

# ╔═╡ 8f5d25dc-34ae-11eb-2aa7-39e601f648a1
md"#### Code: implementing physical constants"

# ╔═╡ 8f493cca-34ae-11eb-3e60-31ab68674568
# Length of rod
L = 0.5

# ╔═╡ 8f149f6a-34ae-11eb-01f2-5983e256a7de
# Thermal conductivity
λ = 45.0

# ╔═╡ 8efa0286-34ae-11eb-1952-e790d141e19b
# Specific heat capacity
c = 480.0

# ╔═╡ 8ecb6bba-34ae-11eb-2c16-819675412725
# Mass density
ρ = 7800.0

# ╔═╡ e6433430-34af-11eb-318d-2199461e5ec9
# Thermal diffusivity
α = λ/(c*ρ)

# ╔═╡ 37c91fae-34b0-11eb-0f82-639173602ac0
md"### Spatial approximation

The one-dimensional rod is discretized as a one-dimensional grid with N points and the finite discretization is noted by

$\Delta x = \frac{L}{N-1}.$

The resulting grid points have the position $x^{0} = 0$, $x^{n} = n~\Delta x$ and $x^{N-1} = L$. The second order derivative is approximated using the [Taylor series](https://en.wikipedia.org/wiki/Taylor_series) as

$\left. \frac{\partial^2 f(x)}{\partial x^2} \right\rvert_{x = \tilde{x}} \approx \frac{1}{\Delta x^2} \left[ f(\tilde{x} - \Delta x) - 2 f(\tilde{x}) + f(\tilde{x} + \Delta x)  \right].$

This [Finite Difference](https://en.wikipedia.org/wiki/Finite_difference_method) scheme is written in matrix-vector form as

$\frac{\partial^2 \vartheta(t,x)}{\partial x^2} \approx \frac{1}{\Delta x^2}
\begin{pmatrix}
-2 & 1 & 0 & \cdots &  & 0 \\
1 & -2 & 1 & 0  & \cdots & \vdots \\
0 & 1 & -2 & 1 & \ddots &  \\
 & & \ddots & \ddots & \ddots &  & \\
& & & 1 & -2 & 1 \\
& & &  & 1 & -2 \\
\end{pmatrix}
~
\begin{pmatrix}
\vartheta(t, x^{0}) \\
\vartheta(t, x^{1}) \\
\vdots \\
\vartheta(t, x^{N-1}) \\
\end{pmatrix}.$
"

# ╔═╡ 65049b1a-3549-11eb-33ee-9dfae609164d
# Number of grid elements
N = 101

# ╔═╡ a36fd440-3549-11eb-0e93-0d36244400ef
# Finite discretization
Δx = L/(N-1)

# ╔═╡ c18a976a-3549-11eb-2991-799f558e4493
# 1-dimensional grid
xspan = 0 : Δx : L

# ╔═╡ d7d5be46-3549-11eb-3a12-afab5ab4bce5
# Diffusion matrix
M = spdiagm(-1 => ones(N-1), 0 => -2*ones(N), 1 => ones(N-1));

# ╔═╡ 045066e2-354a-11eb-164b-09fc7095de47
Matrix(M)[1:5,1:5]

# ╔═╡ 37822ec8-34b0-11eb-175d-7745382c2955
md"### Boundary conditions

The Dirichlet boundary condition states that the value at both sides is fixed as

$\qquad \vartheta(\cdot,0) = \vartheta(\cdot, x^{0}) = 0$

on the left boundary and

$\vartheta(\cdot,L) = \vartheta(\cdot, x^{N-1})  = 0$

on the right boundary. This means, there is no dynamical (e.g. diffusive) behaviour at both boundaries and thus the first and the last row of the diffusion matrix M is set to 0. "

# ╔═╡ 3768b7e0-34b0-11eb-0772-f5f4c95cf8f5
# First row
M[1,1] = M[1,2] = 0;

# ╔═╡ 37529500-34b0-11eb-1ee4-b5837bea23c9
# Last row
M[end,end] = M[end,end-1] = 0;

# ╔═╡ 373652f0-34b0-11eb-2fea-4f14b3ee9140
md"### Heat equation as ODE

After the spatial approximation the heat equation has a form of

$\dot{\theta}(t) = \frac{\alpha}{\Delta x^2} ~ M ~ \theta(t)$

with M as diffusion matrix and $\quad \theta(t) = \left( \vartheta(t, x^{0}), \cdots, \vartheta(t, x^{N-1}) \right)^{\top}$. This is an ordinary differential equation (ODE) that can be solved with common solvers like forward Euler method or Runge-Kutta scheme."

# ╔═╡ 371c6d52-34b0-11eb-12ab-a51e10fe33c5
# Heat Equation as ODE
function heat_eq(dθ, θ, p, t)
    return dθ .= (1/Δx^2) * α * M * θ
end

# ╔═╡ 37013e12-34b0-11eb-3d23-9d43ca3f06a3
md"### Initial Conditions

The initial data of the original heat equation is assumed as $\quad \vartheta_{0}(x) := m \left[ L~x - x^2 \right]~$ with  $m > 0$. This function is approximated to gain the initial conditions of the ODE

$\theta(0) = \left( \vartheta_{0}(x^{0}), \cdots , \vartheta_{0}(x^{N-1}) \right)^{\top}.$
"

# ╔═╡ 78b7fa20-354c-11eb-3754-d5d515d0a2d5
#Initial conditions: θ(0) = m * (L * x - x^2)
function initial_data(x, params)
    # α = params[1] # unused here
    L = params[2]
    m = params[3]

    return m*( L * x - x^2)
end

# ╔═╡ 77906a42-354c-11eb-397e-9dca847ab964
# Initial heat distribution
θ₀ = zeros(N);

# ╔═╡ 93696cf4-354d-11eb-0af3-9b27a8186fbd
# Amplification of initial data
@bind amp html"<input type='range' min='0' max='100' step='1'>"

# ╔═╡ a91c8b80-354d-11eb-03ec-8f4386a69708
m = amp

# ╔═╡ 86f0f21c-354d-11eb-2a32-299183f9118b
# Parameters
param = [α, L, amp]

# ╔═╡ 666a8878-354d-11eb-22dc-e94a72ede22d
let

	for i = 1 : length(xspan)
		θ₀[i] = initial_data(xspan[i], param)
	end

	plot(xspan, θ₀, xlabel="Position x", ylabel="Temperature", legend=false)
end

# ╔═╡ cfc0e592-354d-11eb-2b06-458d198c38e5
md"### Numerical stability

Before the approximated heat equation can be simulated, the [numerical stability](https://en.wikipedia.org/wiki/Numerical_stability) has to be proven with the [von Neumann stability analysis](https://en.wikipedia.org/wiki/Von_Neumann_stability_analysis).

In case of the forward Euler method one finds the approach

$\vartheta(t^{k+1}, x^{n}) = \vartheta(t^{k}, x^{n}) + \alpha \frac{\Delta t}{\Delta x^2} \left( \vartheta(t^{k}, x^{n-1}) - 2 ~ \vartheta(t^{k}, x^{n}) + \vartheta(t^{k}, x^{n+1}) \right)$

or equivalent

$\vartheta(t^{k+1}, x^{n}) = \left(1 - 2 \alpha \frac{\Delta t}{\Delta x^2} \right) \vartheta(t^{k}, x^{n}) + \alpha \frac{\Delta t}{\Delta x^2}  \left( \vartheta(t^{k}, x^{n-1}) + \vartheta(t^{k}, x^{n+1}) \right).$

The sampling time $\Delta t$ has to be chosen to guarantee

$\left(1 - 2 \alpha \frac{\Delta t}{\Delta x^2} \right) > 0.$

Therefore, the upper limit of the sampling time is given by

$\Delta t < \frac{1}{2} \frac{\Delta x^2}{\alpha}.$
"

# ╔═╡ 25789968-3552-11eb-1e25-35c0d1b1c461
# Upper limit of sampling time
ul = 0.5 * Δx^2 / α

# ╔═╡ 56daee6e-3554-11eb-37cd-49ad4ac56805
# Sampling time
Δt = 0.8

# ╔═╡ 56bb225a-3554-11eb-0062-6d03b8ea772f
# time discretization
tspan = (0.0, 1000.0)

# ╔═╡ 569a20f0-3554-11eb-0479-f1e583620682
md"### Code: Solving the ODE"

# ╔═╡ 56790ed8-3554-11eb-3c51-b9b31a4c340e
# Solving the ODE
let
	prob = ODEProblem( heat_eq, θ₀, tspan ) # ODE Problem

	sol = solve(prob,Euler(),dt=Δt,progress=true, save_everystep=false,
				save_start=true) # Solving the ODE

	plot(xspan, sol.u[2], xlabel = "Position x", ylabel="Temperature", legend=false)
end

# ╔═╡ Cell order:
# ╟─baed3ace-34ad-11eb-0cf6-19357238b1de
# ╟─8f5d25dc-34ae-11eb-2aa7-39e601f648a1
# ╠═8f493cca-34ae-11eb-3e60-31ab68674568
# ╠═8f149f6a-34ae-11eb-01f2-5983e256a7de
# ╠═8efa0286-34ae-11eb-1952-e790d141e19b
# ╠═8ecb6bba-34ae-11eb-2c16-819675412725
# ╠═e6433430-34af-11eb-318d-2199461e5ec9
# ╟─37c91fae-34b0-11eb-0f82-639173602ac0
# ╠═37b018c4-34b0-11eb-234d-4fbeaa683873
# ╠═65049b1a-3549-11eb-33ee-9dfae609164d
# ╠═a36fd440-3549-11eb-0e93-0d36244400ef
# ╠═c18a976a-3549-11eb-2991-799f558e4493
# ╠═d7d5be46-3549-11eb-3a12-afab5ab4bce5
# ╠═045066e2-354a-11eb-164b-09fc7095de47
# ╟─37822ec8-34b0-11eb-175d-7745382c2955
# ╠═3768b7e0-34b0-11eb-0772-f5f4c95cf8f5
# ╠═37529500-34b0-11eb-1ee4-b5837bea23c9
# ╟─373652f0-34b0-11eb-2fea-4f14b3ee9140
# ╠═371c6d52-34b0-11eb-12ab-a51e10fe33c5
# ╟─37013e12-34b0-11eb-3d23-9d43ca3f06a3
# ╠═78b7fa20-354c-11eb-3754-d5d515d0a2d5
# ╠═77906a42-354c-11eb-397e-9dca847ab964
# ╠═93696cf4-354d-11eb-0af3-9b27a8186fbd
# ╠═a91c8b80-354d-11eb-03ec-8f4386a69708
# ╠═86f0f21c-354d-11eb-2a32-299183f9118b
# ╠═666a8878-354d-11eb-22dc-e94a72ede22d
# ╟─cfc0e592-354d-11eb-2b06-458d198c38e5
# ╠═25789968-3552-11eb-1e25-35c0d1b1c461
# ╠═56daee6e-3554-11eb-37cd-49ad4ac56805
# ╠═56bb225a-3554-11eb-0062-6d03b8ea772f
# ╟─569a20f0-3554-11eb-0479-f1e583620682
# ╠═56790ed8-3554-11eb-3c51-b9b31a4c340e


"""
Determine the null and alternate hypotheses under t-tests given:

* `ns`: Per-group sample size(s)
* `δ`: Deviation from the null
* `σs`: Per-group standard deviations
"""
function hypotheses(::Type{OneSampleTTest}, n::Real, δ::Real, σ::Real)
    df = n - 1
    ncp = sqrt(n) * δ / σ
    null = TDist(df)
    alt = NoncentralT(df, ncp)
    return null, alt
end

"""
Determine the null and alternate hypotheses under t-tests given:

* `ns`: Per-group sample size(s)
* `δ`: Deviation from the null
* `σs`: Per-group standard deviations
"""
function hypotheses(::Type{EqualVarianceTTest}, n::Real, δ::Real, σ::Real)
    df = 2 * n - 2
    ncp = sqrt(n / 2) * δ / σ
    null = TDist(df)
    alt = NoncentralT(df, ncp)
    return null, alt
end

"""
Determine the null and alternate hypotheses under t-tests given:

* `ns`: Per-group sample size(s)
* `δ`: Deviation from the null
* `σs`: Per-group standard deviations
"""
function hypotheses(::Type{EqualVarianceTTest}, ns::Tuple, δ::Real, σ::Real)
    n1, n2 = ns[1], ns[2]
    df = n1 + n2 - 2
    ncp = (1 / sqrt(1 / n1 + 1 / n2)) * δ / σ
    null = TDist(df)
    alt = NoncentralT(df, ncp)
    return null, alt
end

# @doc """
# Determine the values of t that lead to overestimates of the magnitude of δ.
# """ ->
thresholds(alt::NoncentralT) = -alt.λ, alt.λ


"""
    floatmod(x, y)

This is the equivalent of `x // y` for floating point.
"""
floatmod(x, y) = x - y * floor(x / y)

"""
    fibonacci_points(n)

Constructs lat-long points on a sphere from a Fibonacci sequence.

There are 2n + 1 points on the sphere. It returns a `(2,2n+1)` grid of lat-long.

These points are a good start on an evenly-spaced set of points on the
surface of a sphere. This algorithm is from González [^1]. The usual way
to use these points is to make them a little more even by adjustment
with a successive over-relaxation algorithm. That algorithm moves them
a little at each step due to a repulsive force among neighbors.
Then you make a Voronoi on the sphere and create triangles from this.
The Voronoi will be almost all hexagons, but there will be a few polygons
with 5 and 7 sides.

[^1]: González, Á. (2010). Measurement of areas on a sphere using Fibonacci
and latitude–longitude lattices. Mathematical Geosciences, 42(1), 49.
"""
function fibonacci_points(n)
    ϕ = (1 + sqrt(5)) / 2
    points = zeros(Float64, 2, 2n + 1)
    for i in -n:n
        points[1, i + n + 1] = asin(2i / (2n + 1)) * 180 / π
        lon = floatmod(i, ϕ) * 360 / ϕ
        if lon < -180
            points[2, i + n + 1] = lon + 360
        elseif lon >= 180
            points[2, i + n + 1] = lon - 360
        else
            points[2, i + n + 1] = lon
        end
    end
    points
end


export bisect, cross_edges

"""
`bisect(G::SimpleGraph)` partitions the vertex set of `G` using the
eigenvector associated with the second smallest eigenvalue of the
graph's Laplacian matrix (called `x` below).

This can be invoked as follows:

+ `bisect(G,"user",pivot)` splits the vertices `v` depending on
  `x[v] >= pivot` vs. `x[v] < pivot`.

+ `bisect(G,"zero")` is the same as `bisect(G,"user", 0.0)`.

+ `bisect(G,"median")` is equivalent to `bisect(G,"user",m)` where `m`
  is the median value of `x`.

+ `bisect(G,"equal")` creates a partition in which the two parts have sizes
  the differ by at most 1.

A plain call to `bisect(G)` is equivalent to `bisect(G,"zero")` (which
is the same as `bisect(G,"user", 0.0)`).
"""
function bisect(G::SimpleGraph,
                where::AbstractString="zero",
                pivot::Real=0.0
               )

    verbose = false

    if verbose
        println("G = $G")
        println("where = $where")
        println("pivot = $pivot")
    end

    T = vertex_type(G)
    VV = vlist(G)
    n  = NV(G)
    L  = laplace(G)
    # x  = collect( eig(L)[2][:,2] )
    x = collect( eigen(L).vectors[:,2])

    if verbose
        println(sort(x))
    end

    piv = 0.0

    if where=="equal"
        pairs = sort(collect(zip(x,VV)))
        vtcs  = [ p[2] for p in pairs ]

        if verbose
            println(pairs)
            println(vtcs)
        end

        mid = floor(Int, n/2)
        if verbose
            println("Equal partition of the vertex set: $mid and $(n-mid)")
        end
        A = Set{T}(vtcs[1:mid])
        B = Set{T}(vtcs[mid+1:end])
        return A,B
    end

    if where=="median"
        piv = median(x)
    elseif where=="user"
        piv = pivot
    elseif where=="zero"
        piv = 0.0
    else
        error("Unknown \"where\" specifier: $where")
    end

    if verbose
        println("pivot set to $piv")
    end

    A = Set{T}()
    B = Set{T}()

    for k=1:n
        v = VV[k]
        if x[k] >= piv
            push!(A,v)
        else
            push!(B,v)
        end
    end

    return A,B
end

# import IterTools.product

"""
`cross_edges(G::SimpleGraph,A,B)` returns the set of edges of `G` with
one end in `A` and one end in `B`. Here `A` and `B` are collections
of vertices of `G`.
"""
function cross_edges(G::SimpleGraph, A, B)

    AB = Base.Iterators.product(A,B)

    result = Set(filter(e -> has(G,e[1],e[2]), collect(AB)))
    return result
end


#=##############################################################################
# DESCRIPTION
    Linear algebra tools

# AUTHORSHIP
  * Author    : Eduardo J Alvarez
  * Email     : Edo.AlvarezR@gmail.com
  * Created   : Oct 2021
  * License   : MIT License
=###############################################################################

dot(A, B) = sum(a*b for (a,b) in zip(A, B))
norm(A) = sqrt(mapreduce(x->x^2, +, A))

cross1(A, B) = A[2]*B[3] - A[3]*B[2]
cross2(A, B) = A[3]*B[1] - A[1]*B[3]
cross3(A, B) = A[1]*B[2] - A[2]*B[1]

function cross!(out, A, B)
    out[1] = cross1(A, B)
    out[2] = cross2(A, B)
    out[3] = cross3(A, B)
end

function cross(A, B)
    out = zero(A)
    cross!(out, A, B)
    return out
end


import Distributions: Uniform
import StaticArrays: SVector
import DynamicalSystems: DiscreteDynamicalSystem

"""
    eom_ikeda(u, p, t)

Equations of motion for a discrete two-dimensional Ikeda map system, adapted from [1]
by adding a noise term and allowing the influences from ``x \\to y`` (``c_{xy}``) and
from ``y \\to x`` (``c_{yx}``) to be adjusted. The difference equations are

```math
\\begin{aligned}
x(t+1) = 1 + \\mu(x(t) \\cos{(\\theta)} - c_{yx} y(t) \\sin{(\\theta)}) - min(\\dfrac{\\sigma \\xi_{t}^{(1)})}{(1-x)}, \\xi_{t}^{(2)} \\\\
y(t+1) = \\mu(y(t) \\cos{(\\theta)} - c_{xy} x(t) \\sin{(\\theta)}) - min(\\dfrac{\\sigma \\zeta_{t}^{(1)})}{(1-y)}, \\zeta_{t}^{(2)}
\\end{aligned}
```

## References

1. Cao, Liangyue, Alistair Mees, and Kevin Judd. "Modeling and predicting
    non-stationary time series." International Journal of Bifurcation and
    Chaos 7.08 (1997): 1823-1831.
"""
function eom_ikeda(u, p, t)
    x, y = u[1], u[2]
    c_xy, c_yx, a, b, c, r₁, r₂, σ = p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8]

    θ = a - b/(c + x^2 + y^2)
    μ = r₁*sin(t) - r₂
    d = Uniform(0.1, 0.4)

    dx = 1 + μ*(x*cos(θ) - c_yx*y*sin(θ)) - min(σ*rand(d)/(1-x), rand(d))
    dy = μ*(y*cos(θ) + c_xy*x*sin(θ)) -  min(σ*rand(d)/(1-y), rand(d))

    SVector{2}(dx, dy)
end

"""
    ikeda(; u₀ = rand(2), c_xy = 1.0, c_yx = 1.0, a = 0.8, b = 12, c = 0.9,
        r₁ = rand(Uniform(0.01, 0.3)), r₂ = rand(Uniform(0.01, 0.3)), σ = 0.05)

Initialise a discrete two-dimensional Ikeda map system, adapted from [1]
by adding a noise term and allowing the influences from ``x \\to y`` (``c_{xy}``) and
from ``y \\to x`` (``c_{yx}``) to be adjusted.

As a rule-of-thumb, if parameters `a`, `b`, and `c` are drawn from uniform
distributions on `[0.8, 1.5]`, `[10, 14]` and `[0.1, 0.9]`.

The difference equations are

```math
\\begin{aligned}
x(t+1) = 1 + \\mu(x \\cos{(\\theta)} - c_{yx} y \\sin{(\\theta)}) - min(\\dfrac{\\sigma \\xi_{t}^{(1)})}{(1-x)}, \\xi_{t}^{(2)} \\\\
y(t+1) = \\mu(y \\cos{(\\theta)} - c_{xy} x \\sin{(\\theta)}) - min(\\dfrac{\\sigma \\zeta_{t}^{(1)})}{(1-y)}, \\zeta_{t}^{(2)}
\\end{aligned}
```

## References

1. Cao, Liangyue, Alistair Mees, and Kevin Judd. "Modeling and predicting
    non-stationary time series." International Journal of Bifurcation and
    Chaos 7.08 (1997): 1823-1831.
"""
function ikeda(u₀, c_xy, c_yx, a, b, c, r₁, r₂, σ)
    p = [c_xy, c_yx, a, b, c, r₁, r₂, σ]
    DiscreteDynamicalSystem(eom_ikeda, u₀, p)
end

function ikeda(; u₀ = rand(2), c_xy = 1.0, c_yx = 1.0, a = 0.8, b = 12, c = 0.9,
        r₁ = rand(Uniform(0.01, 0.3)), r₂ = rand(Uniform(0.01, 0.3)), σ = 0.05)
    ikeda(u₀, c_xy, c_yx, a, b, c, r₁, r₂, σ)
end

export eom_ikeda, ikeda


import Pkg;Pkg.add("DifferentialEquations")
import Pkg;Pkg.add("Plots")
using DifferentialEquations
using Plots;gr()

# definition ode
f(y, t) = 3.0 * y + 2.0

# setting initial condiction
y0 = 1.0

# setting time step
tspan = (0.0, 1.0)

# solve of ODEProblem
prob = ODEProblem(f , y0 , tspan)
sol = solve(prob)

# plot
plot(
    sol,
    linewidth=5,
    title="Solution to the linear ODE with a thick line",
    xaxis="Time (t)",
    yaxis="y(t) (in micro.m)",
    label="My Thick Line!"
)

# exact solution
g(t) = 5//3 * exp.(3 * t) - 2//3

# comparison plot
plot!(g, lw=3, ls=:dash, label="True Solution!")


###################################
## Simple Variational Inference
## for 2D Gaussian

using PyPlot

function calc_KL(mu1, lambda1, mu2, lambda2)
    D = size(mu1, 1)
    px_lnqx = 0.5 * logdet(lambda2) - 0.5 * ((mu1 - mu2)' * lambda2 * (mu1 - mu2) + trace(lambda2 * inv(lambda1)))
    px_lnpx = 0.5 * logdet(lambda1) - 0.5 * D
    KL = - (px_lnqx - px_lnpx)
    return KL[1]
end

function plot_results(result, truth)
    N = size(result, 1)
    H = Int(ceil(sqrt(N)))
    W = Int(ceil(N / H))
    f, ax = subplots(H, W)
    for i in 1 : H
        for j in 1 : W
            n = (i - 1) * W + j
            if n <= N
                p = ax[i, j]
                p[:set_title](@sprintf("%d of %d", n, N))
                plot_gaussian(p, result[n][1], result[n][2], "b", "p(z)")
                plot_gaussian(p, truth[1], truth[2], "r", "q(z)")
                p[:set_xlim]([-1.5, 1.0])
                p[:set_ylim]([-0.6, 0.8])
            end
        end
    end
end

function plot_gaussian(p, Mu, Sigma, col, label)
    res = 100
    p[:plot](Mu[1], Mu[2], "x", color=col)

    F = eigfact(Sigma)
    vec = F[:vectors]
    val = F[:values]
    dw = 2*pi/res
    w = dw * (0 : res)

    c = 1.0
    a = sqrt(c * val[1])
    b = sqrt(c * val[2])
    P1 = a*cos(w)
    P2 = b*sin(w)
    P = Mu .+ vec'*vcat(P1', P2')
    p[:plot](P[1, :]', P[2, :]', "-", color=col, label=label)
end

function main()
    ## creat truth distribution
    D = 2 # dimension
    theta = 2.0*pi/12 # tilt
    A = reshape([cos(theta), -sin(theta),
                 sin(theta), cos(theta)],
                2, 2)
    mu = [0.0, 0.0]
    lambda = inv(A * inv(reshape([1,0,0,10], 2, 2)) * A')

    ## initialize
    mu_h = randn(D)
    lambda_h = zeros(D,D)

    ## main iteration
    max_iter = 10
    KL = Vector{Float64}()
    result = Vector()
    for i in 1 : max_iter
        ## update
        mu_h[1] = mu[1] - inv(lambda[1,1])*lambda[1,2] * (mu_h[2] - mu[2])
        lambda_h[1,1] = lambda[1,1]
        mu_h[2] = mu[2] - inv(lambda[2,2])*lambda[2,1] * (mu_h[1] - mu[1])
        lambda_h[2,2] = lambda[2,2]

        ## calculate KL divergeince
        push!(KL, calc_KL(mu_h, lambda_h, mu, lambda))
        #println(mu_h)
        #println(lambda_h)

        ## store the results
        push!(result, (deepcopy(mu_h), deepcopy(inv(lambda_h))))
    end

    ## visualize results
    plot_results(result, (mu, inv(lambda)))

    f, ax = subplots(1, 1)
    plot_gaussian(ax, mu, inv(lambda), "b", latexstring("\$p(z)\$"))
    plot_gaussian(ax, result[end][1], result[end][2], "r", latexstring("\$q(z)\$"))
    ax[:set_xlabel](latexstring("\$z_1\$"), fontsize=16)
    ax[:set_ylabel](latexstring("\$z_2\$"), fontsize=16)
    ax[:legend](fontsize=16)

    ## KL divergence
    f, ax = subplots(1)
    ax[:plot](1:max_iter, KL)
    ax[:set_ylabel]("KL divergence")
    ax[:set_xlabel]("iteration")
    show()
end

main()


# Bregman divergence

"""
Implements the Bregman divergence, a friendly introduction to which can be found
[here](http://mark.reid.name/blog/meet-the-bregman-divergences.html).
Bregman divergences are a minimal implementation of the "mean-minimizer" property.

It is assumed that the (convex differentiable) function F maps vectors (of any type or size) to real numbers.
The inner product used is `Base.dot`, but one can be passed in either by defining `inner` or by
passing in a keyword argument. If an analytic gradient isn't available, Julia offers a suite
of good automatic differentiation packages.

function evaluate(dist::Bregman, p::AbstractVector, q::AbstractVector)
"""
struct Bregman{T1 <: Function, T2 <: Function, T3 <: Function} <: PreMetric
    F::T1
    ∇::T2
    inner::T3
end

# Default costructor.
Bregman(F, ∇) =  Bregman(F, ∇, LinearAlgebra.dot)

# Evaluation fuction
function (dist::Bregman)(p, q)
    # Create cache vals.
    FP_val = dist.F(p)
    FQ_val = dist.F(q)
    DQ_val = dist.∇(q)
    p_size = length(p)
    # Check F codomain.
    if !(isa(FP_val, Real) && isa(FQ_val, Real))
        throw(ArgumentError("F Codomain Error: F doesn't map the vectors to real numbers"))
    end
    # Check vector size.
    if p_size != length(q)
        throw(DimensionMismatch("The vector p ($(size(p))) and q ($(size(q))) are different sizes."))
    end
    # Check gradient size.
    if length(DQ_val) != p_size
        throw(DimensionMismatch("The gradient result is not the same size as p and q"))
    end
    # Return the Bregman divergence.
    return FP_val - FQ_val - dist.inner(DQ_val, p .- q)
end

# Convenience function.
bregman(F, ∇, x, y; inner = LinearAlgebra.dot) = Bregman(F, ∇, inner)(x, y)


# ---
# title: 780. Reaching Points
# id: problem780
# author: Indigo
# date: 2021-01-28
# difficulty: Hard
# categories: Math
# link: <https://leetcode.com/problems/reaching-points/description/>
# hidden: true
# ---
#
# A move consists of taking a point `(x, y)` and transforming it to either `(x,
# x+y)` or `(x+y, y)`.
#
# Given a starting point `(sx, sy)` and a target point `(tx, ty)`, return `True`
# if and only if a sequence of moves exists to transform the point `(sx, sy)` to
# `(tx, ty)`. Otherwise, return `False`.
#
#
#
#     **Examples:**
#     Input: sx = 1, sy = 1, tx = 3, ty = 5
#     Output: True
#     Explanation:
#     One series of moves that transforms the starting point to the target is:
#     (1, 1) -> (1, 2)
#     (1, 2) -> (3, 2)
#     (3, 2) -> (3, 5)
#
#     Input: sx = 1, sy = 1, tx = 2, ty = 2
#     Output: False
#
#     Input: sx = 1, sy = 1, tx = 1, ty = 1
#     Output: True
#
#
#
# **Note:**
#
#   * `sx, sy, tx, ty` will all be integers in the range `[1, 10^9]`.
#
#
## @lc code=start
using LeetCode

function reaching_points(sx, sy, tx, ty)
    while tx >= sx && ty >= sy
        if tx == ty
            break
        end
        if tx > ty
            if ty > sy
                tx %= ty;
            else
                return (tx - sx) % ty == 0;
            end
        else
            if tx > sx
                ty %= tx;
            else
                return (ty - sy) % tx == 0;
            end
        end
    end
    tx == sx && ty == sy
end
## @lc code=end


"""
    cilc_weights(cij, a, b)

This function returns weights (a vector of the number of frequency channels) of the constrained ILC (CILC) method.

*Reference*: Equation (20) of Remazeilles, Delabrouille, Cardoso, MNRAS, 410, 2481 (2011)

# Arguments
- `cij::Array{<:AbstractFloat,2}`: symmetric covariance matrix with the dimention of `(nν, nν)` where `nν` is the number of frequency bands.
- `a::Array{<:AbstractFloat,1}`: vector of the frequency response, for the component to be extracted. E.g., `a = [1,...,1]` for CMB.
- `b::Array{<:AbstractFloat,1}`: vector of the frequency response, for the component to be nulled.
"""
function cilc_weights(
    cij::Array{T,2},
    a::Array{T,1},
    b::Array{T,1},
) where {T<:AbstractFloat}
    if size(cij)[1] ≠ size(cij)[2]
        throw(DimensionMismatch("covariance matrix must be a square matrix"))
    elseif size(cij)[1] ≠ length(a) || size(cij)[1] ≠ length(b)
        throw(DimensionMismatch("dimensions of the covariance matrix and the frequency response vector do not match"))
    elseif a ≈ b
        throw(ErrorException("vectors of the frequency response are too similar"))
    else
        M = Symmetric(cij)
    end
    x = M \ a # R^-1 a
    y = M \ b # R^-1 b
    w = (b'y * x - a'y * y) / (a'x * b'y - (a'y)^2) # ILC weights
end

"""
    cilc_weights(cijℓ, a, b[, ℓid=3])

This function returns weights (a `nν`-by-`nℓ` matrix) of the constrained ILC (CILC) method.

Here, `nν` is the number of frequency channels and `nℓ` is the number of elements in the relevant domain, e.g., multipoles, band-power bins, pixels, etc.

*Reference*: Equation (20) of Remazeilles, Delabrouille, Cardoso, MNRAS, 410, 2481 (2011)

# Arguments
- `cijℓ::Array{<:AbstractFloat,3}`: symmetric covariance matrix with the dimention of `(nℓ, nν, nν)`, `(nν, nℓ, nν)` or `(nν, nν, nℓ)` (default).
- `a::Array{<:AbstractFloat,1}`: vector of the frequency response, for the component to be extracted. E.g., `a = [1,...,1]` for CMB.
- `b::Array{<:AbstractFloat,1}`: vector of the frequency response, for the component to be nulled.

# Optional Arguments
- `ℓid::Integer=3`: location of the index for the `nℓ` domain. `ℓid=1` if `cijℓ[nℓ,nν,nν]`, `ℓid=2` if `cijℓ[nν,nℓ,nν]`, and `ℓid=3` (the default value) if `cijℓ[nν,nν,nℓ]`.
"""
function cilc_weights(
    cijℓ::Array{T,3},
    a::Array{T,1},
    b::Array{T,1},
    ℓid::Integer = 3,
) where {T<:AbstractFloat}
    if ℓid > 3 || ℓid < 1
        throw(DomainError(ℓid, "ℓid must be 1, 2, or 3"))
    end
    if (ℓid == 3 && size(cijℓ)[1] ≠ size(cijℓ)[2]) ||
       (ℓid == 2 && size(cijℓ)[1] ≠ size(cijℓ)[3]) ||
       (ℓid == 1 && size(cijℓ)[2] ≠ size(cijℓ)[3])
        throw(DimensionMismatch("covariance matrix must be a square matrix"))
    end
    nℓ = size(cijℓ)[ℓid]
    nν = ifelse(ℓid == 3, size(cijℓ)[1], size(cijℓ)[3])
    if length(a) ≠ nν || length(b) ≠ nν
        throw(DimensionMismatch("dimensions of the covariance matrix and the frequency response vector do not match"))
    elseif a ≈ b
        throw(ErrorException("vectors of the frequency response are too similar"))
    else
    end
    wℓ = zeros(nν, nℓ) # ILC weights
    for iℓ = 1:nℓ
        if ℓid == 3
            M = Symmetric(cijℓ[:, :, iℓ])
        elseif ℓid == 2
            M = Symmetric(cijℓ[:, iℓ, :])
        else
            M = Symmetric(cijℓ[iℓ, :, :])
        end
        x = M \ a # R^-1 a
        y = M \ b # R^-1 b
        wℓ[:, iℓ] = (b'y * x - a'y * y) / (a'x * b'y - (a'y)^2)
    end
    return wℓ
end


# Matern 1/2 isotropic covariance Function

"""
    Mat12Iso <: MaternISO

Isotropic Matern 1/2 kernel (covariance)
```math
k(x,x') = σ^2 \\exp(-|x-y|/ℓ)
```
with length scale ``ℓ`` and signal standard deviation ``σ``.
"""
mutable struct Mat12Iso{T} <: MaternIso where {T<:Real}
    "Length scale"
    ℓ::T
    "Signal variance"
    σ2::T
    "Priors for kernel parameters"
    priors::Array
end

"""
Matern 1/2 isotropic covariance Function

    Mat12Iso(ll::T, lσ::T)

# Arguments
  - `ll::Real`: length scale (given on log scale)
  - `lσ::Real`: signal standard deviation (given on log scale)
"""
Mat12Iso(ll::T, lσ::T) where T = Mat12Iso{T}(exp(ll), exp(2 * lσ), [])

function set_params!(mat::Mat12Iso, hyp::AbstractVector)
    length(hyp) == 2 || throw(ArgumentError("Matern 1/2 covariance function has two parameters, received $(length(hyp))."))
    mat.ℓ, mat.σ2 = exp(hyp[1]), exp(2 * hyp[2])
end

get_params(mat::Mat12Iso{T}) where T = T[log(mat.ℓ), log(mat.σ2) / 2]
get_param_names(mat::Mat12Iso) = [:ll, :lσ]
num_params(mat::Mat12Iso) = 2

cov(mat::Mat12Iso, r::Number) = mat.σ2 * exp(-r / mat.ℓ)

@inline dk_dll(mat::Mat12Iso, r::Real) = r / mat.ℓ * cov(mat, r)


# indicator of the L2 norm sphere with given radius

"""
  IndSphereL2(r::Real=1.0)

Returns the function `g = ind{x : ||x|| = r}`, for a real parameter `r > 0`.
"""

immutable IndSphereL2{R <: Real} <: IndicatorNonconvex
  r::R
  function IndSphereL2(r::R)
    if r <= 0
      error("parameter r must be positive")
    else
      new(r)
    end
  end
end

IndSphereL2{R <: Real}(r::R=1.0) = IndSphereL2{R}(r)

function (f::IndSphereL2){T <: RealOrComplex}(x::AbstractArray{T})
  if abs(vecnorm(x) - f.r)/f.r > 1e-14
    return +Inf
  end
  return 0.0
end

function prox!{T <: RealOrComplex}(f::IndSphereL2, x::AbstractArray{T}, y::AbstractArray{T}, gamma::Real=1.0)
  normx = vecnorm(x)
  if normx > 0 # zero-zero?
    scal = f.r/normx
    for k in eachindex(x)
      y[k] = scal*x[k]
    end
  else
    normy = 0.0
    for k in eachindex(x)
      y[k] = randn()
      normy += y[k]*y[k]
    end
    normy = sqrt(normy)
    y[:] *= f.r/normy
  end
  return 0.0
end

fun_name(f::IndSphereL2) = "indicator of an L2 norm sphere"
fun_dom(f::IndSphereL2) = "AbstractArray{Real}, AbstractArray{Complex}"
fun_expr(f::IndSphereL2) = "x ↦ 0 if ||x|| = r, +∞ otherwise"
fun_params(f::IndSphereL2) = "r = $(f.r)"

function prox_naive{T <: RealOrComplex}(f::IndSphereL2, x::AbstractArray{T}, gamma::Real=1.0)
  normx = vecnorm(x)
  if normx > 0
    y = x*f.r/normx
  else
    y = randn(size(x))
    y *= f.r/vecnorm(y)
  end
  return y, 0.0
end


#Linear Kalman filter example
#This example closely follows that given on "Greg Czerniak's Website". Namely
#the voltage example on this page: http://greg.czerniak.info/guides/kalman1/

#Let's import the modules required to execute the Kalman Filter
using StateSpace
using Distributions
using DataFrames
using Gadfly
using Colors

#To use the Kalman Filter we need three things:
#1) Some observations
#2) The parameters of the Kalman Filter model describing the system process and
#the observation model.
#3) An initial guess of the state.
#
#The following code is split into those three sections to begin with. We finish
#by performing the Kalman Filter algorithm and finally displaying the results.

################################################################################
#Section: Generate noisy Observations
#-------------------------------------------------------------------------------
#Let's assume that the true voltage value does not change throughout the
#experiment. So we can give the true voltage a value
true_voltage = 1.25

#However our measurements of the true voltage are noisy. Here we assume that the
#noise is normally distributed with zero mean but non-zero variance:
measurement_noise_variance = 0.1

#Given this information we can generate some noisy data (observations). We'll
#also choose the number of observations
number_of_observations = 60
observations = randn(number_of_observations) * sqrt(measurement_noise_variance) + true_voltage

#Note that the observation list has to be transposed because each column of the
#matrix is considered as a new observation.
observations = observations'
#End Section: Generate noisy Observations
################################################################################

################################################################################
#Section: Describe Kalman Filter parameters
#-------------------------------------------------------------------------------
#Now that we have some observations we need to describe the Kalman Filter
#parameters:
#Process matrix - Note that since this is a one-dimensional system I've given
#a floating point value rather than explicitly declaring a matrix with one
#element, although you can do it that way. The value of 1.0 here means that the
#process model suggests that the previous state is the same as the next state.
#i.e. the voltage doesn't change.
process_matrix = 1.0

#Process Covariance - Since it's a single value, the covariance is the same as
#variance. We've made it small here because we are fairly sure of our process
#model that the state doesn't change. If we were less sure that the model was
#correct then we would increase this value.
process_covariance = 0.00001

#Observation matrix - Since we measure the voltage directly (we don't measure
#some quantity related by a function - i.e. we don't measure twice the voltage)
#the observation matrix is equal to 1.0
observation_matrix = 1.0

#Observation Covariance - we set the variance of the measurement noise earlier
#so we'll give it the same value.
observation_covariance = measurement_noise_variance

#Now we can create our Linear State Space Model
linSSM = LinearGaussianSSM(process_matrix, process_covariance, observation_matrix, observation_covariance)
#End Section: Describe Kalman Filter parameters
################################################################################

################################################################################
#Section: Set initial guess of the state
#-------------------------------------------------------------------------------
#We'll begin with a guess that's quite off. We'll assume an initial mean state
#of 3 (i.e. we think we have 3 volts) and give the variance as 1
initial_guess = MvNormal([3.0], [1.0])
#End Section: Set initial guess of the state
################################################################################

################################################################################
#Section: Execute Kalman Filter
#-------------------------------------------------------------------------------
#Now that we have some noisy observations, the Kalman filter parameters and an
#intial guess for the state of the system, we can run the Kalman Filter
filtered_state = filter(linSSM, observations, initial_guess)
#End Section: Execute Kalman Filter
################################################################################

################################################################################
#Section: Plot Filtered results
#-------------------------------------------------------------------------------
#Here we are plotting the filtered results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/

x_data = 1:number_of_observations
state_array = Vector{Float64}(number_of_observations+1)
confidence_array = Vector{Float64}(number_of_observations+1)
state_array[1] = initial_guess.μ[1]
confidence_array[1] = 2*sqrt(initial_guess.Σ.diag[1])
for i in x_data
    current_state = filtered_state.state[i]
    state_array[i+1] = current_state.μ[1]
    confidence_array[i+1] = 2*sqrt(current_state.Σ.mat[1])
end
df_fs = DataFrame(
    x = [0;x_data],
    y = state_array,
    ymin = state_array - confidence_array,
    ymax = state_array + confidence_array,
    f = "Filtered values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))
filtered_state_plot = plot(
    layer(x=x_data, y=filtered_state.observations, Geom.point, Theme(default_color=getColors[2])),
    layer(x=[0;x_data], y=ones(number_of_observations+1)*true_voltage, Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Voltage (Volts)"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Linear Kalman Filter Example")
    )
display(filtered_state_plot)
#End Section: Plot Filtered results
################################################################################

################################################################################
#Section: Execute Kalman Smoother
#-------------------------------------------------------------------------------
#Everything can be performed much better in hindsight and that's essentially
#where the Kalman smoother comes in. Given all of the observations and the
#filtered state (including the Kalman filter parameters, the smoother attempts
#to give better estimates of the system's state. Here we perform smoothing on
#the filtered data.
smoothed_state = smooth(linSSM, filtered_state)
#End Section: Execute Kalman Filter
################################################################################

################################################################################
#Section: Plot Smoothed Results
#-------------------------------------------------------------------------------
#Here we are plotting the smoothed results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/
state_array = Vector{Float64}(number_of_observations)
confidence_array = Vector{Float64}(number_of_observations)
for i in x_data
    current_state = smoothed_state.state[i]
    state_array[i] = current_state.μ[1]
    confidence_array[i] = 2*sqrt(current_state.Σ.mat[1])
end
df_ss = DataFrame(
    x = x_data,
    y = state_array,
    ymin = state_array - confidence_array,
    ymax = state_array + confidence_array,
    f = "Filtered values"
    )

smoothed_state_plot = plot(
    layer(x=x_data, y=smoothed_state.observations, Geom.point, Theme(default_color=getColors[2])),
    layer(x=x_data, y=ones(number_of_observations)*true_voltage, Geom.line, Theme(default_color=getColors[3])),
    layer(df_ss, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Voltage (Volts)"),
    Guide.manual_color_key("Colour Key",["Smoothed Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Linear Kalman Smoother Example")
    )
display(smoothed_state_plot)
#End Section: Plot Filtered results
################################################################################


"""
    norm_data(A; dims=1, use_MAD::Bool=false, corrected::Bool=false)

Computes ``\\frac{A - \\bar A}{\\sigma}``, where `dims` is 1 (the default) if `A` is
column-oriented and 2 if it is row-oriented. If `use_MAD` is `true`, ``\\bar A`` is
`median(A, dims=dims)` and ``\\sigma`` is the MAD version of the standard deviation. If
`use_MAD` is `false`, ``\\bar A`` is `mean(A, dims=dims)` and ``\\sigma`` is the standard
deviation. If `corrected` is `true` then the sum is scaled with `n-1`, whereas the sum is
scaled with `n` if `corrected` is `false`, where `n = size(A, dims)`.
"""
function norm_data(A; dims=1, use_MAD=false, corrected=false)
    A = Matrix{Float64}(A)
    if use_MAD
        return (A .- median(A, dims=dims)) ./ mapslices(mad, A, dims=dims)
    else
        return (A .- mean(A, dims=dims)) ./ std(A, dims=dims, corrected=corrected)
    end
end

"""
    classic_PCA(A; dims=1, standardize::Bool=true, corrected::Bool=false)

Performs PCA on a copy of the ``m × n`` matrix `A`. `dims` is 1 (the default) if `A` is
column-oriented and 2 if it is row-oriented. If `standardize` is `true` (the default), the
copy will be transformed using the MAD version of the standard deviation. If `corrected`
is `true` then the sum is scaled with `n-1`, whereas the sum is scaled with `n` if
`corrected` is `false`, where `n = size(A, dims)`.

Returns `(V, S, prod)`, where ``A = U Σ V^*``, with ``Σ = \\text{diagm}(S)`` being the
diagonal matrix of singular values sorted in descending order and `prod` is the matrix
multiplication of `V` and `A`, with `A` being standardized by default.
"""
function classic_PCA(A; dims=1, standardize=true, corrected=false)
    data = copy(A)
    data = standardize ? norm_data(data, dims=dims, use_MAD=true, corrected=corrected) : data
    U, S, V = svd(cov(data, dims=dims, corrected=corrected))
    prod = dims == 1 ? data * V : V' * data
    return V, S, prod
end


@doc raw"""
    usinc(θ::Real)

Unnormalized version of `sinc` function, i.e. $\operatorname{usinc}(θ) = \frac{\sin(θ)}{θ}$.
This is equivalent to `sinc(θ/π)`.
"""
@inline usinc(θ::Real) = θ == 0 ? one(θ) : isinf(θ) ? zero(θ) : sin(θ) / θ

@doc raw"""
    usinc_from_cos(x::Real)

Unnormalized version of `sinc` function, i.e. $\operatorname{usinc}(θ) = \frac{\sin(θ)}{θ}$,
computed from $x = cos(θ)$.
"""
@inline function usinc_from_cos(x::Real)
    return if x >= 1
        one(x)
    elseif x <= -1
        zero(x)
    else
        sqrt(1 - x^2) / acos(x)
    end
end

@doc raw"""
    nzsign(z[, absz])

Compute a modified `sign(z)` that is always nonzero, i.e. where
````math
\operatorname(nzsign)(z) = \begin{cases}
    1 & \text{if } z = 0\\
    \frac{z}{|z|} & \text{otherwise}
\end{cases}
````
"""
@inline function nzsign(z, absz=abs(z))
    psignz = z / absz
    return ifelse(iszero(absz), one(psignz), psignz)
end

allocate(p, s::Size{S}) where {S} = similar(p, S...)
allocate(p::StaticArray, s::Size{S}) where {S} = similar(p, maybesize(s))
allocate(p, ::Type{T}, s::Size{S}) where {S,T} = similar(p, T, S...)
allocate(p::StaticArray, ::Type{T}, s::Size{S}) where {S,T} = similar(p, T, maybesize(s))

"""
    eigen_safe(x)

Compute the eigendecomposition of `x`. If `x` is a `StaticMatrix`, it is
converted to a `Matrix` before the decomposition.
"""
@inline eigen_safe(x; kwargs...) = eigen(x; kwargs...)
@inline function eigen_safe(x::StaticMatrix; kwargs...)
    s = size(x)
    E = eigen!(Matrix(parent(x)); kwargs...)
    return Eigen(SizedVector{s[1]}(E.values), SizedMatrix{s...}(E.vectors))
end

"""
    log_safe(x)

Compute the matrix logarithm of `x`. If `x` is a `StaticMatrix`, it is
converted to a `Matrix` before computing the log.
"""
@inline log_safe(x) = log(x)
@inline function log_safe(x::StaticMatrix)
    s = Size(x)
    return SizedMatrix{s[1],s[2]}(log(Matrix(parent(x))))
end

# NOTE: workaround until https://github.com/JuliaLang/julia/pull/39973 or similar is merged
"""
    log_safe!(y, x)

Compute the matrix logarithm of `x`. If the eltype of `y` is real, then the imaginary part
of `x` is ignored, and a `DomainError` is raised if `real(x)` has no real logarithm.
"""
function log_safe!(Y, A)
    if eltype(Y) <: Real
        if ishermitian(A)
            eigenF = eigen(Symmetric(real(A)))
            i = findfirst(≤(0), eigenF.values)
            if i !== nothing
                throw(
                    DomainError(
                        eigenF.values[i],
                        "All eigenvalues must be positive to compute a real logarithm.",
                    ),
                )
            end
            mul!(Y, eigenF.vectors .* log.(eigenF.values'), eigenF.vectors')
        elseif istriu(A)
            i = findfirst(≤(0), @view(A[diagind(A)]))
            if i !== nothing
                throw(
                    DomainError(
                        A[i, i],
                        "All eigenvalues must be positive to compute a real logarithm.",
                    ),
                )
            end
            copyto!(Y, real(log(UpperTriangular(A))))
        else
            schurF = schur(convert(Matrix, real(A)))
            i = findfirst(x -> isreal(x) && real(x) ≤ 0, schurF.values)
            if i !== nothing
                throw(
                    DomainError(
                        schurF.values[i],
                        "All eigenvalues must be positive to compute a real logarithm.",
                    ),
                )
            end
            if istriu(schurF.T)
                mul!(Y, schurF.Z, real(log(UpperTriangular(schurF.T))) * schurF.Z')
            else
                schurS = schur(complex(schurF.T))
                Y .= real.(schurS.Z * log(UpperTriangular(schurS.T)) * schurS.Z')
                mul!(Y, schurF.Z * Y, schurF.Z')
            end
        end
    else
        copyto!(Y, log_safe(A))
    end
    return Y
end

"""
    mul!_safe(Y, A, B) -> Y

Call `mul!` safely, that is, `A` and/or `B` are permitted to alias with `Y`.
"""
mul!_safe(Y, A, B) = (Y === A || Y === B) ? copyto!(Y, A * B) : mul!(Y, A, B)

@doc raw"""
    realify(X::AbstractMatrix{T𝔽}, 𝔽::AbstractNumbers) -> Y::AbstractMatrix{<:Real}

Given a matrix $X ∈ 𝔽^{n × n}$, compute $Y ∈ ℝ^{m × m}$, where $m = n \operatorname{dim}_𝔽$,
and $\operatorname{dim}_𝔽$ is the [`real_dimension`](@ref) of the number field $𝔽$, using
the map $ϕ \colon X ↦ Y$, that preserves the matrix product, so that for all
$C,D ∈ 𝔽^{n × n}$,
````math
ϕ(C) ϕ(D) = ϕ(CD).
````
See [`realify!`](@ref) for an in-place version, and [`unrealify!`](@ref) to compute the
inverse of $ϕ$.
"""
function realify(X, 𝔽)
    n = LinearAlgebra.checksquare(X)
    nℝ = real_dimension(𝔽) * n
    Y = allocate(X, real(eltype(X)), nℝ, nℝ)
    return realify!(Y, X, 𝔽, n)
end
realify(X, ::typeof(ℝ)) = X

"""
    realify!(Y::AbstractMatrix{<:Real}, X::AbstractMatrix{T𝔽}, 𝔽::AbstractNumbers)

In-place version of [`realify`](@ref).
"""
realify!(Y, X, 𝔽)

@doc raw"""
    realify!(Y::AbstractMatrix{<:Real}, X::AbstractMatrix{<:Complex}, ::typeof(ℂ))

Given a complex matrix $X = A + iB ∈ ℂ^{n × n}$, compute its realified matrix
$Y ∈ ℝ^{2n × 2n}$, written
where
````math
Y = \begin{pmatrix}A & -B \\ B & A \end{pmatrix}.
````
"""
function realify!(Y, X, ::typeof(ℂ), n=LinearAlgebra.checksquare(X))
    for i in 1:n, j in 1:n
        Xr, Xi = reim(X[i, j])
        Y[i, j] = Y[n + i, n + j] = Xr
        Y[n + i, j] = Xi
        Y[i, n + j] = -Xi
    end
    return Y
end

@doc raw"""
    unrealify!(X::AbstractMatrix{T𝔽}, Y::AbstractMatrix{<:Real}, 𝔽::AbstractNumbers[, n])

Given a real matrix $Y ∈ ℝ^{m × m}$, where $m = n \operatorname{dim}_𝔽$, and
$\operatorname{dim}_𝔽$ is the [`real_dimension`](@ref) of the number field $𝔽$, compute
in-place its equivalent matrix $X ∈ 𝔽^{n × n}$. Note that this function does not check that
$Y$ has a valid structure to be un-realified.

See [`realify!`](@ref) for the inverse of this function.
"""
unrealify!(X, Y, 𝔽)

function unrealify!(X, Y, ::typeof(ℂ), n=LinearAlgebra.checksquare(X))
    for i in 1:n, j in 1:n
        X[i, j] = complex((Y[i, j] + Y[n + i, n + j]) / 2, (Y[n + i, j] - Y[i, n + j]) / 2)
    end
    return X
end
unrealify!(Y, X, ::typeof(ℝ), args...) = copyto!(Y, X)

@generated maybesize(s::Size{S}) where {S} = prod(S) > 100 ? S : :(s)

"""
    select_from_tuple(t::NTuple{N, Any}, positions::Val{P})

Selects elements of tuple `t` at positions specified by the second argument.
For example `select_from_tuple(("a", "b", "c"), Val((3, 1, 1)))` returns
`("c", "a", "a")`.
"""
@generated function select_from_tuple(t::NTuple{N,Any}, positions::Val{P}) where {N,P}
    for k in P
        (k < 0 || k > N) && error("positions must be between 1 and $N")
    end
    return Expr(:tuple, [Expr(:ref, :t, k) for k in P]...)
end

@doc raw"""
    vec2skew!(X, v, k)

create a skew symmetric matrix inplace in `X` of size $k\times k$ from a vector `v`,
for example for `v=[1,2,3]` and `k=3` this
yields
````julia
[  0  1  2;
  -1  0  3;
  -2 -3  0
]
````
"""
function vec2skew!(X, v)
    k = size(X)[1]
    size(X)[2] != k && error("X is of wrong size, expected ($k,$k) got $(size(X)).")
    n = div(k * (k - 1), 2)
    length(v) < n && error("The vector $(v) is too short, expected $(n) got $(length(v)).")
    m = 0
    X .= [i < j ? (m += 1; v[m]) : zero(eltype(v)) for i in 1:k, j in 1:k]
    X .= X - X'
    return X
end
function vec2skew(v, k)
    X = similar(v, k, k)
    vec2skew!(X, v)
    return X
end

@doc raw"""
    isnormal(x; kwargs...) -> Bool

Check if the matrix or number `x` is normal, that is, if it commutes with its adjoint:
````math
x x^\mathrm{H} = x^\mathrm{H} x.
````
By default, this is an equality check. Provide `kwargs` for `isapprox` to perform an
approximate check.
"""
function isnormal(x; kwargs...)
    (isdiag(x) || ishermitian(x)) && return true
    isempty(kwargs) && return x * x' == x' * x
    return isapprox(x * x', x' * x; kwargs...)
end
isnormal(::LinearAlgebra.RealHermSymComplexHerm; kwargs...) = true

"""
    ziptuples(a, b[, c[, d[, e]]])

Zips tuples `a`, `b`, and remaining in a fast, type-stable way. If they have different
lengths, the result is trimmed to the length of the shorter tuple.
"""
@generated function ziptuples(a::NTuple{N,Any}, b::NTuple{M,Any}) where {N,M}
    ex = Expr(:tuple)
    for i in 1:min(N, M)
        push!(ex.args, :((a[$i], b[$i])))
    end
    return ex
end
@generated function ziptuples(
    a::NTuple{N,Any},
    b::NTuple{M,Any},
    c::NTuple{L,Any},
) where {N,M,L}
    ex = Expr(:tuple)
    for i in 1:min(N, M, L)
        push!(ex.args, :((a[$i], b[$i], c[$i])))
    end
    return ex
end
@generated function ziptuples(
    a::NTuple{N,Any},
    b::NTuple{M,Any},
    c::NTuple{L,Any},
    d::NTuple{K,Any},
) where {N,M,L,K}
    ex = Expr(:tuple)
    for i in 1:min(N, M, L, K)
        push!(ex.args, :((a[$i], b[$i], c[$i], d[$i])))
    end
    return ex
end
@generated function ziptuples(
    a::NTuple{N,Any},
    b::NTuple{M,Any},
    c::NTuple{L,Any},
    d::NTuple{K,Any},
    e::NTuple{J,Any},
) where {N,M,L,K,J}
    ex = Expr(:tuple)
    for i in 1:min(N, M, L, K, J)
        push!(ex.args, :((a[$i], b[$i], c[$i], d[$i], e[$i])))
    end
    return ex
end


### A Pluto.jl notebook ###
# v0.19.4

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local iv = try Base.loaded_modules[Base.PkgId(Base.UUID("6e696c72-6542-2067-7265-42206c756150"), "AbstractPlutoDingetjes")].Bonds.initial_value catch; b -> missing; end
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : iv(el)
        el
    end
end

# ╔═╡ 7c860884-5afd-4b4a-b358-4f2bce8bd395
using Pkg; Pkg.activate("../..")

# ╔═╡ 38c4e220-d708-11ec-3968-fbbd41c26155
using PlutoUI, Plots, DifferentialEquations, NonLinearDynamicsCourse

# ╔═╡ 2ac364c2-cbdd-49b4-9f26-9fe89382be5e
TableOfContents(title="📚 Table of Contents", indent=true, depth=4, aside=true)

# ╔═╡ 6b6a5e19-5298-4969-b1c9-699aa1cb2996
html"""

"""

# ╔═╡ 62cba86d-4406-4d16-8715-b29bee7d3178
md"""
# Logistic Equation

$\dot{x}=Rx\left(1-\frac{x}{K}\right)$
"""

# ╔═╡ 6d64cc27-1059-4dd7-8f17-1c6fd9a2eca0
html"""

"""

# ╔═╡ 4dc32b5a-5794-4d2d-844e-cdb80bec2e7b
logistic(x,p,t)=p[1]*x*(1.0-x/p[2])

# ╔═╡ 77766c99-4142-4acb-a855-ab133e67aa66
@bind pars2 (
	PlutoUI.combine() do bind
		md"""
		R: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true))) \
		K: $(bind(Slider(0.01:0.02:2.0,default=0.1;show_value=true))) \
		x0: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true)))
		"""
	end
)

# ╔═╡ 16a156b5-92ab-4e1c-ab70-c3b348186c57
flux1D(logistic,pars2[3],100.0,pars2;xlims=[-0.1,2.0],title="Logistic")

# ╔═╡ ce4da9bf-316b-45d2-8504-f2a63cdda247
md"""
# Logistic Equation with Harvest

$\dot{x}=Rx\left(1-\frac{x}{K}\right)-H$
"""

# ╔═╡ 29474ca0-839b-45de-a7bf-7d91c5ea59aa
logharvest1(x,p,t)=p[1]*x*(1.0-x/p[2])-p[3]

# ╔═╡ 8fc407f4-a242-4cdc-b7e0-062c8f5782d1
@bind pars_harvest (
	PlutoUI.combine() do bind
		md"""
		R: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true))) \
		K: $(bind(Slider(0.01:0.02:2.0,default=0.1;show_value=true))) \
		H: $(bind(Slider(0.01:0.02:2.0,default=0.1;show_value=true))) \
		x0: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true)))
		"""
	end
)

# ╔═╡ 794936ae-d9ae-4319-994f-4282c99c4238
flux1D(logharvest1,pars_harvest[4],300.0,pars_harvest,(u)->(u<0);xlims=[0.0,2.0],title="Logistic with Harvest")

# ╔═╡ b13d261e-1c1b-41b0-ae86-e20cd2304727
md"""
## Critical Slowing Down
"""

# ╔═╡ c3757398-060c-412c-997b-58331b2dee04
@bind pars_csd (
	PlutoUI.combine() do bind
		md"""
		H: $(bind(Slider(0.2:0.002:0.25,default=0.1;show_value=true))) \
		S: $(bind(Slider(0:0.001:0.1,default=0.0;show_value=true)))
		"""
	end
)

# ╔═╡ f1f04dd4-0447-4e48-9bab-f34058a6a0f1
flux1D(logharvest1,0.5+sqrt(0.25-pars_csd[1]),200.0,[1.0,1.0,pars_csd[1]],10.0,pars_csd[2],(u)->(u<0);xlims=[0.0,1.0],title="Log whith Harvest perturbed")

# ╔═╡ c803ed90-958c-4d0e-84d5-3e8cc9c8fbfe
md"""
# Consumer Equation

$\dot{x} = Rx\left(1-\frac{x}{K}\right) - Px$
"""

# ╔═╡ b4ea8364-a582-443c-bdb1-46e75a5c4d4e
# Consumer Equation
consumer(x,p,t)=p[1]*x*(1.0-x/p[2])-p[3]*x

# ╔═╡ 5afa3aff-af15-429b-a0fb-9a0dfcad740d
@bind pars_consumer (
	PlutoUI.combine() do bind
		md"""
		R: $(bind(Slider(0:0.02:1.0,default=0.1;show_value=true))) \
		K: $(bind(Slider(0.01:0.02:2.0,default=0.1;show_value=true))) \
		P: $(bind(Slider(0.0:0.01:0.5,default=0.1;show_value=true))) \
		x0: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true)))
		"""
	end
)

# ╔═╡ 5018d342-0a51-4275-8c0f-1b5d253c2ec0
flux1D(consumer,pars_consumer[4],300.0,pars_consumer,xlims=[0.0,2.0],title="Consumer Equation")

# ╔═╡ 84a596c1-3a0a-4edd-a7ea-f3fcd8839c2a
md"""
# Logistic Equation with Outbreak

$\dot{x} = Rx\left(1-\displaystyle\frac{x}{K}\right)-P\displaystyle\frac{x^2}{1+x^2}$
"""

# ╔═╡ 91b2718a-a921-4bae-99c0-91ec9c7e6479
html"""

"""

# ╔═╡ 5a0b156e-b0b2-43b8-b15e-bcdb65c154bf
logoutbreak(x,p,t)=p[1]*x*(1.0-x/p[2])-p[3]*x*x/(1+x*x)

# ╔═╡ 6528501f-3bb2-48a7-b24e-d32400033538
@bind pars_outbreak (
	PlutoUI.combine() do bind
		md"""
		R: $(bind(Slider(0:0.02:2.0,default=0.1;show_value=true))) \
		K: $(bind(Slider(0.01:0.02:10.0,default=0.1;show_value=true))) \
		P: $(bind(Slider(0.0:0.01:1.0,default=0.1;show_value=true))) \
		x0: $(bind(Slider(0:0.02:8.0,default=0.1;show_value=true)))
		"""
	end
)

# ╔═╡ a3270d8b-f167-4fb9-bcc5-c62c91b39649
flux1D(logoutbreak,pars_outbreak[4],300.0,pars_outbreak;xlims=[-0.2,8.0],title="Log with Outbreak")

# ╔═╡ 0d0d6d2f-56b2-4e01-b9a4-6e469eac90ad
html"""

"""

# ╔═╡ 76cfd5c8-6298-4ae2-9d2d-9e60deb12f49
html"""

"""

# ╔═╡ 0bf40552-b627-4fa7-b3ba-1d8c7ed4a568
html"""

"""

# ╔═╡ Cell order:
# ╟─7c860884-5afd-4b4a-b358-4f2bce8bd395
# ╠═38c4e220-d708-11ec-3968-fbbd41c26155
# ╟─2ac364c2-cbdd-49b4-9f26-9fe89382be5e
# ╟─6b6a5e19-5298-4969-b1c9-699aa1cb2996
# ╟─62cba86d-4406-4d16-8715-b29bee7d3178
# ╟─6d64cc27-1059-4dd7-8f17-1c6fd9a2eca0
# ╠═4dc32b5a-5794-4d2d-844e-cdb80bec2e7b
# ╟─16a156b5-92ab-4e1c-ab70-c3b348186c57
# ╟─77766c99-4142-4acb-a855-ab133e67aa66
# ╟─ce4da9bf-316b-45d2-8504-f2a63cdda247
# ╠═29474ca0-839b-45de-a7bf-7d91c5ea59aa
# ╟─794936ae-d9ae-4319-994f-4282c99c4238
# ╟─8fc407f4-a242-4cdc-b7e0-062c8f5782d1
# ╟─b13d261e-1c1b-41b0-ae86-e20cd2304727
# ╟─f1f04dd4-0447-4e48-9bab-f34058a6a0f1
# ╟─c3757398-060c-412c-997b-58331b2dee04
# ╟─c803ed90-958c-4d0e-84d5-3e8cc9c8fbfe
# ╠═b4ea8364-a582-443c-bdb1-46e75a5c4d4e
# ╟─5018d342-0a51-4275-8c0f-1b5d253c2ec0
# ╟─5afa3aff-af15-429b-a0fb-9a0dfcad740d
# ╟─84a596c1-3a0a-4edd-a7ea-f3fcd8839c2a
# ╟─91b2718a-a921-4bae-99c0-91ec9c7e6479
# ╠═5a0b156e-b0b2-43b8-b15e-bcdb65c154bf
# ╟─a3270d8b-f167-4fb9-bcc5-c62c91b39649
# ╟─6528501f-3bb2-48a7-b24e-d32400033538
# ╟─0d0d6d2f-56b2-4e01-b9a4-6e469eac90ad
# ╟─76cfd5c8-6298-4ae2-9d2d-9e60deb12f49
# ╟─0bf40552-b627-4fa7-b3ba-1d8c7ed4a568


#=
Created on Wednesday 18 March 2020
Last update: Tuesday 23 March 2020

@author: Michiel Stock
michielfmstock@gmail.com

This is an illustration of solving the Krontrovich formulation of the optimal
transport problem. It also demonstrates the use of Convex.jl, a package for
convex optimization problems.
=#

using Convex, SCS

# first give the problem parameters, we use the dessert problem

# portions per person
a = [3.0, 3, 3, 4, 2, 2, 2, 1]
# quantities of each dessert
b = [4.0, 2, 6, 4, 4]

# should both have the same sum
@assert sum(a) ≈ sum(b)

preferences = [2 2 1 0 0;
              0 -2 -2 -2 2;
              1 2 2 2 -1;
              2 1 0 1 -1;
              0.5 2 2 1 0;
              0 1 1 1 -1;
             -2 2 2 1 1;
              2 1 2 1 -1]

C = -preferences

n, m = size(C)

P = Variable(n, m)

problem = minimize(sum(P .* C),  # objective
                    P >= 0,  # non-neg constraints
                    P * ones(m) == a,  # row marginals
                    P' * ones(n) == b)  # column marginals

solve!(problem, () -> SCS.Optimizer(verbose=false))

pstar = problem.optval  # best objective
Pstar = P.value  # distribution

using StatsPlots

groupedbar(Pstar, bar_position = :stack, title="Optimal distribution desserts\nobjective = $pstar")

savefig("figures/kantovorisc.png")


#=##############################################################################
# DESCRIPTION
    Methods for generation of conic sections
# AUTHORSHIP
  * Author    : Eduardo J Alvarez
  * Email     : Edo.AlvarezR@gmail.com
  * Created   : Sep 2019
  * License   : MIT License
=###############################################################################

"""
    `line_intersect(x1, y1, x2, y2, x3, y3, x4, y4)`

    Returns the intersect between two lines, with the first line
passing through the points `(x1, y1), (x2, y2)`, and the second line passing
through the points `(x3, y3), (x4, y4)`.
"""
function line_intersect(x1, y1, x2, y2, x3, y3, x4, y4)
    Px = (
            (x1*y2-y1*x2)*(x3-x4) - (x1-x2)*(x3*y4 - y3*x4)
         ) / (
            (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4)
         )
    Py = (
            (x1*y2-y1*x2)*(y3-y4) - (y1-y2)*(x3*y4-y3*x4)
         ) / (
            (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4)
         )

    return Px, Py
end

"""
    `generate_conic_fun(Ax::Real, Ay::Real, Bx::Real, By::Real, Cx::Real, Cy::Real, Sx::Real, Sy::Real)`

     Returns a conic curve function with control point A, B, and C, and shoulder point S.
        See Raymer's Aircraft Design, lofting chapter (p. 128).
"""
function generate_conic_fun(Ax::Real, Ay::Real, Bx::Real, By::Real,
                            Cx::Real, Cy::Real, Sx::Real, Sy::Real)

    """
        Receives a value between 0 and 1, and returns the
    corresponding point along the conic curve, with 0 being A
    and 1 B.
    """
    function conic_fun(s)
        if s<0 || s>1
            error("Invalid conic parameter $s. Value between 0 and 1 expected.")
        end

        # D: Point in between A and B
        Dx = Ax + s*(Bx-Ax)
        Dy = Ay + s*(By-Ay)

        # E: Intersect between AS and CD
        Ex, Ey = line_intersect(Ax, Ay, Sx, Sy, Cx, Cy, Dx, Dy)

        # F: Intersect between BS and CD
        Fx, Fy = line_intersect(Bx, By, Sx, Sy, Cx, Cy, Dx, Dy)

        # P: Intersect between AF and BE
        Px, Py = line_intersect(Ax, Ay, Fx, Fy, Bx, By, Ex, Ey)

        return Px, Py
    end

    return conic_fun
end

"""
    `generate_conic_fun(Ax::Real, Ay::Real, Bx::Real, By::Real, Cx::Real, Cy::Real, rho::Real)`

    Returns a conic curve function with control point A, B, and C, and shape parameter rho.
        See Raymer's Aircraft Design, lofting chapter (p. 132).

    Hyperbola: rho > 0.5
    Parabola:  rho = 0.5
    Ellipse:   rho < 0.5
    Circle:    rho = 0.4142 and AC = BC
"""
function generate_conic_fun(Ax::Real, Ay::Real, Bx::Real, By::Real,
                            Cx::Real, Cy::Real, rho::Real)

    # D: Midpoint between A and B
    Dx = Ax + 0.5*(Bx-Ax)
    Dy = Ay + 0.5*(By-Ay)

    # S: Shoulder point in between D and C
    Sx = Dx + rho*(Cx-Dx)
    Sy = Dy + rho*(Cy-Dy)

    return generate_conic_fun(Ax, Ay, Bx, By, Cx, Cy, Sx, Sy)
end

"""
    `conic_cross_section(Ps::Array{Array{T1, 1},1}, CPs::Array{Array{T2, 1},1},
rhos::Array{T3, 1}, ss::Array{Array{T4, 1},1})`

    Receives a collection of points `Ps` along an open contour, and
stretching control points `CPs`, shape parameters `rhos`, and probing
parameters `ss` associated to every section of the contour, and it returns
a compound-conic cross section (it is a closed loop, so the number of
sections is equal to the number of points)
"""
function conic_cross_section(Ps::Array{Array{T1, 1},1},
                             CPs::Array{Array{T2, 1},1},
                             rhos::Array{T3, 1},
                             ss::Array{Array{T4, 1},1}
                            ) where {T1<:Real, T2<:Real, T3<:Real, T4<:Real}
    nPs = size(Ps, 1)

    if size(CPs, 1)!=nPs
        error("Invalid CPs. Expected $nPs, got $(size(CPs, 1)).")
    elseif size(rhos, 1)!=nPs
        error("Invalid rhos. Expected $nPs, got $(size(rhos, 1)).")
    elseif size(ss, 1)!=nPs
        error("Invalid ss. Expected $nPs, got $(size(ss, 1)).")
    end

    points = []

    for i in 1:nPs
        A = Ps[i]
        B = Ps[i%nPs + 1]

        conic_fun = generate_conic_fun(Ps[i]..., Ps[i%nPs + 1]...,
                                                CPs[i]..., rhos[i])
        for s in ss[i]
            push!(points, [conic_fun(s)...])
        end
    end

    return points
end


function cart_to_cyl(x,y)
    return [sqrt(x*x + y*y), (atan(y, x) + 2π)%(2π)]
end
function cyl_to_cart(r,t)
    return [r*cos(t), r*sin(t)]
end
function dcart_to_cyl(x,y)
    r2 = x*x + y*y
    r = sqrt(r2)
    return [x/r y/r; -y/r2 x/r2]
end
function dcyl_to_cart(r,t)
    ct, st = cos(t), sin(t)
    return [ct -r*st;st r*ct]
end
function step(x, s, n)
    x_trj = zeros(3, n+1)
    x_trj[:,1] = x
    s0, s1, s2 = s[1], s[2], s[3]
    for i = 2:n+1
           x_trj[:,i] = next(x_trj[:,i-1],s)
    end
    return x_trj
end
function next(u, s)
    x, y, z = u
    s0, s1, s2 = s
    r, t = cart_to_cyl(x, y)
    r1 = s0 + (r - s0)/s1 + cos(t)/2
    t1 = (2*t + s2/4*sin(4*t)) % (2π)
    z1 = z/s1 + sin(t)/2
    x1, y1 = cyl_to_cart(r1,t1)
    return [x1, y1, z1]
end
function dstep(u::Array{Float64,1},s::Array{Float64,1})
    du = zeros(2,2)
    s0, s1, s2 = s
    x, y, z = u
    r, t = cart_to_cyl(x,y)
    r1 = s0 + (r - s0)/s1 + cos(t)/2
    t1 = (2*t + s2/4*sin(4*t)) % (2π)
    z1 = z/s1 + sin(t)/2

    drt1drt = [1/s1 -sin(t)/2;0 2 + s2*cos(4*t)]
    dz1dt = cos(t)/2
    dz1dz = 1/s1

    drtdxy = dcart_to_cyl(x,y)
    dtdx, dtdy = drtdxy[2,1], drtdxy[2,2]
    dxy1dxy = dcyl_to_cart(r1,t1)*drt1drt*drtdxy
    dz1 = [dz1dt*dtdx dz1dt*dtdy dz1dz]

    du = [dxy1dxy [0;0]]
    du = [du;dz1]
    return du
end
function pert(u::Array{Float64,1}, s::Array{Float64,1},
              p::Int64)
    x, y, z = u
    s0, s1, s2 = s
    r, t = cart_to_cyl(x,y)
    r1 = s0 + (r - s0)/s1 + cos(t)/2
    t1 = 2*t + s2/4*sin(4*t)
    z1 = z/s1 + sin(t)/2
    x1, y1 = cyl_to_cart(r1,t1)
    dxyz1drtz1 = [dcyl_to_cart(r1,t1) [0;0]]
    dxyz1drtz1 = [dxyz1drtz1; [0 0 1]]

    if p==1
        drtz1ds = [1-1/s1; 0; 0]
    elseif p==2
        drtz1ds = [-(r - s0)/s1^2; 0; -z/s1^2]
    elseif p==3
        drtz1ds = [0; (1/4)*sin(4*t); 0]
    else
        println("parameter perturbation is only defined
                for indices 1 through 3")
        return 0
    end
    return dxyz1drtz1*drtz1ds
end


"""
    Gamma

* Parametrization
parametrized in \\alpha and \\theta

* Score

* Fisher Information

* `time_varying_params` map.

* Default link
"""
Gamma

function score!(score_til::Matrix{T}, y::T, ::Type{Gamma}, param::Matrix{T}, t::Int) where T
    score_til[t, 1] = log(y) - digamma(param[t, 1]) - log(param[t, 2])
    score_til[t, 2] = y/param[t, 2]^2 - param[t, 1]/param[t, 2]
    return
end

function fisher_information!(aux::AuxiliaryLinAlg{T}, ::Type{Gamma}, param::Matrix{T}, t::Int) where T
    aux.fisher[1, 1] = trigamma(param[t, 1])
    aux.fisher[2, 2] = param[t, 1] / param[t, 2]^2
    aux.fisher[2, 1] = 1/param[t, 2]
    aux.fisher[1, 2] = 1/param[t, 2]
    return
end

function log_likelihood(::Type{Gamma}, y::Vector{T}, param::Matrix{T}, n::Int) where T
    loglik = 0.0
    for t in 1:n
        loglik += (param[t, 1] - 1)*log(y[t]) - y[t]/param[t, 2] - loggamma(param[t, 1]) - param[t, 1]*log(param[t, 2])
    end
    return -loglik
end

# Links
function link!(param_tilde::Matrix{T}, ::Type{Gamma}, param::Matrix{T}, t::Int) where T
    param_tilde[t, 1] = link(LogLink, param[t, 1], zero(T))
    param_tilde[t, 2] = link(LogLink, param[t, 2], zero(T))
    return
end
function unlink!(param::Matrix{T}, ::Type{Gamma}, param_tilde::Matrix{T}, t::Int) where T
    param[t, 1] = unlink(LogLink, param_tilde[t, 1], zero(T))
    param[t, 2] = unlink(LogLink, param_tilde[t, 2], zero(T))
    return
end
function jacobian_link!(aux::AuxiliaryLinAlg{T}, ::Type{Gamma}, param::Matrix{T}, t::Int) where T
    aux.jac[1] = jacobian_link(LogLink, param[t, 1], zero(T))
    aux.jac[2] = jacobian_link(LogLink, param[t, 2], zero(T))
    return
end

# utils
function update_dist(::Type{Gamma}, param::Matrix{T}, t::Int) where T
    small_threshold!(param, SMALL_NUM, t)
    return Gamma(param[t, 1], param[t, 2])
end

function params_sdm(d::Gamma)
    return Distributions.params(d)
end

function num_params(::Type{Gamma})
    return 2
end


"""
    Circle(c,r)

Circle in the complex plane. \$|c-z|=r\$.

#### Arguments
- `c::N`: Center.
- `r::R`: Radius.
"""
struct Circle{N <: Number, R <: Real} <: AbstractCircle
  center::N
  radius::R

  function Circle{N,R}(c0::N, r0::R)  where {N <: Number, R <: Real}
    #if r0 < 0
    #  error("Circle can not have negative radius.")
    #end
    new(c0,r0)
  end
end

Circle(c0::N, r0::R) where {N <: Number, R <: Real} = Circle{N,R}(c0,r0)

center(c::Circle) = c.center
centerC(c::Circle) = c.center
centerR2(c::Circle) = real(c.center), imag(c.center)
centerx(c::Circle) = real(c.center)
centery(c::Circle) = imag(c.center)
radius(c::Circle) = c.radius
radius2(c::Circle) = c.radius*c.radius

Base.show(io::IO, c::Circle{N,R}) where {N,R} =
print(io, "Circle{$N,$R}: |z - (", c.center, ")| = ", c.radius)

"""
    CircleR2(x,y,r)

Circle in the plane \$\\mathbb{R}^2\$. \$|(x0,y0)-(x,y)|=r\$.
"""
struct CircleR2{R <: Real} <: AbstractCircle
  centerx::R
  centery::R
  radius::R

  function CircleR2{R}(x0::R, y0::R, r0::R)  where {R <: Real}
    #if r0 < 0
    #  error("Circle can not have negative radius.")
    #end
    new(x0,y0,r0)
  end
end

CircleR2(x0::R, y0::R, r0::R) where {R <: Real} = CircleR2{R}(x0,y0,r0)
CircleR2(x0::Real, y0::Real, r0::Real) = CircleR2(promote(x0,y0,r0)...)

center(c::CircleR2) = c.centerx, c.centery
centerC(c::CircleR2) = complex(c.centerx, c.centery)
centerR2(c::CircleR2) = c.centerx, c.centery
centerx(c::CircleR2) = c.centerx
centery(c::CircleR2) = c.centery
radius(c::CircleR2) = c.radius
radius2(c::CircleR2) = c.radius*c.radius

Base.show(io::IO, c::CircleR2{R}) where {R} =
print(io, "CircleR2{$R}: |(x,y) - (", c.centerx, ",", c.centery, " )| = ", c.radius)

"""
    Arc(c,p1,p2)

Circular arc the complex plane.

#### Arguments:
- `c::N`: center.
- `p1::N`: Initial point.
- `p2::N`: Final point.
"""
struct Arc{N} <: AbstractArc
  center::N
  p1::N
  p2::N

  function Arc{N}(c0::N, p10::N, p20::N)  where {N <: Number}
    #if r0 < 0
    #  error("Circle can not have negative radius.")
    #end
    new(c0,p10,p20)
  end
end

Arc(c0::N, p10::N, p20::N) where {N <: Number} = Arc{N}(c0,p10,p20)
Arc(c0::Number, p10::Number, p20::Number) = Arc(promote(c0,p10,p20)...)
Arc(c0::Number, r::Real, θ1::Real, θ2::Real) =
Arc(c0, r*(cos(θ1)+sin(θ1)*im), r*(cos(θ2)+sin(θ2)*im))

center(ca::Arc) = ca.center
centerC(ca::Arc) = ca.center
centerR2(ca::Arc) = real(ca.center), imag(ca.center)
radius(ca::Arc) = abs(ca.center - ca.p1)
radius2(ca::Arc) = abs2(ca.center - ca.p1)
initialpoint(ca::Arc) = ca.p1
finalpoint(ca::Arc) = ca.p2
initialpointC(ca::Arc) = ca.p1
finalpointC(ca::Arc) = ca.p2
initialpointR2(ca::Arc) = real(ca.p1), imag(ca.p1)
finalpointR2(ca::Arc) = real(ca.p2), imag(ca.p2)

Base.show(io::IO, ca::Arc{N}) where {N} =
print(io, "Arc{$N}: center=", ca.center, ", extremes ", ca.p1, ", ", ca.p2)

"""
    BiArc(c,p1,p2,p3,p4)

Two arcs in the same circunference.
"""
struct BiArc{N <: Number} <: AbstractCircularCurve
  center::N
  p1::N
  p2::N
  p3::N
  p4::N

  function BiArc{N}(c0::N, p10::N, p20::N, p30::N, p40::N)  where {N <: Number}
    new(c0,p10,p20,p30,p40)
  end
end

BiArc(c0::N, p10::N, p20::N, p30::N, p40::N) where {N <: Number} = BiArc{N}(c0,p10,p20,p30,p40)
BiArc(c0::Number, p10::Number, p20::Number, p30::Number, p40::Number) = BiArc(promote(c0,p10,p20,p30,p40)...)

toarcs(ba::BiArc) = Arc(ba.center,ba.p1,ba.p2), Arc(ba.center,ba.p3,ba.p4)

#=
ToDo!
ArcR2
BiArcR2
=#

"""
Checks if `z` is inside the circle `c` with \$|center-z|^2 < radius^2\$.
"""
isinside(z::Number, c::Circle) = abs2(c.center - z) < (c.radius * c.radius)

"""
Checks if `z` is outside the circle `c` with \$|center-z|^2 > radius^2\$.
"""
isoutside(z::Number, c::Circle) = abs2(c.center - z) > (c.radius * c.radius)

"""
    circlepoints(c, [θ0, θ1; numpts]) -> Array{Tuple{Float64,Float64}}

Create an array of points (`Tuple{Float64,Float64}`) in a given circle

#### Arguments
- `c::Circle`: A circle.
- `θ0::Real`: Initial angle.
- `θ1::Real`: Final angle.
- `numpts::Integer`: Number of points to create.
"""
circlepoints(c::AbstractCircle, θ0::Real=0, θ1::Real=2π; numpts::Integer=100) =
[ (centerx(c) + radius(c)*cos(θ), centery(c) + radius(c)*sin(θ)) for θ ∈ θ0:((θ1 - θ0)/numpts):θ1 ]

circlepoints(x::Real, y::Real, r::Real, θ0::Real=0, θ1::Real=2π; numpts::Integer=100) =
[ (x + r*cos(θ), y + r*sin(θ)) for θ ∈ θ0:((θ1 - θ0)/numpts):θ1 ]

"""
    circlepoints(c, [θ0, θ1; numpts]) -> Array{Complex{Float64}}

Create an array of complex number in a given circle.

#### Arguments
- `c::Circle`: A circle.
- `θ0::Real`: Initial angle.
- `θ1::Real`: Final angle.
- `numpts::Integer`: Number of points to create.
"""
circlecomplexes(c::AbstractCircle, θ0::Real=0, θ1::Real=2π; numpts::Integer=100) =
[ centerC(c) + radius(c)*exp(θ*im) for θ ∈ θ0:((θ1 - θ0)/numpts):θ1 ]

circlecomplexes(z::Number, r::Real, θ0::Real=0, θ1::Real=2π; numpts::Integer=100) =
[ z + r*cos(θ) + r*sin(θ)*im for θ ∈ θ0:((θ1 - θ0)/numpts):θ1 ]


"""
constructDormandPrince()

Constructs the tableau object for the Dormand-Prince Order 4/5 method.
"""
function constructDormandPrince(T::Type = Float64)
  A = [0 0 0 0 0 0 0
      1//5 0 0 0 0 0 0
      3//40 9//40 0 0 0 0 0
      44//45 -56//15 32//9 0 0 0 0
      19372//6561 -25360//2187 64448//6561 -212//729 0 0 0
      9017//3168 -355//33 46732//5247 49//176 -5103//18656 0 0
      35//384 0 500//1113 125//192 -2187//6784 11//84 0]
  c = [0;1//5;3//10;4//5;8//9;1;1]
  α = [35//384;0;500//1113;125//192;-2187//6784;11//84;0]
  αEEst = [5179//57600;0;7571//16695;393//640;-92097//339200;187//2100;1//40]
  A = map(T,A)
  α = map(T,α)
  αEEst = map(T,αEEst)
  c = map(T,c)
  return(ExplicitRKTableau(A,c,α,5,αEEst=αEEst,adaptiveorder=4,fsal=true))
end

"""
ODE_DEFAULT_TABLEAU

Sets the default tableau for the ODE solver. Currently Dormand-Prince 4/5.
"""
const ODE_DEFAULT_TABLEAU = constructDormandPrince()

@inline ODE_DEFAULT_NORM(u) = sqrt(sum(abs2,u) / length(u))
@inline ODE_DEFAULT_NORM(u::Number) = abs(u)
@inline ODE_DEFAULT_ISOUTOFDOMAIN(t,u) = false
@inline ODE_DEFAULT_PROG_MESSAGE(dt,t,u) = "dt="*string(dt)*"\nt="*string(t)*"\nmax u="*string(maximum(abs.(u)))
@inline ODE_DEFAULT_UNSTABLE_CHECK(dt,t,u) = any(isnan,u)


import FractionalCalculus.FracDiffAlg

"""
Riesz sense fractional derivative
"""
abstract type Riesz <: FracDiffAlg end

"""
# Riesz sense symmetric fractional derivative algorithm.

    fracdiff(f, α, end_point, h, Riesz_Symmetric())

Compute fractional derivative of Riesz sense using Triangular Strip Matrix algorithm.

### Example

```julia-repl
julia> fracdiff(x->x, 0.5, 1, 0.01, Riesz_Symmetric())
```
"""
struct Riesz_Symmetric <: Riesz end

################################################################
###                    Type defination done                  ###
################################################################

function fracdiff(f, α, end_point, h, ::Riesz_Symmetric)
    N=Int(floor(end_point/h))

    mat = RieszMatrix(α, N+1, h)
    return mat*f.(collect(0:h:end_point))
end
function RieszMatrix(α, N, h)
    caputo = B(N+1, α)
    caputo = caputo[2:(N+1), 1:N]
    result = 1/2*(caputo+caputo')
    result = h^(-α)*result

    return result
end

function B(N, p)
    result=zeros(N, N)
    temp=omega(N, p)

    @inbounds @simd for i ∈ 1:N
        @views result[i, 1:i]=reverse(temp[1:i])
    end

    return result
end

function omega(n, p)
    omega = zeros(n+1)

    omega[1]=1
    @fastmath @inbounds @simd for i ∈ 1:n
        omega[i+1]=(1-(p+1)/i)*omega[i]
    end

    return omega
end