@inline @inbounds function simplestChaoticFlow(X::AbstractArray, 𝐴::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    𝑥̇ = 𝑦
    𝑦̇ = 𝑧
    𝑧̇ = -𝐴(𝑡)*𝑧 + 𝑦^2 - 𝑥
    return SVector{3}(𝑥̇, 𝑦̇, 𝑧̇)
end

@inline @inbounds function simplestChaoticFlow_J(X::AbstractArray, 𝐴::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    J = @SMatrix [  1.0     0.0     0.0;
                    0.0     1.0     0.0;
                    2.0*𝑦   -𝐴(𝑡)   -1.0]
end

"""
    Sprott's simplest chaotic flow

Sprott, or Sprott1997
"""
function simplestChaoticFlow(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=simplestChaoticFlow_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

simplestChaoticFlowSim = Process(
    process = simplestChaoticFlow,
    X0 = [0.05, 0.05, 0.05], # As in Sprott's paper
    parameter_profile = ramp,
    parameter_profile_parameters = (2.02, 2.07, 0.0, 10000.0),
    # Parameters should only be between ~2.018 and ~2.082 (otherwise, unbounded)
    # Range can be different when parameters are time varying
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.5,
    tmax = 10000.0,
    alg = RK4(),
    solver_opts = Dict(:adaptive => false))
export simplestChaoticFlowSim

simplestChaoticFlowVis = Process(
    process = simplestChaoticFlow,
    X0 = [0.05, 0.05, 0.05],
    parameter_profile = ramp,
    parameter_profile_parameters = (2.03, 2.05, 0.0, 5000.0),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.01,
    tmax = 5000.0,
    alg =  RK4(),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-9))
export simplestChaoticFlowVis

polarReduce(x::Real) = x - 2π*(x÷π);
polarReduce(X::AbstractArray) = X .- 2π.*(X.÷π);

@inline @inbounds function doublePendulum(X::AbstractArray, p, t::Real)
    (θ₁, θ₂, 𝑝₁, 𝑝₂) = X
    (ℓ₁, ℓ₂, m₁, m₂) = p(t)
    𝑔 = 0.0

    C₁(θ₁, θ₂, 𝑝₁, 𝑝₂) = (𝑝₁*𝑝₂*sin(θ₁ - θ₂))/(ℓ₁*ℓ₂*(m₁ + m₂*sin(θ₁ - θ₂)^2))
    C₂(θ₁, θ₂, 𝑝₁, 𝑝₂) = ((ℓ₂^2*m₂*𝑝₁^2 + ℓ₁^2*(m₁ + m₂)*𝑝₂^2 - ℓ₁*ℓ₂*m₂*𝑝₁*𝑝₂*cos(θ₁ - θ₂))/(2*ℓ₁^2*ℓ₂^2*(m₁ + m₂*sin(θ₁ - θ₂)^2)^2))*(sin(2*(θ₁ - θ₂)))

    dθ₁ = (ℓ₂*𝑝₁ - ℓ₁*𝑝₂*cos(θ₁ - θ₂))/(ℓ₁^2*ℓ₂*(m₁ + m₂*sin(θ₁ - θ₂)^2))
    dθ₂ = (ℓ₁*(m₁ + m₂)*𝑝₂ - ℓ₂*m₂*𝑝₁*cos(θ₁ - θ₂))/(ℓ₁*ℓ₂^2*m₂*(m₁ + m₂*sin(θ₁ - θ₂)^2))

    d𝑝₁ = -(m₁ + m₂)*𝑔*ℓ₁*sin(θ₁) - C₁(θ₁, θ₂, 𝑝₁, 𝑝₂) + C₂(θ₁, θ₂, 𝑝₁, 𝑝₂)
    d𝑝₂ = -m₂*𝑔*ℓ₂*sin(θ₂) + C₁(θ₁, θ₂, 𝑝₁, 𝑝₂) - C₂(θ₁, θ₂, 𝑝₁, 𝑝₂)

    return SVector{4}(dθ₁, dθ₂, d𝑝₁, d𝑝₂)
end
function doublePendulum(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters))
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

"""
    Your classic double pendulum, with Hamiltonian equations of motion

    https://scienceworld.wolfram.com/physics/DoublePendulum.html
"""
function cartesianDoublePendulum(P::Process)
    # Should the initial conditions be set in cartesian coordinates?
    sol = doublePendulum(P(process=doublePendulum))
    p = parameterseries(P, transient=true)
    x₁ = p[1, :].*sin.(sol[1, :])
    y₁ = .-p[1, :].*cos.(sol[1, :])
    x₂ = x₁ .+ p[2, :].*sin.(sol[2, :])
    y₂ = y₁ .- p[2, :].*cos.(sol[2, :])
    sol = hcat(x₁, y₁, x₂, y₂)
end

doublePendulumSim = Process(
    process = doublePendulum,
    X0 = [π, 0.01, 0.0, 0.0], # Two angles and two momenta
    parameter_profile = (ramp, constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = ((0.0, 1.0, 0.0), (1.0,), (1.0,), (1.0,), (1.0,)), # (threshold, baseline, stepHeight)
    transient_t0 = -10.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.1,
    tmax = 1000.0,
    alg = AutoVern7(Rodas5()))
export doublePendulumSim

cartesianDoublePendulumSim = Process(
    process = cartesianDoublePendulum,
    X0 = [π/2, π/2, 0.0, 0.0], # Two angles and two momenta
    parameter_profile = (constantParameter, constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = ((1.0,), (1.0,), (1.0,), (1.0,)), # (ℓ₁, ℓ₂, m₁, m₂)
    transient_t0 = -10.0,
    t0 = 0.0,
    dt = 0.00001,
    savedt = 0.1,
    tmax = 100.0,
    alg = AutoVern9(Rodas5()),
    solver_opts =  Dict(:adaptive => true, :reltol => 1e-12, :abstol => 1e-12))
export cartesianDoublePendulumSim

cartesianDoublePendulumVis = Process(
    process = cartesianDoublePendulum,
    X0 = [5π/6, π, 0.0, 0.0], # Two angles (from the downward direction) and two momenta
    parameter_profile = (lorentzian, lorentzian, constantParameter, constantParameter),
    transient_t0 = -10.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.001,
    tmax = 50.0,
    parameter_profile_parameters = ((-1.0, 10.0, 25.0, 1.5), (-1.0, 5.0, 25.0, 2.0), (1.0,), (2.0,)),
    alg = AutoVern9(Rodas5()),
    solver_opts = Dict(:adaptive => false))
export cartesianDoublePendulumVis

cartesianDoublePendulumVis2 = Process(
    process = cartesianDoublePendulum,
    X0 = [5π/6, π, 0.0, 0.0], # Two angles (from the downward direction) and two momenta
    parameter_profile = (lorentzian, lorentzian, constantParameter, constantParameter),
    transient_t0 = -10.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.001,
    tmax = 100.0,
    parameter_profile_parameters = ((-1.0, 18.0, 50.0, 1.5), (-1.0, 9.0, 50.0, 2.0), (1.0,), (2.0,)),
    alg = AutoVern7(Rodas5()),
    solver_opts = Dict(:adaptive => false))
export cartesianDoublePendulumVis2

@inline @inbounds function waveDrivenHarmonic(X::AbstractArray, p::Function, t::Real)
    (ω, ε, k, Ω) = p(t)
    dX2 = -ω^2.0*X[1] + ε*sin(k*X[1] - Ω*t)
    dX1 = X[2]
    return SVector{2}(dX1, dX2)
end

@inline @inbounds function waveDrivenHarmonic_J(X::AbstractArray, p::Function, t::Real)
    (ω, ε, k, Ω) = p(t)
    J = @SMatrix [0.0 1.0; ε*k*cos(k*X[1] - Ω*t)-ω^2 0.0]
end

"""
    An harmonic oscillator driven by a plane wave propagating in the x-direction

Sprott, or Chernikov1988
"""
function waveDrivenHarmonic(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=waveDrivenHarmonic_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

waveDrivenHarmonicSim = Process(# parameters:  (ω, ε, k, Ω)
    process = waveDrivenHarmonic,
    X0 = [0.1, 0.1],
    parameter_profile = (constantParameter, constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = ((1π,), (5π,), (1.0π,), (3π,)),
    #parameter_profile_parameters = ((1π,), (10.0,), (1.0π,), (0.9π)),
    transient_t0 = -10.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.01,
    tmax = 100.0,
    alg = RK4())
export waveDrivenHarmonicSim

# @inline @inbounds function pulseDrivenHarmonic(X::AbstractArray, p::Function, t::Real)
#     (ω, ε, k, Ω) = p(t)
#     dX2 = -ω^2.0*X[1] + ε*sin(k*X[1] - Ω*t)^2
#     dX1 = X[2]
#     return SVector{2}(dX1, dX2)
# end

# @inline @inbounds function pulseDrivenHarmonic_J(X::AbstractArray, p::Function, t::Real)
#     (ω, ε, k, Ω) = p(t)
#     J = @SMatrix [0.0 1.0; 2*ε*k*cos(k*X[1] - Ω*t)-ω^2 0.0]
# end

# """
#     An harmonic oscillator driven by directional 'pulses'
# """
# function pulseDrivenHarmonic(P::Process)
#     seed(P.solver_rng)
#     prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters))#, jac=waveDrivenHarmonic_J)
#     sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
# end

# pulseDrivenHarmonicSim = Process(# parameters:  (ω, ε, k, Ω)
#     process = pulseDrivenHarmonic,
#     X0 = [0.0, 0.0],
#     parameter_profile = (constantParameter, constantParameter, constantParameter, constantParameter),
#     parameter_profile_parameters = ((1π,), (10.0,), (1π,), (1.5π,)),
#     transient_t0 = -10.0,
#     t0 = 0.0,
#     dt = 0.001,
#     savedt = 0.01,
#     tmax = 100.0,
#     alg = RK4())
# export pulseDrivenHarmonicSim

@inline @inbounds function thomasCyclicallySymmetric(X::AbstractArray, 𝑏::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    𝑥̇ = -𝑏(𝑡)*𝑥 + sin(𝑦)
    𝑦̇ = -𝑏(𝑡)*𝑦 + sin(𝑧)
    𝑧̇ = -𝑏(𝑡)*𝑧 + sin(𝑥)
    return SVector{3}(𝑥̇, 𝑦̇, 𝑧̇)
end

@inline @inbounds function thomasCyclicallySymmetric_J(X::AbstractArray, 𝑏::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    J = @SMatrix [  -𝑏(𝑡)   cos(𝑦)   0.0;
                    0.0     -𝑏(𝑡)    cos(𝑧);
                    cos(𝑥)  0.0      -𝑏(𝑡)]
end

"""
    Thomas' cyclically symmetric attractor

From Sprott, or Thomas1999
"""
function thomasCyclicallySymmetric(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=thomasCyclicallySymmetric_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

thomasCyclicallySymmetricSim = Process(
    process = thomasCyclicallySymmetric,
    X0 = [0.1, 0.0, 0.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (0.18,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 1000.0,
    alg = RK4(),
    solver_opts = Dict(:adaptive => true))
export thomasCyclicallySymmetricSim

thomasCyclicallySymmetricVis = Process(
    process = thomasCyclicallySymmetric,
    X0 = [0.1, 0.0, 0.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (0.18,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.05,
    tmax = 5000.0,
    alg = RadauIIA3(),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-12))
export thomasCyclicallySymmetricVis

@inline @inbounds function doubleScroll(X::AbstractArray, 𝑎::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    𝑥̇ = 𝑦
    𝑦̇ = 𝑧
    𝑧̇ = -𝑎(𝑡)*(𝑧 + 𝑦 + 𝑥 - sign(𝑥))
    return SVector{3}(𝑥̇, 𝑦̇, 𝑧̇)
end
function 𝛿(x)
    if x == 0
        return Inf
    else
        return 0
    end
end
@inline @inbounds function doubleScroll_J(X::AbstractArray, 𝑎::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    J = @SMatrix [  0.0   1.0   0.0;
                    0.0   0.0   1.0;
                    -𝑎(𝑡)*(1.0-2.0*𝛿(x))  -𝑎(𝑡)  -𝑎(𝑡)] # Need to worry about integrating delta? Hopefully we never hit 0.0...
end

"""
    Double scroll attractor

From Sprott, or Elwakil2001
"""
function doubleScroll(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=doubleScroll_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

doubleScrollSim = Process(
    process = doubleScroll,
    X0 = [0.01, 0.01, 0.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (0.8,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 1000.0,
    alg = RK4(),
    solver_opts = Dict(:adaptive => true))
export doubleScrollSim

doubleScrollVis = Process(
    process = doubleScroll,
    X0 = [0.01, 0.01, 0.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (0.8,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.01,
    tmax = 5000.0,
    alg = AutoVern7(Rodas5()),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-12))
export doubleScrollVis

@inline @inbounds function lorenz(dX, X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    (𝜎, 𝑟, 𝑏) = p(𝑡)
    dX[1] = 𝜎*(𝑦 - 𝑥)
    dX[2] = -𝑥*𝑧 + 𝑟*𝑥 - 𝑦
    dX[3] = 𝑥*𝑦 - 𝑏*𝑧
end

@inline @inbounds function lorenz_J(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    (𝜎, 𝑟, 𝑏) = p(𝑡)
    J = @SMatrix [  -𝜎      𝜎       0.0;
                    𝑟-𝑧     -1.0    -𝑥;
                    𝑦       𝑥       -𝑏]
end

"""
    Lorenz system

Sprott, Shin1998, Balcerzak2018
"""
function lorenz(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=lorenz_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

lorenzSim = Process(
    process = lorenz,
    X0 = [0.0, -0.01, 9.0],
    parameter_profile = (constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = (10.0, 28.0, 8/3), # Sprott's recomendation
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 500.0,
    alg = AutoVern9(Rodas5()),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-10, :abstol => 1e-10, :maxiters=>1e7))
export lorenzSim

lorenzVis= Process(
    process = lorenz,
    X0 = [0.0, 0.01, 9.0],
    parameter_profile = (constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = (10.0, 28.0, 8/3),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0005,
    savedt = 0.001,
    tmax = 1000.0,
    alg = AutoVern7(Rodas5()),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-15))
export lorenzVis

@inline @inbounds function diffusionlessLorenz(X::AbstractArray, 𝑎::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    𝑥̇ = 𝑦 - 𝑥
    𝑦̇ = -𝑥*𝑧
    𝑧̇ = 𝑥*𝑦 - 𝑎(𝑡)
    return SVector{3}(𝑥̇, 𝑦̇, 𝑧̇)
end
@inline @inbounds function diffusionlessLorenz_J(X::AbstractArray, 𝑎::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    J = @SMatrix [  -1.0   1.0   0.0;
                    -𝑧     0.0   -𝑥;
                    𝑦      𝑥     0.0]
end

"""
    Diffusionless Lorenz system

See Li2014, Sprott2010a (Elegant Chaos), and Schrier2000
"""
function diffusionlessLorenz(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=diffusionlessLorenz_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

diffusionlessLorenzSim = Process(
    process = diffusionlessLorenz,
    X0 = [1.0, 0.0, 1.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (1.0,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 1000.0,
    alg = RK4(),
    solver_opts = Dict(:adaptive => false))
export diffusionlessLorenzSim

diffusionlessLorenzVis = Process(
    process = diffusionlessLorenz,
    X0 = [1.0, 0.0, 1.0],
    parameter_profile = constantParameter,
    parameter_profile_parameters = (1.0,),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.025,
    tmax = 20000.0,
    alg = AutoVern7(Rodas5()),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-8))
export diffusionlessLorenzVis

@inline @inbounds function piecewiseLinearHyperchaos(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧, 𝑢) = X
    (𝑎, 𝑏) = p(𝑡)

    𝑥̇ = 𝑦 - 𝑥
    𝑦̇ = -𝑧*sign(𝑥) + 𝑢
    𝑧̇ = abs(𝑥) - 𝑎
    𝑢̇ = -𝑏*𝑦

    return SVector{4}(𝑥̇, 𝑦̇, 𝑧̇, 𝑢̇)
end
@inline @inbounds function piecewiseLinearHyperchaos_J(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧, 𝑢) = X
    (𝑎, 𝑏) = p(𝑡)
    J = @SMatrix [  -1.0        1.0        0.0          0.0;
                    -2*𝛿(𝑧)     0.0        -sign(𝑥)     1.0; # ? Again, hope we miss the zero
                    sign(𝑥)     0.0        0.0          0.0;
                    0.0         -𝑏         0.0          0.0]
end

"""
    A simple, piecewise-linear, hyperchaotic flow

Li2014
"""
function piecewiseLinearHyperchaos(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=piecewiseLinearHyperchaos_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

piecewiseLinearHyperchaosSim = Process(
    process = piecewiseLinearHyperchaos,
    X0 = [0.2, 0.0, 1.0, 0.0],
    parameter_profile = (constantParameter, constantParameter),
    parameter_profile_parameters = (1.0, 0.25), # * 𝑎 is an amplitude, so set to 1 without loss of generality
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 1000.0,
    alg = Vern9(),
    solver_opts = Dict(:adaptive => false, :reltol => 1e-10, :abstol => 1e-10))
export piecewiseLinearHyperchaosSim

piecewiseLinearHyperchaosVis = Process(
    process = piecewiseLinearHyperchaos,
    X0 = [0.01, 0.01, 0.01, 0.01],
    parameter_profile = (constantParameter, constantParameter),
    parameter_profile_parameters = (1.0, 0.25),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 15000.0,
    alg = AutoVern9(Rodas5()),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-14))
export piecewiseLinearHyperchaosVis

@inline @inbounds function simplifiedLorenz4D(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧, 𝑢) = X
    (𝑎, 𝑏) = p(𝑡)

    𝑥̇ = 𝑦 - 𝑥
    𝑦̇ = -𝑥*𝑧 + 𝑢
    𝑧̇ = 𝑥*𝑦 - 𝑎
    𝑢̇ = -𝑏*𝑦

    return SVector{4}(𝑥̇, 𝑦̇, 𝑧̇, 𝑢̇)
end
@inline @inbounds function simplifiedLorenz4D_J(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧, 𝑢) = X
    (𝑎, 𝑏) = p(𝑡)
    J = @SMatrix [  -1.0        1.0        0.0          0.0;
                    -𝑧          0.0        -𝑥           1.0;
                    𝑦           𝑥          0.0          0.0;
                    0.0         -𝑏         0.0          0.0]
end

"""
    The simplified 4D Lorenz system, whose attractors are exclusively hidden and can be hyperchaotic

Li2014a and Gao2006
"""
function simplifiedLorenz4D(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=simplifiedLorenz4D_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end

simplifiedLorenz4DSim = Process(
    process = simplifiedLorenz4D,
    X0 = [2.0, 4.0, 0.0, 0.0],
    parameter_profile = (constantParameter, constantParameter),
    parameter_profile_parameters = (2.6, 0.44),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.05,
    tmax = 1000.0,
    alg = RK4(),
    solver_opts = Dict(:adaptive => false))
export simplifiedLorenz4DSim

simplifiedLorenz4DVis = Process(
    process = simplifiedLorenz4D,
    X0 = [2.0, 4.0, 0.0, 0.0],
    parameter_profile = (constantParameter, constantParameter),
    parameter_profile_parameters = (2.6, 0.45),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.05,
    tmax = 7500.0,
    alg = Vern9(),
    solver_opts = Dict(:adaptive => false))#, :reltol => 1e-12))
export simplifiedLorenz4DVis

@inline @inbounds function chensSystem(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    (𝑎, 𝑏, 𝑐) = p(𝑡)

    𝑥̇ = 𝑎*(𝑦 - 𝑥)
    𝑦̇ = (𝑐 - 𝑎)*𝑥 - 𝑥*𝑧 + 𝑐*𝑦
    𝑧̇ = 𝑥*𝑦 - 𝑏*𝑧

    return SVector{3}(𝑥̇, 𝑦̇, 𝑧̇)
end
@inline @inbounds function chensSystem_J(X::AbstractArray, p::Function, 𝑡::Real)
    (𝑥, 𝑦, 𝑧) = X
    (𝑎, 𝑏, 𝑐) = p(𝑡)
    J = @SMatrix [-𝑎        𝑎       0.0;
                  𝑐-𝑎-𝑧     𝑐       -𝑥;
                  𝑦         𝑥       -𝑏]
end

"""
    Yet another chaotic attractor

Chen1999, Song2004
"""
function chensSystem(P::Process)
    seed(P.solver_rng)
    prob = ODEProblem(P.process, P.X0, (P.transient_t0, P.tmax), tuplef2ftuple(P.parameter_profile, P.parameter_profile_parameters), jac=chensSystem_J)
    sol = dsolve(prob, P.alg; dt = P.dt, saveat=P.savedt, P.solver_opts...)
end
export chensSystem
yetAnotherChaoticAttractor = chensSystem
export yetAnotherChaoticAttractor

chensSystemSim = Process(
    process = chensSystem,
    X0 = [-3.0, 2.0, 20.0],
    parameter_profile = (constant, constant, constant),
    parameter_profile_parameters = ((44,), (5,), (30,)),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.001,
    savedt = 0.02,
    tmax = 200.0,
    alg = AutoVern9(Rodas5()),
    solver_opts = Dict(:adaptive => true, :abs_tol=>1e-10, :rel_tol=>1e-10))
export chensSystemSim

chensSystemVis = Process(
    process = chensSystem,
    X0 = [-3.0, 2.0, 20.0],
    parameter_profile = (constantParameter, constantParameter, constantParameter),
    parameter_profile_parameters = (35.0, 8.0/3.0, 28.0),
    transient_t0 = -100.0,
    t0 = 0.0,
    dt = 0.0001,
    savedt = 0.001,
    tmax = 1000.0,
    alg = Vern9(),
    solver_opts = Dict(:adaptive => true, :reltol => 1e-9))
export chensSystemVis


"""
    non_backtracking_matrix(g)

Return a non-backtracking matrix `B` and an edgemap storing the oriented
edges' positions in `B`.

Given two arcs ``A_{i j}` and `A_{k l}` in `g`, the
non-backtraking matrix ``B`` is defined as

``B_{A_{i j}, A_{k l}} = δ_{j k} * (1 - δ_{i l})``
"""
function non_backtracking_matrix(g::AbstractGraph)
    # idedgemap = Dict{Int,Edge}()
    edgeidmap = Dict{Edge,Int}()
    m = 0
    for e in edges(g)
        m += 1
        edgeidmap[e] = m
    end

    if !is_directed(g)
        for e in edges(g)
            m += 1
            edgeidmap[reverse(e)] = m
        end
    end

    B = zeros(Float64, m, m)

    for (e, u) in edgeidmap
        i, j = src(e), dst(e)
        for k in inneighbors(g, i)
            k == j && continue
            v = edgeidmap[Edge(k, i)]
            B[v, u] = 1
        end
    end

    return B, edgeidmap
end

"""
    Nonbacktracking{G}

A compact representation of the nonbacktracking operator.

The Nonbacktracking operator can be used for community detection.
This representation is compact in that it uses only ne(g) additional storage
and provides an implicit representation of the matrix B_g defined below.

Given two arcs ``A_{i j}` and `A_{k l}` in `g`, the
non-backtraking matrix ``B`` is defined as

``B_{A_{i j}, A_{k l}} = δ_{j k} * (1 - δ_{i l})``

This type is in the style of GraphMatrices.jl and supports the necessary operations
for computed eigenvectors and conducting linear solves.

Additionally the `contract!(vertexspace, nbt, edgespace)` method takes vectors
represented in the domain of ``B`` and represents them in the domain of the
adjacency matrix of `g`.
"""
struct Nonbacktracking{G <: AbstractGraph}
    g::G
    edgeidmap::Dict{Edge,Int}
    m::Int
end

function Nonbacktracking(g::AbstractGraph)
    edgeidmap = Dict{Edge,Int}()
    m = 0
    for e in edges(g)
        m += 1
        edgeidmap[e] = m
    end
    if !is_directed(g)
        for e in edges(g)
            m += 1
            edgeidmap[reverse(e)] = m
        end
    end
    return Nonbacktracking(g, edgeidmap, m)
end

size(nbt::Nonbacktracking) = (nbt.m, nbt.m)
eltype(nbt::Nonbacktracking) = Float64
issymmetric(nbt::Nonbacktracking) = false

function *(nbt::Nonbacktracking, x::Vector{T}) where T <: Number
    length(x) == nbt.m || error("dimension mismatch")
    y = zeros(T, length(x))
    for (e, u) in nbt.edgeidmap
        i, j = src(e), dst(e)
        for k in inneighbors(nbt.g, i)
            k == j && continue
            v = nbt.edgeidmap[Edge(k, i)]
            y[v] += x[u]
        end
    end
    return y
end
function mul!(C, nbt::Nonbacktracking, B)
    # computs C = A * B
    for i in 1:size(B, 2)
        C[:, i] = nbt * B[:, i]
    end
    return C
end

function coo_sparse(nbt::Nonbacktracking)
    m = nbt.m
    #= I,J = zeros(Int, m), zeros(Int, m) =#
    I, J = zeros(Int, 0), zeros(Int, 0)
    for (e, u) in nbt.edgeidmap
        i, j = src(e), dst(e)
        for k in inneighbors(nbt.g, i)
            k == j && continue
            v = nbt.edgeidmap[Edge(k, i)]
            #= J[u] = v =#
            #= I[u] = u =#
            push!(I, v)
            push!(J, u)
        end
    end
    return I, J, 1.0
end

sparse(nbt::Nonbacktracking) = sparse(coo_sparse(nbt)..., nbt.m, nbt.m)

function *(nbt::Nonbacktracking, x::AbstractMatrix)
    y = zero(x)
    for i in 1:nbt.m
        y[:, i] = nbt * x[:, i]
    end
    return y
end

"""
    contract!(vertexspace, nbt, edgespace)

The mutating version of `contract(nbt, edgespace)`. Modifies `vertexspace`.
"""
function contract!(vertexspace::Vector, nbt::Nonbacktracking, edgespace::Vector)
    T = eltype(nbt.g)
    for i = one(T):nv(nbt.g), j in neighbors(nbt.g, i)
        u = nbt.edgeidmap[i > j ? Edge(j, i) : Edge(i, j)]
        vertexspace[i] += edgespace[u]
    end
end

#TODO: documentation needs work. sbromberger 20170326
"""
    contract(nbt, edgespace)

Integrate out the edges by summing over the edges incident to each vertex.
"""
function contract(nbt::Nonbacktracking, edgespace::Vector)
    y = zeros(eltype(edgespace), nv(nbt.g))
    contract!(y, nbt, edgespace)
    return y
end


using Pkg
Pkg.activate("/home/Morgan/FrankWolfe.jl")

using Test
using FrankWolfe
# include("/home/Morgan/FrankWolfe.jl/src/FrankWolfe.jl")
using LinearAlgebra
import SparseArrays

import FrankWolfe: compute_extreme_point, LpNormLMO, KSparseLMO
# include("/home/Morgan/FrankWolfe.jl/src/norm_oracles.jl")

import GLPK
import MathOptInterface
const MOI = MathOptInterface

import Clp
using Random
import Hypatia

@testset "Simplex LMOs" begin
    n = 6
    direction = zeros(6)
    rhs = 10 * rand()
    lmo_prob = FrankWolfe.ProbabilitySimplexOracle(rhs)
    lmo_unit = FrankWolfe.UnitSimplexOracle(rhs)
    @testset "Choosing improving direction" for idx in 1:n
        direction .= 0
        direction[idx] = -1
        res_point_prob = FrankWolfe.compute_extreme_point(lmo_prob, direction)
        res_point_unit = FrankWolfe.compute_extreme_point(lmo_unit, direction)
        for j in eachindex(res_point_prob)
            if j == idx
                @test res_point_prob[j] == res_point_unit[j] == rhs
            else
                @test res_point_prob[j] == res_point_unit[j] == 0
            end
        end
        # computing dual solutions and testing complementarity
        dual, redC = FrankWolfe.compute_dual_solution(lmo_prob, direction, res_point_prob)
        @test sum((redC .* res_point_prob)) + (dual[1] * (rhs - sum(res_point_prob))) == 0
        dual, redC = FrankWolfe.compute_dual_solution(lmo_unit, direction, res_point_unit)
        @test sum((redC .* res_point_unit)) + (dual[1] * (rhs - sum(res_point_unit))) == 0
    end
    @testset "Choosing least-degrading direction" for idx in 1:n
        # all directions worsening, must pick idx
        direction .= 2
        direction[idx] = 1
        res_point_prob = FrankWolfe.compute_extreme_point(lmo_prob, direction)
        res_point_unit = FrankWolfe.compute_extreme_point(lmo_unit, direction)
        for j in eachindex(res_point_unit)
            @test res_point_unit[j] == 0
            if j == idx
                @test res_point_prob[j] == rhs
            else
                @test res_point_prob[j] == 0
            end
        end
        # computing dual solutions and testing complementarity
        dual, redC = FrankWolfe.compute_dual_solution(lmo_prob, direction, res_point_prob)
        @test sum((redC .* res_point_prob)) + (dual[1] * (rhs - sum(res_point_prob))) == 0
        dual, redC = FrankWolfe.compute_dual_solution(lmo_unit, direction, res_point_unit)
        @test sum((redC .* res_point_unit)) + (dual[1] * (rhs - sum(res_point_unit))) == 0
    end
end

@testset "Lp-norm epigraph LMO" begin
    for n in (1, 2, 5, 10)
        τ = 5 + 3 * rand()
        # tests that the "special" p behaves like the "any" p, i.e. 2.0 and 2
        @testset "$p-norm" for p in (1, 1.0, 1.5, 2, 2.0, Inf, Inf32)
            lmo = LpNormLMO{Float64,p}(τ)
            for _ in 1:100
                c = 5 * randn(n)
                v = FrankWolfe.compute_extreme_point(lmo, c)
                @test norm(v, p) ≈ τ
            end
            c = zeros(n)
            v = FrankWolfe.compute_extreme_point(lmo, c)
            @test !any(isnan, v)
        end
        @testset "K-Norm ball $K" for K in 1:n
            lmo_ball = FrankWolfe.KNormBallLMO(K, τ)
            for _ in 1:20
                c = 5 * randn(n)
                v = FrankWolfe.compute_extreme_point(lmo_ball, c)
                v1 = FrankWolfe.compute_extreme_point(FrankWolfe.LpNormLMO{1}(τ), c)
                v_inf = FrankWolfe.compute_extreme_point(FrankWolfe.LpNormLMO{Inf}(τ / K), c)
                # K-norm is convex hull of union of the two norm epigraphs
                # => cannot do better than the best of them
                @test dot(v, c) ≈ min(dot(v1, c), dot(v_inf, c))
                # test according to original norm definition
                # norm constraint must be tight
                K_sum = 0.0
                for vi in sort!(abs.(v), rev=true)[1:K]
                    K_sum += vi
                end
                @test K_sum ≈ τ
            end
        end
    end
    # testing issue on zero direction
    for n in (1, 5)
        lmo = FrankWolfe.LpNormLMO{Float64,2}(1.0)
        x0 = FrankWolfe.compute_extreme_point(lmo, zeros(n))
        @test all(!isnan, x0)
    end
end

@testset "K-sparse polytope LMO" begin
    @testset "$n-dimension" for n in (1, 2, 10)
        τ = 5 + 3 * rand()
        for K in 1:n
            lmo = KSparseLMO(K, τ)
            x = 10 * randn(n) # dense vector
            v = compute_extreme_point(lmo, x)
            # K-sparsity
            @test count(!iszero, v) == K
            @test sum(abs.(v)) ≈ K * τ
            xsort = sort!(10 * rand(n))
            v = compute_extreme_point(lmo, xsort)
            @test all(iszero, v[1:n-K])
            @test all(abs(vi) ≈ abs(τ * sign(vi)) for vi in v[K:end])
            reverse!(xsort)
            v = compute_extreme_point(lmo, xsort)
            @test all(iszero, v[K+1:end])
            @test all(abs(vi) ≈ abs(τ * sign(vi)) for vi in v[1:K])
        end
    end
    # type stability of K-sparse polytope LMO
    lmo = KSparseLMO(3, 2.0)
    x = 10 * randn(10) # dense vector
    @inferred compute_extreme_point(lmo, x)
end

@testset "Caching on simplex LMOs" begin
    n = 6
    direction = zeros(6)
    rhs = 10 * rand()
    lmo_unit = FrankWolfe.UnitSimplexOracle(rhs)
    lmo_never_cached = FrankWolfe.SingleLastCachedLMO(lmo_unit)
    lmo_cached = FrankWolfe.SingleLastCachedLMO(lmo_unit)
    lmo_multicached = FrankWolfe.MultiCacheLMO{3}(lmo_unit)
    lmo_veccached = FrankWolfe.VectorCacheLMO(lmo_unit)
    @testset "Forcing no cache remains nothing" for idx in 1:n
        direction .= 0
        direction[idx] = -1
        res_point_unit = FrankWolfe.compute_extreme_point(lmo_unit, direction)
        res_point_cached = FrankWolfe.compute_extreme_point(lmo_cached, direction, threshold=0)
        res_point_cached_multi =
            FrankWolfe.compute_extreme_point(lmo_multicached, direction, threshold=-1000)
        res_point_cached_vec =
            FrankWolfe.compute_extreme_point(lmo_veccached, direction, threshold=-1000)
        res_point_never_cached =
            FrankWolfe.compute_extreme_point(lmo_cached, direction, store_cache=false)
        @test res_point_never_cached == res_point_unit
        @test lmo_never_cached.last_vertex === nothing
        @test length(lmo_never_cached) == 0
        empty!(lmo_never_cached)
        @test lmo_cached.last_vertex !== nothing
        @test length(lmo_cached) == 1
        @test count(!isnothing, lmo_multicached.vertices) == min(3, idx) == length(lmo_multicached)
        @test length(lmo_veccached.vertices) == idx == length(lmo_veccached)
        # we set the cache at least at the first iteration
        if idx == 1
            @test lmo_cached.last_vertex == res_point_unit
        end
        # whatever the iteration, last vertex is always the one returned
        @test lmo_cached.last_vertex == res_point_cached
    end
    empty!(lmo_multicached)
    @test length(lmo_multicached) == 0
    empty!(lmo_veccached)
    @test length(lmo_veccached) == 0
end

function _is_doubly_stochastic(m)
    for col in eachcol(m)
        @test sum(col) == 1
    end
    for row in eachrow(m)
        @test sum(row) == 1
    end
end

@testset "Birkhoff polytope" begin
    Random.seed!(42)
    lmo = FrankWolfe.BirkhoffPolytopeLMO()
    for n in (1, 2, 10)
        cost = rand(n, n)
        res = FrankWolfe.compute_extreme_point(lmo, cost)
        _is_doubly_stochastic(res)
        res2 = FrankWolfe.compute_extreme_point(lmo, vec(cost))
        @test res ≈ res2
    end
    cost_mat = [
        2 3 3
        3 2 3
        3 3 2
    ]
    res = FrankWolfe.compute_extreme_point(lmo, cost_mat)
    @test res == I
    @test sum(cost_mat .* res) == 6
    cost_mat = [
        3 2 3
        2 3 3
        3 3 2
    ]
    res = FrankWolfe.compute_extreme_point(lmo, cost_mat)
    @test sum(cost_mat .* res) == 6
    @test res == Bool[
        0 1 0
        1 0 0
        0 0 1
    ]
    cost_mat = [
        3 2 3
        3 3 2
        2 3 3
    ]
    res = FrankWolfe.compute_extreme_point(lmo, cost_mat)
    @test dot(cost_mat, res) == 6
    @test res == Bool[
        0 1 0
        0 0 1
        1 0 0
    ]
end

@testset "Matrix completion and nuclear norm" begin
    nfeat = 50
    nobs = 100
    r = 5
    Xreal = Matrix{Float64}(undef, nobs, nfeat)
    X_gen_cols = randn(nfeat, r)
    X_gen_rows = randn(r, nobs)
    svals = 100 * rand(r)
    for i in 1:nobs
        for j in 1:nfeat
            Xreal[i, j] = sum(X_gen_cols[j, k] * X_gen_rows[k, i] * svals[k] for k in 1:r)
        end
    end
    @test rank(Xreal) == r
    missing_entries = unique!([(rand(1:nobs), rand(1:nfeat)) for _ in 1:1000])
    f(X) =
        0.5 *
        sum((X[i, j] - Xreal[i, j])^2 for i in 1:nobs, j in 1:nfeat if (i, j) ∉ missing_entries)
    function grad!(storage, X)
        storage .= 0
        for i in 1:nobs
            for j in 1:nfeat
                if (i, j) ∉ missing_entries
                    storage[i, j] = X[i, j] - Xreal[i, j]
                end
            end
        end
        return nothing
    end
    # TODO value of radius?
    lmo = FrankWolfe.NuclearNormLMO(sum(svdvals(Xreal)))
    x0 = FrankWolfe.compute_extreme_point(lmo, zero(Xreal))
    gradient = similar(x0)
    grad!(gradient, x0)
    v0 = FrankWolfe.compute_extreme_point(lmo, gradient)
    @test dot(v0 - x0, gradient) < 0
    xfin, vmin, _ = FrankWolfe.frank_wolfe(
        f,
        grad!,
        lmo,
        x0;
        epsilon=1e-6,
        max_iteration=400,
        print_iter=100,
        trajectory=false,
        verbose=false,
        linesearch_tol=1e-7,
        line_search=FrankWolfe.Backtracking(),
        emphasis=FrankWolfe.memory,
    )
    @test 1 - (f(x0) - f(xfin)) / f(x0) < 1e-3
    svals_fin = svdvals(xfin)
    @test sum(svals_fin[r+1:end]) / sum(svals_fin) ≤ 2e-2
    xfin, vmin, _ = FrankWolfe.lazified_conditional_gradient(
        f,
        grad!,
        lmo,
        x0;
        epsilon=1e-6,
        max_iteration=400,
        print_iter=100,
        trajectory=false,
        verbose=false,
        linesearch_tol=1e-7,
        line_search=FrankWolfe.Backtracking(),
        emphasis=FrankWolfe.memory,
    )
end

@testset "Spectral norms" begin
    Random.seed!(42)
    o = Hypatia.Optimizer()
    MOI.set(o, MOI.Silent(), true)
    optimizer = MOI.Bridges.full_bridge_optimizer(
            MOI.Utilities.CachingOptimizer(
            MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
            o,
        ),
        Float64,
    )
    radius = 5.0
    @testset "Spectraplex $n" for n in (2, 10)
        lmo = FrankWolfe.SpectraplexLMO(radius, n)
        direction = Matrix{Float64}(undef, n, n)
        lmo_moi = FrankWolfe.convert_mathopt(lmo, optimizer; side_dimension=n)
        for _ in 1:10
            Random.randn!(direction)
            v = @inferred FrankWolfe.compute_extreme_point(lmo, direction)
            vsym = @inferred FrankWolfe.compute_extreme_point(lmo, direction + direction')
            vsym2 = @inferred FrankWolfe.compute_extreme_point(lmo, Symmetric(direction + direction'))
            @test v ≈ vsym atol=1e-6
            @test v ≈ vsym2 atol=1e-6
            @testset "Vertex properties" begin
                eigen_v = eigen(Matrix(v))
                @test eigmax(Matrix(v)) ≈ radius
                @test norm(eigen_v.values[1:end-1]) ≈ 0 atol=1e-7
                # u can be sqrt(r) * vec or -sqrt(r) * vec
                case_pos = ≈(norm(eigen_v.vectors[:,n] * sqrt(eigen_v.values[n]) - v.u), 0, atol=1e-9)
                case_neg = ≈(norm(eigen_v.vectors[:,n] * sqrt(eigen_v.values[n]) + v.u), 0, atol=1e-9)
                @test case_pos || case_neg
            end
            @testset "Comparison with SDP solution" begin
                v_moi = FrankWolfe.compute_extreme_point(lmo_moi, direction)
                @test norm(v - v_moi) <= 1e-6
            end
        end
    end
    @testset "Unit spectrahedron $n" for n in (2, 10)
        lmo = FrankWolfe.UnitSpectrahedronLMO(radius, n)
        direction = Matrix{Float64}(undef, n, n)
        lmo_moi = FrankWolfe.convert_mathopt(lmo, optimizer; side_dimension=n)
        direction_sym = similar(direction)
        for _ in 1:10
            Random.randn!(direction)
            @. direction_sym = direction + direction'
            v = @inferred FrankWolfe.compute_extreme_point(lmo, direction)
            vsym = @inferred FrankWolfe.compute_extreme_point(lmo, direction_sym)
            @test v ≈ vsym atol=1e-6
            @testset "Vertex properties" begin
                emin = eigmin(direction_sym)
                if emin ≥ 0
                    @test norm(Matrix(v)) ≈ 0
                else
                    eigen_v = eigen(Matrix(v))
                    @test eigmax(Matrix(v)) ≈ radius
                    @test norm(eigen_v.values[1:end-1]) ≈ 0 atol=1e-7
                    # u can be sqrt(r) * vec or -sqrt(r) * vec
                    case_pos = ≈(norm(eigen_v.vectors[:,n] * sqrt(eigen_v.values[n]) - v.u), 0, atol=1e-9)
                    case_neg = ≈(norm(eigen_v.vectors[:,n] * sqrt(eigen_v.values[n]) + v.u), 0, atol=1e-9)
                    @test case_pos || case_neg
                    # make direction PSD
                    direction_sym += 1.1 * abs(emin) * I
                    @assert isposdef(direction_sym)
                    v2 = FrankWolfe.compute_extreme_point(lmo, direction_sym)
                    @test norm(Matrix(v2)) ≈ 0
                end
            end
            @testset "Comparison with SDP solution" begin
                v_moi = FrankWolfe.compute_extreme_point(lmo_moi, direction)
                @test norm(v - v_moi) <= 1e-6
                # forcing PSD direction to test 0 matrix case
                @. direction_sym = direction + direction'
                direction_sym += 1.1 * abs(eigmin(direction_sym)) * I
                @assert isposdef(direction_sym)
                v_moi2 = FrankWolfe.compute_extreme_point(lmo_moi, direction_sym)
                v_lmo2 = FrankWolfe.compute_extreme_point(lmo_moi, direction_sym)
                @test norm(v_moi2 - v_lmo2) <= 1e-6
                @test norm(v_moi2) <= 1e-6
            end
        end
    end
end

@testset "MOI oracle consistency" begin
    Random.seed!(42)
    o = GLPK.Optimizer()
    MOI.set(o, MOI.Silent(), true)
    @testset "MOI oracle consistent with unit simplex" for n in (1, 2, 10)
        MOI.empty!(o)
        x = MOI.add_variables(o, n)
        for xi in x
            MOI.add_constraint(o, xi, MOI.Interval(0.0, 1.0))
        end
        MOI.add_constraint(
            o,
            MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(1.0, x), 0.0),
            MOI.LessThan(1.0),
        )
        lmo = FrankWolfe.MathOptLMO(o)
        lmo_ref = FrankWolfe.UnitSimplexOracle(1.0)
        lmo_moi_ref = FrankWolfe.convert_mathopt(lmo_ref, GLPK.Optimizer(), dimension=n)
        direction = Vector{Float64}(undef, n)
        for _ in 1:5
            Random.randn!(direction)
            vref = compute_extreme_point(lmo_ref, direction)
            v = compute_extreme_point(lmo, direction)
            v_moi = compute_extreme_point(lmo_moi_ref, direction)
            @test vref ≈ v
            @test vref ≈ v_moi
        end
    end
    @testset "MOI consistent probability simplex" for n in (1, 2, 10)
        MOI.empty!(o)
        x = MOI.add_variables(o, n)
        for xi in x
            MOI.add_constraint(o, xi, MOI.Interval(0.0, 1.0))
        end
        MOI.add_constraint(
            o,
            MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(1.0, x), 0.0),
            MOI.EqualTo(1.0),
        )
        lmo = FrankWolfe.MathOptLMO(o)
        lmo_ref = FrankWolfe.ProbabilitySimplexOracle(1.0)
        lmo_moi_ref = FrankWolfe.convert_mathopt(lmo_ref, GLPK.Optimizer(), dimension=n)
        direction = Vector{Float64}(undef, n)
        for _ in 1:5
            Random.randn!(direction)
            vref = compute_extreme_point(lmo_ref, direction)
            v = compute_extreme_point(lmo, direction)
            v_moi = compute_extreme_point(lmo_moi_ref, direction)
            @test vref ≈ v
            @test vref ≈ v_moi
        end
    end
    @testset "Direction with coefficients" begin
        n = 5
        MOI.empty!(o)
        x = MOI.add_variables(o, n)
        for xi in x
            MOI.add_constraint(o, xi, MOI.Interval(0.0, 1.0))
        end
        MOI.add_constraint(
            o,
            MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(1.0, x), 0.0),
            MOI.EqualTo(1.0),
        )
        lmo = FrankWolfe.MathOptLMO(o)
        direction = [MOI.ScalarAffineTerm(-2.0i, x[i]) for i in 2:3]
        v = compute_extreme_point(lmo, direction)
        @test v ≈ [0, 1]
    end
    @testset "Non-settable optimizer with cache" begin
        n = 5
        o = MOI.Utilities.CachingOptimizer(
            MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
            Clp.Optimizer(),
        )
        MOI.set(o, MOI.Silent(), true)
        x = MOI.add_variables(o, 5)
        for xi in x
            MOI.add_constraint(o, xi, MOI.Interval(0.0, 1.0))
        end
        MOI.add_constraint(
            o,
            MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(1.0, x), 0.0),
            MOI.EqualTo(1.0),
        )
        lmo = FrankWolfe.MathOptLMO(o)
        lmo_ref = FrankWolfe.ProbabilitySimplexOracle(1.0)
        direction = Vector{Float64}(undef, n)
        for _ in 1:5
            Random.randn!(direction)
            vref = compute_extreme_point(lmo_ref, direction)
            v = compute_extreme_point(lmo, direction)
            @test vref ≈ v
        end
    end
    @testset "Nuclear norm" for n in (5, 10)
        o = Hypatia.Optimizer()
        MOI.set(o, MOI.Silent(), true)
        optimizer = MOI.Utilities.CachingOptimizer(
            MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
            o,
        )
        MOI.set(optimizer, MOI.Silent(), true)
        nrows = 3n
        ncols = n
        direction = Matrix{Float64}(undef, nrows, ncols)
        τ = 10.0
        lmo = FrankWolfe.NuclearNormLMO(τ)
        lmo_moi =
            FrankWolfe.convert_mathopt(lmo, optimizer, row_dimension=nrows, col_dimension=ncols)
        for _ in 1:10
            randn!(direction)
            v_r = FrankWolfe.compute_extreme_point(lmo, direction)
            flattened = collect(vec(direction))
            push!(flattened, 0)
            v_moi = FrankWolfe.compute_extreme_point(lmo_moi, flattened)
            if v_moi === nothing
                # ignore non-terminating MOI solver results
                continue
            end
            v_moi_mat = reshape(v_moi[1:end-1], nrows, ncols)
            @test v_r ≈ v_moi_mat rtol = 1e-2
        end
    end
end

@testset "MOI oracle on Birkhoff polytope" begin
    o = GLPK.Optimizer()
    o_ref = GLPK.Optimizer()
    for n in (1, 2, 10)
        MOI.empty!(o)
        (x, _) = MOI.add_constrained_variables(o, fill(MOI.Interval(0.0, 1.0), n * n))
        xmat = reshape(x, n, n)
        for idx in 1:n
            # column constraint
            MOI.add_constraint(
                o,
                MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(ones(n), xmat[:, idx]), 0.0),
                MOI.EqualTo(1.0),
            )
            # row constraint
            MOI.add_constraint(
                o,
                MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.(ones(n), xmat[idx, :]), 0.0),
                MOI.EqualTo(1.0),
            )
        end
        direction_vec = Vector{Float64}(undef, n * n)
        lmo_bkf = FrankWolfe.BirkhoffPolytopeLMO()
        lmo_moi = FrankWolfe.MathOptLMO(o)
        lmo_moi_ref = FrankWolfe.convert_mathopt(lmo_bkf, o_ref, dimension=n)
        for _ in 1:10
            randn!(direction_vec)
            direction_mat = reshape(direction_vec, n, n)
            v_moi = FrankWolfe.compute_extreme_point(lmo_moi, direction_vec)
            v_moi_mat = reshape(v_moi, n, n)
            v_bfk = FrankWolfe.compute_extreme_point(lmo_bkf, direction_mat)
            @test all(isapprox.(v_moi_mat, v_bfk, atol=1e-4))
            v_moi_mat2 = FrankWolfe.compute_extreme_point(lmo_moi, direction_mat)
            @test all(isapprox.(v_moi_mat2, v_moi_mat))
        end
    end
end

@testset "MOI oracle and KSparseLMO" begin
    o_base = GLPK.Optimizer()
    cached = MOI.Utilities.CachingOptimizer(
        MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
        o_base,
    )
    o = MOI.Bridges.full_bridge_optimizer(cached, Float64)
    o_ref = MOI.Bridges.full_bridge_optimizer(
        MOI.Utilities.CachingOptimizer(
            MOI.Utilities.UniversalFallback(MOI.Utilities.Model{Float64}()),
            GLPK.Optimizer(),
        ),
        Float64,
    )
    for n in (1, 2, 5, 10)
        for K in 1:3:n
            τ = 10 * rand()
            MOI.empty!(o)
            x = MOI.add_variables(o, n)
            tinf = MOI.add_variable(o)
            MOI.add_constraint(o, MOI.VectorOfVariables([tinf; x]), MOI.NormInfinityCone(n + 1))
            MOI.add_constraint(o, tinf, MOI.LessThan(τ))
            t1 = MOI.add_variable(o)
            MOI.add_constraint(o, MOI.VectorOfVariables([t1; x]), MOI.NormOneCone(n + 1))
            MOI.add_constraint(o, t1, MOI.LessThan(τ * K))
            direction = Vector{Float64}(undef, n)
            lmo_moi = FrankWolfe.MathOptLMO(o)
            lmo_ksp = FrankWolfe.KSparseLMO(K, τ)
            lmo_moi_convert = FrankWolfe.convert_mathopt(lmo_ksp, o_ref, dimension=n)
            for _ in 1:20
                randn!(direction)
                v_moi =
                    FrankWolfe.compute_extreme_point(lmo_moi, MOI.ScalarAffineTerm.(direction, x))
                v_ksp = FrankWolfe.compute_extreme_point(lmo_ksp, direction)
                v_moi_conv = FrankWolfe.compute_extreme_point(
                    lmo_moi_convert,
                    MOI.ScalarAffineTerm.(direction, x),
                )
                for idx in eachindex(v_moi)
                    @test isapprox(v_moi[idx], v_ksp[idx], atol=1e-4)
                    @test isapprox(v_moi_conv[idx], v_ksp[idx], atol=1e-4)
                end
            end
            # verifying absence of a bug
            if n == 5
                direction .= (
                    -0.07020498519126772,
                    0.4298929981513661,
                    -0.8678437699266819,
                    -0.08899938054920563,
                    1.160622285477465,
                )
                v_moi =
                    FrankWolfe.compute_extreme_point(lmo_moi, MOI.ScalarAffineTerm.(direction, x))
                v_ksp = FrankWolfe.compute_extreme_point(lmo_ksp, direction)
                for idx in eachindex(v_moi)
                    @test isapprox(v_moi[idx], v_ksp[idx], atol=1e-4)
                end
            end
        end
    end
end

@testset "Product LMO" begin
    lmo = FrankWolfe.ProductLMO(FrankWolfe.LpNormLMO{Inf}(3.0), FrankWolfe.LpNormLMO{1}(2.0))
    dinf = randn(10)
    d1 = randn(5)
    vtup = FrankWolfe.compute_extreme_point(lmo, (dinf, d1))
    @test length(vtup) == 2
    (vinf, v1) = vtup
    @test sum(abs, vinf) ≈ 10 * 3.0
    @test sum(!iszero, v1) == 1

    vvec = FrankWolfe.compute_extreme_point(lmo, [dinf; d1]; direction_indices=(1:10, 11:15))
    @test vvec ≈ [vinf; v1]
end

@testset "Scaled L-1 norm polytopes" begin
    lmo = FrankWolfe.ScaledBoundL1NormBall(-ones(10), ones(10))
    # equivalent to LMO
    lmo_ref = FrankWolfe.LpNormLMO{1}(1)
    # all coordinates shifted up
    lmo_shifted = FrankWolfe.ScaledBoundL1NormBall(zeros(10), 2 * ones(10))
    lmo_scaled = FrankWolfe.ScaledBoundL1NormBall(-2 * ones(10), 2 * ones(10))
    for _ in 1:100
        d = randn(10)
        v = FrankWolfe.compute_extreme_point(lmo, d)
        vref = FrankWolfe.compute_extreme_point(lmo_ref, d)
        @test v ≈ vref
        vshift = FrankWolfe.compute_extreme_point(lmo_shifted, d)
        @test v .+ 1 ≈ vshift
        v2 = FrankWolfe.compute_extreme_point(lmo_scaled, d)
        @test v2 ≈ 2v
    end
    d = zeros(10)
    v = FrankWolfe.compute_extreme_point(lmo, d)
    vref = FrankWolfe.compute_extreme_point(lmo_ref, d)
    @test v ≈ vref
    @test norm(v) == 1
    # non-uniform scaling
    # validates bugfix
    lmo_nonunif = FrankWolfe.ScaledBoundL1NormBall([-1.0,-1.0], [3.0,1.0])
    direction = [-0.8272727272727383, -0.977272727272718]
    v = FrankWolfe.compute_extreme_point(lmo_nonunif, direction)
    @test v ≈ [3, 0]
    v = FrankWolfe.compute_extreme_point(lmo_nonunif, -direction)
    @test v ≈ [-1, 0]
end

@testset "Scaled L-inf norm polytopes" begin
    # tests ScaledBoundLInfNormBall for the standard hypercube, a shifted one, and a scaled one
    lmo = FrankWolfe.ScaledBoundLInfNormBall(-ones(10), ones(10))
    lmo_ref = FrankWolfe.LpNormLMO{Inf}(1)
    lmo_shifted = FrankWolfe.ScaledBoundLInfNormBall(zeros(10), 2 * ones(10))
    lmo_scaled = FrankWolfe.ScaledBoundLInfNormBall(-2 * ones(10), 2 * ones(10))
    bounds = collect(1.0:10)
    # tests another ScaledBoundLInfNormBall with unequal bounds against a MOI optimizer
    lmo_scaled_unequally = FrankWolfe.ScaledBoundLInfNormBall(-bounds, bounds)
    o = GLPK.Optimizer()
    MOI.set(o, MOI.Silent(), true)
    x = MOI.add_variables(o, 10)
    MOI.add_constraint.(o, x, MOI.GreaterThan.(-bounds))
    MOI.add_constraint.(o, x, MOI.LessThan.(bounds))
    scaled_unequally_opt = FrankWolfe.MathOptLMO(o)
    for _ in 1:100
        d = randn(10)
        v = FrankWolfe.compute_extreme_point(lmo, d)
        vref = FrankWolfe.compute_extreme_point(lmo_ref, d)
        @test v ≈ vref
        vshift = FrankWolfe.compute_extreme_point(lmo_shifted, d)
        @test v .+ 1 ≈ vshift
        v2 = FrankWolfe.compute_extreme_point(lmo_scaled, d)
        @test v2 ≈ 2v
        v3 = FrankWolfe.compute_extreme_point(lmo_scaled_unequally, d)
        v3_test = compute_extreme_point(scaled_unequally_opt, d)
        @test v3 ≈ v3_test
    end
    d = zeros(10)
    v = FrankWolfe.compute_extreme_point(lmo, d)
    vref = FrankWolfe.compute_extreme_point(lmo_ref, d)
    @test v ≈ vref
    @test norm(v, Inf) == 1
end


using DiffEqFlux, Flux, OrdinaryDiffEq, ReverseDiff

# Checks for Shapes and Non-Zero Gradients
u0 = rand(Float32, 6, 1)

hnn = HamiltonianNN(Chain(Dense(6, 12, relu), Dense(12, 1)))
p = hnn.p

@test size(hnn(u0)) == (6, 1)

@test ! iszero(ReverseDiff.gradient(p -> sum(hnn(u0, p)), p))

hnn = HamiltonianNN(Chain(Dense(6, 12, relu), Dense(12, 1)))
p = hnn.p

@test size(hnn(u0)) == (6, 1)

@test ! iszero(ReverseDiff.gradient(p -> sum(hnn(u0, p)), p))

# Test Convergence on a toy problem
t = range(0.0f0, 1.0f0, length = 64)
π_32 = Float32(π)
q_t = reshape(sin.(2π_32 * t), 1, :)
p_t = reshape(cos.(2π_32 * t), 1, :)
dqdt = 2π_32 .* p_t
dpdt = -2π_32 .* q_t

data = cat(q_t, p_t, dims = 1)
target = cat(dqdt, dpdt, dims = 1)

hnn = HamiltonianNN(Chain(Dense(2, 16, relu), Dense(16, 1)))
p = hnn.p

opt = ADAM(0.01)
loss(x, y, p) = sum((hnn(x, p) .- y) .^ 2)

initial_loss = loss(data, target, p)

epochs = 100
for epoch in 1:epochs
    gs = ReverseDiff.gradient(p -> loss(data, target, p), p)
    Flux.Optimise.update!(opt, p, gs)
end

final_loss = loss(data, target, p)

@test initial_loss > final_loss

# Test output and gradient of NeuralHamiltonianDE Layer
tspan = (0.0f0, 1.0f0)

model = NeuralHamiltonianDE(
    hnn, tspan, Tsit5(),
    save_everystep = false, save_start = true,
    saveat = range(tspan[1], tspan[2], length=10)
)
sol = Array(model(data[:, 1]))
@test size(sol) == (2, 10)

ps = Flux.params(model)
gs = Flux.gradient(() -> sum(Array(model(data[:, 1]))), ps)

@test ! iszero(gs[model.p])


using BlockArrays
using ControlSystems
using LinearAlgebra
using PredictiveControl
using Test

function testconstraint( G, L, g, u, x₀, expec )
    res = G*u - g - L*x₀
    @test res ≈ expec atol=1e-4
end

n = 2
m = 1

# This is a controllable and stable discrete-time system
Astab = [0.9 1.0
         0.0 0.9]

B = [0.0;
     1.0]
C = [1.0 0.0;
     0.0 1.0]
D = [0.0;
     0.0]
Ts = 0.1

sysStab = StateSpace( Astab, B, C, D, Ts )

Q = [1.0 1.0;
     1.0 1.0]
R = [1.0]
N = 2

###############################################
## Test only input constraints
###############################################
E = [ 0.0 0.0;
      0.0 0.0 ]
F = [  1.0;
      -1.0 ]
g = [ 5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

# Test sizes
@test size(ḡ) == (2*N,)
@test size(L) == (2*N, n)
@test size(G) == (2*N, m*N)

@test reshape( G[Block(1,1)], (:) ) == F
@test reshape( G[Block(2,2)], (:) ) == F
@test G[Block(1,2)] == zeros( Float64, (2, m) )
@test G[Block(2,1)] == zeros( Float64, (2, m) )

@test L == zeros( Float64, (2*N, n) )

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all inputs in bounds
u  = [ 1.0;
      -1.0 ]
x₀ = [ 1.0;
       1.0 ]

e  = [    u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with all inputs out of bounds
u = [ 6.0;
     -6.0 ]
x₀ = [ 1.0;
       1.0 ]

e  = [    u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with one inbounds, one out of bounds
u = [ 1.0;
     -6.0 ]
x₀ = [ 1.0;
       1.0 ]

e  = [    u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

###############################################
## Test only state constraints
###############################################
E = [ 1.0 0.0;
      0.0 1.0 ]
F = [ 0.0;
      0.0 ]
g = [ 5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

sys = PredictiveControl.getsystem( clqr )

# Test sizes
@test size(ḡ) == (2*N,)
@test size(L) == (2*N, n)
@test size(G) == (2*N, m*N)

@test G == PredictiveControl.prediction( sys, clqr.N )

@test L[Block(1,1)] == -E*sys.A
@test L[Block(2,1)] == -E*sys.A^2

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all states in bounds
u  = [ 0.0;
       0.0 ]
x₀ = [ 0.0;
       0.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [ x₁[1] - g[1];    # States @t=1
       x₁[2] - g[2];    # States @t=1
       x₂[1] - g[1];    # States @t=2
       x₂[2] - g[2] ]   # States @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with all states out of bounds
u  = [ 6.0;
       6.0 ]
x₀ = [ 20.0;
       20.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [ x₁[1] - g[1];    # States @t=1
       x₁[2] - g[2];    # States @t=1
       x₂[1] - g[1];    # States @t=2
       x₂[2] - g[2] ]   # States @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with one inbounds, one out of bounds
u  = [  5.0;
        5.0 ]
x₀ = [ 0.0;
       0.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [ x₁[1] - g[1];    # States @t=1
       x₁[2] - g[2];    # States @t=1
       x₂[1] - g[1];    # States @t=2
       x₂[2] - g[2] ]   # States @t=2

testconstraint( G, L, ḡ, u, x₀, e )

###############################################
## Test the inclusion of both state and input constraints
###############################################
E = [ 1.0 0.0;
      0.0 1.0;
      0.0 0.0;
      0.0 0.0 ]
F = [ 0.0;
      0.0;
      1.0;
     -1.0 ]
g = [ 5.0;
      5.0;
      5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

sys = PredictiveControl.getsystem( clqr )

# Test sizes
@test size(ḡ) == (4*N,)
@test size(L) == (4*N, n)
@test size(G) == (4*N, m*N)

@test G[Block(1,1)] == E*sys.B + F
@test G[Block(2,2)] == E*sys.B + F
@test G[Block(2,1)] == E*sys.A*sys.B
@test G[Block(1,2)] == zeros( Float64, (4, m) )

@test L[Block(1,1)] == -E*sys.A
@test L[Block(2,1)] == -E*sys.A^2

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all states and inputs in bounds
u  = [ 0.0;
       0.0 ]
x₀ = [ 0.0;
       0.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [   x₁[1] - g[1];      # States @t=1
         x₁[2] - g[2];      # States @t=1
          u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
         x₂[1] - g[1];      # States @t=2
         x₂[2] - g[2];      # States @t=2
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with all inputs out of bounds, states out of bounds
u  = [ 6.0;
       6.0 ]
x₀ = [ 20.0;
       20.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [   x₁[1] - g[1];      # States @t=1
         x₁[2] - g[2];      # States @t=1
          u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
         x₂[1] - g[1];      # States @t=2
         x₂[2] - g[2];      # States @t=2
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

# Test with one inbounds, one out of bounds
u  = [ 5.0;
       6.0 ]
x₀ = [ 0.0;
       0.0 ]

x₁ = sysStab.A*x₀ + sysStab.B*u[1]
x₂ = sysStab.A*x₁ + sysStab.B*u[2]

e  = [   x₁[1] - g[1];      # States @t=1
         x₁[2] - g[2];      # States @t=1
          u[1] - g[1];      # Inputs @t=1
       -( u[1] + g[2] );    # Inputs @t=1
         x₂[1] - g[1];      # States @t=2
         x₂[2] - g[2];      # States @t=2
          u[2] - g[1];      # Inputs @t=2
       -( u[2] + g[2] ) ]   # Inputs @t=2

testconstraint( G, L, ḡ, u, x₀, e )

###############################################
## Test the inclusion of input constraints with a pre-stabilized system
###############################################
E = [ 0.0 0.0;
      0.0 0.0 ]
F = [ 1.0;
     -1.0 ]
g = [ 5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, K = :dlqr, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

sys = PredictiveControl.getsystem( clqr )

# Test sizes
@test size(ḡ) == (2*N,)
@test size(L) == (2*N, n)
@test size(G) == (2*N, m*N)

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all inputs in bounds
v  = [ 0.0;
       0.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    u₀ - g[1];      # Inputs @t=0
       -( u₀ + g[2] );    # Inputs @t=0
          u₁ - g[1];      # Inputs @t=1
       -( u₁ + g[2] ) ]   # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )

# Test with all inputs out of bounds
v  = [ 40.0;
       30.0 ]
x₀ = [ 20.0;
       20.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    u₀ - g[1];      # Inputs @t=0
       -( u₀ + g[2] );    # Inputs @t=0
          u₁ - g[1];      # Inputs @t=1
       -( u₁ + g[2] ) ]   # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )

# Test with one inbounds, one out of bounds
v  = [ 5.0;
       10.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    u₀ - g[1];      # Inputs @t=0
       -( u₀ + g[2] );    # Inputs @t=0
          u₁ - g[1];      # Inputs @t=1
       -( u₁ + g[2] ) ]   # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )

###############################################
## Test the inclusion of state constraints with a pre-stabilized system
###############################################
E = [ 1.0 0.0;
      0.0 1.0 ]
F = [ 0.0;
      0.0 ]
g = [ 5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, K = :dlqr, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

sys = PredictiveControl.getsystem( clqr )

# Test sizes
@test size(ḡ) == (2*N,)
@test size(L) == (2*N, n)
@test size(G) == (2*N, m*N)

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all states in bounds
v  = [ 0.0;
       0.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [ x₁[1] - g[1];    # State @t=1
       x₁[2] - g[2];    # State @t=1
       x₂[1] - g[1];    # State @t=2
       x₂[2] - g[2] ]   # State @t=2

testconstraint( G, L, ḡ, v, x₀, e )

# Test with all states out of bounds
v  = [ 30.0;
       30.0 ]
x₀ = [ 20.0;
       20.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [ x₁[1] - g[1];    # State @t=1
       x₁[2] - g[2];    # State @t=1
       x₂[1] - g[1];    # State @t=2
       x₂[2] - g[2] ]   # State @t=2

testconstraint( G, L, ḡ, v, x₀, e )

# Test with one inbounds, one out of bounds
v  = [ 5.0;
       10.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [ x₁[1] - g[1];    # State @t=1
       x₁[2] - g[2];    # State @t=1
       x₂[1] - g[1];    # State @t=2
       x₂[2] - g[2] ]   # State @t=2

testconstraint( G, L, ḡ, v, x₀, e )

###############################################
## Test the inclusion of both state and input constraints on a pre-stabilized system
###############################################
E = [ 1.0 0.0;
      0.0 1.0;
      0.0 0.0;
      0.0 0.0 ]
F = [ 0.0;
      0.0;
      1.0;
     -1.0 ]
g = [ 5.0;
      5.0;
      5.0;
      5.0 ]

clqr = ConstrainedTimeInvariantLQR( sysStab, N, Q, R, :Q, K = :dlqr, F = F , E = E, g = g )
G, L, ḡ = PredictiveControl.inequalityconstraints( clqr, PrimalFullCondensing )

sys = PredictiveControl.getsystem( clqr )

# Test sizes
@test size(ḡ) == (4*N,)
@test size(L) == (4*N, n)
@test size(G) == (4*N, m*N)

@test ḡ[Block(1)] == g
@test ḡ[Block(2)] == g

# Test with all states and inputs in bounds
v  = [ 0.0;
       0.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    x₁[1] - g[1];     # State  @t=1
          x₁[2] - g[2];     # State  @t=1
          u₀    - g[3];     # Inputs @t=0
       -( u₀    + g[4] );   # Inputs @t=0
          x₂[1] - g[1];     # State  @t=2
          x₂[2] - g[2]      # State  @t=2
          u₁    - g[3];     # Inputs @t=1
       -( u₁    + g[4] ) ]  # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )

# Test with all inputs out of bounds, states out of bounds
v  = [ 6.0;
       6.0 ]
x₀ = [ 20.0;
       20.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    x₁[1] - g[1];     # State  @t=1
          x₁[2] - g[2];     # State  @t=1
          u₀    - g[3];     # Inputs @t=0
       -( u₀    + g[4] );   # Inputs @t=0
          x₂[1] - g[1];     # State  @t=2
          x₂[2] - g[2]      # State  @t=2
          u₁    - g[3];     # Inputs @t=1
       -( u₁    + g[4] ) ]  # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )

# Test with one inbounds, one out of bounds
v  = [ 5.0;
       6.0 ]
x₀ = [ 0.0;
       0.0 ]

u₀ = -(clqr.K*x₀)[1] + v[1]
x₁ = sysStab.A*x₀ + sysStab.B*u₀
u₁ = -(clqr.K*x₁)[1] + v[2]
x₂ = sysStab.A*x₁ + sysStab.B*u₁

e  = [    x₁[1] - g[1];     # State  @t=1
          x₁[2] - g[2];     # State  @t=1
          u₀    - g[3];     # Inputs @t=0
       -( u₀    + g[4] );   # Inputs @t=0
          x₂[1] - g[1];     # State  @t=2
          x₂[2] - g[2]      # State  @t=2
          u₁    - g[3];     # Inputs @t=1
       -( u₁    + g[4] ) ]  # Inputs @t=1

testconstraint( G, L, ḡ, v, x₀, e )


"""`meanvalueform` performs the second order extension on the given function `f` with its optional derivative
`f_prime` and initial intervals `x` and  its midpoints `x_mid`."""

function meanvalueform(f::Function, f_prime::Function, x::IntervalBox, x_mid::IntervalBox)

    mvf = f(x_mid) + dot(f_prime(x), (x - x_mid).v)

    return mvf

end

# use automatic differentiation if no derivative function given:

meanvalueform(f::Function, x::IntervalBox, x_mid::IntervalBox) = meanvalueform(f, x->∇(f,x.v), x, x_mid)

# determines the midpoints of x if x_mid is not given:

meanvalueform(f::Function, f_prime, x::IntervalBox) = meanvalueform(f, f_prime, x, IntervalBox(mid(x)))

# use automatic differentiation if no derivative function given and determines the midpoints of x if x_mid is not given:

meanvalueform(f::Function, x::IntervalBox) = meanvalueform(f, x->∇(f,x.v), x, IntervalBox(mid(x)))

"""`smear` performs the election of the variable with the largest value of semar like direction to bisect on the given function `f`
with its optional derivative `f_prime` and initial intervals `x`."""

function smear(f::Function, f_prime::Function, x::IntervalBox)

    smear_magnitude = abs.(f_prime(x)) .* diam.(X)

    return argmax([X.hi for X in ub])

end

# use automatic differentiation if no derivative function given:

smear(f::Function, x::IntervalBox) = smear(f, x->∇(f,x.v), x)

"""`localoptim` performs a local optimization of a given function `f` on intervals `x`."""

function localoptim(f::Function, X::StaticArray)

    xc = X
    gamma = 0.01
    max_iters = 10

    for i in 1:max_iters

        x = xc
        xc = x - gamma * ∇(f, x)

        if f(xc) > f(x)

            break

        end

    end

    return (f(xc), xc)

end

"""`random` generates random numbers over intervals `x`."""

randnum(r::SArray, x::IntervalBox) = (r * (x.hi - x.lo)) + x.lo

function random(X::IntervalBox{N,T}, n::Int64) where {N,T}

    Random_numbers = typeof(mid(X))[ ]

    for i in 1:n

        rand_numb = @SVector rand(N)

        interval_rand_numb = randnum.(rand_numb, X)

        push!(Random_numbers, interval_rand_numb)

    end

    return Random_numbers

end


using LinearAlgebraicRepresentation
Lar = LinearAlgebraicRepresentation
using ViewerGL
GL = ViewerGL

n = 200 #3000 #1000 #20000
t = 0.35 #0.15 #0.4 #0.15

function randlines(n=1000,t=0.2)
	V = zeros(Float64,2,2*n)
	EV = [zeros(Int64,2) for k=1:n]
	for k=1:n
		v1 = rand(Float64,2)
		v2 = rand(Float64,2)
		vm = (v1+v2)/2
		transl = rand(Float64,2)
		V[:,k] = (v1-vm)*t + transl
		V[:,n+k] = (v2-vm)*t + transl
		EV[k] = [k,n+k]
	end
	V = GL.normalize2(V)
	model = (V,EV)
	return model
end

model = randlines(n,t)
Sigma = Lar.spaceindex(model)
GL.VIEW([ GL.GLLines(model...)] )

W,EW = Lar.fraglines(1.5,1.5,1.5)(model)
GL.VIEW([ GL.GLLines(W,EW,GL.COLORS[1])] );

W,EW = Lar.fragmentlines(model)
V,EVs = Lar.biconnectedComponent((W,EW::Lar.Cells)) # 2-connected components (H & T)

graphs = [ GL.GLLines(V,EVs[k],GL.COLORS[(k-1)%12+1]) for k=1:length(EVs) ]
GL.VIEW(graphs);


using Convex
using Convex: DotMultiplyAtom
using Test
using SDPA_GMP
using MathOptInterface
using Random
const MOI = MathOptInterface
using GenericLinearAlgebra
using LinearAlgebra
import Random.shuffle
import Statistics.mean

TOL = 1e-3
eye(n) = Matrix(1.0I, n, n)

# Seed random number stream to improve test reliability
Random.seed!(2)

solvers = Any[]

push!(solvers, SDPA_GMP.Optimizer{BigFloat}())

# If Gurobi is installed, uncomment to test with it:
#using Gurobi
#push!(solvers, GurobiSolver(OutputFlag=0))

# If Mosek is installed, uncomment to test with it:
#using Mosek
#push!(solvers, MosekSolver(LOG=0))

@testset "Convex" begin
    # include("test_utilities.jl")
    # include(joinpath("test","test_const.jl"))
    include(joinpath("test_affine.jl"))
    include(joinpath("test_lp.jl"))
    # include("test_socp.jl")
    # include(joinpath("test_sdp.jl"))
    # include("test_exp.jl")
    # include("test_sdp_and_exp.jl")
    # include("test_mip.jl")
end


#*******************************************************/
#* Copyright(c) 2021 by Artelys                        */
#* This source code is subject to the terms of the     */
#* MIT Expat License (see LICENSE.md)                  */
#*******************************************************/

#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This example demonstrates how to use Knitro to solve the following
# simple nonlinear optimization problem.  This model is test problem
# HS40 from the Hock & Schittkowski collection.
#
# max   x0*x1*x2*x3        (obj)
# s.t.  x0^3 + x1^2 = 1    (c0)
#       x0^2*x3 - x2 = 0   (c1)
#       x3^2 - x1 = 0      (c2)
#
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

using KNITRO, Test

function example_nlp2noderivs(; verbose=true)
    #*------------------------------------------------------------------*
    #*     FUNCTION callbackEvalFC                                      *
    #*------------------------------------------------------------------*
    # The signature of this function matches KNITRO.KN_eval_callback in knitro.h.
    # Only "obj" and "c" are set in the KNITRO.KN_eval_result structure.
    function callbackEvalFC(kc, cb, evalRequest, evalResult, userParams)
        x = evalRequest.x

        # Evaluate nonlinear term in objective
        evalResult.obj[1] = x[1] * x[2] * x[3] * x[4]

        # Evaluate nonlinear terms in constraints
        evalResult.c[1] = x[1] * x[1] * x[1]
        evalResult.c[2] = x[1] * x[1] * x[4]

        return 0
    end

    #*------------------------------------------------------------------*
    #*     main                                                         *
    #*------------------------------------------------------------------*

    # Create a new Knitro solver instance.
    kc = KNITRO.KN_new()

    # Initialize Knitro with the problem definition.

    # Add the variables and specify initial values for them.
    # Note: any unset lower bounds are assumed to be
    # unbounded below and any unset upper bounds are
    # assumed to be unbounded above.
    xIndices = KNITRO.KN_add_vars(kc, 4)
    for x in xIndices
        KNITRO.KN_set_var_primal_init_value(kc, x, 0.8)
    end

    # Add the constraints and set the rhs and coefficients
    KNITRO.KN_add_cons(kc, 3)
    KNITRO.KN_set_con_eqbnds_all(kc, [1.0, 0.0, 0.0])

    # Coefficients for 2 linear terms
    lconIndexCons = Int32[1, 2]
    lconIndexVars = Int32[2, 1]
    lconCoefs = [-1.0, -1.0]
    KNITRO.KN_add_con_linear_struct(kc, lconIndexCons, lconIndexVars, lconCoefs)

    # Coefficients for 2 quadratic terms

    # 1st term:  x1^2 term in c0
    # 2nd term:  x3^2 term in c2
    qconIndexCons = Int32[0, 2]
    qconIndexVars1 = Int32[1, 3]
    qconIndexVars2 = Int32[1, 3]
    qconCoefs = [1.0, 1.0]

    KNITRO.KN_add_con_quadratic_struct(
        kc,
        qconIndexCons,
        qconIndexVars1,
        qconIndexVars2,
        qconCoefs,
    )

    # Add callback to evaluate nonlinear(non-quadratic) terms in the model:
    #    x0*x1*x2*x3  in the objective
    #    x0^3         in first constraint c0
    #    x0^2*x3      in second constraint c1
    cb = KNITRO.KN_add_eval_callback(kc, true, Int32[0, 1], callbackEvalFC)

    # Set minimize or maximize(if not set, assumed minimize)
    KNITRO.KN_set_obj_goal(kc, KNITRO.KN_OBJGOAL_MAXIMIZE)

    # Set option to println output after every iteration.
    kn_outlev = verbose ? KNITRO.KN_OUTLEV_ITER : KNITRO.KN_OUTLEV_NONE
    KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_OUTLEV, kn_outlev)

    # Solve the problem.
    #
    # Return status codes are defined in "knitro.h" and described
    # in the Knitro manual.
    nStatus = KNITRO.KN_solve(kc)
    nStatus, objSol, x, lambda_ = KNITRO.KN_get_solution(kc)
    varbndInfeas, varintInfeas, varviols = KNITRO.KN_get_var_viols(kc, Cint[0, 1, 2, 3])
    coninfeas, conviols = KNITRO.KN_get_con_viols(kc, Cint[0, 1, 2])
    err = KNITRO.KN_get_presolve_error(kc)

    if verbose
        println()
        println("Knitro converged with final status = ", nStatus)
        # An example of obtaining solution information.
        println("  optimal objective value  = ", objSol)
        println("  optimal primal values x  = ", x)
        println("  feasibility violation    = ", KNITRO.KN_get_abs_feas_error(kc))
        println("  KKT optimality violation = ", KNITRO.KN_get_abs_opt_error(kc))
        println("Variables bound violations = ", varbndInfeas)
        println("Variables integrality violations = ", varintInfeas)
        println("Variables violation values = ", varviols)
        println("Constraints bound violations = ", coninfeas)
        println("Constraints violation values = ", conviols)
    end

    @testset "Example HS40 nlp1noderivs" begin
        @test varbndInfeas == [0, 0, 0, 0]
        @test varintInfeas == [0, 0, 0, 0]
        @test varviols ≈ [0.0, 0.0, 0.0, 0.0] atol = 1e-6
        @test coninfeas == [0, 0, 0]
        @test conviols ≈ [0.0, 0.0, 0.0] atol = 1e-6
        @test KNITRO.KN_get_abs_feas_error(kc) == max(conviols...)
    end

    # Delete the Knitro solver instance.
    return KNITRO.KN_free(kc)
end

if KNITRO.KNITRO_VERSION >= v"12.4"
    example_nlp2noderivs(; verbose=isdefined(Main, :KN_VERBOSE) ? KN_VERBOSE : true)
else
    println("Example `nlp2noderivs.jl` is only available with Knitro >= 12.4")
end


# ---
# title: 377. Combination Sum IV
# id: problem377
# author: Tian Jun
# date: 2020-10-31
# difficulty: Medium
# categories: Dynamic Programming
# link: <https://leetcode.com/problems/combination-sum-iv/description/>
# hidden: true
# ---
#
# Given an integer array with all positive numbers and no duplicates, find the
# number of possible combinations that add up to a positive integer target.
#
# **Example:**
#
#
#
#     _**nums**_ = [1, 2, 3]
#     _**target**_ = 4
#
#     The possible combination ways are:
#     (1, 1, 1, 1)
#     (1, 1, 2)
#     (1, 2, 1)
#     (1, 3)
#     (2, 1, 1)
#     (2, 2)
#     (3, 1)
#
#     Note that different sequences are counted as different combinations.
#
#     Therefore the output is _**7**_.
#
#
#
#
# **Follow up:**
# What if negative numbers are allowed in the given array?
# How does it change the problem?
# What limitation we need to add to the question to allow negative numbers?
#
# **Credits:**
# Special thanks to [@pbrother](https://leetcode.com/pbrother/) for adding this
# problem and creating all test cases.
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


import Rotations.RotMatrix

"""
    degree_rotation(deg)

Create a rotation matrix for the angle given in degrees.
"""
function degree_rotation(deg::Real)
    RotMatrix(pi * (deg / 180))
end

"""
    frac_rotation(frac)

Create a rotation matrix for the angle given as a fraction,
i.e. 1.0 represents a full rotation and 0.5 a half rotation.
"""
function frac_rotation(frac::Real)
    RotMatrix(2 * pi * frac)
end

"""
    radian_rotation(rad)

Create a rotation matrix for the angle given in radians.
"""
function radian_rotation(rad::Real)
    RotMatrix(rad)
end

export degree_rotation, frac_rotation, radian_rotation