include(Pkg.dir("HybridSystems", "examples", "cruise_control.jl"));

const va = 15.6
const vb = 24.5
const vc = 29.5
const v = (va, vb, vc)
const U = 4.
const m0 = 500
const T = 2
const N = 10
const M = 1
const H = 0.8;

macro _time(x)
    quote
        y = @timed $(esc(x))
        # y[1] is returned value
        # y[2] is time in seconds
        y[2]
    end
end

using Gurobi
lpsolver = GurobiSolver(OutputFlag=0);

function liftu(S, sys::HybridSystems.DiscreteLinearControlSystem)
    [sys.A sys.B] \ S
end
function new_constraint(hs, S, q, t)
    @assert source(hs, t) == q
    σ = symbol(hs, t)
    r = target(hs, t)
    ABset = liftu(S[1], hs.resetmaps[σ])
    project(ABset, 1:statedim(hs, q))
end
function new_constraints(hs, S, q)
    map(t -> new_constraint(hs, S, q, t), out_transitions(hs, q))
end
function add_hrep!(S, h::HalfSpace)
    # I was using LP cycling errors when using CDD's LP solver
    if issubset(S, h) # If S ⊆ h, then adding h will not change affect S
        false
    else
        push!(S, SimpleHRepresentation(reshape(h.a, 1, length(h.a)), [h.β]))
        true
    end
end
function add_constraint!(S, P)
    added = count(map(h -> add_hrep!(S, h), ineqs(P))) + count(map(h -> add_hrep!(S, h), eqs(P)))
    removehredundancy!(S) # CDD throws LP cycling error
    added
end
function add_constraints!(S::Polyhedron, Ps::Vector{<:Polyhedron})
    sum(P -> add_constraint!(S, P), Ps)
end
function set_iteration!(hs, S)
    Ps = map(q -> new_constraints(hs, S, q), states(hs))
    added = add_constraints!.(S, Ps)
    @show added
end
function iterate!(hs, S, nit)
    map(i -> (gc(); @_time set_iteration!(hs, S)), 1:nit)
end

Mmax = 1
nit = 2
t = zeros(Mmax, nit)
Hs = Vector{HybridSystem}(Mmax)
CIS = Vector{Vector{Polyhedron}}(Mmax)
for m in 1:Mmax
    Hs[m] = cruise_control_example(N, m, vmin = 5., v=(va, vb, vc), U=U, H=H, sym=false, m0=500);
    I0 = Hs[m].invariants;
    @show nineqs(I0[1])
    CIS[m] = deepcopy(I0);
    @show m
    t[m, :] = iterate!(Hs[m], CIS[m], nit)
    @show t[m, :]
end
t

Mmax = size(t, 1)
nit = 1
previt = size(t, 2)
t = [t zeros(Mmax, nit)]
totit = size(t, 2)
for m in 1:Mmax
    t[m, previt+(1:nit)] = iterate!(Hs[m], CIS[m], nit)
    @show t[m, :]
end
t

import Plots
Plots.pyplot()

D = [1, 2]
Plots.plot(project(Hs[1].invariants[1], D))
Plots.plot!(project(CIS[1][1], D))

Plots.savefig("dist_trailerspeed.png")

t