function [mpcobj,hasSpacingCtrl,UseExternalModel,DataType,NumEgoStates,hasLag] = ...
    pfcblock_MaskInitFcn(blk,EgoModelMatrixA,EgoModelMatrixB,EgoModelMatrixC,...
                         umax,umin,amax,amin,Vx0,L0,...
                         psteps,ControlHorizon,Ts,maxiter,Wa,Wu,Wv,We,...
                         m,Iz,lf,lr,Cf,Cr,lag,tau)
% Mask initialization function for the Path Following Control System block.
%
% Inputs:
%   EgoModelMatrix A,B,C:    continuous time model from long. acceleration, steering angle to long. velocity, lateral velocity and yaw angle rate
%   umax, umin:              maximum steering angle, minimum steering angle
%   amax, amin:              maximum long. acceleration, minimum long. acceleration
%   Vx0:                     initial longitudinal velocity
%   L0:                      default spacing when long. velocity is zero
%   psteps:                  prediction horizon (# of sampling steps)
%   ControlHorizon:          control horizon
%   Ts:                      sample time
%   maxiter:                 maximum iteration number for MPC optimization
%   Wa:                      weight on change of long. acceleration
%   Wu:                      weight on change of steering angle
%   Wv:                      weight on long. velocity tracking
%   We:                      weight on lateral error
%   m,Iz,lf,lr,Cf,Cr:        vehicle parameters for lateral dynamics
%                         m: vehicle mass 
%                        Iz: yaw moment of inertia
%                        lf: longitudinal distance to the front axis
%                        lr: longitudinal distance to the rear axix
%                        Cf: cornering stiffness of front tires
%                        Cr: cornering stiffness of rear tires
%  lag:                      (lumped) transport lag caused by actuator, sensor and communication lags
%  tau:                      time constant for long. acceleration tracking
% Outputs:
%   mpcobj:                  MPC object under the mask
%   hasSpacingCtrl:          zero or one, indicating if spacing control is enabled
%   UseExternalModel:        zero or one, indicating if an external vehicle model is prodived
%   DataType:                data type
%   NumEgoStates:            number of states for ego lateral dynamics
%   hasLag:                  zero or one, indicating if lag dynamics is included                  

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

%% Check parameter validity
pfcblock_utilParameterCheck(blk); 

%% Set variant for MPC models and MPC controllers
mdlVairants = [blk '/MPC Models'];
ctrlVariants = [blk '/MPC Controllers'];
if strcmp(get_param(blk,'spaceCtrl'),'off')
    % no spacing control
    hasSpacingCtrl = false;
    set_param(mdlVairants, 'OverrideUsingVariant', 'pathFollowingNoSpacingCtrl');
    set_param(ctrlVariants, 'OverrideUsingVariant', 'pathFollowingNoSpacingCtrl');
else
    % has spacing control
    hasSpacingCtrl = true;
    set_param(mdlVairants, 'OverrideUsingVariant', 'pathFollowingHasSpacingCtrl');
    set_param(ctrlVariants, 'OverrideUsingVariant', 'pathFollowingHasSpacingCtrl');
end

% handling lag
hasLag = (lag>0);

% Update mask ports for spacing control
updateSpacingCtrlPorts(blk,hasSpacingCtrl);

%% Vehicle dynamics
UseExternalModel = false;
vehMdlVairants = [blk '/Vehicle Models'];
% longitudinal dynamics
[A1,B1,C1] = adasblocks_utilGetLongVehModelFromParam(tau);
% lateral dynamics
[A2,B2,C2] = adasblocks_utilGetLatVehModelFromParam(m,Iz,lf,lr,Cf,Cr,Vx0);
% combine longitudinal and lateral dynamics
[Ag,Bg,Cg] = append(A1,B1,C1,0,A2,B2,C2,zeros(2,1));
% update vehicle info
if strcmp(get_param(blk,'ModelType'),slsvInternal('slsvGetEnStringFromCatalog','MPC:adasblocks:promptPFCUseVehicleModel'))
    UseExternalModel = true;
    set_param(vehMdlVairants, 'OverrideUsingVariant', 'useVehicleModel');
    Ag = EgoModelMatrixA;
    Bg = EgoModelMatrixB;
    Cg = EgoModelMatrixC;
else
    set_param(vehMdlVairants, 'OverrideUsingVariant', 'useVehicleParameters');
end

% number of states for vehicle model
NumEgoStates = size(Bg,1); 

%% Set MPC block data type
DataType = get_param(blk,'datatypeopt');
% In the variant with spacing control
pfSpacingController = [blk '/MPC Controllers/Path Following Controller with Spacing Control'];
set_param([pfSpacingController '/Adaptive MPC Controller'],'BlockDataType',DataType);
% In the vairant without spacing control
pfController = [blk '/MPC Controllers/Path Following Controller'];
set_param([pfController '/Adaptive MPC Controller'],'BlockDataType',DataType);

%% Update ports for the mask
updateOptionalPorts(blk,DataType,hasSpacingCtrl,UseExternalModel,NumEgoStates);

%% Define internal model for MPC design
if hasSpacingCtrl
    % Convert vehicle model to MPC model with spacing control
    [As,Bs,Cs,Ds] = pfcblock_utilGetDiscreteModelForMPCHasSpacingCtrl(Ts,Vx0,Ag,Bg,Cg,lag,hasLag);
    % Internal model for for MPC design
    sys1 = ss(As,Bs,Cs,Ds,Ts);
    % Generate MPC controller under the mask
    mpcobj = generateMPCHasSpacingCtrl(sys1,umax,umin,amax,amin,Vx0,L0,psteps,Ts,Wa,Wu,Wv,We,ControlHorizon);
else
    % Convert vehicle model to MPC model with no spacing control
    [A,B,C,D] = pfcblock_utilGetDiscreteModelForMPCNoSpacingCtrl(Ts,Vx0,Ag,Bg,Cg,lag,hasLag);
    % Internal model for for MPC design
    sys2 = ss(A,B,C,D,Ts);
    % Generate MPC controller under the mask
    mpcobj = generateMPCNoSpacingCtrl(sys2,umax,umin,amax,amin,Vx0,psteps,Ts,Wa,Wu,Wv,We,ControlHorizon);
end

%% Use suboptimal solution for MPC
if strcmp(get_param(blk,'suboptimal'),'on')
    mpcobj.Optimizer.UseSuboptimalSolution = 1;
    mpcobj.Optimizer.ActiveSetOptions.MaxIterations = maxiter;
end

%% Update MPC block for external MV
if strcmp(get_param(blk,'trackmode'),'on')
    set_param([pfSpacingController '/Adaptive MPC Controller'],'mv_inport','on');
    set_param([pfController '/Adaptive MPC Controller'],'mv_inport','on');
    connectMPCextmv(pfSpacingController);
    connectMPCextmv(pfController);    
else
    disconnectMPCextmv(pfSpacingController);
    disconnectMPCextmv(pfController);
    set_param([pfSpacingController '/Adaptive MPC Controller'],'mv_inport','off');
    set_param([pfController '/Adaptive MPC Controller'],'mv_inport','off');
end



%-------------------Local functions-------------------------
%
%% Local function: generate MPC controller with spacing control
function  mpc1 = generateMPCHasSpacingCtrl(sys,umax,umin,amax,amin,v0_ego,default_spacing,psteps,Ts,Wa,Wu,Wv,We,ControlHorizon)

status = mpcverbosity('off');

% Specify the MPC signal types in the plant.
sys = setmpcsignals(sys,'MV',[1,2],'MD',[3,4]); % set signal types
mpc1 = mpc(sys,Ts); % start mpc design

% Specify the prediction horizon.
mpc1.PredictionHorizon = psteps;
% Specify the control horizon.
mpc1.ControlHorizon = ControlHorizon; %round(max(0.1*mpc1.PredictionHorizon,2));

%% Specify the nominal values based on the simulation initial conditions.
%  plant inputs at operating point
mpc1.Model.Nominal.U(3) = v0_ego;                               % lead car velocity
%  plant outputs at operating point
mpc1.Model.Nominal.Y(1) = 1.4*v0_ego + default_spacing;         % relative distance (use time gap 1.4 sec)
mpc1.Model.Nominal.Y(2) = v0_ego;                               % longitudinal velocity

%% Specify scale factors based on the operating ranges of the variables
%--- Normalize dynamic ranges to improve optimization
mpc1.MV(1).ScaleFactor = amax - amin;     % acceleration
mpc1.MV(2).ScaleFactor = umax - umin;     % steering

mpc1.OV(1).ScaleFactor = 50;              % relative distance
mpc1.OV(2).ScaleFactor = 30;              % ego longitudinal velocity
mpc1.OV(3).ScaleFactor = 1;               % lateral deviation
mpc1.OV(4).ScaleFactor = 0.2;             % yaw angle deviation

mpc1.DV(1).ScaleFactor = 30;              % lead car velocity
mpc1.DV(2).ScaleFactor = 0.1;             % road yaw angle rate = v0_ego*curvature

%% Specify constraints
mpc1.MV(1).Min = amin;                         % min acceleration 
mpc1.MV(1).Max = amax;                         % max acceleration
mpc1.MV(2).Min = umin;                         % min steering
mpc1.MV(2).Max = umax;                         % max steering
mpc1.OV(1).Min = 1.4*v0_ego + default_spacing; % safe distance (use time gap 1.4 sec)

%% Specify weights for optimization
alpha = 1; % non-zero
mpc1.Weights.MVRate = [Wa Wu]*alpha;    % robustness for acceleration and steering
mpc1.Weights.OV = [0 Wv We 0]/alpha;    % outputs = [relative distance, ego velocity,
                                        %            lateral deviation, yaw angle deviation]
mpcverbosity(status);

%% Local function: generate MPC controller with no spacing control
function  mpc1 = generateMPCNoSpacingCtrl(sys,umax,umin,amax,amin,v0_ego,psteps,Ts,Wa,Wu,Wv,We,ControlHorizon)

status = mpcverbosity('off');

%% Specify the MPC signal types in the plant.
sys = setmpcsignals(sys,'MV',[1,2],'MD',3); % accleration, steering and desired yaw rate
mpc1 = mpc(sys,Ts);

%% Specify prediction and control horizon.
% Specify the prediction horizon.
mpc1.PredictionHorizon = psteps;
% Specify the control horizon.
mpc1.ControlHorizon = ControlHorizon;% round(max(0.1*mpc1.PredictionHorizon,2));

%% Specify constraints
mpc1.MV(1).Min = amin; % min acceleration 
mpc1.MV(1).Max = amax; % max acceleration
mpc1.MV(2).Min = umin; % min steering
mpc1.MV(2).Max = umax; % max steering

%% Specify nominal values.
mpc1.Model.Nominal.Y(1) = v0_ego; % longitudinal velocity

%% Specify scale factors based on the operating ranges of the variables
%--- Normalize dynamic ranges to improve optimization
mpc1.MV(1).ScaleFactor = amax - amin;     % acceleration
mpc1.MV(2).ScaleFactor = umax - umin;     % steering

mpc1.OV(1).ScaleFactor = 30;              % Typical value of longitudinal velocity
mpc1.OV(2).ScaleFactor = 1;               % Range for lateral deviation
mpc1.OV(3).ScaleFactor = 0.2;             % Range for relative yaw angle

mpc1.DV.ScaleFactor = 0.1;                % Range of deried yaw rate


%% Specify weights for optimization
alpha = 1; % non-zero
mpc1.Weights.MVRate = [Wa Wu]*alpha;  % robustness for acceleration and steering
mpc1.Weights.OV = [Wv We 0]/alpha;    % outputs = [ego velocity,lateral deviation, yaw angle deviation]

mpcverbosity(status);

%% Local function: update mask inports/outports
function updateOptionalPorts(blk,DataType,hasSpacingCtrl,UseExternalModel,NumEgoStates)
% Update optional inports and outports
inport_names = {...
    'Minimum longitudinal acceleration',...
    'Maximum longitudinal acceleration',...
    'Minimum steering angle',...
    'Maximum steering angle',...
    'Enable optimization',...
    'External control signal',...
    };
AvailableBlocks = get_param(blk,'Blocks');
% what exists in the block (old)
type_in_block = ismember(inport_names,AvailableBlocks);
% what is specified in the dialog (new)
type_in_mask = [...
    strcmp(get_param(blk,'amin_ext'),'on'), ...
    strcmp(get_param(blk,'amax_ext'),'on'), ...
    strcmp(get_param(blk,'umin_ext'),'on'), ...
    strcmp(get_param(blk,'umax_ext'),'on'), ...
    strcmp(get_param(blk,'optmode'),'on'),...
    strcmp(get_param(blk,'trackmode'),'on'), ...
    ];
% Update Inport
port = 9;     % default has 8 inports. Optional ports start at 9.
if ~hasSpacingCtrl
    port = 6; % default has 5 inports. Optional ports start at 6. 
end
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},port,1,'MinAcceleration',DataType);
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},port,2,'MaxAcceleration',DataType);
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},port,3,'MinSteering',DataType);
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,4,inport_names{4},port,4,'MaxSteering',DataType);
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,5,inport_names{5},port,5,'1',DataType);
port = adasblocks_utilUpdateInport(blk,'Data Processing for MPC Controller',AvailableBlocks,type_in_mask,type_in_block,6,inport_names{6},port,6,'[0;0]',DataType);
% Update dynamis ports (A, B, C)
updateExternalModelPorts(blk,UseExternalModel,port)
% Update dimensions for ports (A,B,C)
if UseExternalModel
    updatePortsDimensions(blk,NumEgoStates);
end

%% Local function: connect ext.mv for MPC
function connectMPCextmv(subsystemblk)
name = 'Track mv terminator';
AvailableBlocks = get_param(subsystemblk,'Blocks');
if ismember(name,AvailableBlocks)
    termblk = [subsystemblk '/' name];
    % delete line to terminator block
    delete_line(subsystemblk,['mpc_ext_mv' '/1'],[name '/1']);
    % delete terminator block
    delete_block(termblk);
    % connect to ext.mv port
    add_line(subsystemblk,['mpc_ext_mv' '/1'],['Adaptive MPC Controller' '/5']);
end

%% Local function: disconnect ext.mv for MPC
function disconnectMPCextmv(subsystemblk)
if strcmp(get_param([subsystemblk '/Adaptive MPC Controller'],'mv_inport'),'on')
    % delete line to MPC block
    delete_line(subsystemblk,['mpc_ext_mv' '/1'],['Adaptive MPC Controller' '/5']);
    % add terminator block
    name = 'Track mv terminator';
    termblk = [subsystemblk '/' name];
    add_block('built-in/Terminator',termblk);
    % connect to ext.mv port
    add_line(subsystemblk,['mpc_ext_mv' '/1'],[name '/1']);
    % set position of the terminator block
    pos = get_param([subsystemblk '/' 'mpc_ext_mv'],'position');
    newpos = pos + [70 -18 90 18];
    set_param(termblk,'position',newpos);
end

%% Local function: update spacing control ports
function updateSpacingCtrlPorts(blk,hasSpacingCtrl)
% Update optional inports and outports
inport_names = {...
    'Relative distance',...
    'Time gap',...
    'Relative velocity',...
    };
AvailableBlocks = get_param(blk,'Blocks');
% what exists in the block (old)
type_in_block = ismember(inport_names,AvailableBlocks);
% what is specified in the dialog (new)
type_in_mask = ones(1,3)*hasSpacingCtrl;
% Update Inport
port = 1;
port = pfcblock_updateGroundInport(blk,'MPC Controllers',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},2,2);
port = pfcblock_updateGroundInport(blk,'MPC Controllers',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},3,1);
port = pfcblock_updateGroundInport(blk,'MPC Controllers',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},4,3);

%% Local function: update spacing control ports
function updateExternalModelPorts(blk,UseExternalModel,port)
% Update optional inports and outports
inport_names = {...
    'Vehicle dynamics matrix A',...
    'Vehicle dynamics matrix B',...
    'Vehicle dynamics matrix C',...
    };
AvailableBlocks = get_param(blk,'Blocks');
% what exists in the block (old)
type_in_block = ismember(inport_names,AvailableBlocks);
% what is specified in the dialog (new)
type_in_mask = ones(1,3)*UseExternalModel;
% Update Inport
port = pfcblock_updateGroundInport(blk,'Vehicle Models',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},port,1);
port = pfcblock_updateGroundInport(blk,'Vehicle Models',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},port,2);
port = pfcblock_updateGroundInport(blk,'Vehicle Models',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},port,3);

%% Local function: set PortDimensions for dynamics ports (A,B,C)
function updatePortsDimensions(blk,NumEgoStates)
PortAName = [blk '/' 'Vehicle dynamics matrix A'];
PortBName = [blk '/' 'Vehicle dynamics matrix B'];
PortCName = [blk '/' 'Vehicle dynamics matrix C'];
set_param(PortAName,'PortDimensions',['[' num2str(NumEgoStates) ',' num2str(NumEgoStates) ']']);
set_param(PortBName,'PortDimensions',['[' num2str(NumEgoStates) ',' '2' ']']);
set_param(PortCName,'PortDimensions',['[' '3' ',' num2str(NumEgoStates) ']']);









