module powertrain_control
using ReachabilityAnalysis, ModelingToolkit
#using Polyhedra, CDDLib # ??

vars = @variables p, lam, pe, i, t

@taylorize function startup!(dx, x, params, t)
    p, lam, pe, i = x
    dx[1] = 0.41328 * (2 * 247 * (-2.3421 * p * p + 2.7799 * p - 0.3273) -
                       0.9 * (-3.66 + 0.08979 * 104.71975511 * p - 0.0337 * 104.71975511 * p * p +
                              0.0001 * 104.71975511 * 104.71975511 * p))
    dx[2] = 4 * (13.893 -
                 35.2518 * 1 *
                 ((1 / 14.7) * (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                                0.0001 * pe * 104.71975511 * 104.71975511)) +
                 20.7364 * 1 * 1 *
                 ((1 / 14.7) * (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                                0.0001 * pe * 104.71975511 * 104.71975511)) *
                 ((1 / 14.7) * (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                                0.0001 * pe * 104.71975511 * 104.71975511)) +
                 2.6287 * (0.9 * (-3.66 + 0.08979 * 104.71975511 * p - 0.0337 * 104.71975511 * p * p +
                                  0.0001 * p * 104.71975511 * 104.71975511)) -
                 1.592 *
                 (0.9 * (-3.66 + 0.08979 * 104.71975511 * p - 0.0337 * 104.71975511 * p * p +
                         0.0001 * p * 104.71975511 * 104.71975511)) * 1 *
                 ((1 / 14.7) * (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                                0.0001 * pe * 104.71975511 * 104.71975511)) - lam)
    dx[3] = 0.41328 * (2 * 1 * (247) * (-2.3421 * p * p + 2.7799 * p - 0.3273) -
                       (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                        0.0001 * pe * 104.71975511 * 104.71975511))
    dx[4] = zero(i)
    return dx
end

@taylorize function normal!(dx, x, params, t)
    p, lam, pe, i = x
    dx[1] = 0.41328 * (2 * 1 * (247) * (-2.3421 * p * p + 2.7799 * p - 0.3273) -
                       (-3.66 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                        0.0001 * pe * 104.71975511 * 104.71975511))
    dx[2] = 4 * (13.893 -
                 35.2518 * 1 *
                 ((1 / 14.7) * (1 + ivalue + 0.04 * (1 * lam - 14.7)) *
                  (-0.366 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                   0.0001 * pe * 104.71975511 * 104.71975511)) +
                 20.7364 * 1 * 1 *
                 ((1 / 14.7) * (1 + ivalue + 0.04 * (1 * lam - 14.7)) *
                  (-0.366 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                   0.0001 * pe * 104.71975511 * 104.71975511)) *
                 ((1 / 14.7) * (1 + ivalue + 0.04 * (1 * lam - 14.7)) *
                  (-0.366 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                   0.0001 * pe * 104.71975511 * 104.71975511)) +
                 2.6287 * (0.9 * (-0.366 + 0.08979 * 104.71975511 * p - 0.0337 * 104.71975511 * p * p +
                                  0.0001 * p * 104.71975511 * 104.71975511)) -
                 1.592 *
                 (0.9 * (-0.366 + 0.08979 * 104.71975511 * p - 0.0337 * 104.71975511 * p * p +
                         0.0001 * p * 104.71975511 * 104.71975511)) * 1 *
                 ((1 / 14.7) * (1 + ivalue + 0.04 * (1 * lam - 14.7)) *
                  (-0.366 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                   0.0001 * pe * 104.71975511 * 104.71975511)) - lam)
    dx[3] = 0.41328 * (2 * 1 * (247.0) * (-2.3421 * p * p + 2.7799 * p - 0.3273) -
                       (-0.366 + 0.08979 * 104.71975511 * pe - 0.0337 * 104.71975511 * pe * pe +
                        0.0001 * pe * 104.71975511 * 104.71975511))
    dx[4] = 0.14 * (1 * lam - 14.7)
    return dx
end

function powertrain_control_hybrid()
    n = 4 + 1 # variables

    automaton = GraphAutomaton(2)
    add_transition!(automaton, 1, 2, 1)

    # mode 1 "startup"
    invariant = HPolyhedron([HalfSpace(t >= 9.5, vars), HalfSpace(t <= 20, vars)])
    mode1 = @system(x' = startup!(x), dim:5, x ∈ invariant)
    # mode 1 "normal"
    invariant = Universe(n)
    mode2 = @system(x' = normal!(x), dim:5, x ∈ invariant)
    modes = [mode1, mode2]

    reset = Dict(n => 0.0)

    # transition startup -> normal
    guard = HalfSpace(t >= 9.5, vars)
    trans1 = ConstrainedResetMap(n, guard, reset)
    resetmaps = [trans1]

    return HybridSystems.HybridSystem(automaton, modes, resetmaps, [AutonomousSwitching()])
end

function model(X0)
    H = powertrain_control_hybrid()
    return IVP(H, [(1, X0)])
end
end # module

Main.powertrain_control