text:
@maintain_type struct BicycleControl{T} <: FieldVector{3,T}    # common to all models below
    δ::T    # steering angle
    Fxf::T  # front tire longitudinal force (tire frame)
    Fxr::T  # rear tire longidutinal force (tire frame)
end

struct BicycleModelParams{T}
    # Dimensions
    L::T    # wheelbase
    a::T    # distance from front axle to CG
    b::T    # distance from rear axle to CG
    h::T    # CG height

    # Mass and yaw moment of inertia
    G::T    # gravitational acceleration
    m::T    # vehicle mass
    Izz::T  # yaw moment of inertia

    # Tire model parameters
    μ::T    # coefficient of friction
    Cαf::T  # front tire (pair) cornering stiffness
    Cαr::T  # rear tire (pair) cornering stiffness

    # Longitudinal Drag Force Parameters (FxDrag = Cd0 + Cd1*Ux + Cd2*Ux^2)
    Cd0::T  # rolling resistance
    Cd1::T  # linear drag coefficint
    Cd2::T  # quadratic "aero" drag coefficint
end
BicycleModelParams(vehicle::Dict) = BicycleModelParams((vehicle[n] for n in fieldnames(BicycleModelParams))...)

abstract type AbstractBicycleModel{T} <: DifferentialDynamics end
Base.getproperty(B::AbstractBicycleModel, name::Symbol) = getproperty(getfield(B, :params), name)    # potentially not a great idea...

# Coupled Tire Forces - Simplified Model
@inline function fialatiremodel(α, Cα, μ, Fx, Fz)
    F_max = μ*Fz
    abs(Fx) >= F_max ? zero(F_max) : _fialatiremodel(tan(α), Cα, sqrt(F_max*F_max - Fx*Fx))
end

@inline function _fialatiremodel(tanα, Cα, Fy_max)
    tanα_slide = 3*Fy_max/Cα
    ratio = abs(tanα/tanα_slide)
    if ratio <= 1
        -Cα*tanα*(1 - ratio + ratio*ratio/3)
    else
        -Fy_max*sign(tanα)
    end
end

@inline function invfialatiremodel(Fy, Cα, μ, Fx, Fz)
    F_max = μ*Fz
    Fy_max = sqrt(F_max*F_max - Fx*Fx)
    atan(_invfialatiremodel(Fy, Cα, Fy_max))
end

@inline function _invfialatiremodel(Fy, Cα, Fy_max)    # returns tanα
    if abs(Fy) >= Fy_max
        -(3*Fy_max/Cα)*sign(Fy)
    else
        -(1 + cbrt(abs(Fy)/Fy_max - 1))*sign(Fy)
    end
end

@inline function lateral_tire_forces(B::AbstractBicycleModel, αf, αr, Fxf, Fxr, sδ, cδ, num_iters=3)
    L, a, b, h, m, μ, Cαf, Cαr, G = B.L, B.a, B.b, B.h, B.m, B.μ, B.Cαf, B.Cαr, B.G
    Fyf = zero(Fxf)
    Fx = Fxf*cδ - Fyf*sδ + Fxr
    for i in 1:num_iters
        Fzf = (m*G*b - h*Fx)/L
        Fyf = fialatiremodel(αf, Cαf, μ, Fxf, Fzf)
        Fx = Fxf*cδ - Fyf*sδ + Fxr
    end
    Fzr = (m*G*a + h*Fx)/L
    Fyr = fialatiremodel(αr, Cαr, μ, Fxr, Fzr)
    Fyf, Fyr
end

@inline function lateral_tire_forces(B::AbstractBicycleModel, q::StaticVector{6}, u::StaticVector{3}, num_iters=3)
    Ux, Uy, r = q[4], q[5], q[6]
    δ, Fxf, Fxr = u[1], u[2], u[3]
    a, b = B.a, B.b

    sδ, cδ = sincos(δ)
    αf = atan(Uy + a*r, Ux) - δ
    αr = atan(Uy - b*r, Ux)
    lateral_tire_forces(B, αf, αr, Fxf, Fxr, sδ, cδ, num_iters)
end

# Bicycle Model
struct BicycleModel{T} <: AbstractBicycleModel{T}
    params::BicycleModelParams{T}
end
BicycleModel(vehicle::Dict) = BicycleModel(BicycleModelParams(vehicle))

@maintain_type struct BicycleState{T} <: FieldVector{6,T}
    E::T    # world frame "x" position of CM
    N::T    # world frame "y" position of CM
    ψ::T    # world frame heading of vehicle
    Ux::T   # body frame longitudinal speed
    Uy::T   # body frame lateral speed
    r::T    # yaw rate (dψ/dt)
end

@maintain_type struct LocalRoadGeometry{T} <: FieldVector{4,T}
    ψ::T    # nominal trajectory heading
    κ::T    # nominal trajectory (local) curvature
    θ::T    # nominal trajectory (local) pitch grade
    ϕ::T    # nominal trajectory (local) roll grade
end

function (B::BicycleModel{T})((E, N, ψ, Ux, Uy, r)::StaticVector{6},
                              (δ, Fxf, Fxr)::StaticVector{3},
                              (ψᵣ, κ, θ, ϕ)::StaticVector{4}=zeros(LocalRoadGeometry{T})) where {T}
    a, b, m, Izz, Cd0, Cd1, Cd2 = B.a, B.b, B.m, B.Izz, B.Cd0, B.Cd1, B.Cd2

    sψ, cψ = sincos(ψ)
    sδ, cδ = sincos(δ)
    αf = atan(Uy + a*r, Ux) - δ
    αr = atan(Uy - b*r, Ux)
    Fyf, Fyr = lateral_tire_forces(B, αf, αr, Fxf, Fxr, sδ, cδ)
    Fx_drag = -Cd0 - Ux*(Cd1 + Cd2*Ux)
    Fx_grade = 0    # TODO: figure out how roll/pitch are ordered
    Fy_grade = 0
    F̃xf = Fxf*cδ - Fyf*sδ
    F̃yf = Fyf*cδ + Fxf*sδ
    SVector(
        -Ux*sψ - Uy*cψ, # Ux*cψ - Uy*sψ,  # (ψ measured from N)
        Ux*cψ - Uy*sψ,  # Ux*sψ + Uy*cψ,
        r,
        (F̃xf + Fxr + Fx_drag + Fx_grade)/m + r*Uy,
        (F̃yf + Fyr + Fy_grade)/m - r*Ux,
        (a*F̃yf - b*Fyr)/Izz
    )
end
(B::BicycleModel{T})(q::StaticVector{6}, ur::StaticVector{7}) where {T} = B(q, ur[SUnitRange(1,3)], ur[SUnitRange(4,7)])

# Tracking Bicycle Model
struct TrackingBicycleModel{T} <: AbstractBicycleModel{T}
    params::BicycleModelParams{T}
end
TrackingBicycleModel(vehicle::Dict) = TrackingBicycleModel(BicycleModelParams(vehicle))

@maintain_type struct TrackingBicycleState{T} <: FieldVector{6,T}
    Δs::T   # longitudinal error (w.r.t. nominal trajectory)
    Ux::T   # body frame longitudinal speed
    Uy::T   # body frame lateral speed
    r::T    # yaw rate
    Δψ::T   # heading error (w.r.t. nominal trajectory)
    e::T    # lateral error (w.r.t. nominal trajectory)
end

@maintain_type struct TrackingBicycleParams{T} <: FieldVector{4,T}
    V::T    # nominal trajectory speed
    κ::T    # nominal trajectory (local) curvature
    θ::T    # nominal trajectory (local) pitch grade
    ϕ::T    # nominal trajectory (local) roll grade
end

function (B::TrackingBicycleModel{T})((Δs, Ux, Uy, r, Δψ, e)::StaticVector{6},
                                      (δ, Fxf, Fxr)::StaticVector{3},
                                      (V, κ, θ, ϕ)::StaticVector{4}=zeros(TrackingBicycleParams{T})) where {T}
    a, b, m, Izz, Cd0, Cd1, Cd2 = B.a, B.b, B.m, B.Izz, B.Cd0, B.Cd1, B.Cd2

    sΔψ, cΔψ = sincos(Δψ)
    sδ, cδ = sincos(δ)
    αf = atan(Uy + a*r, Ux) - δ
    αr = atan(Uy - b*r, Ux)
    Fyf, Fyr = lateral_tire_forces(B, αf, αr, Fxf, Fxr, sδ, cδ)
    Fx_drag = -Cd0 - Ux*(Cd1 + Cd2*Ux)
    Fx_grade = 0    # TODO: figure out how roll/pitch are ordered
    Fy_grade = 0
    F̃xf = Fxf*cδ - Fyf*sδ
    F̃yf = Fyf*cδ + Fxf*sδ
    SVector(
        Ux*cΔψ - Uy*sΔψ - V,
        (F̃xf + Fxr + Fx_drag + Fx_grade)/m + r*Uy,
        (F̃yf + Fyr + Fy_grade)/m - r*Ux,
        (a*F̃yf - b*Fyr)/Izz,
        r - (Ux*cΔψ - Uy*sΔψ)*κ,
        Ux*sΔψ + Uy*cΔψ
    )
end
(B::TrackingBicycleModel{T})(q::StaticVector{6}, ur::StaticVector{7}) where {T} = B(q, ur[SUnitRange(1,3)], ur[SUnitRange(4,7)])

# Lateral Tracking Bicycle Model
struct LateralTrackingBicycleModel{T} <: AbstractBicycleModel{T}
    params::BicycleModelParams{T}
end
LateralTrackingBicycleModel(vehicle::Dict) = LateralTrackingBicycleModel(BicycleModelParams(vehicle))

@maintain_type struct LateralTrackingBicycleState{T} <: FieldVector{4,T}
    Uy::T   # body frame lateral speed
    r::T    # yaw rate
    Δψ::T   # heading error (w.r.t. nominal trajectory)
    e::T    # lateral error (w.r.t. nominal trajectory)
end

@maintain_type struct LateralTrackingBicycleParams{T} <: FieldVector{4,T}
    Ux::T   # body frame longitudinal speed
    κ::T    # nominal trajectory (local) curvature
    θ::T    # nominal trajectory (local) pitch grade
    ϕ::T    # nominal trajectory (local) roll grade
end

function (B::LateralTrackingBicycleModel{T})((Uy, r, Δψ, e)::StaticVector{4},
                                             (δ, Fxf, Fxr)::StaticVector{3},
                                             (Ux, κ, θ, ϕ)::StaticVector{4}=zeros(LateralTrackingBicycleParams{T})) where {T}
    a, b, m, Izz = B.a, B.b, B.m, B.Izz

    sΔψ, cΔψ = sincos(Δψ)
    sδ, cδ = sincos(δ)
    αf = atan(Uy + a*r, Ux) - δ
    αr = atan(Uy - b*r, Ux)
    Fyf, Fyr = lateral_tire_forces(B, αf, αr, Fxf, Fxr, sδ, cδ)
    Fy_grade = 0
    F̃yf = Fyf*cδ + Fxf*sδ
    SVector(
        (F̃yf + Fyr + Fy_grade)/m - r*Ux,
        (a*F̃yf - b*Fyr)/Izz,
        r - Ux*κ,
        Ux*sΔψ + Uy*cΔψ
    )
end
(B::LateralTrackingBicycleModel{T})(q::StaticVector{4}, up::StaticVector{7}) where {T} = B(q, up[SUnitRange(1,3)], up[SUnitRange(4,7)])

# Uy/r Stability Envelope
@inline function stable_limits(B::AbstractBicycleModel, Ux, Fxf, Fxr)
    L, a, b, h, m, μ, Cαf, Cαr, G = B.L, B.a, B.b, B.h, B.m, B.μ, B.Cαf, B.Cαr, B.G

    Fx = Fxf + Fxr
    Fy_grade = 0
    Fzf = (m*G*b - h*Fx)/L
    Fzr = (m*G*a + h*Fx)/L
    Ff_max = μ*Fzf
    Fr_max = μ*Fzr
    Fyf_max = abs(Fxf) > Ff_max ? zero(Ff_max) : sqrt(Ff_max*Ff_max - Fxf*Fxf)
    Fyr_max = abs(Fxr) > Fr_max ? zero(Fr_max) : sqrt(Fr_max*Fr_max - Fxr*Fxr)
    tanαf_slide = 3*Fyf_max/Cαf
    tanαr_slide = 3*Fyr_max/Cαr
    αf_slide = atan(tanαf_slide)
    αr_slide = atan(tanαr_slide)

    # https://ddl.stanford.edu/sites/default/files/publications/2012_Thesis_Bobier_A_Phase_Portrait_Approach_to_Vehicle_Stabilization_and_Envelope_Control.pdf
    δ_max = atan(L*( μ*G + Fy_grade/m)/(Ux*Ux) - tanαr_slide) + αf_slide    # corresponds to U̇y = 0; αr < 0; αf < 0; δ > 0; r > 0
    δ_min = atan(L*(-μ*G + Fy_grade/m)/(Ux*Ux) + tanαr_slide) - αf_slide    # corresponds to U̇y = 0; αr > 0; αf > 0; δ < 0; r < 0
    rC  = ( μ*G + Fy_grade/m)/Ux
    UyC = -Ux*tanαr_slide + b*rC
    rD  = Ux/L*(tan( αf_slide + δ_max) - tanαr_slide)     # αr > 0; αf > 0; δ > 0;
    UyD = Ux*tanαr_slide + b*rD
    mCD = (rD - rC)/(UyD - UyC)
    rE  = Ux/L*(tan(-αf_slide + δ_min) + tanαr_slide)    # αr < 0; αf < 0; δ < 0;
    UyE = -Ux*tanαr_slide + b*rE
    rF  = (-μ*G + Fy_grade/m)/Ux
    UyF = Ux*tanαr_slide + b*rF
    mEF = (rF - rE)/(UyF - UyE)

    H_veh = @SMatrix [ 1/Ux -b/Ux;    # β max (≈Uy/Ux)
                      -1/Ux  b/Ux;    # β min (≈Uy/Ux)
                       -mCD     1;    # r max
                        mEF    -1]    # r min
    G_veh = SVector(αr_slide, αr_slide, rC - UyC*mCD, -rF + UyF*mEF)
    δ_min, δ_max, H_veh, G_veh
end

# Bicycle Model with Combined Longitudinal Force Input (Fx = Fxf + Fxr)
@maintain_type struct BicycleControl2{T} <: FieldVector{2,T}
    δ::T    # steering angle
    Fx::T   # combined front/rear tire longitudinal force (sum of each in respective tire frames; approximately, total is applied longitudinally in body frame)
end
BicycleControl2((δ, Fxf, Fxr)::StaticVector{3}) = BicycleControl2(δ, Fxf + Fxr)

struct LongitudinalActuationParams{T}
    fwd_frac::T
    rwd_frac::T
    fwb_frac::T
    rwb_frac::T
end
LongitudinalActuationParams(vehicle::Dict) = LongitudinalActuationParams((vehicle[n] for n in fieldnames(LongitudinalActuationParams))...)
function longitudinal_tire_forces(LP::LongitudinalActuationParams, Fx)
    Fx > 0 ? (Fx*LP.fwd_frac, Fx*LP.rwd_frac) :
             (Fx*LP.fwb_frac, Fx*LP.rwb_frac)
end
BicycleControl(LP::LongitudinalActuationParams, (δ, Fx)::StaticVector{2}) = BicycleControl(δ, longitudinal_tire_forces(LP, Fx)...)

struct ControlLimits{T}
    Fx_max::T
    Fx_min::T
    Px_max::T
    δ_max::T
    κ_max::T
end
ControlLimits(vehicle::Dict) = ControlLimits((vehicle[n] for n in fieldnames(ControlLimits))...)
function apply_control_limits(CL::ControlLimits, (δ, Fx)::StaticVector{2}, Ux)
    Fx_max, Fx_min, Px_max, δ_max = CL.Fx_max, CL.Fx_min, CL.Px_max, CL.δ_max
    Ux = ForwardDiff.value(Ux)    # important for ForwardDiff/linearization, since Ux is technically a state variable
    BicycleControl2(clamp(δ, -δ_max, δ_max),
                    max(min(Fx, Fx_max, Px_max/Ux), Fx_min))
end

struct VehicleModel{T,B<:AbstractBicycleModel{T}} <: DifferentialDynamics
    bicycle_model::B
    longitudinal_params::LongitudinalActuationParams{T}
    control_limits::ControlLimits{T}
end
VehicleModel(vehicle::Dict, B=BicycleModel(vehicle)) = VehicleModel(B, LongitudinalActuationParams(vehicle), ControlLimits(vehicle))

get_Ux(B::BicycleModel, q, u, p) = q[4]
get_Ux(B::TrackingBicycleModel, q, u, p) = q[2]
get_Ux(B::LateralTrackingBicycleModel, q, u, p) = p[1]
function (X::VehicleModel{T})(q, u, p=zeros(SVector{4,T})) where {T}    # TODO: not necessarily always the right-sized p
    Ux = get_Ux(X.bicycle_model, q, u, p)
    X.bicycle_model(q, BicycleControl(X.longitudinal_params, apply_control_limits(X.control_limits, u, Ux)), p)
end
(X::VehicleModel{T,BicycleModel{T}})(q::StaticVector{6}, ur::StaticVector{6}) where {T} = X(q, ur[SUnitRange(1,2)], ur[SUnitRange(3,6)])
(X::VehicleModel{T,TrackingBicycleModel{T}})(q::StaticVector{6}, ur::StaticVector{6}) where {T} = X(q, ur[SUnitRange(1,2)], ur[SUnitRange(3,6)])
(X::VehicleModel{T,LateralTrackingBicycleModel{T}})(q::StaticVector{4}, ur::StaticVector{6}) where {T} = X(q, ur[SUnitRange(1,2)], ur[SUnitRange(3,6)])

# Estimates for Longitudinal Dynamics
function steady_state_estimates(X::VehicleModel{T}, V, A_tan, κ;
                                num_iters=4, r=V*κ, β0=T(0), δ0=T(0), Fyf0=T(0)) where {T}
    # assuming vehicle CM tracks nominal trajectory exactly; constant sideslip w.r.t. nominal trajectory
    B, LP, CL = X.bicycle_model, X.longitudinal_params, X.control_limits
    L, a, b, h, m, Izz, μ, G = B.L, B.a, B.b, B.h, B.m, B.Izz, B.μ, B.G
    Cαf, Cαr, Cd0, Cd1, Cd2 = B.Cαf, B.Cαr, B.Cd0, B.Cd1, B.Cd2
    fwd_frac, rwd_frac, fwb_frac, rwb_frac = LP.fwd_frac, LP.rwd_frac, LP.fwb_frac, LP.rwb_frac
    Fx_max, Px_max = CL.Fx_max, CL.Px_max

    A_rad = V*V*κ
    A_mag = hypot(A_tan, A_rad)
    A_max = μ*G
    if A_mag > A_max             # nominal trajectory total acceleration exceeds friction limit
        if abs(A_rad) > A_max    # nominal trajectory lateral acceleration exceeds friction limit
            A_rad = A_max*sign(A_rad)
            A_tan = zero(A_rad)
            # error("TODO: case when nominal trajectory lateral acceleration exceeds friction limit")
        else    # prioritize radial acceleration for path tracking; reduce intended acceleration along path to compensate
            A_tan = sqrt(A_max*A_max - A_rad*A_rad)*sign(A_tan)
        end
    end
    ṙ = A_tan*κ

    i = 1
    β, δ, Fyf = β0, δ0, Fyf0
    local Ux, Uy, Fxr, Fxf
    while true
        sβ, cβ = sincos(β)
        sδ, cδ = sincos(δ)
        Ux, Uy = V*cβ, V*sβ
        Fx_drag = -Cd0 - Ux*(Cd1 + Cd2*Ux)
        Fx_grade = 0    # TODO: figure out how roll/pitch are ordered
        Fy_grade = 0

        Ax = A_tan*cβ - A_rad*sβ             # Ax = U̇x - r*Uy
        Ay = A_tan*sβ + A_rad*cβ             # Ay = U̇y + r*Ux
        Fx = Ax*m - Fx_drag - Fx_grade       # Fx = F̃xf + Fxr = Fxf*cδ - Fyf*sδ + Fxr
        Fx = min(Fx, min(Fx_max, Px_max/Ux)*(rwd_frac + fwd_frac*cδ) - Fyf*sδ)    # braking force saturated by friction, not engine, limits
        Fzr, Fzf = (m*G*a + h*Fx)/L, (m*G*b - h*Fx)/L
        Fr_max, Ff_max = μ*Fzr, μ*Fzf

        Fxr = clamp((Fx + Fyf*sδ)*ifelse(Fx > 0, rwd_frac/(rwd_frac + fwd_frac*cδ),
                                                 rwb_frac/(rwb_frac + fwb_frac*cδ)),
                    -Fr_max, Fr_max)
        Fyr_max = sqrt(Fr_max*Fr_max - Fxr*Fxr)
        Fyr = (Ay*m - Fy_grade - ṙ*Izz/a)/(1 + b/a)
        Fyr = clamp(Fyr, -Fyr_max, Fyr_max)    # TODO: maybe reduce A_tan in the case that Fyr exceeds Fyr_max
        tanαr = _invfialatiremodel(Fyr, Cαr, Fyr_max)

        F̃xf = clamp(Fx - Fxr, -Ff_max, Ff_max)
        F̃yfmax = sqrt(Ff_max*Ff_max - F̃xf*F̃xf)
        F̃yf = clamp((b*Fyr + ṙ*Izz)/a, -F̃yfmax, F̃yfmax)
        Fxf = F̃xf*cδ + F̃yf*sδ
        Fyf = F̃yf*cδ - F̃xf*sδ
        Fyf_max = sqrt(Ff_max*Ff_max - Fxf*Fxf)
        αf = atan(_invfialatiremodel(Fyf, Cαf, Fyf_max))
        δ = atan(Uy + a*r, Ux) - αf

        if i == num_iters
            Ax = (Fxf*cδ - Fyf*sδ + Fxr + Fx_drag + Fx_grade)/m
            Ay = (Fyf*cδ + Fxf*sδ + Fyr + Fy_grade)/m
            A_tan = Ax*cβ + Ay*sβ
            break
        end
        i = i + 1
        β = atan(tanαr + b*r/Ux)
    end

    sβ, cβ = sincos(β)
    Ux, Uy = V*cβ, V*sβ
    (β=β, Ux=Ux, Uy=Uy, r=r, A=A_tan, δ=δ, Fxf=Fxf, Fxr=Fxr)
end

max_stars_repo_name: Michelle-NYX/CollisionAvoidance

max_issues_repo_name: Michelle-NYX/CollisionAvoidance

max_forks_repo_name: Michelle-NYX/CollisionAvoidance


text:
function rsch_integ_rk4!(
    E::Float64,
    Z::Int64, l::Int64, rmesh, V, P, Q;
    MAXVAL_STOP=1e6
)
    Nr = size(rmesh,1)

    # Calculate Vmid
    Vmid = zeros(Float64, Nr-1)
    for i in 1:Nr-1
        rmid = 0.5*( rmesh[i] + rmesh[i+1] )
        Vmid[i] = radial_interp(rmesh, V, rmid, i+1)
    end

    #@printf("Some Vmid\n")
    #for i in 1:4
    #    @printf("%8d %20.10e\n", i, Vmid[i])
    #end
    #@printf("...\n")
    #for i in Nr-4:Nr-1
    #    @printf("%8d %20.10e\n", i, Vmid[i])
    #end

    # BC at r -> 0
    y0 = zeros(Float64,2)
    if l == 0
        y0[1] = 1.0 - Z*rmesh[1]
        y0[2] = -Z
    else
        y0[1] = rmesh[1]^l
        y0[2] = l*rmesh[1]^(l-1)
    end

    C1 = zeros(Float64,Nr)
    C2 = zeros(Float64,Nr)
    for i in 1:Nr
        C1[i] = 2*(V[i] - E) + l*(l + 1)/rmesh[i]
        C2[i] = -2/rmesh[i]
    end

    C1mid = zeros(Float64,Nr-1)
    C2mid = zeros(Float64,Nr-1)
    for i in 1:Nr-1
        rmid = 0.5*(rmesh[i] + rmesh[i+1])
        C1mid[i] = 2*(Vmid[i] - E) + l*(l + 1)/rmid^2
        C2mid[i] = -2/rmid
    end

    y1 = zeros(Float64,Nr)
    y2 = zeros(Float64,Nr)
    imax = sch_rk4_step!( rmesh, y0, C1, C2, C1mid, C2mid, y1, y2, MAXVAL_STOP )

    for i in 1:imax
        P[i] = y1[i]*rmesh[i] # P(r) = r*R(r)
        Q[i] = y2[i]*rmesh[i] + y1[i]
    end

    return imax
end

ext: jl


text:
@testset "iter" begin
    x1 = [1,2,3,4,5]
    x2 = pyjl(x1)
    x3 = pylist(x2)
    x4 = pyconvert(Vector{Int}, x3)
    @test x1 == x4
end


text:
# Arithmetic
#XYZ and LMS are linear vector spaces
const Linear3 = Union{XYZ, LMS}
+(a::Linear3, b::Linear3) = mapc(+, a, b)
-(a::Linear3, b::Linear3) = mapc(-, a, b)
-(a::Linear3) = mapc(-, a)
*(k::Number, a::Linear3) = mapc(v -> k * v, a)
*(a::Linear3, k::Number) = k * a
/(a::Linear3, k::Number) = mapc(v -> v / k, a)

# Algorithms relating to color processing and generation

# Chromatic Adaptation / Whitebalancing
# -------------------------------------

"""
    whitebalance(c, src_white, ref_white)

Whitebalance a color.

Input a source (adopted) and destination (reference) white. For example, if you want a photo
taken under fluorescent lighting to appear correct in regular sunlight, you might do
something like `whitebalance(c, WP_F2, WP_D65)`.

# Arguments

- `c`: An observed color.
- `src_white`: Adopted or source white corresponding to `c`
- `ref_white`: Reference or destination white.

Returns a whitebalanced color.
"""
function whitebalance(c::T, src_white::Color, ref_white::Color) where T <: Color
    c_lms = convert(LMS, c)
    src_wp = convert(LMS, src_white)
    dest_wp = convert(LMS, ref_white)

    # This is sort of simplistic, it sets the degree of adaptation term in
    # CAT02 to 0.
    # Setting the degree of adaptation to 0 is rather odd. Wouldn’t setting
    # it to 1.0 make more sense as a temporary default value?
    ans = LMS(c_lms.l * dest_wp.l / src_wp.l,
              c_lms.m * dest_wp.m / src_wp.m,
              c_lms.s * dest_wp.s / src_wp.s)

    convert(T, ans)
end

# Simulation of color deficiency (color "blindness")
# ----------------------------

# This method is due to:
# Brettel, H., Viénot, F., & Mollon, J. D. (1997).  Computerized simulation of
# color appearance for dichromats. Josa A, 14(10), 2647–2655.
#
# These functions add to Brettel's method a parameter p in [0, 1] giving the
# degree of photopigment loss. At p = 1, the complete loss of a particular
# photopigment is simulated, where 0 < p < 1 represents partial loss.

# This is supposed to be "the brightest possible metamer of an equal-energy
# stimulus". I'm punting a bit and just calling that RGB white.
const default_brettel_neutral = convert(LMS, RGB(1, 1, 1))

# Helper function for Brettel conversions.
function brettel_abc(neutral::LMS, anchor::LMS)
    a = neutral.m * anchor.s - neutral.s * anchor.m
    b = neutral.s * anchor.l - neutral.l * anchor.s
    c = neutral.l * anchor.m - neutral.m * anchor.l
    (a, b, c)
end

"""
    protanopic(c)
    protanopic(c, p)

Convert a color to simulate protanopic color deficiency (lack of the
long-wavelength photopigment). `c` is the input color; the optional
argument `p` is the fraction of photopigment loss, in the range 0 (no
loss) to 1 (complete loss).
"""
function protanopic(q::T, p, neutral::LMS) where T <: Color
    q = convert(LMS, q)
    anchor_wavelen = q.s / q.m < neutral.s / neutral.m ? 575 : 475
    anchor = colormatch(anchor_wavelen)
    anchor = convert(LMS, anchor)
    (a, b, c) = brettel_abc(neutral, anchor)

    q = LMS((one(p) - p) * q.l + p * (-(b*q.m + c*q.s)/a),
            q.m,
            q.s)
    convert(T, q)
end

"""
    deuteranopic(c)
    deuteranopic(c, p)

Convert a color to simulate deuteranopic color deficiency (lack of the
middle-wavelength photopigment). See [`protanopic`](@ref) for detail about the arguments.
"""
function deuteranopic(q::T, p, neutral::LMS) where T <: Color
    q = convert(LMS, q)
    anchor_wavelen = q.s / q.l < neutral.s / neutral.l ? 575 : 475
    anchor = colormatch(anchor_wavelen)
    anchor = convert(LMS, anchor)
    (a, b, c) = brettel_abc(neutral, anchor)

    q = LMS(q.l,
            (one(p) - p) * q.m + p * (-(a*q.l + c*q.s)/b),
            q.s)
    convert(T, q)
end

"""
    tritanopic(c)
    tritanopic(c, p)

Convert a color to simulate tritanopic color deficiency (lack of the
short-wavelength photopigment). See [`protanopic`](@ref) for detail about
the arguments.
"""
function tritanopic(q::T, p, neutral::LMS) where T <: Color
    q = convert(LMS, q)
    anchor_wavelen = q.m / q.l < neutral.m / neutral.l ? 660 : 485
    anchor = colormatch(anchor_wavelen)
    anchor = convert(LMS, anchor)
    (a, b, c) = brettel_abc(neutral, anchor)

    q = LMS(q.l,
            q.m,
            (one(p) - p) * q.l + p * (-(a*q.l + b*q.m)/c))
    convert(T, q)
end

protanopic(c::Color, p)   = protanopic(c, p, default_brettel_neutral)
deuteranopic(c::Color, p) = deuteranopic(c, p, default_brettel_neutral)
tritanopic(c::Color, p)   = tritanopic(c, p, default_brettel_neutral)

protanopic(c::Color)   = protanopic(c, 1.0)
deuteranopic(c::Color) = deuteranopic(c, 1.0)
tritanopic(c::Color)   = tritanopic(c, 1.0)

# MSC - Most Saturated Colorant for given hue h
# ---------------------

"""
    MSC(h)
    MSC(h, l; linear=false)

Calculate the most saturated color in sRGB gamut for any given hue `h` by
finding the corresponding corner in LCHuv space. Optionally, the lightness `l`
may also be specified.

# Arguments

- `h`: Hue [0,360] in LCHuv space
- `l`: Lightness [0,100] in LCHuv space

# Keyword arguments

- `linear` : If true, the saturation is linearly interpolated between black/
  white and `MSC(h)` as the gamut is approximately triangular in L-C section.

!!! note
    `MSC(h)` returns an `LCHuv` color, but `MSC(h, l)` returns a saturation
    value. This behavior might change in a future release.

"""
function MSC(h)

    #Wrap h to [0, 360] range
    h = normalize_hue(Float64(h))

    #Selecting edge of RGB cube; R=1 G=2 B=3
    # p #variable
    # o #min
    # t #max
    if     h < 12.173988685914473 #convert(LCHuv,RGB(1,0,0)).h
        p=3; t=1
    elseif h < 85.872748860776770 #convert(LCHuv,RGB(1,1,0)).h
        p=2; t=1
    elseif h < 127.72355046632740 #convert(LCHuv,RGB(0,1,0)).h
        p=1; t=2
    elseif h < 192.17397321802082 #convert(LCHuv,RGB(0,1,1)).h
        p=3; t=2
    elseif h < 265.87273498040290 #convert(LCHuv,RGB(0,0,1)).h
        p=2; t=3
    elseif h < 307.72354567594960 #convert(LCHuv,RGB(1,0,1)).h
        p=1; t=3
    else
        p=3; t=1
    end

    alpha=-sind(h)
    beta=cosd(h)

    #un &vn are calculated based on reference white (D65)
    #X=0.95047; Y=1.0000; Z=1.08883
    un = 0.19783982482140777 #4.0X/(X+15.0Y+3.0Z)
    vn = 0.46833630293240970 #9.0Y/(X+15.0Y+3.0Z)

    #sRGB matrix
    M = [0.4124564  0.3575761  0.1804375;
         0.2126729  0.7151522  0.0721750;
         0.0193339  0.1191920  0.9503041]'

    m_tx=M[t,1]
    m_ty=M[t,2]
    m_tz=M[t,3]
    m_px=M[p,1]
    m_py=M[p,2]
    m_pz=M[p,3]

    f1 = 4alpha*m_px+9beta*m_py
    a1 = 4alpha*m_tx+9beta*m_ty
    f2 = m_px+15m_py+3m_pz
    a2 = m_tx+15m_ty+3m_tz

    cp=((alpha*un+beta*vn)*a2-a1)/(f1-(alpha*un+beta*vn)*f2)

    col = zeros(3)
    col[p] = clamp01(srgb_compand(cp))
    # col[o] = 0.0
    col[t] = 1.0

    return convert(LCHuv, RGB(col[1],col[2],col[3]))
end

# Maximum saturation for given lightness and hue
# ----------------------

# Maximally saturated color for a specific hue and lightness
# is found by looking for the edge of LCHuv space.
function MSC(h, l; linear::Bool=false)
    if linear
        pmid = MSC(h)
        pend_l = l > pmid.l ? 100.0 : 0.0
        return (pend_l - l) / (pend_l - pmid.l) * pmid.c
    end
    return find_maximum_chroma(LCHuv{Float64}(l, 0, h))
end

# This function finds the maximum chroma for the lightness `c.l` and hue `c.h`
# by means of the binary search. Even though this requires more than 20
# iterations, somehow, this is fast.
function find_maximum_chroma(c::C,
                             low::Real=0,
                             high::Real=180) where {T, C<:Union{LCHab{T}, LCHuv{T}}}
    err = convert(T, 1e-6)
    l, h = convert(T, low), convert(T, high)
    h - l < err && return l

    mid = convert(T, (l + h) / 2)
    min(mid - l, h - mid) == zero(T) && return l
    lchm = C(c.l, mid, c.h)
    rgbm = convert(RGB{T}, lchm)
    clamped = max(red(rgbm), green(rgbm), blue(rgbm)) > 1-err ||
              min(red(rgbm), green(rgbm), blue(rgbm)) < err
    if clamped
        return find_maximum_chroma(c, l, mid)::T
    else
        return find_maximum_chroma(c, mid, h)::T
    end
end

function find_maximum_chroma(c::LCHab{T}) where T
    maxc = find_maximum_chroma(c, 0, 135)

    # The sRGB gamut in LCHab space has a *hollow* around the yellow corner.
    # Since the following boundary is based on the D65 white point, the values
    # should be modified on other conditions.
    if 97 < c.h < 108 && c.l > 92
        err = convert(T, 1e-6)
        len = 10000
        dh = convert(T, (100 - maxc) / len)
        h_yellow = 102.85124420310268 # convert(LCHab,RGB{Float64}(1,1,0)).h
        chroma = maxc
        for i = 1:len
            chroma += dh
            rgb = convert(RGB, LCHab(c.l, chroma, c.h))
            blue(rgb) < err && continue
            y = c.h < h_yellow ? red(rgb) : green(rgb)
            if y < 1-err
                maxc = chroma
            end
        end
    end
    return maxc
end
find_maximum_chroma(c::Lab) = find_maximum_chroma(convert(LCHab, c))
find_maximum_chroma(c::Luv) = find_maximum_chroma(convert(LCHuv, c))


text:
module KuboOscillator

    export kubo_oscillator_sde_1, kubo_oscillator_psde_1, kubo_oscillator_spsde_1
    export kubo_oscillator_sde_2, kubo_oscillator_psde_2, kubo_oscillator_spsde_2
    export kubo_oscillator_sde_3, kubo_oscillator_psde_3, kubo_oscillator_spsde_3

    using GeometricIntegrators.Equations

    q_init_A=[0.5, 0.0]
    q_init_B=[0.5 0.0 -0.5; 0.0 0.5 0.0]

    noise_intensity = 0.1

    Δt  = 0.01
    nt  = 10

    function kubo_oscillator_sde_v(t,q, v_out)
        v_out[1]=  q[2]
        v_out[2]= -q[1]
    end

    function kubo_oscillator_sde_B(t,q, B_out; col=0)
        noise_intensity = 0.1

        if col==0
            B_out[1,1]=  noise_intensity*q[2]
            B_out[2,1]= -noise_intensity*q[1]
        elseif col==1
            B_out[1]=  noise_intensity*q[2]
            B_out[2]= -noise_intensity*q[1]
        end
    end

    function kubo_oscillator_sde_1()
        # q_init_A - single deterministic initial condition
        # Generating 3 sample paths
        # 1-dimensional noise
        SDE(1, 3, kubo_oscillator_sde_v, kubo_oscillator_sde_B, q_init_A)
    end

    function kubo_oscillator_sde_2()
        # q_init_B - interpreted as a single random initial condition
        # The 3 columns correspond to 3 sample paths
        # 1-dimensional noise
        SDE(1, kubo_oscillator_sde_v, kubo_oscillator_sde_B, q_init_B)
    end

    function kubo_oscillator_sde_3()
        # q_init_B - interpreted as multiple deterministic initial conditions
        # The 3 columns correspond to 3 different deterministic initial conditions
        # Generating 3 sample paths for each initial condition
        # 1-dimensional noise
        SDE(1, 3, kubo_oscillator_sde_v, kubo_oscillator_sde_B, q_init_B)
    end

    # PSDE

    q_init_C=[0.5]
    p_init_C=[0.0]

    q_init_D=[0.5 0.0 -0.5]
    p_init_D=[0.0 0.5 0.0]

    function kubo_oscillator_psde_v(t, q, p, v_out)
        v_out[1]=  p[1]
    end

    function kubo_oscillator_psde_f(t, q, p, f_out)
        f_out[1]=  -q[1]
    end

    function kubo_oscillator_psde_B(t, q, p, B_out)
        B_out[1,1]= noise_intensity*p[1]
    end

    function kubo_oscillator_psde_G(t, q, p, G_out)
        G_out[1,1]= -noise_intensity*q[1]
    end

    function kubo_oscillator_psde_1()
        # q_init_C - single deterministic initial condition
        # Generating 3 sample paths
        # 1-dimensional noise
        PSDE(1, 3, kubo_oscillator_psde_v, kubo_oscillator_psde_f, kubo_oscillator_psde_B, kubo_oscillator_psde_G, q_init_C, p_init_C)
    end

    function kubo_oscillator_psde_2()
        # q_init_D - interpreted as a single random initial condition
        # The 3 columns correspond to 3 sample paths
        # 1-dimensional noise
        PSDE(1, kubo_oscillator_psde_v, kubo_oscillator_psde_f, kubo_oscillator_psde_B, kubo_oscillator_psde_G, q_init_D, p_init_D)
    end

    function kubo_oscillator_psde_3()
        # q_init_D - interpreted as multiple deterministic initial conditions
        # The 3 columns correspond to 3 different deterministic initial conditions
        # Generating 3 sample paths for each initial condition
        # 1-dimensional noise
        PSDE(1, 3, kubo_oscillator_psde_v, kubo_oscillator_psde_f, kubo_oscillator_psde_B, kubo_oscillator_psde_G, q_init_D, p_init_D)
    end

    # SPSDE

    function kubo_oscillator_spsde_v(t, q, p, v_out)
        v_out[1]=  p[1]
    end

    function kubo_oscillator_spsde_f1(t, q, p, f_out)
        f_out[1]=  -q[1]
    end

    function kubo_oscillator_spsde_f2(t, q, p, f_out)
        f_out[1]=  0.0
    end

    function kubo_oscillator_spsde_B(t, q, p, B_out)
        B_out[1,1]= noise_intensity*p[1]
    end

    function kubo_oscillator_spsde_G1(t, q, p, G_out)
        G_out[1,1]= -noise_intensity*q[1]
    end

    function kubo_oscillator_spsde_G2(t, q, p, G_out)
        G_out[1,1]= 0.0
    end

    function kubo_oscillator_spsde_1()
        # q_init_C - single deterministic initial condition
        # Generating 3 sample paths
        # 1-dimensional noise
        SPSDE(1, 3, kubo_oscillator_spsde_v, kubo_oscillator_spsde_f1, kubo_oscillator_spsde_f2, kubo_oscillator_spsde_B, kubo_oscillator_spsde_G1, kubo_oscillator_spsde_G2, q_init_C, p_init_C)
    end

    function kubo_oscillator_spsde_2()
        # q_init_D - interpreted as a single random initial condition
        # The 3 columns correspond to 3 sample paths
        # 1-dimensional noise
        SPSDE(1, kubo_oscillator_spsde_v, kubo_oscillator_spsde_f1, kubo_oscillator_spsde_f2, kubo_oscillator_spsde_B, kubo_oscillator_spsde_G1, kubo_oscillator_spsde_G2, q_init_D, p_init_D)
    end

    function kubo_oscillator_spsde_3()
        # q_init_D - interpreted as multiple deterministic initial conditions
        # The 3 columns correspond to 3 different deterministic initial conditions
        # Generating 3 sample paths for each initial condition
        # 1-dimensional noise
        SPSDE(1, 3, kubo_oscillator_spsde_v, kubo_oscillator_spsde_f1, kubo_oscillator_spsde_f2, kubo_oscillator_spsde_B, kubo_oscillator_spsde_G1, kubo_oscillator_spsde_G2, q_init_D, p_init_D)
    end

end


text:
module Test_klfapps

using Random
using LinearAlgebra
using MatrixPencils
using Test

@testset "Matrix Pencils Applications" begin

Random.seed!(2351);

@testset "Matrix Pencils Structure Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time info  = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(3,0); N = zeros(3,0);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [0, 0, 0] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(0,3); N = zeros(0,3);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [0, 0, 0] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(1,1); N = ones(1,1);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 1 && info.nrank == 1

M = ones(1,1); N = zeros(1,1);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [1] && info.nf == 0 && info.nrank == 1

M = ones(1,1); N = nothing;
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];
@time info = pkstruct(M, N)
@test info.rki == [] && info.lki == [] && info.id == [2] && info.nf == 0 && info.nrank == 2

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time info = pkstruct(M, N)
rki, lki, id, nf, nrank = info
@test rki == [0] && lki == [0] && id == [1] && nf == 3 && nrank == 4

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time info = pkstruct(M, N, fast = true, atol1 = 1.e-7, atol2 = 1.e-7)
@test info.rki == [0, 0, 1, 2] && info.lki == [0, 3] && info.id == [1,2] && info.nf == 3 && info.nrank == 12

#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
Ty = Float64
Ty = Complex{Float64}
for Ty in (Float64, Complex{Float64})

#Ty = Complex{Float64}
abstol = sqrt(eps(one(real(Ty))))

# given structure
mr = 2; nr = 3; ni = 4; nf = 10; ml = 5; nl = 4;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M = Q*M*Z;
N = Q*N*Z;

atol1 = 1.e-7; atol2 = 1.e-7;
@time info = pkstruct(M, N, fast = fast, atol1 = atol1, atol2 = atol2)
@test info.rki == [mr] && info.lki == [nl] && info.id == [ni] && info.nf == nf && info.nrank == mr+nl+ni+nf

end
end
end

Random.seed!(2351);

@testset "Matrix Pencils Eigenvalues Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time val, kinfo  = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 0, 0)

M = zeros(3,0); N = zeros(3,0);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], [0, 0, 0], Int64[], 0, 0)

M = zeros(0,3); N = zeros(0,3);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 0], Int64[], Int64[], 0, 0)

M = zeros(1,1); N = ones(1,1);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [0] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 1, 1)

N = zeros(1,1); M = ones(1,1);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [Inf] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [1], 0, 1)

M = ones(1,1); N = nothing;
@time val, info = peigvals(M, N, fast = fast)
@test val == Float64[] && info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];

@time val, kinfo = peigvals(M, N)
@test val == [Inf;Inf] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [2], 0, 2)

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time val, kinfo = peigvals(M, N)
@test length(val) == 4 &&
      length(filter(y-> y == true,isinf.(val))) == 1 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0], [0], [1], 3, 4)

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time val, kinfo = peigvals(M, N, fast = true, atol1 = 1.e-7, atol2 = 1.e-7)
@test length(val) == 6 &&
      length(filter(y-> y == true,isinf.(val))) == 3 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 1, 2], [0, 3], [1, 2], 3, 12)

#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
#Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

# given structure
mr = 2; nr = 3; ni = 4; nf = 10; ml = 5; nl = 4;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M = Q*M*Z;
N = Q*N*Z;

@time val, kinfo = peigvals(M, N, fast = fast, atol1 = 1.e-5, atol2 = 1.e-7)
@test length(val) == ni+nf &&
      length(filter(y-> y == true,isinf.(val))) == ni &&
      length(filter(y-> y == true,isfinite.(val))) == nf &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([mr], [nl], [ni], nf, mr+nl+ni+nf)

end
end
end

Random.seed!(2151);

@testset "Matrix Pencils Zeros Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 0, 0)

M = zeros(3,0); N = zeros(3,0);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], [0, 0, 0], Int64[], 0, 0)

M = zeros(0,3); N = zeros(0,3);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 0], Int64[], Int64[], 0, 0)

M = zeros(1,1); N = ones(1,1);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [0] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 1, 1)

N = zeros(1,1); M = ones(1,1);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [1], 0, 1)

M = ones(1,1); N = nothing;
@time val, iz, info = pzeros(M, N, fast = fast)
@test val == Float64[] && iz == [] && info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0  && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [Inf] && iz == [1] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [2], 0, 2)

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time val, iz, kinfo   = pzeros(M, N, fast = fast)
@test length(val) == 3 &&
      length(filter(y-> y == true,isinf.(val))) == 0 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0], [0], [1], 3, 4)

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time val, iz, kinfo   = pzeros(M, N, fast = fast, atol1 = 1.e-7, atol2 = 1.e-7)
@test length(val) == 4 &&
      length(filter(y-> y == true,isinf.(val))) == 1 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      iz == [1] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 1, 2], [0, 3], [1, 2], 3, 12)

#Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

abstol = sqrt(eps(one(real(Ty))))

# given structure
mr = 2; nr = mr+1; ni = 4; nf = 8; nl = 4; ml = nl+1;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M2 = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N2 = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M2 = Q*M2*Z;
N2 = Q*N2*Z;

M = copy(M2); N = copy(N2);
atol1 = 1.e-7; atol2 = 1.e-7;
@time val, iz, kinfo  = pzeros(M, N, fast = fast, atol1 = atol1, atol2 = atol2)
@test length(val) == ni+nf-1 &&
      length(val[isinf.(val)]) == ni-1 &&
      length(val[isfinite.(val)]) == nf && iz == (ni > 1 ? [ni-1] : [ ]) &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([mr], [nl], ni > 0 ? [ni] : [], nf, mr+nl+ni+nf)

end
end
end # pzeros

@testset "Matrix Pencils Rank Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@test prank(M, N, fastrank = fast) == 0

M = zeros(3,0); N = zeros(3,0);
@test prank(M, N, fastrank = fast) == 0

M = zeros(0,3); N = zeros(0,3);
@test prank(M, N, fastrank = fast) == 0

M = zeros(1,1); N = ones(1,1);
@test prank(M, N, fastrank = fast) == 1

@test prank(N, M, fastrank = fast) == 1

M = ones(1,1); N = nothing;
@test prank(M, N, fastrank = fast) == 1

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];
@test prank(M, N, fastrank = fast) == 4

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);

@test prank(M, N, fastrank = fast) == 12

Ty = Float64
Ty = Complex{Float64}
#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
for Ty in (Float64, Complex{Float64})

abstol = sqrt(eps(one(real(Ty))))

# full row rank
r = 5; n = 20;
M = rand(Ty,r,n)
N = rand(Ty,r,n)

@time prnk = prank(M, N, fastrank = fast)
@test prnk == r

# full column rank
r = 5; m = 20;
M = rand(Ty,m,r)
N = rand(Ty,m,r)

@time prnk = prank(M, N, fastrank = fast)
@test prnk == r

# given rank
r = 5; m = 20; n = 30;
Q = rand(Ty,m,m);
Z = rand(Ty,n,n);
M = Q*[ rand(Ty,r,r) zeros(Ty,r,n-r); zeros(Ty,m-r,n) ]*Z;
N = Q*[ rand(Ty,r,r) zeros(Ty,r,n-r); zeros(Ty,m-r,n) ]*Z;

@time prnk = prank(M, N, atol1 = 100*eps(opnorm(M,1)), atol2 = 100*eps(opnorm(N,1)), fastrank = fast)
@test prnk == r

end
end
end  # prank

end  # applications testset

end  # module


text:
mutable struct DisjointSetNode
    rank::Int
    p::DisjointSetNode
    DisjointSetNode() = (obj = new(0); obj.p = obj;)
end

findset(x::DisjointSetNode) = x == x.p ? x.p : x.p = findset(x.p)

function union!(x::DisjointSetNode, y::DisjointSetNode)
	xset = findset(x)
	yset = findset(y)
	if xset.rank > yset.rank
		yset.p = xset
		return
	elseif xset.rank == yset.rank
		yset.rank += 1
	end
	xset.p = yset
end

function hammingdistance(s1::String, s2::String)
	sum = 0
	for (i, j) in zip(s1, s2) sum += i == j ? 0 : 1 end
	return sum
end

function count_sets(nodes::Array{DisjointSetNode, 1})::Int
	sets::Array{DisjointSetNode, 1} = []
	for i in nodes
		if !(findset(i) in sets)
			push!(sets, findset(i))
		end
	end
	return length(sets)
end

function findclusters(E::Vector{Tuple{Int, Int, Int}}, n::Int, k::Int)
	edges_to_find::Int = length(E) - (k - 1)
	sort!(E)
	nodes::Array{DisjointSetNode, 1} = Array{DisjointSetNode, 1}(undef, n)
	for i in 1 : n
		nodes[i] = DisjointSetNode()
	end
	i = 1
	while count_sets(nodes) > k
		if findset(nodes[E[i][2]]) != findset(nodes[E[i][3]])
			union!(nodes[E[i][2]], nodes[E[i][3]])
		end
		i += 1
	end
	klusters::Dict{DisjointSetNode, Array{Int, 1}} = Dict()
	for i in 1 : n
		if !haskey(klusters, findset(nodes[i]))
			push!(klusters, findset(nodes[i]) => [i])
		else
			push!(klusters[findset(nodes[i])], i)
		end
	end
	return collect(values(klusters))
end

function findanimalgroups(animals::Vector{Tuple{String, String}}, k::Int64)
	E::Vector{Tuple{Int, Int, Int}} = []
	for i in 1 : length(animals)
		for j in i + 1 : length(animals)
			push!(E, (hammingdistance(animals[i][2], animals[j][2]), i, j))
		end
	end
	groups = findclusters(E, length(animals), k)
	animgroups::Array{Array{String, 1}, 1} = []
	for i in groups
		push!(animgroups, [])
		for j in i
			push!(animgroups[length(animgroups)], animals[j][1])
		end
	end
	return animgroups
end


text:
#
#  Use git:                    git status ....;   git add <filenames>;   git commit -m "..";   git push;   git rm  <filenames>
#  Use Jupyter notebooks:      using IJulia;   notebook()
#  Activation:                 ];   pkg> up;   pkg> activate
#  Working with JAC:           using Revise;   using JAC;   include("../src/jac.jl");   pkg> test
#
"""
`module JAC`
    ... Jena Atomic Calculator (JAC) provides tools for performing atomic (structure) calculations at various degrees of complexity
        and sophistication. It has been designed to not only calculate atomic level structures and properties [such as g-factors or
        hyperfine and isotope-shift parameters] but also transition amplitudes between bound-state levels [for the anapole moment, dipole
        operator, electron electric-dipole moment, parity non-conservation, etc.] and, in particular, (atomic) transition probabilities,
        Auger rates, photoionization cross sections, radiative and dielectronic recombination rates as well as cross sections for many
        other (elementary) processes. In the future, JAC will also facilitate interactive computations, the simulation of atomic cascades,
        the time-evolution of statistical tensors as well as various semi-empirical estimates of atomic properties. --
        In addition, the JAC module supports the display of level energies, electron and photon spectra, radial orbitals and
        and other atomic data.

**`Perform (atomic) computations of different complexity:`**
    JAC will eventually support **eight kinds** of computations which can be summarized as follows:

   + Atomic computations, based on explicitly specified electron configurations.
   + Restricted active-space computations (RAS; not yet properly implemented).
   + Interactive computations.
   + Atomic cascade computations (not yet fully implemented).
   + Atomic responses (not yet implemented).
   + Time-evolution of statistical tensors in (intense) light pusles (not yet implemented).
   + Semi-empirical estimates of cross sections, etc. (not yet properly implemented).
   + Symbolic evaluation of expressions from Racah's algebra, etc. (not yet properly implemented).

**`Further details and information`**

        + Kinds of atomic implementation                                       [cf. ? Details.kindsOfComputation]
        + Atomic amplitudes (partly) implemented in JAC                        [cf. ? Details.amplitudes]
        + Atomic level properties (partly) implemented in JAC                  [cf. ? Details.properties]
        + Atomic processes (partly) implemented in JAC                         [cf. ? Details.processes]
        + Interactive use of JAC procedures                                    [cf. ? Details.interactive]
        + Design principles and limitations of the JAC program                 [cf. ? Details.design]
        + Data types, structs and name conventions of the JAC module           [cf. ? Details.datatypes]
        + Atomic cascade computations and approximations                       [cf. ? Details.decayCascades]
        + Use of (em) light pulses in the time evolution of statist. tensors   [cf. ? Details.pulses]
        + Why Julia ?                                                          [cf. ? Details.whyJulia]

"""
module JAC

using  Dates, Printf,  LinearAlgebra, Interact, SpecialFunctions, FortranFiles, GaussQuadrature, QuadGK, GSL, JLD

export @racahsum,
       add, analyze, AlphaVariation, AnapoleMoment, AngularJ64, AngularM64, AngularJ, AngularMomentum,
       AsfSettings, Atomic, Auger, AutoIonization,
       Basics, Basis,
       compute, convertUnits, Cascade, Configuration, ConfigurationR, Continuum, CsfR, CoulombExcitation, CoulombIonization,
       diagonalize, Defaults, DecayYield, Details, Dielectronic, DoubleAuger,
       estimate, ElectricDipoleMoment, Einstein, EmMultipole,
       E1, M1, E2, M2, E3, M3, E4, M4,
       FormFactor,
       generate, Green, GreenFunction, getDefaults, Gui,
       Hfs, HydrogenicIon,
       interpolate, integrate, ImpactExcitation, ImpactExcitationAutoion, ImpactIonization, InternalConversion, IsotopeShift,
       LandeZeeman, Level, LevelSymmetry, LSjj, LSjjSettings,
       ManyElectron, Model, modify, MultiPhotonDeExcitation, MultiPhotonDoubleIon, MultiPhotonIonization, MultipoleMoment,
       MultipolePolarizibility, Multiplet,
       NoAmplitude, NoProcess, Nuclear, NoneQed,
       Orbital,
       perform, provide, PairAnnihilation1Photon, PairAnnihilation2Photon, PairProduction, ParityNonConservation,
       PhotoEmission, PhotoExcitation, PhotoExcitationAutoion, PhotoExcitationFluores, PhotoIonization, PhotoIonizationFluores,
       PhotoIonizationAutoion, PhotoRecombination, PlasmaShift,
       QedPetersburg, QedSydney,
       Radial, RadialIntegrals, Radiative, RadiativeAuger, RayleighCompton, REDA, READI,
       SchiffMoment, Shell, Subshell, setDefaults,
       tabulate, tools,
       UseCoulomb, UseBabushkin, UseGauge

# Basic data and data structures
include("module-Basics.jl")
include("module-Radial.jl")
include("module-Math.jl")
include("module-Defaults.jl")
include("module-ManyElectron.jl")
include("module-Nuclear.jl")

# Specialized functions/methods to manipulate these data
include("module-AngularMomentum.jl")
include("module-AngularCoefficients-Ratip2013.jl")
include("module-Bsplines.jl")
include("module-Continuum.jl")
include("module-Details.jl")
include("module-RadialIntegrals.jl")
include("module-HydrogenicIon.jl")
include("module-InteractionStrength.jl")
include("module-InteractionStrengthQED.jl")
include("module-PeriodicTable.jl")
include("module-TableStrings.jl")
include("module-Tools.jl")
include("module-LSjj.jl")

# Functions/methods for atomic amplitudes
include("module-MultipoleMoment.jl")
include("module-ParityNonConservation.jl")

include("module-PhotoEmission.jl")

# Functions/methods for atomic properties
include("module-Einstein.jl")
include("module-Hfs.jl")
include("module-IsotopeShift.jl")
include("module-LandeZeeman.jl")
include("module-AlphaVariation.jl")
include("module-FormFactor.jl")
include("module-DecayYield.jl")
include("module-GreenFunction.jl")
include("module-MultipolePolarizibility.jl")
include("module-PlasmaShift.jl")

# Functions/methods for atomic processes
include("module-PhotoExcitation.jl")
include("module-PhotoIonization.jl")
include("module-PhotoRecombination.jl")
include("module-AutoIonization.jl")
include("module-Dielectronic.jl")
include("module-PhotoExcitationFluores.jl")
include("module-PhotoExcitationAutoion.jl")
include("module-RayleighCompton.jl")
include("module-MultiPhotonDeExcitation.jl")
include("module-CoulombExcitation.jl")
include("module-PhotoIonizationFluores.jl")
include("module-PhotoIonizationAutoion.jl")
include("module-CoulombIonization.jl")
include("module-ImpactExcitation.jl")
include("module-ImpactExcitationAutoion.jl")
include("module-RadiativeAuger.jl")
include("module-MultiPhotonIonization.jl")
include("module-MultiPhotonDoubleIon.jl")
include("module-InternalConversion.jl")      ## up to here + up to jac-tools
#= Further processes, not yet included into the code
include("module-DoubleAuger.jl")
include("module-REDA.jl")
include("module-READI.jl")
include("module-PairProduction.jl")
include("module-PairAnnihilation1Photon.jl")
include("module-PairAnnihilation2Photon.jl")  =#

# Functions/methods for atomic and cascade computations
include("module-Atomic.jl")
include("module-Cascade.jl")

# Functions/methods for atomic time evolutions
# include("module-Pulse.jl")
# include("module-Statistical.jl")

# Functions/methods for semi-empirical estimations
# include("module-ImpactIonization.jl")
# include("module-Semiempirical.jl")

# Functions/methods for symbolic computations
# include("module-Racah.jl")

# Basic functions/methods to manipulate these data
include("inc-module-Basics.jl")

# Specialized macros
include("macro-racahsum.jl")

# All test functions/methods stay with the JAC root module
include("jac-test.jl")

function __init__()
    # The following variables need to be initialized at runtime to enable precompilation
    global JAC_SUMMARY_IOSTREAM = stdout
    global JAC_TEST_IOSTREAM    = stdout
end

println("\nWelcome to JAC:  A fresh computational approach to atomic structures, processes, casacdes and time evolutions [(C) Copyright by Stephan Fritzsche, Jena (2019)].")

end


text:
using Random
using Random: GLOBAL_RNG

abstract type AbstractExplorationPolicy end

"""
    AlwaysExplore()

Policy for always exploring sub-optimal transitions.
"""
struct AlwaysExplore <: AbstractExplorationPolicy
    rng
end
AlwaysExplore() = AlwaysExplore(GLOBAL_RNG)

(::AlwaysExplore)() = true
(p::AlwaysExplore)(state::OracleState) = rand(p.rng, state.A)

Base.show(io::IO, ::AlwaysExplore) = print(io, "AlwaysExplore")

"""
    NeverExplore()

Policy for never exploring sub-optimal transitions.
"""
struct NeverExplore <: AbstractExplorationPolicy
    rng
end
NeverExplore() = NeverExplore(GLOBAL_RNG)

(::NeverExplore)() = false
(p::NeverExplore)(state::OracleState) = rand(p.rng, state.G)

Base.show(io::IO, ::NeverExplore) = print(io, "NeverExplore")

"""
    ExplorationPolicy(k, p)

Simple exploration policy from Goldberg & Nivre, 2012. Returns true at rate p.
"""
struct ExplorationPolicy <: AbstractExplorationPolicy
    p::Float64
    rng

    function ExplorationPolicy(p, rng=GLOBAL_RNG)
        @assert 0 <= p <= 1
        new(p, rng)
    end
end

(p::ExplorationPolicy)() =
    rand(p.rng) >= 1 - p.p
(p::ExplorationPolicy)(state::OracleState) =
    rand(p.rng) >= 1 - p.p ? rand(state.A) : rand(state.G)