text:
#####################
# 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

max_stars_repo_name: sisl/SatelliteDynamics

max_issues_repo_name: sisl/SatelliteDynamics

max_forks_repo_name: sisl/SatelliteDynamics


text:
mutable struct TwoBodyDB <: Database
    m::Int
    n::Int

    rawDB
    chart::Vector{Tuple}
    degeneracy::Dict

    fx::Vector{Number}
    fy::Vector{Number}
    fz::Vector{Number}

    idx::Dict
    idxset::Dict

    xi::Dict
    yi::Dict
    zi::Dict

    ri::Dict

    xi1::Dict
    yi1::Dict
    zi1::Dict

    ri1::Dict

    xi_i1::Dict
    yi_i1::Dict
    zi_i1::Dict

    rii1::Dict
end

function TwoBodyDB(numbers::Array{Int, 1})
    chart, degeneracy = getchart(Tuple(numbers), 2)
    m = length(chart)
    n = 0

    fx = Vector{Number}([])
    fy = Vector{Number}([])
    fz = Vector{Number}([])

    # Indexing to force
    idx = Dict{Tuple{Int, Int}, Vector{Int}}([(key, Vector{Int}([])) for key in chart])
    idxset = Dict{Tuple{Int, Int}, Set}([(key, Set()) for key in chart])

    xi = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    ri = Dict{Tuple{Int, Int}, Array{Array{Float64, 1}, 1}}([(key, Array{Array{Float64, 1}, 1}([])) for key in chart])

    xi1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    # ri1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    ri1 = Dict{Tuple{Int, Int}, Array{Array{Float64, 1}, 1}}([(key, Array{Array{Float64, 1}, 1}([])) for key in chart])

    xi_i1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi_i1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi_i1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    rii1 = Dict{Tuple{Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    TwoBodyDB(m, n, [], chart, degeneracy, fx, fy, fz, idx, idxset,
              xi, yi, zi, ri, xi1, yi1, zi1, ri1, xi_i1, yi_i1, zi_i1, rii1)
end

update!(db::TwoBodyDB,
        coords::Cartesian{T1, 2},
        forces::Array{T2, 2}; kwargs...) where {T1<:Number, T2<:Number} = #=
        =# update!(db, coords[nax, :, :], forces[nax, :, :]; kwargs...)
update!(db::TwoBodyDB, coords::Cartesian{T1, 3},
        forces::Array{T2, 3}; kwargs...) where {T1<:Number, T2<:Number}#=
    =# = update!(db, convert(TwoBody, coords; kwargs...), forces)

function update!(db::TwoBodyDB, coords::SparseAtomic, forces::Array{T, 3}) where {T<:Number}
    M, A, _ = size(forces)
    # size between coords and forces is inconsistent
    @assert (M, A) == Tuple(size(coords)[1:2])
    # "No data found"
    @assert M > 0

    chart = keys(db.idx)
    numbers = coords.cache["numbers"]

    count = db.n + 1

    for m=1:M
        for a=1:A
            push!(db.rawDB, coords[m, a])
            push!(db.fx, forces[m, a, 1]);
            push!(db.fy, forces[m, a, 2]);
            push!(db.fz, forces[m, a, 3])
            datum = coords[m, a].descriptor
            for cluster in chart
                append!(db.xi[cluster],    datum[cluster]["xi"])
                append!(db.yi[cluster],    datum[cluster]["yi"])
                append!(db.zi[cluster],    datum[cluster]["zi"])
                append!(db.ri[cluster],    datum[cluster]["ri"])
                append!(db.xi1[cluster],   datum[cluster]["xi1"])
                append!(db.yi1[cluster],   datum[cluster]["yi1"])
                append!(db.zi1[cluster],   datum[cluster]["zi1"])
                append!(db.ri1[cluster],   datum[cluster]["ri1"])
                append!(db.xi_i1[cluster], datum[cluster]["xi_i1"])
                append!(db.yi_i1[cluster], datum[cluster]["yi_i1"])
                append!(db.zi_i1[cluster], datum[cluster]["zi_i1"])
                append!(db.rii1[cluster],  datum[cluster]["rii1"])

                idx = similar(Vector{Int}, length(datum[cluster]["xi"]))
                fill!(idx, count)
                append!(db.idx[cluster], idx)
                union!(db.idxset[cluster], count)
            end

            count += 1
            db.n += 1
        end
    end
end

"""
 idx refers to the number where its enviroment located in the array.
"""
mutable struct ThreeBodyDB <: Database
    m::Int                  # Number of rows
    n::Int                  # Number of columns

    rawDB
    chart::Vector{Tuple}
    degeneracy::Dict

    fx::Vector{Number}
    fy::Vector{Number}
    fz::Vector{Number}

    idx::Dict
    idxset::Dict

    xi::Dict
    yi::Dict
    zi::Dict
    ri::Dict

    xi1::Dict
    yi1::Dict
    zi1::Dict
    ri1::Dict

    xi2::Dict
    yi2::Dict
    zi2::Dict
    ri2::Dict

    xi_i1::Dict
    yi_i1::Dict
    zi_i1::Dict

    xi_i2::Dict
    yi_i2::Dict
    zi_i2::Dict

    xi1_i2::Dict
    yi1_i2::Dict
    zi1_i2::Dict

    rii1::Dict
    rii2::Dict
    ri1i2::Dict

end

function ThreeBodyDB(numbers::Array{Int, 1})
    chart, degeneracy = getchart(Tuple(numbers), 3)
    m = length(chart)
    n = 0

    fx = Vector{Number}([])
    fy = Vector{Number}([])
    fz = Vector{Number}([])

    # Indexing to force
    idx = Dict{Tuple{Int, Int, Int}, Vector{Int}}([(key, Vector{Int}([])) for key in chart])
    idxset = Dict{Tuple{Int, Int, Int}, Set}([(key, Set()) for key in chart])

    xi = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    ri = Dict{Tuple{Int, Int, Int}, Array{Array{Float64, 1}, 1}}([(key, Array{Array{Float64, 1}, 1}([])) for key in chart])

    xi1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    ri1 = Dict{Tuple{Int, Int, Int}, Array{Array{Float64, 1}, 1}}([(key, Array{Array{Float64, 1}, 1}([])) for key in chart])

    xi2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    ri2 = Dict{Tuple{Int, Int, Int}, Array{Array{Float64, 1}, 1}}([(key, Array{Array{Float64, 1}, 1}([])) for key in chart])

    xi_i1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi_i1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi_i1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    xi_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    xi1_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    yi1_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    zi1_i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    rii1 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    rii2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])
    ri1i2 = Dict{Tuple{Int, Int, Int}, Vector{Number}}([(key, Vector{Number}([])) for key in chart])

    ThreeBodyDB(m, n, [], chart, degeneracy, fx, fy, fz, idx, idxset, xi, yi, zi, ri, xi1, yi1, zi1, ri1, xi2, yi2, zi2, ri2,
                xi_i1, yi_i1, zi_i1, xi_i2, yi_i2, zi_i2, xi1_i2, yi1_i2, zi1_i2,
                rii1, rii2, ri1i2)
end

update!(db::ThreeBodyDB, coords::Cartesian{T, 2}, forces::Array{Number, 2}; kwargs...) where {T<:Number} = update!(db, coords[nax, :, :], forces[nax, :, :]; kwargs...)
update!(db::ThreeBodyDB, coords::Cartesian{T, 3}, forces::Array{Number, 3}; kwargs...) where {T<:Number} = update!(db, convert(TwoBody, coords; kwargs...), forces)

function update!(db::ThreeBodyDB, coords::SparseAtomic, forces::Array{T, 3}) where {T<:Number}
    M, A, _ = size(forces)
    # "size between coords and forces is inconsistent"
    @assert (M, A) == Tuple(size(coords)[1:2])
    # No data found"
    @assert M > 0

    chart = keys(db.idx)
    numbers = coords.cache["numbers"]

    count = db.n + 1
    for m=1:M
        for a=1:A
            push!(db.rawDB, coords[m, a])
            push!(db.fx, forces[m, a, 1]); push!(db.fy, forces[m, a, 2]);
            push!(db.fz, forces[m, a, 3])

            datum = coords[m, a].descriptor
            for cluster in chart
                append!(db.xi[cluster],    datum[cluster]["xi"])
                append!(db.yi[cluster],    datum[cluster]["yi"])
                append!(db.zi[cluster],    datum[cluster]["zi"])
                append!(db.ri[cluster],    datum[cluster]["ri"])
                append!(db.xi1[cluster],   datum[cluster]["xi1"])
                append!(db.yi1[cluster],   datum[cluster]["yi1"])
                append!(db.zi1[cluster],   datum[cluster]["zi1"])
                append!(db.ri1[cluster],   datum[cluster]["ri1"])
                append!(db.xi2[cluster],   datum[cluster]["xi2"])
                append!(db.yi2[cluster],   datum[cluster]["yi2"])
                append!(db.zi2[cluster],   datum[cluster]["zi2"])
                append!(db.ri2[cluster],   datum[cluster]["ri2"])
                append!(db.xi_i1[cluster], datum[cluster]["xi_i1"])
                append!(db.yi_i1[cluster], datum[cluster]["yi_i1"])
                append!(db.zi_i1[cluster], datum[cluster]["zi_i1"])
                append!(db.xi_i2[cluster], datum[cluster]["xi_i2"])
                append!(db.yi_i2[cluster], datum[cluster]["yi_i2"])
                append!(db.zi_i2[cluster], datum[cluster]["zi_i2"])
                append!(db.xi1_i2[cluster],datum[cluster]["xi1_i2"])
                append!(db.yi1_i2[cluster],datum[cluster]["yi1_i2"])
                append!(db.zi1_i2[cluster],datum[cluster]["zi1_i2"])
                append!(db.rii1[cluster],  datum[cluster]["rii1"])
                append!(db.rii2[cluster],  datum[cluster]["rii2"])
                append!(db.ri1i2[cluster], datum[cluster]["ri1i2"])

                idx = similar(Vector{Int}, length(datum[cluster]["xi"]))
                fill!(idx, count)
                append!(db.idx[cluster], idx)
                union!(db.idxset[cluster], count)
            end

            count += 1
            db.n += 1
        end
    end
end

mutable struct TwoThreeBodyDB <: Database
    twobody::TwoBodyDB
    threebody::ThreeBodyDB
end

function TwoThreeBodyDB(numbers)
    twobody = TwoBodyDB(numbers)
    threebody = ThreeBodyDB(numbers)
    TwoThreeBodyDB(twobody, threebody)
end

update!(db::TwoThreeBodyDB, coords::Cartesian{T, 2},
        forces::Array{Number, 2}; kwargs...) where {T<:Number}#=
        =# = update!(db, coords[nax, :, :], forces[nax, :, :]; kwargs...)
update!(db::TwoThreeBodyDB, coords::Cartesian{T, 3},
        forces::Array{Number, 3}; kwargs...) where {T<:Number}#=
        =# = update!(db, convert(TwoBody, coords; kwargs...), forces)

function update!(db::TwoThreeBodyDB, coords, forces)
    update!(db.twobody, coords, forces)
    update!(db.threebody, coords, forces)
end

ext: jl

max_stars_repo_name: schinavro/taps

max_issues_repo_name: schinavro/taps

max_forks_repo_name: schinavro/taps


text:
randdual(x, ::Val{N}=Val(3)) where {N} = ForwardDiff.Dual(x, ntuple(_ -> randn(), Val(N))...)
@time @testset "ForwardDiff.jl" begin
    m = 53
    n = 63
    k = 73

    A1 = rand(Float64, m, k)
    B1 = rand(Float64, k, n)
    C1 = rand(Float64, m, n)

    A2 = deepcopy(A1)
    B2 = deepcopy(B1)
    C2 = deepcopy(C1)

    α = Float64(2.0)
    β = Float64(2.0)

    Octavian.matmul!(C1, A1, B1, α, β)
    LinearAlgebra.mul!(C2, A2, B2, α, β)
    @test C1 ≈ C2

    @testset "real array from the left" begin
        config = ForwardDiff.JacobianConfig(nothing, C1, B1)
        I = LinearAlgebra.I(size(B1, 2))

        J1 = ForwardDiff.jacobian((C, B) -> Octavian.matmul!(C, A1, B), C1, B1, config)
        @test J1 ≈ kron(I, A1)

        J2 = ForwardDiff.jacobian((C, B) -> LinearAlgebra.mul!(C, A2, B), C2, B2, config)
        @test J1 ≈ kron(I, A2)
        @test J1 ≈ J2

        J3 = ForwardDiff.jacobian((C, B) -> Octavian.matmul_serial!(C, A1, B), C1, B1, config)
        @test J3 ≈ kron(I, A1)
        @test J3 ≈ J2

    end

    @testset "real array from the right" begin
        # dense and column-major arrays
        config = ForwardDiff.JacobianConfig(nothing, C1, A1)

        J1 = ForwardDiff.jacobian((C, A) -> Octavian.matmul!(C, A, B1), C1, A1, config)
        J2 = ForwardDiff.jacobian((C, A) -> LinearAlgebra.mul!(C, A, B2), C2, A2, config)
        J3 = ForwardDiff.jacobian((C, A) -> Octavian.matmul_serial!(C, A, B1), C1, A1, config)
        @test J1 ≈ J2 ≈ J3

        # transposed arrays
        A1new = Matrix(A1')'
        A2new = Matrix(A2')'
        config = ForwardDiff.JacobianConfig(nothing, C1, A1new)

        J1 = ForwardDiff.jacobian((C, A) -> Octavian.matmul!(C, A, B1), C1, A1new, config)
        J2 = ForwardDiff.jacobian((C, A) -> LinearAlgebra.mul!(C, A, B2), C2, A2new, config)
        J3 = ForwardDiff.jacobian((C, A) -> Octavian.matmul_serial!(C, A, B1), C1, A1new, config)
        @test J1 ≈ J2 ≈ J3

        # direct version using dual numbers
        A1dual = zeros(eltype(config), reverse(size(A1))...)
        A1dual .= A1'
        C1dual = zeros(eltype(config), size(C1)...)

        A2dual = deepcopy(A1dual)
        C2dual = deepcopy(C1dual)
        C3dual = similar(C1dual); C4dual = similar(C2dual)
        Octavian.matmul!(C1dual, A1dual', B1)
        Octavian.matmul!(C2dual, A2dual', B2)
        Octavian.matmul_serial!(C3dual, A1dual', B1)
        Octavian.matmul_serial!(C4dual, A2dual', B2)
        @test C1dual ≈ C2dual ≈ C3dual ≈ C4dual
    end

  @testset "two dual arrays" begin
    A1d = randdual.(A1)
    B1d = randdual.(B1)
    @test reinterpret(Float64, Octavian.matmul(A1d, B1d, 1.3)) ≈ reinterpret(Float64, Octavian.matmul_serial(A1d, B1d, 1.3)) ≈ reinterpret(Float64, (A1d * B1d) .* 1.3)
    @test reinterpret(Float64, Octavian.matmul(@view(A1d[begin:end-1,:]), B1d)) ≈ reinterpret(Float64, Octavian.matmul_serial(@view(A1d[begin:end-1,:]), B1d)) ≈ reinterpret(Float64, @view(A1d[begin:end-1,:]) * B1d)
  end
end


text:
@doc raw"""
# Harmonic Oscillator

"""
module HarmonicOscillator

    using GeometricBase
    using GeometricEquations
    using Parameters

    export harmonic_oscillator_ode, harmonic_oscillator_iode, harmonic_oscillator_pode, harmonic_oscillator_hode, harmonic_oscillator_sode,
           harmonic_oscillator_dae, harmonic_oscillator_idae, harmonic_oscillator_pdae, harmonic_oscillator_hdae

    export hamiltonian, compute_energy_error

    const Δt = 0.1
    const nt = 10

    const k = 0.5
    const ω = √k
    const p = (k=k, ω=ω)

    ϑ₁(t,q) = q[2]
    ϑ₂(t,q) = zero(eltype(q))

    function ϑ(q)
        p = zero(q)
        p[1] = ϑ₁(0,q)
        p[2] = ϑ₂(0,q)
        return p
    end

    function hamiltonian(t, q, params)
        @unpack k = params
        q[2]^2 / 2 + k * q[1]^2 / 2
    end

    function hamiltonian(t, q, p, params)
        @unpack k = params
        p[1]^2 / 2 + k * q[1]^2 / 2
    end

    function lagrangian(t, q, v, params)
        ϑ₁(t,q) * v[1] + ϑ₂(t,q) * v[2] - hamiltonian(t, q, params)
    end

    t₀=0.0
    q₀=[0.5, 0.0]
    z₀=[0.5, 0.0, 0.5]
    p₀=ϑ(q₀)

    A = sqrt(q₀[2]^2 / k + q₀[1]^2)
    ϕ = asin(q₀[1] / A)

    refq = A * sin(ω * Δt * nt + ϕ)
    refp = ω * Δt * nt * A * cos(ω * Δt * nt + ϕ)
    refx = [refq, refp]

    function oscillator_ode_v(t, x, v, params)
        @unpack k = params
        v[1] = x[2]
        v[2] = -k*x[1]
        nothing
    end

    function harmonic_oscillator_ode(x₀=q₀, params=p)
        # @assert size(x₀,1) == 2
        ODE(oscillator_ode_v, x₀; invariants=(h=hamiltonian,), parameters=params)
    end

    function oscillator_pode_v(t, q, p, v, params)
        v[1] = p[1]
        nothing
    end

    function oscillator_pode_f(t, q, p, f, params)
        @unpack k = params
        f[1] = -k*q[1]
        nothing
    end

    function harmonic_oscillator_pode(q₀=[q₀[1]], p₀=[p₀[1]], params=p)
        # @assert length(q₀) == length(p₀)
        # @assert all([length(q) == length(p) == 1 for (q,p) in zip(q₀,p₀)])
        # @assert size(q₀,1) == size(p₀,1) == 1
        PODE(oscillator_pode_v, oscillator_pode_f, q₀, p₀; invariants=(h=hamiltonian,), parameters=params)
    end

    function harmonic_oscillator_hode(q₀=[q₀[1]], p₀=[p₀[1]], params=p)
        # @assert length(q₀) == length(p₀)
        # @assert all([length(q) == length(p) == 1 for (q,p) in zip(q₀,p₀)])
        # @assert size(q₀,1) == size(p₀,1) == 1
        HODE(oscillator_pode_v, oscillator_pode_f, hamiltonian, q₀, p₀; parameters=params)
    end

    function oscillator_sode_v_1(t, q, v, params)
        v[1] = q[2]
        v[2] = 0
        nothing
    end

    function oscillator_sode_v_2(t, q, v, params)
        @unpack k = params
        v[1] = 0
        v[2] = -k*q[1]
        nothing
    end

    function oscillator_sode_q_1(t, q̄, q, h, params)
        q[1] = q̄[1] + h * q̄[2]
        q[2] = q̄[2]
        nothing
    end

    function oscillator_sode_q_2(t, q̄, q, h, params)
        @unpack k = params
        q[1] = q̄[1]
        q[2] = q̄[2] - h * k*q̄[1]
        nothing
    end

    function harmonic_oscillator_sode(q₀=q₀, params=p)
        SODE((oscillator_sode_v_1, oscillator_sode_v_2),
             (oscillator_sode_q_1, oscillator_sode_q_2),
             q₀; parameters=params)
    end

    function oscillator_iode_ϑ(t, q, p, params)
        p[1] = q[2]
        p[2] = 0
        nothing
    end

    function oscillator_iode_ϑ(t, q, v, p, params)
        oscillator_iode_ϑ(t, q, p, params)
    end

    function oscillator_iode_f(t, q, v, f, params)
        @unpack k = params
        f[1] = -k*q[1]
        f[2] = v[1] - q[2]
        nothing
    end

    function oscillator_iode_g(t, q, λ, g, params)
        g[1] = 0
        g[2] = λ[1]
        nothing
    end

    function oscillator_iode_v(t, q, v, params)
        @unpack k = params
        v[1] = q[2]
        v[2] = -k*q[1]
        nothing
    end

    function harmonic_oscillator_iode(q₀=q₀, p₀=ϑ(q₀), params=p)
        # @assert size(q₀,1) == size(p₀,1) == 2
        IODE(oscillator_iode_ϑ, oscillator_iode_f,
             oscillator_iode_g, q₀, p₀;
             invariants=(h=hamiltonian,), parameters=params,
             v̄=oscillator_iode_v)
    end

    function oscillator_dae_v(t, z, v, params)
        @unpack k = params
        v[1] = z[2]
        v[2] = -k*z[1]
        v[3] = z[2] - k*z[1]
        nothing
    end

    function oscillator_dae_u(t, z, λ, u, params)
        u[1] = -λ[1]
        u[2] = -λ[1]
        u[3] = +λ[1]
    end

    function oscillator_dae_ϕ(t, z, ϕ, params)
        ϕ[1] = z[3] - z[1] - z[2]
    end

    function harmonic_oscillator_dae(z₀=z₀, λ₀=[zero(eltype(z₀))], params=p)
        # @assert size(z₀,1) == 3
        # @assert size(λ₀,1) == 1
        # @assert all([length(z) == 3 for z in z₀])
        # @assert all([length(λ) == 1 for λ in λ₀])
        DAE(oscillator_dae_v, oscillator_dae_u, oscillator_dae_ϕ,
            z₀, λ₀; invariants=(h=hamiltonian,), parameters=params, v̄=oscillator_ode_v)
    end

    function oscillator_idae_u(t, q, p, λ, u, params)
        u[1] = λ[1]
        u[2] = λ[2]
        nothing
    end

    function oscillator_idae_g(t, q, p, λ, g, params)
        g[1] = 0
        g[2] = λ[1]
        nothing
    end

    function oscillator_hdae_ū(t, q, p, λ, u, params)
        u[1] = λ[1]
        u[2] = λ[2]
        nothing
    end

    function oscillator_hdae_ḡ(t, q, p, λ, g, params)
        g[1] = 0
        g[2] = λ[1]
        nothing
    end

    function oscillator_idae_ϕ(t, q, p, ϕ, params)
        ϕ[1] = p[1] - q[2]
        ϕ[2] = p[2]
        nothing
    end

    function oscillator_hdae_ψ(t, q, p, v, f, ψ, params)
        ψ[1] = f[1] - v[2]
        ψ[2] = f[2]
        nothing
    end

    function harmonic_oscillator_idae(q₀=q₀, p₀=ϑ(q₀), λ₀=zero(q₀), params=p)
        # @assert size(q₀,1) == size(p₀,1) == size(λ₀,1) == 2
        IDAE(oscillator_iode_ϑ, oscillator_iode_f,
             oscillator_idae_u, oscillator_idae_g, oscillator_idae_ϕ,
             q₀, p₀, λ₀; invariants=(h=hamiltonian,), parameters=params, v̄=oscillator_iode_v)
    end

    function oscillator_pdae_v(t, q, p, v, params)
        @unpack k = params
        v[1] = q[2]
        v[2] = -k*q[1]
        nothing
    end

    function oscillator_pdae_f(t, q, p, f, params)
        @unpack k = params
        f[1] = -k*q[1]
        f[2] = p[1] - q[2]
        nothing
    end

    function harmonic_oscillator_pdae(q₀=q₀, p₀=ϑ(q₀), λ₀=zero(q₀), params=p)
        # @assert size(q₀,1) == size(p₀,1) == 2
        PDAE(oscillator_pdae_v, oscillator_pdae_f,
             oscillator_idae_u,  oscillator_idae_g, oscillator_idae_ϕ,
             q₀, p₀, λ₀; invariants=(h=hamiltonian,), parameters=params)
    end

    function harmonic_oscillator_hdae(q₀=q₀, p₀=ϑ(q₀), λ₀=zero(q₀), params=p)
        # @assert size(q₀,1) == size(p₀,1) == 2
        HDAE(oscillator_pdae_v, oscillator_pdae_f,
             oscillator_idae_u, oscillator_idae_g, oscillator_idae_ϕ,
             oscillator_idae_ū, oscillator_idae_ḡ, oscillator_idae_ψ,
             hamiltonian, q₀, p₀, λ₀; parameters=params)
    end

    function compute_energy_error(t, q::AbstractDataSeries{T}, parameters) where {T}
        h = DataSeries(T, q.nt)
        e = DataSeries(T, q.nt)

        for i in axes(q,2)
            h[i] = hamiltonian(t[i], q[:,i], parameters)
            e[i] = (h[i] - h[0]) / h[0]
        end

        (h, e)
    end

end


text:
# Guide
# Pkg.dev("SixDOF")
# Pkg.add("SixDOF")
# Pkg.test("SixDOF")
# include("SixDOF.jl")

using Plots
```

First, we import the module.

```
using SixDOF
```

There are several structs we need to define.  The inputs used in this example correspond to the Zagi flying wing in Appendix E of Small Unmanned Aircraft: Theory and Practice by Beard and McLain.  We specify the mass properties:

```
```

For this example:
```
m = 13.5
Ixx = .8244
Iyy = 1.135
Izz = 1.759
Ixz = 0.1204
mp = MassProp(m, Ixx, Iyy, Izz, Ixz)
nothing # hide
```

Next, we specify reference areas and lengths:

```
Sref = 0.55
bref = 2.8956
cref = 0.18994
ref = Reference(Sref, bref, cref)
nothing # hide

controller = ConstantController(0.0, 0.0, 0.0, 0.0, 0.8)
nothing # hide

Wi = [0.0, 0.0, 0.0]
Wb = [0.0, 0.0, 0.0]
rho = 1.2682
asound = 300.0
g = 9.81
atm = ConstantAtmosphere(Wi, Wb, rho, asound, g)
nothing # hide

CL0 = 0.28 # Zero-alpha lift
CLalpha = 3.45  # lift curve slope
CLq = 0.0 # Pitch rate derivative
CLM = 0.0 # Mach derivative
CLdf = 0.0  # flaps derivative
CLde = -0.36  # elevator derivative
CLmax = 1.4  # max CL (stall)
CLmin = -0.9  # min CL (neg stall)
alphas = 20*pi/180

CD0 = 0.03  # zero-lift drag coerff
U0 = 30.0  # velocity corresponding to Reynolds number of CD0
exp_Re = -0.2  # exponent in Reynolds number calc
e = 0.9  # Oswald efficiency
Mcc = 0.7  # crest critcal Mach number
CDdf = 0.0  # flaps
CDde = 0.0  # elevator
CDda = 0.0  # aileron
CDdr = 0.0  # rudder

CYbeta = -0.98 # Sideslip derivative
CYp = 0.0  # Roll rate derivative
CYr = 0.0 # Yaw rate derivative
CYda = 0.0 # Roll control (aileron) derivative
CYdr = -0.17 # Yaw control (rudder) derivative

Clbeta = -0.12  # Sideslip derivative
Clp = -0.26  # Roll rate derivative
Clr = 0.14  # Yaw rate derivative
Clda = 0.08  # Roll (aileron) control derivative
Cldr = 0.105  #Yaw (rudder) control derivative

Cm0 = -0.02338 # Zero-alpha pitch
Cmalpha = -0.38 # Alpha derivative
Cmq = -3.6 # Pitch rate derivative
CmM = 0.0
Cmdf = 0.0
Cmde = -0.5 # Pitch control derivative

Cnbeta = 0.25  # Slideslip derivative
Cnp = 0.022  # Roll rate derivative
Cnr = -0.35  # Yaw rate derivative
Cnda = 0.06  # Roll (aileron) control derivative
Cndr = 0.0  # Yaw (rudder) control derivative

sd = StabilityDeriv(CL0, CLalpha, CLq, CLM, CLdf, CLde, alphas,
    CD0, U0, exp_Re, e, Mcc, CDdf, CDde, CDda, CDdr,
    CYbeta, CYp, CYr, CYda, CYdr,
    Clbeta, Clp, Clr, Clda, Cldr,
    Cm0, Cmalpha, Cmq, CmM, Cmdf, Cmde,
    Cnbeta, Cnp, Cnr, Cnda, Cndr)
nothing # hide

CT0 = 0.11221
CT1 = -0.13803
CT2 = -0.047394
CQ0 = 0.0062
CQ1 = 0.00314
CQ2 = -0.015729
D = 10*0.0254
num = 2
type = COCOUNTER
R = 0.5
Kv = 2500.0 * pi/30
i0 = 0.3
voltage = 8.0
propulsion = MotorPropBatteryDataFit(CT2, CT1, CT0, CQ2, CQ1, CQ0, D, num, type, R, Kv, i0, voltage)
nothing # hide

inertial2 = UniformGravitationalField()
nothing # hide

Vinf = U0
alpha = 3.0*pi/180
s0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Vinf*cos(alpha), 0.0, Vinf*sin(alpha), 0.0, 0.0, 0.0]
tspan = (0.0, 4.0)
p = mp, ref, sd, propulsion, inertial2, atm, controller

# using DifferentialEquations
# prob = DifferentialEquations.ODEProblem(sixdof!, s0, tspan, p)
# sol = DifferentialEquations.solve(prob)
# nothing # hide

#Tyson testing his stuff
JACOBIAN=finite_jacobian(s0,p)
s0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Vinf*cos(alpha), 0.0, Vinf*sin(alpha), 0.0, 0.0, 0.0]
p = mp, ref, sd, propulsion, inertial2, atm, controller
NEWJACOBIAN=getjacobian(s0,p)

#=
struct wing{TV, TF}
    pos::TV
    dir::TV
    cr::TF
    ct::TF
    tilt::TF
    twist::TF
end
=#

leftWing = Wing([0.0 0.0 0.0],[-.1 -1 -.05], .3, .1, 0.0, 3.0)
rightWing = Wing([0.0 0.0 0.0],[-.1  1 -.05], .3, .1, 0.0, 3.0)

planeModel = (leftWing,rightWing)

animvtk(JACOBIAN, planeModel,"Aerosonde")
animvtk(NEWJACOBIAN, planeModel,"Aerosonde2")


text:
"""
    nontrivial_pegiun(x, p, n) -> Function

Iterate a 2d discrete system with nonlinear, nontrivial coupling. This system
is from [Péguin-Feissolle & Teräsvirta (1999)](https://www.amse-aixmarseille.fr/sites/default/files/_dt/greqam/99a42.pdf), and
was also studied in [Chávez et al. (2003)](https://www.sciencedirect.com/science/article/pii/S0165027002003679).
The version implemented here allows for tweaking the parameters of the equations.

The difference equations are

```math
\\begin{aligned}
x(t+1) &= p_2 + p_3 x(t-2) + \\dfrac{p_4 - p_5 y(t-3)}{1 + e^{-p_6 y(t-3)}} + \\xi_1(t) \\
y(t+1) &= p_1 y(t) + \\xi_2(t).
\\end{aligned}
```

Here, ``\\xi_{1,2}(t)`` are two independent normally distributed noise processes
with zero mean and standard deviations ``\\sigma_1`` and ``\\sigma_2``. The
``\\xi_{1,2}(t)`` terms represent dynamical noise.

# References

Péguin-Feissolle, A., & Teräsvirta, T. (1999). A General Framework for
Testing the Granger Noncausaality Hypothesis. Universites d’Aix-Marseille II
et III. [https://www.amse-aixmarseille.fr/sites/default/files/_dt/greqam/99a42.pdf](https://www.amse-aixmarseille.fr/sites/default/files/_dt/greqam/99a42.pdf)

Chávez, M., Martinerie, J., & Le Van Quyen, M. (2003). Statistical
assessment of nonlinear causality: application to epileptic EEG signals.
Journal of Neuroscience Methods, 124(2), 113–128.
doi:10.1016/s0165-0270(02)00367-9
[https://www.sciencedirect.com/science/article/pii/S0165027002003679](https://www.sciencedirect.com/science/article/pii/S0165027002003679)
"""
function eom_nontrivial_pegiun(u, p, n)
    n = n + 10
    O = zeros(Float64, n + 3, 2)
    x, y = (u...,)
    p₁, p₂, p₃, p₄, p₅, p₆, σ₁, σ₂ = (p...,)

    # Propagate initial condition to the three first time steps.
    for i = 1:3
        O[i, 1] = x
        O[i, 2] = y
    end
    for i = 4:n
        y1 = O[i-1, 2]
        x2 = O[i-2, 1]
        y3 = O[i-3, 2]

        ξ₁ = rand(Normal(0, σ₁))
        ξ₂ = rand(Normal(0, σ₂))
        ynew = p₁*y1 + ξ₁
        xnew = p₂ + p₃*x2 + (p₄ - p₅*y3)/(1 + exp(-p₆*y3)) + ξ₂
        O[i, 1] = xnew
        O[i, 2] = ynew
    end
    O = O[10+3:end-10, :]
    O[:, 1] .= O[:, 1] .- mean(O[:, 1])
    O[:, 2] .= O[:, 2] .- mean(O[:, 2])
    O[:, 1] .= O[:, 1] ./ std(O[:, 1])
    O[:, 2] .= O[:, 2] ./ std(O[:, 2])
    return Dataset(O)
end

function nontrivial_pegiun(uᵢ, p₁, p₂, p₃, p₄, p₅, p₆, σ₁, σ₂, n::Int)
    p = [p₁, p₂, p₃, p₄, p₅, p₆, σ₁, σ₂]
    eom_nontrivial_pegiun(uᵢ, p, n)
end

"""
    nontrivial_pegiun(;uᵢ = rand(2), σ₁ = 0.1, σ₂ = 0.1,
        p₁ = 0.7, p₂ = 0.1, p₃ = 0.4, p₄ = 2.4, p₅ = 0.9, p₆ = 4, n = 100) -> Dataset

Create a 2d discrete systems with nonlinear, nontrivial coupling from [1] .
This version allows for tweaking the parameters of the equations. The variables
are each normalised to zero mean and unit variance.

The difference equations are

```math
\\begin{aligned}
x(t+1) &= p_2 + p_3 x(t-2) + \\dfrac{p_4 - p_5 y(t-3)}{1 + e^{-p_6 y(t-3)}} + \\xi_1(t) \\
y(t+1) &= p_1 y(t) + \\xi_2(t).
\\end{aligned}
```
Here, ``\\xi_{1,2}(t)`` are two independent normally distributed noise processes
with zero mean and standard deviations ``\\sigma_1`` and ``\\sigma_2``. The
``\\xi_{1,2}(t)`` terms represent dynamical noise.

# References
1. Chávez, M., Martinerie, J., & Le Van Quyen, M. (2003). Statistical
assessment of nonlinear causality: application to epileptic EEG signals.
Journal of Neuroscience Methods, 124(2), 113–128. doi:10.1016/s0165-0270(02)00367-9
"""
function nontrivial_pegiun(;uᵢ = rand(2), σ₁ = 0.1, σ₂ = 0.1,
        p₁ = 0.7, p₂ = 0.1, p₃ = 0.4, p₄ = 2.4, p₅ = 0.9, p₆ = 4, n = 100)
    eom_nontrivial_pegiun(uᵢ, [p₁, p₂, p₃, p₄, p₅, p₆, σ₁, σ₂], n)
end


text:
function regenerate_test()
    download("https://drive.google.com/u/0/uc?id=1jx7FZi9HwGq3gJPc4fjjI1bLxjmZ5Rrn&export=download", "fi_lib.jl")
    download("https://drive.google.com/u/0/uc?id=1wpeFbQ3lW59WVi2NTl9XjXGb8xNyACwQ&export=download", "abs_rev.jl")
    download("https://drive.google.com/u/0/uc?id=13bgyu2o0lE6vbChBp9l7VRD-GktzWbFP&export=download", "atan2.jl")
    download("https://drive.google.com/u/0/uc?id=1lNVYLO2MoxpK_t8HZevw4Kyb-Xq49aSN&export=download", "c-xsc.jl")
    download("https://drive.google.com/u/0/uc?id=13oYgM8lkp4KK5VHc15Mp-PqNwU0BA3MW&export=download", "ieee1788-constructors.jl")
    download("https://drive.google.com/u/0/uc?id=1Yzq4PZxSz0rdMDcZelR3DxcpNlxPcyKA&export=download", "ieee1788-exceptions.jl")
    download("https://drive.google.com/u/0/uc?id=1CtoBPf_yUErJr78sNCBy78CD9v00OOFz&export=download", "libieeep1788_bool.jl")
    download("https://drive.google.com/u/0/uc?id=1FUltmfYTbkF6kDdV5Smh6naulID5J2_f&export=download", "libieeep1788_cancel.jl")
    download("https://drive.google.com/u/0/uc?id=1F3ySXep023Bff3apm5M9DjJaOlVYi6c2&export=download", "libieeep1788_class.jl")
    download("https://drive.google.com/u/0/uc?id=1IOqjsxUPg0LVh7-K2pC3lBxB9_dh_cp-&export=download", "libieeep1788_elem.jl")
    download("https://drive.google.com/u/0/uc?id=1NSKDxu7mMevG0_YWn91YsQthyu_rORY3&export=download", "libieeep1788_num.jl")
    download("https://drive.google.com/u/0/uc?id=1BizPXSTzg6Pli9PzOWjXHqpg8UiGubPz&export=download", "libieeep1788_overlap.jl")
    download("https://drive.google.com/u/0/uc?id=1r6gW6ssTwYbvZOK1pSk2thidSJSEnA97&export=download", "libieeep1788_rec_bool.jl")
    download("https://drive.google.com/u/0/uc?id=1EBdvzA5YTXFJAqHdEX0zn5BGf8fgnO8o&export=download", "libieeep1788_reduction.jl")
    download("https://drive.google.com/u/0/uc?id=1hYiYVWFZGaFnxpdOv2358h6SrzYQkJSy&export=download", "libieeep1788_rev.jl")
    download("https://drive.google.com/u/0/uc?id=1XxvBmdCiCwUrtNn9qN1gH2l5hjroGYjD&export=download", "libieeep1788_set.jl")
    download("https://drive.google.com/u/0/uc?id=1Ci6nPtdl2MkTi0ethqJXHIKdGEJveO8x&export=download", "mpfi.jl")
end


text:
function decode_msg(line::AbstractString)
    pos = findfirst(':', line)
    pos2 = findnext(':', line, pos+1)

    msg_id = line[1:pos-1]
    msg_cmd = line[pos+1:pos2-1]
    msg_body_encoded = line[pos2+1:end]
    msg_body = String(Base64.base64decode(msg_body_encoded))
    return msg_id, msg_cmd, msg_body
end

function send_notification(conn, msg_cmd::AbstractString, msg_body::AbstractString="")
    encoded_msg_body = Base64.base64encode(msg_body)
    println(conn, msg_cmd, ":notification:", encoded_msg_body)
end

function send_response(conn, msg_id::AbstractString, msg_body::AbstractString)
    encoded_msg_body = Base64.base64encode(msg_body)
    println(conn, "RESPONSE:", msg_id, ':', encoded_msg_body)
end

max_stars_repo_name: caleb-allen/julia-vscode

max_issues_repo_name: caleb-allen/julia-vscode

max_forks_repo_name: caleb-allen/julia-vscode


text:
# # 3D system with sliding mode controller
#
#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/models/Sherlock-Benchmark-7.ipynb)
#
# The model was first proposed in [^YH12], where the authors originally designed a
# discontinuous sliding mode controller for this system.

# ## Model
#
# The model is a three-dimensional system given by the following equations:
#
# ```math
# \begin{aligned}
# \dot{x}_1 &= x_3^3 - x_2 + w \\
# \dot{x}_2 &= x_3 \\
# \dot{x}_3 &= u
# \end{aligned}
# ```
# where w is a bounded error in the range ``[−0.01, 0.01]``. A neural network controller
# was trained for this system, using a model predictive controller as a demonstrator.
# The trained network had 2 hidden layers, with 300 neurons in the first layer,
# and 200 in the second layer. The sampling time for this controller was 0.5s.

using NeuralNetworkAnalysis

# The spatial variables are aguemte
@taylorize function f!(dx, x, p, t)
    x₁, x₂, x₃, x₄, w, u = x

    dx[1] = x₃^3 - x₂ + w
    dx[2] = x₃
    dx[3] = u
    dx[4] = zero(x₄) # w
    dx[5] = zero(x₅) # u
    return dx
end

# define the initial-value problem
X₀ = Hyperrectangle(low=[0.35, 0.45, 0.25], high=[0.45, 0.55, 0.35])
W₀ = Interval(-0.01, 0.01)
U₀ = Interval(2.0, 2.0)
prob = @ivp(x' = f!(x), dim: 5, x(0) ∈ X₀ × W₀ × U₀);

# solve it
##sol = solve(prob, T=2.0);

# ## Specifications
#
# The verification problem here is that of target reachability. For an initial set
# of ``x₁ ∈ [0.35, 0.45]``, ``x₂ ∈ [0.45, 0.55]``, ``x₃ ∈ [0.25, 0, 35]``, it is
# required to prov that the system converges to ``x ∈ [−0.032, 0.032]^3`` within ``2s``.

# ## Results

# ## References

# [^YH12]: Dong-Hae Yeom and Young Hoon Joo. [*Control Lyapunov function design by
#          cancelling input singularity.*](http://www.koreascience.or.kr/article/JAKO201220962918909.page).
#          International Journal of Fuzzy Logic and Intelligent Systems, 12, 06 2012.
#


text:
using LDUFacts
using LinearAlgebra

using Test

@testset "LDUFacts.jl" begin
    include("ldufact.jl")
    include("lduupdate.jl")
end


text:
export MOVE_FORWARD, TURN_LEFT, TURN_RIGHT, LRUD, WALL

using Crayons
using Colors

const COLORS = (:red, :green, :blue, :magenta, :yellow, :white)

#####
# Actions
#####

struct MoveForward end
const MOVE_FORWARD = MoveForward()

struct Up end
const UP = Up()
(x::Up)(p::CartesianIndex{2}) = p + CartesianIndex(-1, 0)

struct Down end
const DOWN = Down()
(x::Down)(p::CartesianIndex{2}) = p + CartesianIndex(1, 0)

struct Left end
const LEFT = Left()
(x::Left)(p::CartesianIndex{2}) = p + CartesianIndex(0, -1)

struct Right end
const RIGHT = Right()
(x::Right)(p::CartesianIndex{2}) = p + CartesianIndex(0, 1)

const LRUD = Union{Left, Right, Up, Down}

struct TurnRight end
const TURN_RIGHT = TurnRight()
struct TurnLeft end
const TURN_LEFT = TurnLeft()

(x::TurnRight)(::Left) = UP
(x::TurnRight)(::Up) = RIGHT
(x::TurnRight)(::Right) = DOWN
(x::TurnRight)(::Down) = LEFT
(x::TurnLeft)(::Left) = DOWN
(x::TurnLeft)(::Up) = LEFT
(x::TurnLeft)(::Right) = UP
(x::TurnLeft)(::Down) = RIGHT

#####
# Objects
#####

abstract type AbstractObject end

Base.show(io::IO, x::AbstractObject) = print(io, Crayon(foreground=get_color(x), reset=true), convert(Char, x))

struct Empty <: AbstractObject end
const EMPTY = Empty()
Base.convert(::Type{Char}, ::Empty) = '⋅'
get_color(::Empty) = :white

struct Wall <: AbstractObject end
const WALL = Wall()
Base.convert(::Type{Char}, ::Wall) = '█'
get_color(::Wall) = :white

struct Goal <: AbstractObject end
const GOAL = Goal()
Base.convert(::Type{Char}, ::Goal) = '♥'
get_color(::Goal) = :red

struct Door{C} <: AbstractObject end
Door(c) = Door{c}()
Base.convert(::Type{Char}, ::Door) = '🚪'
get_color(::Door{C}) where C = C

Base.@kwdef mutable struct Agent <: AbstractObject
    color::Symbol=:red
    dir::LRUD
end
function Base.convert(::Type{Char}, a::Agent)
    if        a.dir === UP
        '↑'
    elseif  a.dir === DOWN
        '↓'
    elseif  a.dir === LEFT
        '←'
    elseif a.dir === RIGHT
        '→'
    end
end
get_color(a::Agent) = a.color
get_dir(a::Agent) = a.dir
set_dir!(a::Agent, d) = a.dir = d


text:
"""
A simple optimal growth model, for testing the DiscreteDP class.

Filename: finite_dp_og.jl

"""

"""
Set up R, Q and beta, the three elements that define an instance of
the DiscreteDP object.
"""
mutable struct SimpleOG
	#-Paramters-#
	B::Int64
	M::Int64
	alpha::Float64
	beta::Float64
	#-Internal Variables-#
	n::Int64
	m::Int64
	R::Array{Float64}
	Q::Array{Float64}

	function SimpleOG(B::Int64, M::Int64, alpha::Float64, beta::Float64, u::Function)
		n = B + M + 1
		m = M + 1
		R = zeros(Float64, n, m)
		Q = zeros(Float64, n, m, n)
		#-Populate R-#
		populate_R!(n,m,R,u,alpha)
		populate_Q!(m,Q,B)
		return new(B,M,alpha,beta,n,m,R,Q)
	end
end

SimpleOG() = SimpleOG(10,5,0.5,0.9,u)
SimpleOG(B::Int64, M::Int64, alpha::Float64, beta::Float64) = SimpleOG(B,M,alpha,beta,u)

#-Support Functions-#

function u(c, alpha)
    return c^alpha
end

function populate_R!(n,m,R,u,alpha)
    for s in 1:n
        for a in 1:m
            if a <= s
                R[s, a] = u(s-a,alpha)
            else
                R[s, a] = -Inf
            end
        end
    end
end

function populate_Q!(m,Q,B)
    for a in 1:m
        Q[:, a, a:(a + B)] .= 1.0 / (B+1)
    end
end


text:
using Documenter, Functors

DocMeta.setdocmeta!(Functors, :DocTestSetup, :(using Functors); recursive = true)

makedocs(modules = [Functors],
         doctest = VERSION == v"1.6",
         sitename = "Functors.jl",
         pages = ["Home" => "index.md",
                  "API" => "api.md"],
         format = Documenter.HTML(
             analytics = "UA-36890222-9",
             assets = ["assets/flux.css"],
             prettyurls = get(ENV, "CI", nothing) == "true"),
         )

deploydocs(repo = "github.com/FluxML/Functors.jl.git",
           target = "build",
           push_preview = true)


text:
"""
    function compare_tan_inv_RDC(args...; kwargs)

Compare the inverse tangent of the residual demand curve functions for both bid boundaries
(Lower bid "bid_lo" and higher bid "bid_hi"). Then it returns if the functions are the same,
if they intersect, and if yes, the point where they intersect. The comparison is done by
evaluating both functions from the Slack generator minimum active power (Pmin) to the
maximum active power (Pmax) according to its operational limits. The functions work as an
approximation of the LMP for a certain range, and they are calculated using the Transmission
Constrained Residual Demand Derivative (TCRDD) [1].

[1] L. Xu, R. Baldick and Y. Sutjandra, "Bidding Into Electricity Markets:
    A Transmission-Constrained Residual Demand Derivative Approach," in IEEE
    Transactions on Power Systems, vol. 26, no. 3, pp. 1380-1388, Aug. 2011,
    doi: 10.1109/TPWRS.2010.2083702.

# Arguments
- `sys::System`:                            Power system in p.u. (from PowerSystems.jl)
- `BaseMVA::Float64`:                       Apparent Power Base for the system [MVA]
- `lmp_lo::DataFrame`:                      LMP of all buses of the system (lower bid)
- `lmp_hi::DataFrame`:                      LMP of all buses of the system (higher bid)
- `tcrdd_slack_lo::Float64`:                TCRDD [pu]  (lower bid)
- `tcrdd_slack_hi::Float64`:                TCRDD [pu]  (higher bid)
- `bid_lo::Float64`:                        Lower Bid for the slack generator [pu]
- `bid_hi::Float64`:                        Higher Bid for the slack generator [pu]
- `Pmin_orig::Float64`:                     Active power minimum limit Slack gen [pu]
- `Pmax_orig::Float64`:                     Active power maximum limit Slack gen [pu]

# Keywords
- `segm_bid_argmax_profit::Int64 = 50000:   Segments to evaluate the approx profit function
- `epsilon::Float64 = 0.01`:                Tolerance to identify if they intersect and/or
                                             if they are the same function.
- `print_plots::Bool = true`:               Flag to print plots

# Throws
- `NotFoundError`:                          No Slack bus found in the system
"""
function compare_tan_inv_RDC(
    sys::System,
    BaseMVA::Float64,
    res_lo::PowerSimulations.OperationsProblemResults,
    res_hi::PowerSimulations.OperationsProblemResults,
    lmp_lo::DataFrame,
    lmp_hi::DataFrame,
    tcrdd_slack_lo::Float64,
    tcrdd_slack_hi::Float64,
    bid_lo::Float64,
    bid_hi::Float64,
    Pmin_orig::Float64,
    Pmax_orig::Float64;
    segm_bid_argmax_profit::Int64 = 50000,
    epsilon::Float64 = 0.01,
    print_plots::Bool = true
    )

    # Get slack bus
    bus_slack_name = "Empty"
    has_slack = false
    buses = get_components(Bus, sys)
    for bus in buses
        if get_bustype(bus) == BusTypes.REF
            has_slack = true
            bus_slack_name = bus.name
        end
    end

    # Storage the lmp of the slack bus
    if has_slack
        lmp_slack_lo = lmp_lo[1, bus_slack_name]
        lmp_slack_hi = lmp_hi[1, bus_slack_name]
    else
        error("NotFoundError: No Slack found in the system")
    end

    # Get Slack Generator component ,ID and name
    (gen_thermal_slack,gen_thermal_slack_id,gen_thermal_slack_name)=get_thermal_slack(sys)

    # Get Optimised Pg of slack lo and hi
    all_PGenThermal_lo = get_variables(res_lo)[:P__ThermalStandard] #Optimised PGen
    Pg_slack_lo = all_PGenThermal_lo[1, gen_thermal_slack_name]
    all_PGenThermal_hi = get_variables(res_hi)[:P__ThermalStandard] #Optimised PGen
    Pg_slack_hi = all_PGenThermal_hi[1, gen_thermal_slack_name]
    # Correct tcrdd pu using BaseMVA
    tcrdd_slack_hi = tcrdd_slack_hi/(BaseMVA^2)
    tcrdd_slack_lo = tcrdd_slack_lo/(BaseMVA^2)

    #----------Evaluate both functions for all segments of Pg using the tcrdd----------
    segment = 0
    segm_div = segm_bid_argmax_profit
    bids = Array{Float64}(undef,segm_div + 1)
    tan_inv_RDC_lo = Array{Float64}(undef, segm_div + 1)
    tan_inv_RDC_hi = Array{Float64}(undef, segm_div + 1)
    dif_tans = Array{Float64}(undef, segm_div+1)
    same_tan_inv_lo_hi_num = ones(segm_div + 1, 1) #false 0, true 1
    intersect_tan_inv_lo_hi_num = zeros(segm_div + 1, 1) #false 0, true 1
    step = (Pmax_orig - Pmin_orig)/segm_div

    for Pg in Pmin_orig:step:Pmax_orig
        segment = segment + 1
        tan_inv_RDC_lo[segment] = (((1/tcrdd_slack_lo)*(Pg-Pg_slack_lo)) + lmp_slack_lo)
        tan_inv_RDC_hi[segment] = (((1/tcrdd_slack_hi)*(Pg-Pg_slack_hi)) + lmp_slack_hi)
        bids[segment] = Pg
        dif_tans[segment] = abs(tan_inv_RDC_hi[segment] - tan_inv_RDC_lo[segment])
        # Same function? or #Intersect
        if dif_tans[segment] > (epsilon*BaseMVA)
            same_tan_inv_lo_hi_num[segment] = 0.0
        else
            intersect_tan_inv_lo_hi_num[segment] = 1.0
        end
    end

    # Flag if they are the same function
    if sum(same_tan_inv_lo_hi_num) == segm_div + 1.0
        same_tan_inv_lo_hi = true
    else
        same_tan_inv_lo_hi = false
    end

    # Flag if they intersect
    if sum(intersect_tan_inv_lo_hi_num) ≠ 0.0
        intersect_tan_inv_lo_hi = true
    else
        intersect_tan_inv_lo_hi = false
    end

    # Find where do they intersect if they do
    (int_flag, segm_intersect) = findmax(intersect_tan_inv_lo_hi_num)
    bid_intersect = bids[segm_intersect]

    # Plot both functions
    if print_plots == true
        tan_inv_RDC = Array{Float64}(undef,(segment,2))
        tan_inv_RDC[:, 1] = tan_inv_RDC_lo
        tan_inv_RDC[:, 2] = tan_inv_RDC_hi
        plotly()
        p=plot(bids,tan_inv_RDC, title = "bids vs lmp_aprox_TCRDD",
            label = ["lmp_apx_lo" "lmp_apx_hi"], legend = :bottomright);
        xlabel!("bids(pu)");
        ylabel!("lmp")
        display(p)
    end

    return same_tan_inv_lo_hi, intersect_tan_inv_lo_hi, bid_intersect
end

max_stars_repo_name: invenia/MaximumGeneratorProfitTCRDD

max_issues_repo_name: invenia/MaximumGeneratorProfitTCRDD

max_forks_repo_name: invenia/MaximumGeneratorProfitTCRDD


text:
function jt06()
#
#  test polynomial suggested by Jenkins and Traub
#
    p = poly([.1;1.001;.998;1.00002;.99999])
    z = [.1;1.001;.998;1.00002;.99999];
    z = [z ones(5)]
    p, PolyZeros(z)
end


text:
#links function inputs and outputs
struct MappedFunction
    fun
    fmap
end


text:
# using Base: cache_dependencies
"""
    load_config(input_file, det_metadata, sim_config_file)

AbstractString or Table, AbstractString, AbstractString -> PropDict

Merge simulation inputs and settings into one simulation config to pass around
"""
function load_config(det_metadata::AbstractString, sim_config_file::AbstractString)
    # function load_config(input_file::Union{AbstractString, Table}, det_metadata::AbstractString, sim_config_file::AbstractString)
    # construct default cached name
    cached_name = filename(sim_config_file)

    # add detector info as simulation input
    sim_inputs = PropDict(
        :detector_metadata => det_metadata,
        :simulation => PropDict( :cached_name => cached_name )
    )

    # contains only simulation settings
    sim_config = propdict(sim_config_file)

    # merge
    # after merging, if cached_name was present in sim_config, it will override the default one above
    sim_total = merge(sim_inputs, sim_config)

    # add detector name to cached filename
    # what? this doesn't work?? can i make it mutable?
    # sim_total.pss.cached_name = "$(filename(sim_total.detector_metadata))_$(sim_total.pss.cached_name)"
    # workaround
    temp = PropDict(:pss => PropDict(:cached_name => "$(filename(sim_total.detector_metadata))_$(sim_total.simulation.cached_name)"))
    sim_total = merge(sim_total, temp)
    @info "(Future) cached basename: $(sim_total.simulation.cached_name)"

    # load_config(input_file, sim_total)
    sim_total
end

function load_config(input_file::Union{AbstractString, Table}, sim_config::PropDict)
    # add input
    sim_inputs = PropDict(
        :input_file => input_file,
    )

    # merge
    # new input file will override previous input if present
    sim_total = merge(sim_config, sim_inputs)

    #@info "Your final simulation configuration" sim_total

    sim_total
end

function load_config(input_file::Union{AbstractString, Table}, det_metadata::AbstractString, sim_config_file::AbstractString)
    sim_config = load_config(det_metadata, sim_config_file)

    load_config(input_file, sim_config)
end

@with_kw struct Environment
    "Medium"
    medium::String = "vacuum"

    "Crystal temperature in K"
    crystal_t::Real = 0

    "Oprating voltage in V"
    op_voltage::Real = 0
end

"""
Simulation parameters related to the environment
"""
function Environment(sim_conf::PropDict)
    Environment(
        sim_conf.environment.medium,
        sim_conf.environment.crystal_t,
        sim_conf.environment.op_voltage
    )
end


text:
################################################# FILE DESCRIPTION #########################################################

# This file contains tests for edge indexing

############################################################################################################################

@testset "SparseMatrixCSC Generation" begin
   Nv = 10
   Ne = 90

   x = randindxs(Nv, Ne)
   @test size(x) == (Nv,Nv)
   @test Ne / 2 <= nnz(x) <= 2 * Ne
   @test nonzeros(x) == collect(1 : nnz(x))

   x = completeindxs(Nv)
   @test size(x) == (Nv,Nv)
   @test nnz(x) == Nv * (Nv - 1)
   @test nonzeros(x) == collect(1 : nnz(x))
end

@testset "SparseMatrixCSC Indexing" begin
   x = completeindxs(10)
   e = 1=>2
   es = EdgeID[1=>2, 3=>6, 4=>7, 5=>9]
   eit = EdgeIter(4, [1,3,4,5], [2,6,7,9])

   @test x[e] == 1
   @test x[es] == [1, 23, 33, 44]
   @test x[eit] == [1, 23, 33, 44]

   x[e] = 2
   @test x[e] == 2

   x[es] = 5
   @test x[es] == [5, 5, 5, 5]
   @test x[eit] == [5, 5, 5, 5]

   x[eit] = 7
   @test x[es] == [7, 7, 7, 7]
   @test x[eit] == [7, 7, 7, 7]

   x[es] = [1,2,3,4]
   @test x[es] == [1, 2, 3, 4]
   @test x[eit] == [1, 2, 3, 4]

   x[eit] = [4, 3, 2, 1]
   @test x[es] == [4, 3, 2, 1]
   @test x[eit] == [4, 3, 2, 1]
end

@testset "SparseMatrixCSC Adjacency" begin
   x = completeindxs(10)
   v = rand(1:10)
   vs = rand(1:10, 5)
   adj = setdiff(1:10, v)

   @test fadj(x, v) == adj
   @test fadj!(x, v, zeros(Int, 9)) == adj

   @test indegree(x, v) == 9
   @test indegree(x, vs) == [indegree(x, v) for v in vs]
   @test indegree(x) == fill(9, 10)

   @test outdegree(x, v) == 9
   @test outdegree(x, vs) == [outdegree(x, v) for v in vs]
   @test outdegree(x) == fill(9, 10)
end

@testset "SparseMatrixCSC Mutation" begin
   x = completeindxs(10)

   y = addvertex!(x)
   @test nv(y) == 11
   @test EdgeIter(x) == EdgeIter(y)

   y,erows = rmvertex!(y, 11)
   @test x == y
   @test erows == Int[]

   e = 1=>1
   es = EdgeID[2=>2, 3=>3, 4=>4, 5=>5]

   addedge!(x, e, 91)
   @test x[e] == 91

   rmedge!(x, e)
   @test x[e] == 0

   addedge!(x, es, [92,93,94,95])
   @test x[es] == [92,93,94,95]

   rmedge!(x, es)
   @test x[es] == [0,0,0,0]
end

@testset "SparseMatrixCSC Subgraph" begin
   x = completeindxs(10)
   vs = 1:5
   eit = EdgeIter(x)
   terows = [1,2,3,4,10,11,12,13,19,20,21,22,28,29,30,31,37,38,39,40]

   x1,erows = subgraph(x, vs)
   @test nv(x1) == 5
   @test ne(x1) == 20
   @test erows == terows

   x2,erows = subgraph(x, eit)
   @test nv(x2) == 10
   @test ne(x2) == 90
   @test eit == EdgeIter(x2)
   @test erows == collect(1:90)

   x3,erows = subgraph(x, vs, eit)
   @test nv(x3) == 5
   @test ne(x3) == 20
   @test x1 == x3
   @test erows == terows
end


text:
# Test bounds checking
let b1 = codeunits("abcdef")
    @test_throws BoundsError check_string(b1, -10)
    @test_throws BoundsError check_string(b1, 0)
    @test_throws BoundsError check_string(b1, 7)
    @test_throws BoundsError check_string(b1, 3, -10)
    @test_throws BoundsError check_string(b1, 3, 0)
    @test_throws BoundsError check_string(b1, 3, 7)
    @test_throws UnicodeError check_string(b1, 3, 1)
end

let str = UTF8Str("this is a test\uff")
    @test_throws UnicodeError UTF8Str(vcat(codeunits("this is a test"), [0xed, 0x80]))
    # This next test was broken by #24999
    #@test next(str, 15) == ('\ufffd', 16)
    @test_throws BoundsError getindex(str, 0:3)
    @test_throws BoundsError getindex(str, 17:18)
    @test_throws BoundsError getindex(str, 2:17)
    # This next test was broken by #24999
    #@test string(Char(0x110000)) == "\ufffd"
    #=
    sa = SubString{ASCIIStr}(ascii("This is a silly test"), 1, 14)
    s8 = convert(SubString{UTF8Str}, sa)
    @test typeof(s8) == SubString{UTF8Str}
    @test s8 == "This is a sill"
    =#
    @test convert(UTF8Str, [0xed, 0x80, 0x80]) == "\ud000"
end


text:
int main()
{
   int x = 15;
   int b[x];
   double c[-1];
   return b[16];
}

max_stars_repo_name: lukaszcz/javalette

max_issues_repo_name: lukaszcz/javalette

max_forks_repo_name: lukaszcz/javalette


text:
function Base.show(io::IO, x::EXPR{T}, indent = 0) where T
    indent == 3 && return
    name = sprint(show, T)
    print(io, "  "^indent, T, "  ", x.span)
    if isempty(x.defs)
        println()
    else
        println("  {", join((a.id for a in x.defs), ","), "}")
    end
    for (i, a) in enumerate(x.args)
        show(io, a, indent + 1)
    end
end

function Base.show(io::IO, x::Scope{T}, indent = 0) where {T}
    print(io, "  "^indent, "↘ ", T, " (", length(x.args), ") ")
    println("[", join(collect(a.id.val for a in x.args if a isa Variable), ", "), "]")
    for a in x.args
        if a isa Scope
            show(io, a, indent + 1)
        end
    end
end

function Base.show(io::IO, x::Variable, indent = 0)
    println(io, " "^indent, "Var → $(x.id)::$(x.t)")
end


text:
"""
    Element1D <: AbstractElement{1}

Stores information for 1D element.

# Fields
- `low::typeof(1.0u"bohr")` : lowest value within element
- `high::typeof(1.0u"bohr")` : highest value within element
"""
struct Element1D <: AbstractElement{1}
    low::typeof(1.0u"bohr")
    high::typeof(1.0u"bohr")
    function Element1D(low::Unitful.Length, high::Unitful.Length)
        @assert high > low
        new(low, high)
    end
end

function Element1D(low, high)
    @assert dimension(low) == dimension(high) == NoDims
    return Element1D(low*u"bohr", high*u"bohr")
end

"""
    CubicElement <: AbstractElement{3}

Cubic 3D element.

# Fields
- `center::SVector{3,Float64}`  : center location of element
- `a::typeof(1.0u"bohr")` : side length of the cube
"""
struct CubicElement <: AbstractElement{3}
    center::SVector{3,typeof(1.0u"bohr")}
    a::typeof(1.0u"bohr")
end

"""
    CubicElement(a) -> CubicElement

Return `CubicElement` with center at origin and side length `a`.
"""
function CubicElement(a)
    return CubicElement((0.,0.,0.).*u"bohr", a)
end

"""
    CubicElement(xe::Element1D, ye::Element1D, ze::Element1D) -> CubicElement

Construct `CubicElement` from three `Element1D`. Center is taken from elements,
which need to have same `elementsize`.
"""
function CubicElement(xe::Element1D, ye::Element1D, ze::Element1D)
    @assert elementsize(xe) == elementsize(ye) == elementsize(ze)
    center = SVector(getcenter(xe), getcenter(ye), getcenter(ze))
    return CubicElement(center, elementsize(xe))
end

"""
    CubicElementArray <: AbstractElementArray{CubicElement,3}

Cubic element that is divided to smaller cubic elments.

# Fields
- `a::typeof(1.0u"bohr")`                    : cubes side lenght
- `center::::SVector{3,typeof(1.0u"bohr")}`  : cube center
- `npoints`                                  : number of elements per degree of freedom
"""
struct CubicElementArray <: AbstractElementArray{CubicElement,3}
    a::typeof(1.0u"bohr")
    center::SVector{3,typeof(1.0u"bohr")}
    npoints::Int
    function CubicElementArray(a, npoints::Int; center=zeros(3).*u"bohr")
        @assert a > 0u"bohr"  "Element needs to have positive size"
        @assert npoints > 0 "Number of elements needs to be more than zero"
        new(a, center, npoints)
    end
end

"""
    ElementVector <: AbstractElementArray{Element1D,1}

Holds several 1D elements that are next to each other.
In practice this just a array of `Element1D` with a condition that
they are next to each other.

# Fields
- `v::Vector{Element1D}`   :   Vector for elements

# Creation
    ElementVector(el::Element1D...)
    ElementVector(start, bounds...)

- `start`   :  lowest boundary value for elements
- `bounds`  :  boundaries for next elements, needs to have increasing values
"""
struct ElementVector <: AbstractElementArray{Element1D,1}
    v::Vector{Element1D}
    function ElementVector(el::Element1D...)
        if length(el) > 1
            s = el[begin].high
            for e in el[begin+1:end]
                @assert s == e.low  "Elements are not next to each other"
                s = e.high
            end
        end
        new(Vector(el))
    end
    function ElementVector(start, bounds...)
        s = start
        for b in bounds
            @assert s < b  "Invalid element boundaries"
            s = b
        end
        v = [Element1D(start, bounds[begin])]
        if length(bounds) > 1
            s = bounds[begin]
            for b in bounds[begin+1:end]
                push!(v, Element1D(s,b))
                s = b
            end
        end
        new(v)
    end
end

Base.size(ev::ElementVector) = size(ev.v)
Base.getindex(ev::ElementVector, i) = ev.v[i]

"""
    elementsize(ce::CubicElementArray) -> Float64

Cubes 1D side length
"""
elementsize(ce::CubicElementArray) = ce.a/ce.npoints

"""
    elementsize(e::Element1D) -> Float64

Element length/size
"""
elementsize(e::Element1D) = e.high - e.low

"""
    getcenter(e::Element1D) -> Float64

Center location of element
"""
getcenter(e::Element1D) = 0.5*(e.high + e.low)

function get_1d_element(ce::CubicElementArray, i::Int)
    @assert 0 < i <= ce.npoints
    s = elementsize(ce)
    low = -0.5ce.a+(i-1)*s
    return Element1D(low, low+s)
end

function Base.getindex(ca::CubicElementArray, I::Int, J::Int, K::Int)
    low = ca.center .-  0.5 .* ca.a
    step = ca.a / ca.npoints
    c0 = low .+ (0.5.*step)
    nc = c0 .+ step .* ([I,J,K].-1)
    return CubicElement(nc, step)
end

function Base.show(io::IO, ce::CubicElementArray)
    print(io, "Cubic elements=$(size(ce))")
end

Base.size(ce::CubicElementArray) = (ce.npoints, ce.npoints, ce.npoints)

Base.show(io::IO, ce::CubicElement) = print(io, "Cubic element a = $(ce.a)")

"""
    get_center(ce::CubicElement)
    get_center(ce::CubicElementArray)

Return element center.
"""
get_center(ce::CubicElement) = ce.center
get_center(ce::CubicElementArray) = ce.center

## Element grids

"""
    CubicElementGrid{NG, NE} <: AbstractElementGrid{6}

Cube that is divided to smaller cubes that have integration grid.
Elements and integration points are symmetric for x-, y- and z-axes.

# Fields
- `elements::CubicElements` : cubic elements
- `ecenters::Vector{Float64}` : center locations for the subcube elements
- `gpoints::Vector{Float64}`  : integration grid definition, in 1D
- `w::Vector{Float64}`  :  integration weights
- `origin::SVector{3}{Float64}`  : origin location of the supercube

# Example
```jldoctest
julia> CubicElementGrid(5u"pm", 4, 32; origin=[1., 0., 0.].*u"nm)
Cubic elements grid with 4^3 elements with 32^3 Gauss points
```
"""
struct CubicElementGrid{NG, NE} <: AbstractElementGrid{6}
    elements::CubicElementArray
    ecenters::Vector{Float64}
    gpoints::Vector{Float64}
    w::Vector{Float64}
    origin::SVector{3}{Float64}
    function CubicElementGrid(a::Unitful.Length, nelements::Int, ngpoints::Int;
                              origin=SVector(0.,0.,0.).*unit(a))
        ce = CubicElementArray(a, nelements)
        x, w = gausspoints(ce, ngpoints)

        #TODO make this to take units properly
        ecenters = map( x-> austrip(get_center(x)[1]) , ce[:,1,1])
        new{ngpoints, nelements}(ce, ecenters, x, w, austrip.(origin))
    end
end

function Base.show(io::IO, ceg::CubicElementGrid)
    print(io, "Cubic elements grid with $(size(ceg.elements,1))^3 elements with $(length(ceg.gpoints))^3 Gauss points")
end

function Base.show(io::IO, ::MIME"text/plain", ceg::CubicElementGrid)
    print(io, "Cubic elements grid with $(size(ceg.elements,1))^3 elements with $(length(ceg.gpoints))^3 Gauss points")
end

function Base.size(c::CubicElementGrid)
    ng = length(c.gpoints)
    ne = size(c.elements)[1]
    return (ng, ng, ng, ne, ne, ne)
end

function Base.getindex(c::CubicElementGrid, i::Int,j::Int,k::Int, I::Int,J::Int,K::Int)
    return SVector(c.ecenters[I]+c.gpoints[i], c.ecenters[J]+c.gpoints[j], c.ecenters[K]+c.gpoints[k] )
end

"""
    xgrid(ceg::CubicElementGrid) -> Matrix{Float64}

x-axis coordinated of for elements and grids.
First index is for grids and second for elements.
"""
xgrid(ceg::CubicElementGrid) = [x+X+ceg.origin[1] for x in ceg.gpoints, X in ceg.ecenters ]

"""
    ygrid(ceg::CubicElementGrid) -> Matrix{Float64}

y-axis coordinated of for elements and grids.
First index is for grids and second for elements.
"""
ygrid(ceg::CubicElementGrid) = [x+X+ceg.origin[2] for x in ceg.gpoints, X in ceg.ecenters ]

"""
    zgrid(ceg::CubicElementGrid) -> Matrix{Float64}

z-axis coordinated of for elements and grids.
First index is for grids and second for elements.
"""
zgrid(ceg::CubicElementGrid) = [x+X+ceg.origin[3] for x in ceg.gpoints, X in ceg.ecenters ]

"""
    grid1d(ceg::CubicElementGrid) -> Matrix{Float64}

Return matrix of grid points in elements.
First index is for grids and second for elements.
Differs from [`xgrid`](@ref), [`ygrid`](@ref) and [`zgrid`](@ref) in that
the origin of grid is not specified. Which is equal to origin at 0,0,0.
"""
grid1d(ceg::CubicElementGrid) = [x+X for x in ceg.gpoints, X in ceg.ecenters ]

## 1D grids
# Plan is to build noncubic grids based on these

struct ElementGridVector <: AbstractMatrix{Float64}
    elements::ElementVector
    r::Matrix{Float64}
    w::Matrix{Float64}
    function ElementGridVector(ev::ElementVector, ng)
        tmp = gausspoints.(ev, ng)
        r = hcat([ x[1] for x in tmp ]...)
        w = hcat([ x[2] for x in tmp ]...)
        new(ev, r, w)
    end
end

Base.size(egv::ElementGridVector) = size(egv.r)

Base.getindex(egv::ElementGridVector, i, j) = egv.r[i,j]

## Gauss points for integration

function gausspoints(n; elementsize=(-1.0, 1.0))
    x, w = gausslegendre(n)
    esize = austrip.(elementsize)
    shift = (esize[2]+esize[1])./2
    x = x .* (esize[2]-esize[1])./2 .+ shift
    w .*= (esize[2]-esize[1])/2
    return x, w
end

"""
    gausspoints(el::Element1D, n::Int) -> (SVector, SVector)

Create `n` Gauss-Legendre points for element.

Returns a tuple with first index having Gauss points and
second integration weights.
"""
function gausspoints(el::Element1D, n::Int)
    return gausspoints(n; elementsize=austrip.((el.low, el.high)) )
end

"""
    gausspoints(ce::CubicElements, npoints::Int) -> (SVector, SVector)

Create `npoints` Gauss-Legendre points for element.

Returns a tuple with first index having Gauss points and
second integration weights.

Only one set of Gauss points that can be used for all elements is returned.
The returned points have center at 0 and need to be shifted for different elements.
"""
function gausspoints(ce::CubicElementArray, npoints::Int)
    s = elementsize(ce)/2
    return gausspoints(npoints; elementsize=(-s, s))
end


text:
const MONGOC_LOG_LEVEL_ERROR = 0
const MONGOC_LOG_LEVEL_CRITICAL = 1
const MONGOC_LOG_LEVEL_WARNING = 2
const MONGOC_LOG_LEVEL_MESSAGE = 3
const MONGOC_LOG_LEVEL_INFO = 4
const MONGOC_LOG_LEVEL_DEBUG = 5
const MONGOC_LOG_LEVEL_TRACE = 6

function _log_handler(level::Cint, domain::Ptr{UInt8}, message::Ptr{UInt8}, ::Ptr{Cvoid})
    jlevel = if level == MONGOC_LOG_LEVEL_ERROR
        Logging.Error
    elseif level == MONGOC_LOG_LEVEL_CRITICAL
        Logging.Error
    elseif level == MONGOC_LOG_LEVEL_WARNING
        Logging.Warn
    elseif level == MONGOC_LOG_LEVEL_MESSAGE
        Logging.Info
    elseif level == MONGOC_LOG_LEVEL_INFO
        Logging.Info
    elseif level == MONGOC_LOG_LEVEL_DEBUG
        Logging.Debug
    elseif level == MONGOC_LOG_LEVEL_TRACE
        Logging.Debug
    else
        error("Unexpected mongoc log level $level")
    end
    domain = unsafe_string(domain)
    message = unsafe_string(message)
    @logmsg jlevel "$domain $message"
    nothing
end

function init_log_handler()
    mongoc_set_log_handler(@cfunction(_log_handler, Cvoid, (Cint, Ptr{UInt8}, Ptr{UInt8}, Ptr{Cvoid})))
end