construct.jl

const MSG_LINMODEL_ERR = "estim.model type must be a LinModel, see ManualEstimator docstring "*
                         "to use a nonlinear state estimator with a linear controller"

struct PredictiveControllerBuffer{NT<:Real}
    u ::Vector{NT}
    Z̃ ::Vector{NT}
    D̂ ::Vector{NT}
    Ŷ ::Vector{NT}
    U ::Vector{NT}
    D̂e::Vector{NT}
    Ŷe::Vector{NT}
    Ue::Vector{NT}
    Ẽ ::Matrix{NT}
    P̃u::Matrix{NT}
    empty::Vector{NT}
end

@doc raw"""
    PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)

Create a buffer for `PredictiveController` objects.

The buffer is used to store intermediate results during computation without allocating.
"""
function PredictiveControllerBuffer(
    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp::Int, Hc::Int, nϵ::Int
) where NT <: Real
    nu, ny, nd = estim.model.nu, estim.model.ny, estim.model.nd
    nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
    u  = Vector{NT}(undef, nu)
    Z̃  = Vector{NT}(undef, nZ̃)
    D̂  = Vector{NT}(undef, nd*Hp)
    Ŷ  = Vector{NT}(undef, ny*Hp)
    U  = Vector{NT}(undef, nu*Hp)
    D̂e = Vector{NT}(undef, nd*(Hp+1))
    Ŷe = Vector{NT}(undef, ny*(Hp+1))
    Ue = Vector{NT}(undef, nu*(Hp+1))
    Ẽ  = Matrix{NT}(undef, ny*Hp, nZ̃)
    P̃u = Matrix{NT}(undef, nu*Hp, nZ̃)
    empty = Vector{NT}(undef, 0)
    return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, D̂e, Ŷe, Ue, Ẽ, P̃u, empty)
end

"Include all the objective function weights of [`PredictiveController`](@ref)"
struct ControllerWeights{
    NT<:Real,
    # parameters to support both dense and Diagonal matrices (with specialization):
    MW<:AbstractMatrix{NT}, 
    NW<:AbstractMatrix{NT},  
    LW<:AbstractMatrix{NT}, 
}
    M_Hp::Hermitian{NT, MW}
    Ñ_Hc::Hermitian{NT, NW}
    L_Hp::Hermitian{NT, LW}
    E   ::NT
    iszero_M_Hp::Vector{Bool}
    iszero_Ñ_Hc::Vector{Bool}
    iszero_L_Hp::Vector{Bool}
    iszero_E::Bool
    isinf_C ::Bool
    function ControllerWeights{NT}(
        M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt, Ewt
    ) where {
        NT<:Real, 
        MW<:AbstractMatrix{NT}, 
        NW<:AbstractMatrix{NT}, 
        LW<:AbstractMatrix{NT}
    }
        nΔU = size(N_Hc, 1)
        C = Cwt
        isinf_C = isinf(C)
        if !isinf_C  
            # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable)
            Ñ_Hc = [N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C]
            isdiag(N_Hc) && (Ñ_Hc = Diagonal(Ñ_Hc)) # NW(Ñ_Hc) does not work on Julia 1.10
        else
            # ΔŨ = ΔU (only hard constraints)
            Ñ_Hc = N_Hc
        end
        M_Hp = Hermitian(M_Hp, :L)
        Ñ_Hc = Hermitian(Ñ_Hc, :L)
        L_Hp = Hermitian(L_Hp, :L)
        E = Ewt
        iszero_M_Hp = [iszero(M_Hp)]
        iszero_Ñ_Hc = [iszero(Ñ_Hc)]
        iszero_L_Hp = [iszero(L_Hp)]
        iszero_E = iszero(E)
        return new{NT, MW, NW, LW}(
            M_Hp, Ñ_Hc, L_Hp, E, 
            iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E, isinf_C
        )
    end
end

"Outer constructor to validate and convert weight matrices if necessary."
function ControllerWeights(
        model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
    ) where {NT<:Real}
    validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
    M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp)
    return ControllerWeights{NT}(M_Hp, N_Hc, L_Hp, Cwt, Ewt)
end

"Validate predictive controller weight and horizon specified values."
function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing)
    nu, ny = model.nu, model.ny
    nM, nN, nL = ny*Hp, nu*Hc, nu*Hp
    Hp < 1  && throw(ArgumentError("Prediction horizon Hp should be ≥ 1"))
    Hc < 1  && throw(ArgumentError("Control horizon Hc should be ≥ 1"))
    Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp"))
    size(M_Hp) ≠ (nM,nM) && throw(DimensionMismatch("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)"))
    size(N_Hc) ≠ (nN,nN) && throw(DimensionMismatch("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)"))
    size(L_Hp) ≠ (nL,nL) && throw(DimensionMismatch("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)"))
    (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative"))
    (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative"))
    (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative"))
    !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian"))
    !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian"))
    !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian"))
    size(C) ≠ ()    && throw(DimensionMismatch("Cwt should be a real scalar"))
    C < 0           && throw(ArgumentError("Cwt weight should be ≥ 0"))
    !isnothing(E) && size(E) ≠ () && throw(DimensionMismatch("Ewt should be a real scalar"))
end

"Include all the data for the constraints of [`PredictiveController`](@ref)"
struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
    # matrices for the terminal constraints:
    ẽx̂      ::Matrix{NT}
    fx̂      ::Vector{NT}
    gx̂      ::Matrix{NT}
    jx̂      ::Matrix{NT}
    kx̂      ::Matrix{NT}
    vx̂      ::Matrix{NT}
    bx̂      ::Vector{NT}
    # matrices for the zero defect constraints (N/A for single shooting transcriptions):
    ẼS      ::Matrix{NT}
    FS      ::Vector{NT}
    GS      ::Matrix{NT}
    JS      ::Matrix{NT}
    KS      ::Matrix{NT}
    VS      ::Matrix{NT}
    BS      ::Vector{NT}
    # custom linear equality constraints:
    Ẽw      ::Matrix{NT}
    Fw      ::Vector{NT}
    W̄y      ::SparseMatrixCSC{NT, Int}
    W̄u      ::SparseMatrixCSC{NT, Int}
    W̄d      ::SparseMatrixCSC{NT, Int}
    W̄r      ::SparseMatrixCSC{NT, Int}
    nw      ::Int
    # bounds over the prediction horizon (deviation vectors from operating points): 
    U0min   ::Vector{NT}
    U0max   ::Vector{NT}
    ΔŨmin   ::Vector{NT}
    ΔŨmax   ::Vector{NT}
    Y0min   ::Vector{NT}
    Y0max   ::Vector{NT}
    Wmin    ::Vector{NT}
    Wmax    ::Vector{NT}
    x̂0min   ::Vector{NT}
    x̂0max   ::Vector{NT}
    # A matrices for the linear inequality constraints:
    A_Umin  ::SparseMatrixCSC{NT, Int}
    A_Umax  ::SparseMatrixCSC{NT, Int}
    A_ΔŨmin ::SparseMatrixCSC{NT, Int}
    A_ΔŨmax ::SparseMatrixCSC{NT, Int}
    A_Ymin  ::Matrix{NT}
    A_Ymax  ::Matrix{NT}
    A_Wmin  ::Matrix{NT}
    A_Wmax  ::Matrix{NT}
    A_x̂min  ::Matrix{NT}
    A_x̂max  ::Matrix{NT}
    A       ::Matrix{NT}
    # b vector for the linear inequality constraints:
    b       ::Vector{NT}
    # indices of finite numbers in the b vector (linear inequality constraints):
    i_b     ::BitVector
    # A matrices for the linear equality constraints:
    Aeq     ::Matrix{NT}
    # b vector for the linear equality constraints:
    beq     ::Vector{NT}
    # nonlinear equality constraints:
    neq     ::Int
    # constraint softness parameter vectors needing seperate storage:
    C_ymin  ::Vector{NT}
    C_ymax  ::Vector{NT}
    C_wmin  ::Vector{NT}
    C_wmax  ::Vector{NT}
    c_x̂min  ::Vector{NT}
    c_x̂max  ::Vector{NT}
    # indices of finite numbers in the g vector (nonlinear inequality constraints):
    i_g     ::BitVector
    # custom nonlinear inequality constraints:
    gc!     ::GCfunc
    nc      ::Int
end

@doc raw"""
    setconstraint!(mpc::PredictiveController; <keyword arguments>) -> mpc

Set the bound constraint parameters of the [`PredictiveController`](@ref) `mpc`.

The predictive controllers support both soft and hard constraints, defined by:
```math 
\begin{alignat*}{3}
    \mathbf{u_{min}  - c_{u_{min}}}  ϵ ≤&&\       \mathbf{u}(k+j) &≤ \mathbf{u_{max}  + c_{u_{max}}}  ϵ &&\qquad  j = 0, 1 ,..., H_p - 1 \\
    \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\      \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad  j = 0, 1 ,..., H_c - 1 \\
    \mathbf{y_{min}  - c_{y_{min}}}  ϵ ≤&&\       \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max}  + c_{y_{max}}}  ϵ &&\qquad  j = 1, 2 ,..., H_p     \\
    \mathbf{w_{min}  - c_{w_{min}}}  ϵ ≤&&\       \mathbf{w}(k+j) &≤ \mathbf{w_{max}  + c_{w_{max}}}  ϵ &&\qquad  j = 0, 1 ,..., H_p     \\
    \mathbf{x̂_{min}  - c_{x̂_{min}}}  ϵ ≤&&\   \mathbf{x̂}_i(k+H_p) &≤ \mathbf{x̂_{max}  + c_{x̂_{max}}}  ϵ &&\qquad
\end{alignat*}
```
and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the
end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints
for details on bounds and softness parameters ``\mathbf{c}``. The penultimate line with the
``\mathbf{w}`` vector is for custom linear inequality constraints. See Extended Help for 
details on this and time-varying bounds. The output, custom and terminal constraints are all
soft by default.

# Arguments
!!! info
    The keyword arguments `Δumin`, `Δumax`, `c_Δumin`, `c_Δumax`, `x̂min`, `x̂max`, `c_x̂min`,
    `c_x̂max` and their capital letter versions have non-Unicode alternatives e.g. 
    *`Deltaumin`*, *`xhatmax`* and *`C_Deltaumin`*

    The default constraints are mentioned here for clarity but omitting a keyword argument 
    will not re-assign to its default value (defaults are set at construction only).

- `mpc::PredictiveController` : predictive controller to set constraints
- `umin=fill(-Inf,nu)` / `umax=fill(+Inf,nu)` : manipulated input bound ``\mathbf{u_{min/max}}``
- `Δumin=fill(-Inf,nu)` / `Δumax=fill(+Inf,nu)` : manipulated input increment bound ``\mathbf{Δu_{min/max}}``
- `ymin=fill(-Inf,ny)` / `ymax=fill(+Inf,ny)` : predicted output bound ``\mathbf{y_{min/max}}``
- `wmin=fill(-Inf,nw)` / `wmax=fill(+Inf,nw)` : custom linear constraint bound ``\mathbf{w_{min/max}}``
- `x̂min=fill(-Inf,nx̂)` / `x̂max=fill(+Inf,nx̂)` : terminal constraint bound ``\mathbf{x̂_{min/max}}``
- `c_umin=fill(0.0,nu)` / `c_umax=fill(0.0,nu)` : `umin` / `umax` softness weight ``\mathbf{c_{u_{min/max}}}``
- `c_Δumin=fill(0.0,nu)` / `c_Δumax=fill(0.0,nu)` : `Δumin` / `Δumax` softness weight ``\mathbf{c_{Δu_{min/max}}}``
- `c_ymin=fill(1.0,ny)` / `c_ymax=fill(1.0,ny)` : `ymin` / `ymax` softness weight ``\mathbf{c_{y_{min/max}}}``
- `c_wmin=fill(1.0,nw)` / `c_wmax=fill(1.0,nw)` : `wmin` / `wmax` softness weight ``\mathbf{c_{w_{min/max}}}``
- `c_x̂min=fill(1.0,nx̂)` / `c_x̂max=fill(1.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}``
- all the keyword arguments above but with a first capital letter, except for the terminal
  constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help)

# Examples
```jldoctest
julia> mpc = LinMPC(setop!(LinModel(tf(3, [30, 1]), 4), uop=[50], yop=[25]));

julia> mpc = setconstraint!(mpc, umin=[0], umax=[100], Δumin=[-10], Δumax=[+10])
LinMPC controller with a sample time Ts = 4.0 s:
├ estimator: SteadyKalmanFilter
├ model: LinModel
├ optimizer: OSQP
├ transcription: SingleShooting
└ dimensions:
  ├ 10 prediction steps Hp
  ├  2 control steps Hc
  ├  1 slack variable ϵ (control constraints)
  ├  1 manipulated inputs u (0 integrating states)
  ├  2 estimated states x̂
  ├  1 measured outputs ym (1 integrating states)
  ├  0 unmeasured outputs yu
  └  0 measured disturbances d
```

# Extended Help
!!! details "Extended Help"
    Terminal constraints provide closed-loop stability guarantees on the nominal plant model.
    They can render an unfeasible problem however. In practice, a sufficiently large
    prediction horizon ``H_p`` without terminal constraints is typically enough for 
    stability. If `mpc.estim.direct==true`, the estimator computes the states at ``i = k`` 
    (the current time step), otherwise at ``i = k - 1``. Note that terminal constraints are
    applied on the augmented state vector ``\mathbf{x̂}`` (see [`SteadyKalmanFilter`](@ref)
    for details on augmentation).

    For variable constraints, the bounds can be modified after calling [`moveinput!`](@ref),
    that is, at runtime, but not the softness parameters ``\mathbf{c}``. It is not possible
    to modify `±Inf` bounds at runtime.

    !!! tip
        To keep a variable unconstrained while maintaining the ability to add a constraint
        later at runtime, set the bound to an absolute value sufficiently large when you
        create the controller (but different than `±Inf`).

    It is also possible to specify time-varying constraints over the horizons. In such
    cases, they are all defined by:
    ```math 
    \begin{alignat*}{3}
        \mathbf{U_{min}  - C_{u_{min}}}  ϵ ≤&&\ \mathbf{U}  &≤ \mathbf{U_{max}  + C_{u_{max}}}  ϵ \\
        \mathbf{ΔU_{min} - C_{Δu_{min}}} ϵ ≤&&\ \mathbf{ΔU} &≤ \mathbf{ΔU_{max} + C_{Δu_{max}}} ϵ \\
        \mathbf{Y_{min}  - C_{y_{min}}}  ϵ ≤&&\ \mathbf{Ŷ}  &≤ \mathbf{Y_{max}  + C_{y_{max}}}  ϵ \\
        \mathbf{W_{min}  - C_{w_{min}}}  ϵ ≤&&\ \mathbf{W}  &≤ \mathbf{W_{max}  + C_{w_{max}}}  ϵ 
    \end{alignat*}
    ```
    For this, use the same keyword arguments as above but with a first capital letter:

    - `Umin`  / `Umax`  / `C_umin`  / `C_umax`  : ``\mathbf{U}`` constraints `(nu*Hp,)`.
    - `ΔUmin` / `ΔUmax` / `C_Δumin` / `C_Δumax` : ``\mathbf{ΔU}`` constraints `(nu*Hc,)`.
    - `Ymin`  / `Ymax`  / `C_ymin`  / `C_ymax`  : ``\mathbf{Ŷ}`` constraints `(ny*Hp,)`.
    - `Wmin`  / `Wmax`  / `C_wmin`  / `C_wmax`  : custom linear constraints `(nw*(Hp+1),)`.
    
    The custom linear inequality constraints are all gathered in the vector:
    ```math   
    \begin{aligned}                                                                                     
    \mathbf{W}
        &=                                                                                                \begin{bmatrix} 
        \mathbf{w}(k+0)      \\ \mathbf{w}(k+1)      \\ \vdots               \\ \mathbf{w}(k+H_p)         \end{bmatrix}   \\
        &=                                                                                                \begin{bmatrix}
        \mathbf{W_y ŷ}(k+0)   + \mathbf{W_u u}(k+0)   + \mathbf{W_d d}(k+0)   + \mathbf{W_r r_y}(k+0)     \\
        \mathbf{W_y ŷ}(k+1)   + \mathbf{W_u u}(k+1)   + \mathbf{W_d d̂}(k+1)   + \mathbf{W_r r̂_y}(k+1)     \\
        \vdots                                                                                            \\
        \mathbf{W_y ŷ}(k+H_p) + \mathbf{W_u u}(k+H_p) + \mathbf{W_d d̂}(k+H_p) + \mathbf{W_r r̂_y}(k+H_p)   \end{bmatrix} 
    \end{aligned}
    ```
    The matrices ``\mathbf{W_y}``, ``\mathbf{W_u}``, ``\mathbf{W_d}`` and ``\mathbf{W_r}``
    must have `nw` rows and are provided at construction time. The terms with
    ``\mathbf{W_y}`` are present only if the model is a [`LinModel`](@ref). Any `nothing`
    value for these matrices is treated as a zero matrix.
"""
function setconstraint!(
    mpc::PredictiveController; 
    umin        = nothing, umax        = nothing,
    Deltaumin   = nothing, Deltaumax   = nothing,
    ymin        = nothing, ymax        = nothing,
    wmin        = nothing, wmax        = nothing,
    xhatmin     = nothing, xhatmax     = nothing,
    c_umin      = nothing, c_umax      = nothing,
    c_Deltaumin = nothing, c_Deltaumax = nothing,
    c_ymin      = nothing, c_ymax      = nothing,
    c_wmin      = nothing, c_wmax      = nothing,
    c_xhatmin   = nothing, c_xhatmax   = nothing,
    Umin        = nothing, Umax        = nothing,
    DeltaUmin   = nothing, DeltaUmax   = nothing,
    Ymin        = nothing, Ymax        = nothing,
    Wmin        = nothing, Wmax        = nothing,
    C_umax      = nothing, C_umin      = nothing,
    C_Deltaumax = nothing, C_Deltaumin = nothing,
    C_ymax      = nothing, C_ymin      = nothing,
    C_wmax      = nothing, C_wmin      = nothing,
    Δumin   = Deltaumin,   Δumax = Deltaumax,
    x̂min    = xhatmin,     x̂max = xhatmax,
    c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax,
    c_x̂min  = c_xhatmin,   c_x̂max = c_xhatmax,
    ΔUmin   = DeltaUmin,   ΔUmax = DeltaUmax,
    C_Δumin = C_Deltaumin, C_Δumax = C_Deltaumax,
)
    model, con =  mpc.estim.model, mpc.con
    transcription, optim = mpc.transcription, mpc.optim
    nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc
    nϵ, nw, nc = mpc.nϵ, con.nw, con.nc
    notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED)
    if isnothing(Umin) && !isnothing(umin)
        size(umin) == (nu,) || throw(DimensionMismatch("umin size must be $((nu,))"))
        for i = 1:nu*Hp
            con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i]
        end
    elseif !isnothing(Umin)
        size(Umin) == (nu*Hp,) || throw(DimensionMismatch("Umin size must be $((nu*Hp,))"))
        con.U0min .= Umin .- mpc.Uop
    end
    if isnothing(Umax) && !isnothing(umax)
        size(umax) == (nu,) || throw(DimensionMismatch("umax size must be $((nu,))"))
        for i = 1:nu*Hp
            con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i]
        end
    elseif !isnothing(Umax)
        size(Umax)   == (nu*Hp,) || throw(DimensionMismatch("Umax size must be $((nu*Hp,))"))
        con.U0max .= Umax .- mpc.Uop
    end
    if isnothing(ΔUmin) && !isnothing(Δumin)
        size(Δumin) == (nu,) || throw(DimensionMismatch("Δumin size must be $((nu,))"))
        for i = 1:nu*Hc
            con.ΔŨmin[i] = Δumin[(i-1) % nu + 1]
        end
    elseif !isnothing(ΔUmin)
        size(ΔUmin)  == (nu*Hc,) || throw(DimensionMismatch("ΔUmin size must be $((nu*Hc,))"))
        con.ΔŨmin[1:nu*Hc] .= ΔUmin
    end
    if isnothing(ΔUmax) && !isnothing(Δumax)
        size(Δumax) == (nu,) || throw(DimensionMismatch("Δumax size must be $((nu,))"))
        for i = 1:nu*Hc
            con.ΔŨmax[i] = Δumax[(i-1) % nu + 1]
        end
    elseif !isnothing(ΔUmax)
        size(ΔUmax)  == (nu*Hc,) || throw(DimensionMismatch("ΔUmax size must be $((nu*Hc,))"))
        con.ΔŨmax[1:nu*Hc] .= ΔUmax
    end
    if isnothing(Ymin) && !isnothing(ymin)
        size(ymin) == (ny,) || throw(DimensionMismatch("ymin size must be $((ny,))"))
        for i = 1:ny*Hp
            con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i]
        end
    elseif !isnothing(Ymin)
        size(Ymin) == (ny*Hp,) || throw(DimensionMismatch("Ymin size must be $((ny*Hp,))"))
        con.Y0min .= Ymin .- mpc.Yop
    end
    if isnothing(Ymax) && !isnothing(ymax)
        size(ymax) == (ny,) || throw(DimensionMismatch("ymax size must be $((ny,))"))
        for i = 1:ny*Hp
            con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i]
        end
    elseif !isnothing(Ymax)
        size(Ymax) == (ny*Hp,) || throw(DimensionMismatch("Ymax size must be $((ny*Hp,))"))
        con.Y0max .= Ymax .- mpc.Yop
    end

    if isnothing(Wmin) && !isnothing(wmin)
        size(wmin) == (nw,) || throw(DimensionMismatch("wmin size must be $((nw,))"))
        for i = 1:nw*(Hp+1)
            con.Wmin[i] = wmin[(i-1) % nw + 1]
        end
    elseif !isnothing(Wmin)
        size(Wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmin size must be $((nw*(Hp+1),))"))
        con.Wmin .= Wmin
    end
    if isnothing(Wmax) && !isnothing(wmax)
        size(wmax) == (nw,) || throw(DimensionMismatch("wmax size must be $((nw,))"))
        for i = 1:nw*(Hp+1)
            con.Wmax[i] = wmax[(i-1) % nw + 1]
        end
    elseif !isnothing(Wmax)
        size(Wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmax size must be $((nw*(Hp+1),))"))
        con.Wmax .= Wmax
    end
    if !isnothing(x̂min)
        size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))"))
        con.x̂0min .= x̂min .- mpc.estim.x̂op
    end
    if !isnothing(x̂max)
        size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))"))
        con.x̂0max .= x̂max .- mpc.estim.x̂op
    end
    allECRs = (
        c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax,
        C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax,
        c_x̂min, c_x̂max,
    )
    if any(ECR -> !isnothing(ECR), allECRs)
        nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
        notSolvedYet || error("Cannot set softness parameters after calling moveinput!")
    end
    if notSolvedYet
        isnothing(C_umin)   && !isnothing(c_umin)   && (C_umin  = repeat(c_umin,  Hp))
        isnothing(C_umax)   && !isnothing(c_umax)   && (C_umax  = repeat(c_umax,  Hp))
        isnothing(C_Δumin)  && !isnothing(c_Δumin)  && (C_Δumin = repeat(c_Δumin, Hc))
        isnothing(C_Δumax)  && !isnothing(c_Δumax)  && (C_Δumax = repeat(c_Δumax, Hc))
        isnothing(C_ymin)   && !isnothing(c_ymin)   && (C_ymin  = repeat(c_ymin,  Hp))
        isnothing(C_ymax)   && !isnothing(c_ymax)   && (C_ymax  = repeat(c_ymax,  Hp))
        isnothing(C_wmin)   && !isnothing(c_wmin)   && (C_wmin  = repeat(c_wmin,  Hp+1))
        isnothing(C_wmax)   && !isnothing(c_wmax)   && (C_wmax  = repeat(c_wmax,  Hp+1))
        if !isnothing(C_umin)
            size(C_umin) == (nu*Hp,) || throw(DimensionMismatch("C_umin size must be $((nu*Hp,))"))
            any(<(0), C_umin) && error("C_umin weights should be non-negative")
            con.A_Umin[:, end] .= -C_umin
        end
        if !isnothing(C_umax)
            size(C_umax) == (nu*Hp,) || throw(DimensionMismatch("C_umax size must be $((nu*Hp,))"))
            any(<(0), C_umax) && error("C_umax weights should be non-negative")
            con.A_Umax[:, end] .= -C_umax
        end
        if !isnothing(C_Δumin)
            size(C_Δumin) == (nu*Hc,) || throw(DimensionMismatch("C_Δumin size must be $((nu*Hc,))"))
            any(<(0), C_Δumin) && error("C_Δumin weights should be non-negative")
            con.A_ΔŨmin[1:end-1, end] .= -C_Δumin 
        end
        if !isnothing(C_Δumax)
            size(C_Δumax) == (nu*Hc,) || throw(DimensionMismatch("C_Δumax size must be $((nu*Hc,))"))
            any(<(0), C_Δumax) && error("C_Δumax weights should be non-negative")
            con.A_ΔŨmax[1:end-1, end] .= -C_Δumax
        end
        if !isnothing(C_ymin)
            size(C_ymin) == (ny*Hp,) || throw(DimensionMismatch("C_ymin size must be $((ny*Hp,))"))
            any(<(0), C_ymin) && error("C_ymin weights should be non-negative")
            con.C_ymin .= C_ymin
            size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel
        end
        if !isnothing(C_ymax)
            size(C_ymax) == (ny*Hp,) || throw(DimensionMismatch("C_ymax size must be $((ny*Hp,))"))
            any(<(0), C_ymax) && error("C_ymax weights should be non-negative")
            con.C_ymax .= C_ymax
            size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel
        end
        if !isnothing(C_wmin)
            size(C_wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmin size must be $((nw*(Hp+1),))"))
            any(<(0), C_wmin) && error("C_wmin weights should be non-negative")
            con.C_wmin .= C_wmin
            con.A_Wmin[:, end] .= -C_wmin
        end
        if !isnothing(C_wmax)
            size(C_wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmax size must be $((nw*(Hp+1),))"))
            any(<(0), C_wmax) && error("C_wmax weights should be non-negative")
            con.C_wmax .= C_wmax
            con.A_Wmax[:, end] .= -C_wmax
        end
        if !isnothing(c_x̂min)
            size(c_x̂min) == (nx̂,) || throw(DimensionMismatch("c_x̂min size must be $((nx̂,))"))
            any(<(0), c_x̂min) && error("c_x̂min weights should be non-negative")
            con.c_x̂min .= c_x̂min
            size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel
        end
        if !isnothing(c_x̂max)
            size(c_x̂max) == (nx̂,) || throw(DimensionMismatch("c_x̂max size must be $((nx̂,))"))
            any(<(0), c_x̂max) && error("c_x̂max weights should be non-negative")
            con.c_x̂max .= c_x̂max
            size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
        end
    end
    i_Umin,  i_Umax  = .!isinf.(con.U0min), .!isinf.(con.U0max)
    i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax)
    i_Ymin,  i_Ymax  = .!isinf.(con.Y0min), .!isinf.(con.Y0max)
    i_Wmin,  i_Wmax  = .!isinf.(con.Wmin),  .!isinf.(con.Wmax)
    i_x̂min,  i_x̂max  = .!isinf.(con.x̂0min), .!isinf.(con.x̂0max)
    if notSolvedYet
        con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc(
            model, transcription, nc,
            i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
            i_Ymin, i_Ymax, i_Wmin, i_Wmax,
            i_x̂min, i_x̂max,
            con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, 
            con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax,
            con.A_x̂min, con.A_x̂max,
            con.Aeq
        )
        A = con.A[con.i_b, :]
        b = con.b[con.i_b]
        Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
        JuMP.delete(optim, optim[:linconstraint])
        JuMP.unregister(optim, :linconstraint)
        @constraint(optim, linconstraint, A*Z̃var .≤ b)
        reset_nonlincon!(mpc)
    else
        i_b, i_g = init_matconstraint_mpc(
            model, transcription, nc,
            i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
            i_Ymin, i_Ymax, i_Wmin, i_Wmax,
            i_x̂min, i_x̂max,
        )
        if i_b ≠ con.i_b || i_g ≠ con.i_g
            error("Cannot modify ±Inf constraints after calling moveinput!")
        end
    end
    return mpc
end

"By default, no nonlinear constraints, return nothing."
reset_nonlincon!(::PredictiveController) = nothing

"""
    default_Hp(model::LinModel)

Estimate the default prediction horizon `Hp` for [`LinModel`](@ref).
"""
default_Hp(model::LinModel) = DEFAULT_HP0 + estimate_delays(model)
"Throw an error when model is not a [`LinModel`](@ref)."
function default_Hp(::SimModel)
    msg = "Prediction horizon Hp must be explicitly specified if model is not a LinModel."
    throw(ArgumentError(msg))
end

"""
    estimate_delays(model::LinModel)

Estimate the number of delays in `model` with a security margin.
"""
function estimate_delays(model::LinModel)
    # TODO: also check for settling time (poles)
    # TODO: also check for non minimum phase systems (zeros)
    # TODO: replace sum with max delay between all the I/O
    # TODO: use this nk value for default N value in sim!
    poles = eigvals(model.A)
    # atol=1e-3 to overestimate the number of delays : for closed-loop stability, it is
    # better to overestimate the default value of Hp, as a security margin.
    nk = sum(isapprox.(abs.(poles), 0.0, atol=1e-3)) # number of delays
    return nk
end
"Return `0` when model is not a [`LinModel`](@ref)."
estimate_delays(::SimModel) = 0


@doc raw"""
    move_blocking(Hp::Int, Hc::Vector{Int}) -> nb

Get the move blocking vector `nb` from the `Hc` argument, and modify it to match `Hp`.

This feature is also known as manipulated variable blocking. The argument `Hc` is
interpreted as the move blocking vector `nb`. It specifies the length of each step (or
"block") in the ``\mathbf{ΔU}`` vector, to customize the pattern (in time steps, thus
strictly positive integers):
```math
    \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}'
```
Introducing the notation:
```math
j_ℓ =                                   \begin{cases}
    0                   & ℓ = 0         \\
    ∑_{i=1}^{ℓ} n_i     & ℓ > 0         \end{cases}
```
to convert from block lengths to discrete time steps, the vector that includes all the free
moves of the manipulated input is then defined as:
```math
\mathbf{ΔU} = \begin{bmatrix}
    \mathbf{Δu}(k + j_0)                      \\
    \mathbf{Δu}(k + j_1)                      \\
    \vdots                                    \\
    \mathbf{Δu}(k + j_{H_c-1})   
\end{bmatrix}
```
The provided `nb` vector is modified to ensure `sum(nb) == Hp`:
- If `sum(nb) < Hp`, a new element is pushed to `nb` with the value `Hp - sum(nb)`.
- If `sum(nb) > Hp`, the intervals are truncated until `sum(nb) == Hp`. For example, if
  `Hp = 10` and `nb = [1, 2, 3, 6, 7]`, then `nb` is truncated to `[1, 2, 3, 4]`.
""" 
function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int})
    Hp = Hp_arg
    nb = Hc_arg
    all(>(0), nb) || throw(ArgumentError("Move blocking vector must be strictly positive integers."))
    if sum(nb) < Hp
        newblock = [Hp - sum(nb)]
        nb = [nb; newblock]
    elseif sum(nb) > Hp
        nb = nb[begin:findfirst(≥(Hp), cumsum(nb))]
        if sum(nb) > Hp
            # if the last block is too large, it is truncated to fit Hp:
            nb[end] = Hp - @views sum(nb[begin:end-1])
        end
    end
    return nb
end

"""
    move_blocking(Hp::Int, Hc::Int) -> nb

Construct a move blocking vector `nb` that match the provided `Hp` and `Hc` integers.

The vector is filled with `1`s, except for the last element which is `Hp - Hc + 1`.
"""
function move_blocking(Hp_arg::Int, Hc_arg::Int)
    Hp, Hc = Hp_arg, Hc_arg
    nb = fill(1, Hc)
    if Hc > 0 # if Hc < 1, it will crash later with a clear error message
        nb[end] = Hp - Hc + 1
    end
    return nb
end

"Get the actual control Horizon `Hc` (integer) from the move blocking vector `nb`."
get_Hc(nb::AbstractVector{Int}) = length(nb)

"Validate the custom linear constraint matrices dimensions."
function validate_custom_lincon(model::SimModel{NT}, Wy, Wu, Wd, Wr) where NT<:Real
    nu, nd, ny = model.nu, model.nd, model.ny
    if !isa(model, LinModel) && !isnothing(Wy)
        throw(ArgumentError("Wy matrix can be specified only with LinModel."))
    end
    nw = if !isnothing(Wy)
        size(Wy, 1)
    elseif !isnothing(Wu)
        size(Wu, 1)
    elseif !isnothing(Wd)
        size(Wd, 1)
    elseif !isnothing(Wr)
        size(Wr, 1)
    else
        0
    end
    Wy = isnothing(Wy) ? zeros(NT, nw, ny) : Wy
    Wu = isnothing(Wu) ? zeros(NT, nw, nu) : Wu
    Wd = isnothing(Wd) ? zeros(NT, nw, nd) : Wd
    Wr = isnothing(Wr) ? zeros(NT, nw, ny) : Wr
    size(Wy, 2) == ny || throw(DimensionMismatch("Wy must have $ny columns."))
    size(Wu, 2) == nu || throw(DimensionMismatch("Wu must have $nu columns."))
    size(Wd, 2) == nd || throw(DimensionMismatch("Wd must have $nd columns."))
    size(Wr, 2) == ny || throw(DimensionMismatch("Wr must have $ny columns."))
    if size(Wy, 1) != nw || size(Wu, 1) != nw || size(Wd, 1) != nw || size(Wr, 1) != nw
        msg = "all custom linear constraint matrices must have the same number of rows."
        throw(DimensionMismatch(msg))
    end
    return Wy, Wu, Wd, Wr
end

"""
    validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u)

Check the dimensions of the arguments of [`moveinput!`](@ref).
"""
function validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u)
    ny, nd, nu, Hp = mpc.estim.model.ny, mpc.estim.model.nd, mpc.estim.model.nu, mpc.Hp
    size(ry) ≠ (ny,)    && throw(DimensionMismatch("ry size $(size(ry)) ≠ output size ($ny,)"))
    size(d)  ≠ (nd,)    && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)"))
    size(lastu) ≠ (nu,) && throw(DimensionMismatch("lastu size $(size(lastu)) ≠ manip. input size ($nu,)"))
    size(D̂)  ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)"))
    size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)"))
    size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
end

@doc raw"""
    init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu

Init decision variables to input increments over ``H_c`` conversion matrix `PΔu`.

The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input
increments over ``H_c``, is computed by:
```math
\mathbf{ΔU} = \mathbf{P_{Δu}} \mathbf{Z}
```

in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section.

# Extended Help
!!! details "Extended Help"
    Following the decision variable definition of the [`TranscriptionMethod`](@ref), the
    conversion matrix ``\mathbf{P_{Δu}}``, we have:
    - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref)
    - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise.
    The matrix is store as as `SparseMatrixCSC` to support both cases efficiently.
"""
function init_ZtoΔU(
    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc
) where {NT<:Real}
    I_nu_Hc = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc))
    nZ = get_nZ(estim, transcription, Hp, Hc)
    nΔU = estim.model.nu*Hc
    PΔu = [I_nu_Hc spzeros(NT, nΔU, nZ - nΔU)]
    return PΔu
end

@doc raw"""
    init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu

Init decision variables to inputs over ``H_p`` conversion matrices.

The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{U}``, the manipulated
inputs over ``H_p``, is computed by:
```math
\mathbf{U} = \mathbf{P_u} \mathbf{Z} + \mathbf{T_u} \mathbf{u}(k-1)
```
The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended Help section.

# Extended Help
!!! details "Extended Help"
    With ``n_i``, the ``i``th element of the ``\mathbf{n_b}`` vector defined in [`move_blocking`](@ref)
    documentation, we introduce the ``\mathbf{Q}(n_i)`` matrix of size `(nu*ni, nu)`:
    ```math
    \mathbf{Q}(n_i) =       \begin{bmatrix}
        \mathbf{I}          \\
        \mathbf{I}          \\
        \vdots              \\
        \mathbf{I}          \end{bmatrix}            
    ```
    The ``\mathbf{U}`` vector and the conversion matrices are defined as:
    ```math
    \mathbf{U} = \begin{bmatrix}
        \mathbf{u}(k + 0)                                                                   \\
        \mathbf{u}(k + 1)                                                                   \\
        \vdots                                                                              \\
        \mathbf{u}(k + H_p - 1)                                                             \end{bmatrix} , \quad
    \mathbf{P_u^†} = \begin{bmatrix}
        \mathbf{Q}(n_1)         & \mathbf{0}            & \cdots    & \mathbf{0}            \\
        \mathbf{Q}(n_2)         & \mathbf{Q}(n_2)       & \cdots    & \mathbf{0}            \\
        \vdots                  & \vdots                & \ddots    & \vdots                \\
        \mathbf{Q}(n_{H_c})     & \mathbf{Q}(n_{H_c})   & \cdots    & \mathbf{Q}(n_{H_c})   \end{bmatrix} , \quad
    \mathbf{T_u} = \begin{bmatrix}
        \mathbf{I}                                                                          \\
        \mathbf{I}                                                                          \\
        \vdots                                                                              \\
        \mathbf{I}                                                                          \end{bmatrix}
    ```
    and, depending on the transcription method, we have:
    - ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref)
    - ``\mathbf{P_u} = [\begin{smallmatrix}\mathbf{P_u^†} & \mathbf{0} \end{smallmatrix}]``
      if `transcription` is a [`MultipleShooting`](@ref)
    The conversion matrices are stored as `SparseMatrixCSC` since it was benchmarked that it
    is generally more performant than normal dense matrices, even for small `nu`, `Hp` and 
    `Hc` values. Using `Bool` element type and `BitMatrix` is also slower.
"""
function init_ZtoU(
    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb
) where {NT<:Real}
    nu = estim.model.nu
    I_nu = sparse(Matrix{NT}(I, nu, nu))
    PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc)
    for i=1:Hc
        ni    = nb[i]
        Q_ni  = repeat(I_nu, ni, 1)
        iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1])
        PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))]
    end
    PuDagger = sparse(PuDagger)
    nZ = get_nZ(estim, transcription, Hp, Hc)
    Pu = [PuDagger spzeros(NT, nu*Hp, nZ - nu*Hc)]
    Tu = repeat(I_nu, Hp)
    return Pu, Tu
end

@doc raw"""
    init_quadprog(
        model::LinModel, transcriptions::TranscriptionMethod, weights::ControllerWeights, 
        Ẽ, P̃Δu, P̃u; warn_cond=1e6
    ) -> H̃

Init the quadratic programming Hessian `H̃` for MPC.

The matrix appear in the quadratic general form:
```math
    J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r 
```
The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): 
```math
    \mathbf{H̃} = 2 (   \mathbf{Ẽ'}      \mathbf{M}_{H_p} \mathbf{Ẽ} 
                     + \mathbf{P̃_{Δu}'} \mathbf{Ñ}_{H_c} \mathbf{P̃_{Δu}} 
                     + \mathbf{P̃_{u}'}  \mathbf{L}_{H_p} \mathbf{P̃_{u}}     )
```
in which ``\mathbf{Ẽ}``, ``\mathbf{P̃_{Δu}}`` and ``\mathbf{P̃_{u}}`` matrices are defined
at [`relaxŶ`](@ref), [`relaxΔU`](@ref) and [`relaxU`](@ref) documentation, respectively. The
vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see
[`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
optimization but required to evaluate the minimal ``J`` value. A `@warn` will be displayed
if the condition number `cond(H̃) > warn_cond` and `transcription` is a `SingleShooting` 
(`warn_cond=Inf` for no warning).
"""
function init_quadprog(
    ::LinModel, transcription::TranscriptionMethod, weights::ControllerWeights, 
    Ẽ, P̃Δu, P̃u; warn_cond=1e6
)
    M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp
    H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + P̃Δu'*Ñ_Hc*P̃Δu + P̃u'*L_Hp*P̃u), :L)
    verify_cond(transcription, H̃, warn_cond)
    return H̃
end
"Return empty matrix if `model` is not a [`LinModel`](@ref)."
function init_quadprog(
    ::SimModel{NT}, ::TranscriptionMethod, ::ControllerWeights, args...; kwargs...
) where {NT<:Real}
    H̃ = Hermitian(zeros(NT, 0, 0), :L)
    return H̃
end

"Check the condition number of `H̃` and display a `@warn` if `cond(H̃) > warn_cond`."
function verify_cond(::SingleShooting, H̃, warn_cond)
    if !isinf(warn_cond)
        cond_H̃ = cond(H̃)
        cond_H̃ > warn_cond && @warn(
            "The Hessian condition number cond_H̃ > $warn_cond. The optimization "*
            "problem may be ill-conditioned.\n Consider changing the tunings, using "*
            "MultipleShooting, or using an optimizer more robust to this like DAQP.",
            cond_H̃,
        )
    end
    return nothing
end
"No check if `transcription` is not a `SingleShooting`."
verify_cond(::TranscriptionMethod,_,_) = nothing

"""
    init_defaultcon_mpc(
        estim::StateEstimator, 
        weights::ControllerWeights
        transcription::TranscriptionMethod,
        Hp, Hc, 
        PΔu, Pu, E, 
        ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
        ES, GS, JS, KS, VS, BS,
        Wy, Wu, Wd, Wr,
        gc!=nothing, nc=0
    ) -> con, nϵ, P̃Δu, P̃u, Ẽ

Init `ControllerConstraint` struct with default parameters based on estimator `estim`.

Also return `P̃Δu`, `P̃u` and `Ẽ` matrices for the the augmented decision vector `Z̃`.
"""
function init_defaultcon_mpc(
    estim::StateEstimator{NT}, 
    weights::ControllerWeights,
    transcription::TranscriptionMethod,
    Hp,  Hc, 
    PΔu, Pu, E, 
    ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
    ES, GS, JS, KS, VS, BS,
    Wy, Wu, Wd, Wr,
    gc!::GCfunc = nothing, nc = 0
) where {NT<:Real, GCfunc<:Union{Nothing, Function}}
    model = estim.model
    nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
    nw = size(Wy, 1)
    nW = nw*(Hp+1)
    nS = size(ES, 1)
    nϵ = weights.isinf_C ? 0 : 1
    u0min,      u0max   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
    Δumin,      Δumax   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
    y0min,      y0max   = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny)
    wmin,       wmax    = fill(convert(NT,-Inf), nw), fill(convert(NT,+Inf), nw)
    x̂0min,      x̂0max   = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂)
    c_umin,     c_umax  = fill(zero(NT), nu), fill(zero(NT), nu)
    c_Δumin,    c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu)
    c_ymin,     c_ymax  = fill(one(NT), ny),  fill(one(NT), ny)
    c_wmin,     c_wmax  = fill(one(NT), nw),  fill(one(NT), nw)
    c_x̂min,     c_x̂max  = fill(one(NT), nx̂),  fill(one(NT), nx̂)
    U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax = 
        repeat_constraints(
            Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max, wmin, wmax
        )
    C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax = 
        repeat_constraints(
            Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax
        )
    W̄y = sparse(repeatdiag(Wy, Hp+1))
    W̄u = sparse(repeatdiag(Wu, Hp+1))
    W̄d = sparse(repeatdiag(Wd, Hp+1))
    W̄r = sparse(repeatdiag(Wr, Hp+1))
    A_Umin,  A_Umax, P̃u  = relaxU(Pu, C_umin, C_umax, nϵ)
    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ)
    A_Ymin,  A_Ymax, Ẽ  = relaxŶ(E, C_ymin, C_ymax, nϵ)
    A_Wmin,  A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ)
    A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ)
    Aeq, ẼS = augmentdefect(ES, nϵ)
    i_Umin,  i_Umax  = .!isinf.(U0min), .!isinf.(U0max)
    i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
    i_Ymin,  i_Ymax  = .!isinf.(Y0min), .!isinf.(Y0max)
    i_Wmin,  i_Wmax  = .!isinf.(Wmin),  .!isinf.(Wmax)
    i_x̂min,  i_x̂max  = .!isinf.(x̂0min), .!isinf.(x̂0max)
    i_b, i_g, A, Aeq, neq = init_matconstraint_mpc(
        model, transcription, nc,
        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max,
        A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min,
        Aeq
    )
    # dummy fx̂, Fw and FS vectors (updated just before optimization)
    fx̂, Fw, FS = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nS)
    # dummy b and beq vectors (updated just before optimization)
    b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1))
    con = ControllerConstraint{NT, GCfunc}(
        ẽx̂      , fx̂     , gx̂      , jx̂       , kx̂     , vx̂     , bx̂     ,
        ẼS      , FS     , GS      , JS       , KS     , VS     , BS     ,
        Ẽw      , Fw     , W̄y      , W̄u       , W̄d     , W̄r     , nw     ,
        U0min   , U0max  , ΔŨmin   , ΔŨmax    , 
        Y0min   , Y0max  , Wmin    , Wmax     , x̂0min  , x̂0max  , 
        A_Umin  , A_Umax , A_ΔŨmin , A_ΔŨmax  , 
        A_Ymin  , A_Ymax , A_Wmin  , A_Wmax   , A_x̂min , A_x̂max , 
        A       , b      , i_b     , 
        Aeq     , beq    ,
        neq     ,
        C_ymin  , C_ymax , C_wmin  , C_wmax   , c_x̂min , c_x̂max , 
        i_g     ,
        gc!     , nc
    )
    return con, nϵ, P̃Δu, P̃u, Ẽ
end

"Repeat predictive controller constraints over their respective horizons."
function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax, wmin, wmax)
    Umin  = repeat(umin, Hp)
    Umax  = repeat(umax, Hp)
    ΔUmin = repeat(Δumin, Hc)
    ΔUmax = repeat(Δumax, Hc)
    Ymin  = repeat(ymin, Hp)
    Ymax  = repeat(ymax, Hp)
    Wmin  = repeat(wmin, Hp+1)
    Wmax  = repeat(wmax, Hp+1)
    return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax, Wmin, Wmax
end

@doc raw"""
    relaxU(Pu, C_umin, C_umax, nϵ) -> A_Umin, A_Umax, P̃u

Augment manipulated inputs constraints with slack variable ϵ for softening.

Denoting the decision variables augmented with the slack variable
``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the
augmented conversion matrix ``\mathbf{P̃_u}``, similar to the one described at
[`init_ZtoU`](@ref). It also returns the ``\mathbf{A}`` matrices for the inequality
constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{U_{min}}} \\ 
    \mathbf{A_{U_{max}}} 
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{U_{min} + T_u u}(k-1) \\
    + \mathbf{U_{max} - T_u u}(k-1)
\end{bmatrix}
```
in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains
``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times.
"""
function relaxU(Pu::AbstractMatrix{NT}, C_umin, C_umax, nϵ) where NT<:Real
    if nϵ == 1 # Z̃ = [Z; ϵ]
        # ϵ impacts Z → U conversion for constraint calculations:
        A_Umin, A_Umax = -[Pu  C_umin], [Pu -C_umax] 
        # ϵ has no impact on Z → U conversion for prediction calculations:
        P̃u = [Pu zeros(NT, size(Pu, 1))]
    else # Z̃ = Z (only hard constraints)
        A_Umin, A_Umax = -Pu,  Pu
        P̃u = Pu
    end
    return A_Umin, A_Umax, P̃u
end

@doc raw"""
    relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu

Augment input increments constraints with slack variable ϵ for softening.

Denoting the decision variables augmented with the slack variable 
``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the
augmented conversion matrix ``\mathbf{P̃_{Δu}}``, similar to the one described at
[`init_ZtoΔU`](@ref), but extracting the input increments augmented with the slack variable
``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}] = \mathbf{P̃_{Δu} Z̃}``.
Also, knowing that ``0 ≤ ϵ ≤ ∞``, it also returns the augmented bounds 
``\mathbf{ΔŨ_{min}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ 0 \end{smallmatrix}]`` and
``\mathbf{ΔŨ_{max}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ ∞ \end{smallmatrix}]``,
and the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{ΔŨ_{min}}} \\ 
    \mathbf{A_{ΔŨ_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{ΔŨ_{min}} \\
    + \mathbf{ΔŨ_{max}}
\end{bmatrix}
```
Note that strictly speaking, the lower bound on the slack variable ϵ is a decision variable
bound, which is more precise than a linear inequality constraint. However, it is more
convenient to treat it as a linear inequality constraint since the optimizer `OSQP.jl` does
not support pure bounds on the decision variables.
"""
function relaxΔU(PΔu::AbstractMatrix{NT}, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) where NT<:Real
    nZ = size(PΔu, 2)
    if nϵ == 1 # Z̃ = [Z; ϵ]
        ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] # 0 ≤ ϵ ≤ ∞
        A_ϵ = [zeros(NT, 1, nZ) NT[1.0]]
        A_ΔŨmin, A_ΔŨmax = -[PΔu  C_Δumin; A_ϵ], [PΔu -C_Δumax; A_ϵ]
        P̃Δu = [PΔu zeros(NT, size(PΔu, 1), 1); zeros(NT, 1, size(PΔu, 2)) NT[1.0]]
    else # Z̃ = Z (only hard constraints)
        ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
        A_ΔŨmin, A_ΔŨmax = -PΔu,  PΔu
        P̃Δu = PΔu
    end
    return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu
end

@doc raw"""
    relaxŶ(E, C_ymin, C_ymax, nϵ) -> A_Ymin, A_Ymax, Ẽ

Augment linear output prediction constraints with slack variable ϵ for softening.

Denoting the decision variables augmented with the slack variable 
``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the 
``\mathbf{Ẽ}`` matrix that appears in the linear model prediction equation 
``\mathbf{Ŷ_0 = Ẽ Z̃ + F}``, and the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{Y_{min}}} \\ 
    \mathbf{A_{Y_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{(Y_{min} - Y_{op}) + F} \\
    + \mathbf{(Y_{max} - Y_{op}) - F} 
\end{bmatrix}
```
in which ``\mathbf{Y_{min}, Y_{max}}`` and ``\mathbf{Y_{op}}`` vectors respectively contains
``\mathbf{y_{min}, y_{max}}`` and ``\mathbf{y_{op}}`` repeated ``H_p`` times.
"""
function relaxŶ(E::AbstractMatrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real
    if nϵ == 1 # Z̃ = [Z; ϵ]
        if iszero(size(E, 1))
            # model is not a LinModel, thus Ŷ constraints are not linear:
            C_ymin = C_ymax = zeros(NT, 0, 1)
        end
        # ϵ impacts predicted output constraint calculations:
        A_Ymin, A_Ymax = -[E  C_ymin], [E -C_ymax] 
        # ϵ has no impact on output predictions:
        Ẽ = [E zeros(NT, size(E, 1), 1)] 
    else # Z̃ = Z (only hard constraints)
        Ẽ = E
        A_Ymin, A_Ymax = -E,  E
    end
    return A_Ymin, A_Ymax, Ẽ
end

@doc raw"""
    relaxW(E, Pu, nw, W̄y, W̄u, C_wmin, C_wmax, nϵ) -> A_Wmin, A_Wmax, Ẽw

Construct and augment the custom linear constraints with slack variable ϵ for softening.

By introducing the following block-diagonal matrices with ``H_p + 1`` blocks:
```math
\begin{aligned}
\mathbf{W̄_y} &= \mathrm{diag}(\mathbf{W_y}, \mathbf{W_y}, \dots, \mathbf{W_y}) \\
\mathbf{W̄_u} &= \mathrm{diag}(\mathbf{W_u}, \mathbf{W_u}, \dots, \mathbf{W_u}) \\
\mathbf{W̄_d} &= \mathrm{diag}(\mathbf{W_d}, \mathbf{W_d}, \dots, \mathbf{W_d}) \\
\mathbf{W̄_r} &= \mathrm{diag}(\mathbf{W_r}, \mathbf{W_r}, \dots, \mathbf{W_r}) 
\end{aligned}
```
the ``\mathbf{W}`` vector defined in the Extended Help section of [`setconstraint!`](@ref)
can be expressed as:
```math
\mathbf{W} = \mathbf{E_w} \mathbf{Z} + \mathbf{F_w}
```
in which:
```math
\mathbf{E_w} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{0}   \\ \mathbf{E}   \end{smallmatrix}] + 
               \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}]
```
The ``\mathbf{E}`` matrix appears in the linear output prediction equation 
``\mathbf{Ŷ_0 = E Z + F}``, and ``\mathbf{P_u}``, in the conversion equation 
``\mathbf{U = P_u Z + T_u u}(k-1)``. The ``\mathbf{p_u}`` matrix corresponds to the last
`nu` rows of ``\mathbf{P_u}``. The ``\mathbf{W̄_u}`` term assumes that ``H_c ≤ H_p``, hence
``\mathbf{Δu}(k + H_c) = \mathbf{0}`` and ``\mathbf{u}(k + H_c) = \mathbf{u}(k + H_c - 1)``.
The ``\mathbf{F_w}`` vector is updated at each control period `k` in [`linconstraint!`](@ref)
method, and is defined as:
```math
\mathbf{F_w} =  
    \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{ŷ}(k)       \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + 
    \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1)     \end{smallmatrix}] +
    \mathbf{W̄_d} [\begin{smallmatrix} \mathbf{d}(k)       \\ \mathbf{D̂}          \end{smallmatrix}] +
    \mathbf{W̄_r} [\begin{smallmatrix} \mathbf{r_y}(k)     \\ \mathbf{R̂_y}        \end{smallmatrix}]
```
Denoting the decision variables augmented with the slack variable
``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, the function
returns the ``\mathbf{Ẽ_w}`` matrix that appears in the custom constraint equation
``\mathbf{W = Ẽ_w Z̃ + F_w}``, and the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{W_{min}}} \\ 
    \mathbf{A_{W_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{W_{min} + F_w} \\
    + \mathbf{W_{max} - F_w}
\end{bmatrix}
```
"""
function relaxW(E::AbstractMatrix{NT}, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) where {NT<:Real}
    nW = size(W̄y, 1)
    ny = size(W̄y, 2) ÷ (Hp + 1)
    nu = size(Pu, 1) ÷ Hp    
    if iszero(size(E, 1))
        # model is not a LinModel, thus no Wy terms in the custom constraints:
        Wy_terms = zeros(NT, nW, size(E, 2))
    else
        Wy_terms = W̄y*[zeros(NT, ny, size(E, 2)); E]
    end
    Wu_terms = W̄u*[Pu; Pu[end-nu+1:end, :]]
    Ew = Wy_terms + Wu_terms
    if nϵ == 1 # Z̃ = [Z; ϵ]
        # ϵ impacts custom constraint calculations:
        A_Wmin, A_Wmax = -[Ew  C_wmin], [Ew -C_wmax]
        # ϵ has no impact on custom constraint predictions:
        Ẽw = [Ew zeros(NT, nW, 1)]
    else # Z̃ = Z (only hard constraints)
        Ẽw = Ew
        A_Wmin, A_Wmax = -Ew, Ew
    end
    return A_Wmin, A_Wmax, Ẽw
end

@doc raw"""
    relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) -> A_x̂min, A_x̂max, ẽx̂

Augment terminal state constraints with slack variable ϵ for softening.

Denoting the decision variables augmented with the slack variable 
``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the 
``\mathbf{ẽ_{x̂}}`` matrix that appears in the terminal state equation 
``\mathbf{x̂_0}(k + H_p) = \mathbf{ẽ_x̂ Z̃ + f_x̂}``, and the ``\mathbf{A}`` matrices for 
the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{x̂_{min}}} \\ 
    \mathbf{A_{x̂_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{(x̂_{min} - x̂_{op}) + f_x̂} \\
    + \mathbf{(x̂_{max} - x̂_{op}) - f_x̂}
\end{bmatrix}
```
"""
function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where {NT<:Real}
    if nϵ == 1 # Z̃ = [Z; ϵ]
        if iszero(size(ex̂, 1))
            # model is not a LinModel and transcription is a SingleShooting, thus terminal
            # state constraints are not linear:
            c_x̂min = c_x̂max = zeros(NT, 0, 1)
        end
        # ϵ impacts terminal state constraint calculations:
        A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max]
        # ϵ has no impact on terminal state predictions:
        ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] 
    else # Z̃ = Z (only hard constraints)
        ẽx̂ = ex̂
        A_x̂min, A_x̂max = -ex̂,  ex̂
    end
    return A_x̂min, A_x̂max, ẽx̂
end

@doc raw"""
    augmentdefect(ES, nϵ) -> Aeq, ẼS

Augment defect equality constraints with slack variable ϵ if `nϵ == 1`.

It returns the ``\mathbf{Ẽ_S}`` matrix that appears in the linear defect equation 
``\mathbf{Ẽ_S Z̃ + F_S}`` and the ``\mathbf{A}`` matrix for the equality constraints:
```math
\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S}
```
"""
function augmentdefect(ES::AbstractMatrix{NT}, nϵ) where NT<:Real
    if nϵ == 1 # Z̃ = [Z; ϵ]
        ẼS = [ES zeros(NT, size(ES, 1), 1)]
    else # Z̃ = Z (only hard constraints)
        ẼS = ES
    end
    Aeq = ẼS
    return Aeq, ẼS
end

@doc raw"""
    init_stochpred(estim::InternalModel, Hp) -> Ks, Ps

Init the stochastic prediction matrices for [`InternalModel`](@ref).

`Ks` and `Ps` matrices are defined as:
```math
    \mathbf{Ŷ_s} = \mathbf{K_s x̂_s}(k) + \mathbf{P_s ŷ_s}(k)
```
Current stochastic outputs ``\mathbf{ŷ_s}(k)`` comprises the measured outputs 
``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and unmeasured 
``\mathbf{ŷ_s^u}(k) = \mathbf{0}``. See [^2].

[^2]: Desbiens, A., D. Hodouin & É. Plamondon. 2000, "Global predictive control: a unified
    control structure for decoupling setpoint tracking, feedforward compensation and 
    disturbance rejection dynamics", *IEE Proceedings - Control Theory and Applications*, 
    vol. 147, no 4, <https://doi.org/10.1049/ip-cta:20000443>, p. 465–475, ISSN 1350-2379.
"""
function init_stochpred(estim::InternalModel{NT}, Hp) where NT<:Real
    As, B̂s, Cs = estim.As, estim.B̂s, estim.Cs
    ny  = estim.model.ny
    nxs = estim.nxs
    Ks = Matrix{NT}(undef, ny*Hp, nxs)
    Ps = Matrix{NT}(undef, ny*Hp, ny)
    for i = 1:Hp
        iRow = (1:ny) .+ ny*(i-1)
        Ms = Cs*As^(i-1)*B̂s
        Ks[iRow,:] = Cs*As^i - Ms*Cs
        Ps[iRow,:] = Ms
    end
    return Ks, Ps 
end

"Return empty matrices if `estim` is not a [`InternalModel`](@ref)."
function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real
    return zeros(NT, 0, estim.nxs), zeros(NT, 0, estim.model.ny)
end