function [A,B,C,D] = lkablock_utilGetDTModelHasLagForMPC(Ts,Vx,Ag,Bg,Cg,lag)
%#codegen
%
% Get discrete state-space model for Adaptive MPC. Called by the "Lane
% Keeping Assist System" Block.

% Inputs:
%   Ts:                mask variable for sample time
%   Vx:                mask inport for longitudinal velocity
%   Ag,Bg,Cg:          continuous or discrete vehicle model
%   lag:               lumped lag in the model

% Outputs:
%   A,B,C,D:           discrete time model for MPC

%   Author: Meng Xia, Rong Chen
%   Copyright 2017-2021 The MathWorks, Inc.

% Get full model for MPC
%   Vehicle model (continuous or discrete) with input delta (steering
%   angle) and outputs [vy,phidot] (lateral velocity and yaw angle rate).
%   The first step is to get model Gp from input [delta,phidotdes] to
%   [vy,phidot,phidotdes] with the desired yaw angle rate phidotdes.
%   The second step is to get continuous model Ge from
%   [vy,phidot,phidotdes] to [e1,e2] (lateral error and yaw angle error).
%   If discrete-time vehicle model is specified, then Ge need to be
%   discretized as well.
%   The overall model Gf is the series connection of Gp and Ge, from input
%   [delta,phidotdes] to output [e1,e2]. If continuous-time vehicle is
%   specified, then the model Gf need to be discretized as well. 

% Augement the vehicle model into the MPC internal model
[Af,Bf] = getFullModel(Ag,Bg,Cg,Vx);
nx = size(Bg,1); 
Cf = [zeros(2,nx),eye(2)];
Df = zeros(2);
% Add lag dynamics when vehicle model specied by parameters
[Ac,Bc,Cc,Dc] = addLagModel(Af,Bf,Cf,Df,lag);
% Convert continuous to discrete time
[A, B] = adasblocks_utilDicretizeModel(Ac, Bc, Ts);
C = Cc;
D = Dc;

%% local functions
% Augement the vehicle model into the MPC internal model
function [Af,Bf] = getFullModel(Ag,Bg,Cg,Vx)
% dynamics for Gp from [delta,phidotdes] to [vy,phidot,phidotdes]
Ap = Ag;
n = size(Bg,1);                                                
Bp = [Bg,zeros(n,1)];
Cp = [Cg;zeros(1,n)];
Dp = [zeros(2);0,1];

% continuous dynamics for Ge from [vy,phidot,phidotdes] to [e1,e2]
Ae = [0,Vx;0,0];
Be = [1,0,0;0,1,-1];

% series connection of Gp and Ge
Af = [Ap,zeros(n,2);Be*Cp,Ae];
Bf = [Bp;Be*Dp];

% Augment the lag dynamics
function [Az,Bz,Cz,Dz] = addLagModel(Af,Bf,Cf,Df,lag)
% lag dynamics Gl
Al = -1/lag*eye(2);
Bl = 1/lag*eye(2);
Cl = eye(2);
Dl = zeros(2);

% series connection of Gf and and Gl
n = size(Bf,1);                                                
Az = [Af,zeros(n,2);Bl*Cf,Al];
Bz = [Bf;Bl*Df];
Cz = [Dl*Cf,Cl];
Dz = Dl*Df;

