function [As,Bs,Cs,Ds] = pfcblock_utilAugmentVehicleMdlHasSpacingCtrl(Ag,Bg,Cg,Vx)
%#codegen
%
% Augment vehicle model to MPC model. This function is called by "Path
% Following Control System" block. 
%
% Vehicle model state-space matrices (Ag,Bg,Cg). 
% Vehicle longitudinal velocity Vx. 
% MPC model with spacing control state-space matrices (As,Bs,Cs,Ds). 

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

%% Add feedthrough terms of measured disturbance (Gm: 4*5 model)
NumEgoStates = size(Bg,1); % number of states in vehicle model
Am = Ag;
Bm = [Bg,zeros(NumEgoStates,2)];
Cm = [Cg;zeros(2,NumEgoStates)];
Dm = [zeros(3,4);zeros(2),eye(2)];

%% Model with spacing control outputs (Gp: 5*4 model)
Ap = zeros(3);
Ap(2,3) = Vx;
Bp = [-1,0,0,1,0;...
      0,1,0,0,0;...
      0,0,1,0,-1];
Cp = zeros(4,3);
Cp(1,1) = 1;
Cp(3,2) = 1;
Cp(4,3) = 1;
Dp = zeros(4,5);
Dp(2,1) = 1;

%% MPC internal model with spacing control (Gs = Gm*Gp: 4*4 model)
As = [Am,zeros(NumEgoStates,3);Bp*Cm,Ap];
Bs = [Bm;Bp*Dm];
Cs = [Dp*Cm,Cp];
Ds = Dp*Dm;