function [mpcobj,UseExternalModel,DataType,NumEgoStates,hasLag] = ...
    lkablock_MaskInitFcn(blk,EgoModelMatrixA,EgoModelMatrixB,EgoModelMatrixC,...
                         umax,umin,Vx0,psteps,Ts,maxiter,weightratio,m,Iz,lf,lr,Cf,Cr,lag)
% Mask initialization function for the Lane Keeping Assist System block.
%
% Inputs:
%   EgoModelMatrix A,B,C:    continuous time model from steering angle to lateral velocity and yaw angle rate
%   umax, umin:              maximum steering angle, minimum steering angle
%   Vx0:                     initial longitudinal velocity
%   psteps:                  prediction horizon (# of sampling steps)
%   Ts:                      sample time
%   maxiter:                 maximum iteration number for MPC optimization
%   weightratio:             balancing factor for controller behavior (between 0 and 1, 0.5 for balanced)
%   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
% Outputs:
%   mpcobj:                  MPC object under the mask
%   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.

%% Slider text under slider
maskObj = Simulink.Mask.get(blk);
object = maskObj.getDialogControl('SliderText');
object.Prompt = [getString(message('MPC:adasblocks:promptLKAWeightTextLeft')),blanks(12),...
                getString(message('MPC:adasblocks:promptLKAWeightTextRight')),blanks(12)];
            
%% Check parameter validity
lkablock_utilParameterCheck(blk);

%% Define vehicle lateral model
% default values
UseExternalModel = false;
[Ag,Bg,Cg] = getVehModelFromParam(m,Iz,lf,lr,Cf,Cr,Vx0);
% update vehicle info
if strcmp(get_param(blk,'ModelType'),slsvInternal('slsvGetEnStringFromCatalog','MPC:adasblocks:promptLKASpecifyVehicleModel'))
    UseExternalModel = true;
    Ag = EgoModelMatrixA;
    Bg = EgoModelMatrixB;
    Cg = EgoModelMatrixC;
end
% handling lag
hasLag = (lag>0);
% number of states for vehicle model
NumEgoStates = size(Bg,1); 

%% Set MPC block data type
DataType = get_param(blk,'datatypeopt');
set_param([blk '/Adaptive MPC for LKA/Adaptive MPC Controller'],'BlockDataType',DataType);

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

%% Define internal model for MPC design
[A,B,C,D] = lkablock_utilGetDiscreteModelForMPC(Ts,Vx0,Ag,Bg,Cg,lag,hasLag);
% Internal model for for MPC design
sys = ss(A,B,C,D,Ts);

%% generate MPC controller under the mask
mpcobj = generateMPC(sys,umax,umin,psteps,Ts,weightratio);

%% 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
mpcblk = [blk '/Adaptive MPC for LKA/Adaptive MPC Controller'];
if strcmp(get_param(blk,'trackmode'),'on')
    set_param(mpcblk,'mv_inport','on');
    connectMPCextmv(blk);
else
    disconnectMPCextmv(blk);
    set_param(mpcblk,'mv_inport','off');
end

%% Connect vehicle model
if UseExternalModel
    connectExternalVehicleMatrices(blk);
else    
    disconnectExternalVehicleMatrices(blk);
end


%-------------------Local functions-------------------------
%
%% Local function: generate MPC controller
function  mpc1 = generateMPC(sys,umax,umin,psteps,Ts,weightratio)

status = mpcverbosity('off');

% Specify the MPC signal types in the plant.
sys = setmpcsignals(sys,'MV',1,'MD',2);
%% MPC Design
mpc1 = mpc(sys,Ts);
% Specify the prediction horizon.
mpc1.PredictionHorizon = psteps;
% Specify the control horizon.
mpc1.ControlHorizon = round(max(0.1*mpc1.PredictionHorizon,2));
% Specify MV constraints: max/min acceleraion.
mpc1.MV.Min = umin;
mpc1.MV.Max = umax;
% Specify scale factors based on the operating ranges of the variables.
mpc1.MV.ScaleFactor = umax-umin; % range of the steering angle
mpc1.DV.ScaleFactor = 0.01; % scale factor of road yaw angle rate 
mpc1.OV(1).ScaleFactor = 0.5; % scale factor of lateral deviation w.r.t road
mpc1.OV(2).ScaleFactor = 0.1; % scale factor of relative yaw angle w.r.t. road
% Specify weights. 
alpha = getWeightParam(weightratio); % relating slider on mask to weights in MPC design
mpc1.Weights.MVRate = 0.1*alpha;
mpc1.Weights.OV = [1 0.1]/alpha; % tighter control on lateral deviation over relative yaw angle 

mpcverbosity(status);

%% Local function: update mask inports/outports
function updateOptionalPorts(blk,DataType,UseExternalModel,NumEgoStates)
% Update optional inports and outports
inport_names = {'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,'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 = 5;
port = adasblocks_utilUpdateInport(blk,'Adaptive MPC for LKA',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},port,4,'MinSteering',DataType);
port = adasblocks_utilUpdateInport(blk,'Adaptive MPC for LKA',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},port,5,'MaxSteering',DataType);
port = adasblocks_utilUpdateInport(blk,'Adaptive MPC for LKA',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},port,7,'1',DataType);
port = adasblocks_utilUpdateInport(blk,'Adaptive MPC for LKA',AvailableBlocks,type_in_mask,type_in_block,4,inport_names{4},port,6,'0',DataType);
% Update dynamis ports (A, B, C)
updateDynamicPorts(blk,DataType,port,UseExternalModel);
% Update dimensions for ports (A,B,C)
if UseExternalModel
    updatePortsDimensions(blk,NumEgoStates);
end

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

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

%% Local function: update MPC weight parameter from slider value
function alpha = getWeightParam(beta)
weightrange = 50;      % must be greater than one
alpha = 1/weightrange^(beta-0.5);

%% Local function: update optional dynamics ports
function updateDynamicPorts(blk,DataType,portNum,UseExternalModel)
% 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 = portNum;
port = adasblocks_utilUpdateInport(blk,'Model for Adaptive MPC',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},port,3,'zeros(2)',DataType);
port = adasblocks_utilUpdateInport(blk,'Model for Adaptive MPC',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},port,4,'zeros(2,1)',DataType);
port = adasblocks_utilUpdateInport(blk,'Model for Adaptive MPC',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},port,5,'zeros(2)',DataType);

%% Local function: get continuous vehicle lateral model from parameters
function [Ag,Bg,Cg] = getVehModelFromParam(m,Iz,lf,lr,Cf,Cr,Vx)
% Get continuous vehicle model from vehicle parameters with steering angle
% as input and the first output is the lateal velocity and the second
% output is the yaw angle rate.

% Specify vehicle state-space model with state varaibles [vy,phidot]. 
%   vy:       lateral velocity
%   phidot:   yaw angle rate
%   dynamics: dx = [a1,a2;a3,a4]x + [b1;b2]u.
a1 = -(2*Cf+2*Cr)/m/Vx;
a2 = -(2*Cf*lf-2*Cr*lr)/m/Vx - Vx;
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/Vx;
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/Vx;
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;
% Specify the vehicle model matrices with states [vy,phidot].
Ag = [a1,a2;a3,a4];
Bg = [b1;b2];
Cg = eye(2);

%% Local function: connect external vehicle model to "Adaptive Model" subsystem
function connectExternalVehicleMatrices(blk)
% terminator names
nameA = 'A terminator';
nameB = 'B terminator';
nameC = 'C terminator';
% blocks
subsystemblk = [blk '/Model for Adaptive MPC'];
termblkA = [subsystemblk '/' nameA];
termblkB = [subsystemblk '/' nameB];
termblkC = [subsystemblk '/' nameC];
% get source block to terminator A
ph = get_param(termblkA,'PortHandles');
srcline = get_param(ph.Inport,'Line');
srcblk = getfullname(get_param(srcline,'SrcBlockHandle'));
% connect vehicle model if source block is not from Parameters
if ~strcmp(srcblk,[subsystemblk '/' 'Vehicle Dynamics from Parameters'])
    % delete lines to terminator
    delete_line(subsystemblk,['Signal Specification A' '/1'],[nameA '/1']);
    delete_line(subsystemblk,['Signal Specification B' '/1'],[nameB '/1']);
    delete_line(subsystemblk,['Signal Specification C' '/1'],[nameC '/1']);
    % delete lines to "Adaptive Model" subsystem
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/1'],['Adaptive Model' '/3']);
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/2'],['Adaptive Model' '/4']);
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/3'],['Adaptive Model' '/5']);
    % move terminator block up
    pos = get_param([subsystemblk '/' 'Vehicle Dynamics from Parameters'],'position');
    newpos = pos - [-185 -17 -110 137];
    set_param(termblkA,'position',newpos);
    set_param(termblkB,'position',newpos + [0 60 0 60]);
    set_param(termblkC,'position',newpos + 2*[0 60 0 60]);
    % connect "Parameters" to terminator
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/1'],[nameA '/1']);
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/2'],[nameB '/1']);
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/3'],[nameC '/1']);
    % connect line to "Adaptive Model" subsystem
    add_line(subsystemblk,['Signal Specification A' '/1'],['Adaptive Model' '/3'],'autorouting','smart');
    add_line(subsystemblk,['Signal Specification B' '/1'],['Adaptive Model' '/4'],'autorouting','smart');
    add_line(subsystemblk,['Signal Specification C' '/1'],['Adaptive Model' '/5'],'autorouting','smart');
end

%% Local function: disconnect external vehicle model to "Adaptive Model" subsystem
function disconnectExternalVehicleMatrices(blk)
% terminator names
nameA = 'A terminator';
nameB = 'B terminator';
nameC = 'C terminator';
% blocks
subsystemblk = [blk '/Model for Adaptive MPC'];
termblkA = [subsystemblk '/' nameA];
termblkB = [subsystemblk '/' nameB];
termblkC = [subsystemblk '/' nameC];
% get source block to terminator A
ph = get_param(termblkA,'PortHandles');
srcline = get_param(ph.Inport,'Line');
srcblk = getfullname(get_param(srcline,'SrcBlockHandle'));
% disconnect vehicle model if source block is from Parameters
if strcmp(srcblk,[subsystemblk '/' 'Vehicle Dynamics from Parameters'])
    % delete lines to terminator
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/1'],[nameA '/1']);
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/2'],[nameB '/1']);
    delete_line(subsystemblk,['Vehicle Dynamics from Parameters' '/3'],[nameC '/1']);
    % delete line to "Adaptive Model" subsystem
    delete_line(subsystemblk,['Signal Specification A' '/1'],['Adaptive Model' '/3']);
    delete_line(subsystemblk,['Signal Specification B' '/1'],['Adaptive Model' '/4']);
    delete_line(subsystemblk,['Signal Specification C' '/1'],['Adaptive Model' '/5']);
    % move terminator block down
    pos = get_param([subsystemblk '/' 'Signal Specification A'],'position');
    newpos = pos - [-190 0 -130 0];
    set_param(termblkA,'position',newpos);
    set_param(termblkB,'position',newpos + [0 60 0 60]);
    set_param(termblkC,'position',newpos + 2*[0 60 0 60]);
    % add lines to terminator
    add_line(subsystemblk,['Signal Specification A' '/1'],[nameA '/1']);
    add_line(subsystemblk,['Signal Specification B' '/1'],[nameB '/1']);
    add_line(subsystemblk,['Signal Specification C' '/1'],[nameC '/1']);
    % add lines to "Adaptive Model" subsystem
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/1'],['Adaptive Model' '/3'],'autorouting','smart');
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/2'],['Adaptive Model' '/4'],'autorouting','smart');
    add_line(subsystemblk,['Vehicle Dynamics from Parameters' '/3'],['Adaptive Model' '/5'],'autorouting','smart');
end

%% 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) ',' '1' ']']);
set_param(PortCName,'PortDimensions',['[' '2' ',' num2str(NumEgoStates) ']']);






