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 = LightAutomaton(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.)

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

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

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

Main.powertrain_control