function pfcblock_callbackCreateSubsystemButton(blk)
% Callback function of the "Create PFC subsystem" button in the Path
% Following Control System block dialog.

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

%% create new Simulink model
% find existing diagram name
AllDiagrams = find_system('Type','block_diagram');
AllNames = evalin('base','who');
Exclusion = union(AllDiagrams, AllNames);
% make unique name
modelname = matlab.lang.makeUniqueStrings('pfcsubsystem', Exclusion);
while true
    filename = evalin('base',['which(''-all'',''' modelname, ''');']);
    if isempty(filename)
        break;
    else
        Exclusion = union(Exclusion, modelname);
        modelname = matlab.lang.makeUniqueStrings('pfcsubsystem', Exclusion); 
    end
end
% create new model
new_system(modelname);

%% add ACC subsystem block
name = 'PFC subsystem';
subsystemname = [modelname,'/' name];
subsystem = add_block(blk,subsystemname);
% change size of the model
set_param(modelname, 'ZoomFactor','FitSystem');
% disable link to library
set_param(subsystem,'LinkStatus','inactive');
% break link to library
set_param(subsystem,'LinkStatus','none');


%% export the data struct to workspace
[~,structname] = pfcblock_utilExportDataStruct(blk,modelname,AllNames);

%% unmask the block
set_param(subsystem,'Mask','off');

%% handle lag
maskWSVars = get_param(blk,'MaskWSVariables');
blkWSVarNames = {maskWSVars.Name};
hasLag = maskWSVars(strcmp(blkWSVarNames,'hasLag')).Value;
updateMLfcnblk(subsystemname,hasLag)

%% assign values for PFC subsystem
hasSpacingCtrl = maskWSVars(strcmp(blkWSVarNames,'hasSpacingCtrl')).Value;
UseExternalModel = maskWSVars(strcmp(blkWSVarNames,'UseExternalModel')).Value;
setPFCParam(blk,subsystemname,structname,hasSpacingCtrl,UseExternalModel);

%% open acc subsystem
open_system(modelname);




%% local function: set PFC parameters using the mpcpfc struct
function setPFCParam(blk,subsystemname,structname,hasSpacingCtrl,UseExternalModel)
%% always-have constant blocks
blocknames = { 'Vehicle mass constant',...
               'Vehicle yaw inertia constant',...
               'Vehicle length to front constant',...
               'Vehicle length to rear constant',...
               'Vehicle front tire cornering stiffness constant',...
               'Vehicle rear tire cornering stiffness constant',...
               'Vehicle acceleration tracking time constant',...
               'Sample time constant',...
               'Transport lag constant',...
               'Default spacing constant',...
               };
           
% Vehicle dynamics parameters
% lateral dynamics
latblk = [subsystemname '/Vehicle Models/Vehicle parametric model/Vehicle lateral dynamics'];
set_param([latblk '/' blocknames{1}],'Value',[structname '.VehicleMass']);
set_param([latblk '/' blocknames{2}],'Value',[structname '.VehicleYawInertia']);
set_param([latblk '/' blocknames{3}],'Value',[structname '.LengthToFront']);
set_param([latblk '/' blocknames{4}],'Value',[structname '.LengthToRear']);
set_param([latblk '/' blocknames{5}],'Value',[structname '.FrontTireStiffness']);
set_param([latblk '/' blocknames{6}],'Value',[structname '.RearTireStiffness']);
% longitudinal dynamics
longblk = [subsystemname '/Vehicle Models/Vehicle parametric model/Vehicle longitudinal dynamics'];
set_param([longblk '/' blocknames{7}],'Value',[structname '.AccelTimeConstant']);

% Sample time, transport lag and default spacing   
set_param([subsystemname '/' blocknames{8}],'Value',[structname '.Ts']);
set_param([subsystemname '/' blocknames{9}],'Value',[structname '.TransportLag']);
set_param([subsystemname '/' blocknames{10}],'Value',[structname '.DefaultSpacing']);

%% optional constant blocks
if strcmp(get_param(blk,'umin_ext'),'off')
    set_param([subsystemname '/' 'Minimum steering angle' ' constant'],'Value',[structname '.MinSteering']);
end
if strcmp(get_param(blk,'umax_ext'),'off')
    set_param([subsystemname '/' 'Maximum steering angle' ' constant'],'Value',[structname '.MaxSteering']);
end
if strcmp(get_param(blk,'amin_ext'),'off')
    set_param([subsystemname '/' 'Minimum longitudinal acceleration' ' constant'],'Value',[structname '.MinAcceleration']);
end
if strcmp(get_param(blk,'amax_ext'),'off')
    set_param([subsystemname '/' 'Maximum longitudinal acceleration' ' constant'],'Value',[structname '.MaxAcceleration']);
end

%% dimension for U,Y, X and DX for Adaptive MPC
if hasSpacingCtrl
    mdlSpacingCtrlblk = [subsystemname '/MPC Models/MPC Model with Spacing Control'];
    set_param([mdlSpacingCtrlblk '/' 'U Constant'],'Value', ['[0;0;' [structname '.InitialLongVel'] ';0]']);
    set_param([mdlSpacingCtrlblk '/' 'Y Constant'],'Value', ['[1.4*' [structname '.InitialLongVel'] '+' [structname '.DefaultSpacing'] ';' [structname '.InitialLongVel'] ';0;0]']);
    set_param([mdlSpacingCtrlblk '/' 'X Constant'],'Value', ['zeros(' [structname '.NumEgoStates'] '+3+4*' [structname '.hasLag'] ',1)']);
    set_param([mdlSpacingCtrlblk '/' 'DX Constant'],'Value',['zeros(' [structname '.NumEgoStates'] '+3+4*' [structname '.hasLag'] ',1)']);
else
    mdlblk = [subsystemname '/MPC Models/MPC Model'];
    set_param([mdlblk '/' 'U Constant'],'Value', '[zeros(3,1)]');
    set_param([mdlblk '/' 'Y Constant'],'Value', ['[' [structname '.InitialLongVel'] ';0;0]']);
    set_param([mdlblk '/' 'X Constant'],'Value', ['zeros(' [structname '.NumEgoStates'] '+2+3*' [structname '.hasLag']  ',1)']);
    set_param([mdlblk '/' 'DX Constant'],'Value',['zeros(' [structname '.NumEgoStates'] '+2+3*' [structname '.hasLag'] ',1)']);
end

%% dimension for dynamics ports (A,B,C)
if UseExternalModel
    PortAName = [subsystemname '/' 'Vehicle dynamics matrix A'];
    PortBName = [subsystemname '/' 'Vehicle dynamics matrix B'];
    PortCName = [subsystemname '/' 'Vehicle dynamics matrix C'];
    set_param(PortAName,'PortDimensions',['[' [structname '.NumEgoStates'] ',' [structname '.NumEgoStates'] ']']);
    set_param(PortBName,'PortDimensions',['[' [structname '.NumEgoStates'] ',' '2' ']']);
    set_param(PortCName,'PortDimensions',['[' '3' ',' [structname '.NumEgoStates'] ']']);
end

%% data type 
% in the "Convert" blocks
convertBlocks = find_system(subsystemname,...
    'MatchFilter', @Simulink.match.allVariants, ... 
    'BlockType','DataTypeConversion');
numConvertBlk = length(convertBlocks);
for ct = 1:numConvertBlk    
    set_param(convertBlocks{ct},'OutDataTypeStr',[structname '.DataType']);
end
% in the "Constant" blocks
constantBlocks = find_system(subsystemname,...
    'MatchFilter', @Simulink.match.allVariants, ... 
    'BlockType','Constant');
numConstantBlk = length(constantBlocks);
for ct = 1:numConstantBlk    
    set_param(constantBlocks{ct},'OutDataTypeStr',[structname '.DataType']);
end
% in the "MPCDataTypeAutoConversion" blocks
MPCConvertBlocks = find_system(subsystemname, ...
   'MatchFilter', @Simulink.match.allVariants, ...
   'MaskType', 'MPCDataTypeAutoConversion');
numMPCConvertBlk = length(MPCConvertBlocks);
for ct = 1:numMPCConvertBlk    
    set_param(MPCConvertBlocks{ct},'datatypestr',[structname '.DataType']);
end
% in the "MPCDataTypeAutoConversion2D" blocks
MPCConvertBlocks = find_system(subsystemname, ...
   'MatchFilter', @Simulink.match.allVariants, ...
   'MaskType', 'MPCDataTypeAutoConversion2D');
numMPCConvertBlk = length(MPCConvertBlocks);
for ct = 1:numMPCConvertBlk    
    set_param(MPCConvertBlocks{ct},'datatypestr',[structname '.DataType']);
end
% in the "MPCCurvatureConversion" blocks
MPCConvertBlocks = find_system(subsystemname, ...
   'MatchFilter', @Simulink.match.allVariants, ...
   'MaskType', 'MPCCurvatureConversion');
numMPCConvertBlk = length(MPCConvertBlocks);
for ct = 1:numMPCConvertBlk    
    set_param(MPCConvertBlocks{ct},'datatypestr',[structname '.DataType']);
    set_param(MPCConvertBlocks{ct},'p',[structname '.Controller.PredictionHorizon']);
end

%% mpcobj
if hasSpacingCtrl
    pfSpacingController = [subsystemname '/MPC Controllers/Path Following Controller with Spacing Control'];
    set_param([pfSpacingController '/' 'Adaptive MPC Controller'],'mpcobj',[structname '.Controller']);
else
    pfController = [subsystemname '/MPC Controllers/Path Following Controller'];
    set_param([pfController '/' 'Adaptive MPC Controller'],'mpcobj',[structname '.Controller']);
end


%% Local function: update Matlab function block
function updateMLfcnblk(subsystemname,hasLag)
if hasLag
    % -----------------------------------------
    % set MPC Model with Spacing Control variant
    Code = [...
        "function [A,B,C,D] = getAdaptiveMPCModel(Ts,Vx, Ag, Bg, Cg, lag)"
        "% Get discrete state-space model for Adaptive MPC with sample time Ts."
        "% Called by the 'Path Following Control System' Block."
        ""
        "% Inputs:"
        "%   Ts:               mask variable for sample time"
        "%   Vx:               mask inport for longitudinal velocity"
        "%   Ag,Bg,Cg:         continuous or discrete vehicle model with sample time Ts"       
        "% Outputs:"
        "%   A,B,C,D:          discrete time model for adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = pfcblock_utilGetDTModelHasLagForMPCHasSpacingCtrl(Ts,Vx,Ag,Bg,Cg,lag);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/MPC Models/MPC Model with Spacing Control/Adaptive Model with Spacing Control'],'-isa','Stateflow.EMChart');
    ch.Script = Code;
    % -----------------------------------------
    % set MPC Model without Spacing control variant
    Code = [...
        "function [A,B,C,D] = getAdaptiveMPCModel(Ts,Vx, Ag, Bg, Cg, lag)"
        "% Get discrete state-space model for Adaptive MPC with sample time Ts."
        "% Called by the 'Path Following Control System' Block."
        ""
        "% Inputs:"
        "%   Ts:               mask variable for sample time"
        "%   Vx:               mask inport for longitudinal velocity"
        "%   Ag,Bg,Cg:         continuous or discrete vehicle model with sample time Ts"       
        "% Outputs:"
        "%   A,B,C,D:          discrete time model for adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = pfcblock_utilGetDTModelHasLagForMPCNoSpacingCtrl(Ts,Vx,Ag,Bg,Cg,lag);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/MPC Models/MPC Model/Adaptive Model'],'-isa','Stateflow.EMChart');
    ch.Script = Code;    
else
    % -----------------------------------------
    % set MPC Model with Spacing Control variant
    Code = [...
        "function [A,B,C,D] = getAdaptiveMPCModel(Ts,Vx, Ag, Bg, Cg, lag)"
        "% Get discrete state-space model for Adaptive MPC with sample time Ts."
        "% Called by the 'Path Following Control System' Block."
        ""
        "% Inputs:"
        "%   Ts:               mask variable for sample time"
        "%   Vx:               mask inport for longitudinal velocity"
        "%   Ag,Bg,Cg:         continuous or discrete vehicle model with sample time Ts"
        "% Outputs:"
        "%   A,B,C,D:          discrete time model for adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = pfcblock_utilGetDTModelNoLagForMPCHasSpacingCtrl(Ts,Vx,Ag,Bg,Cg);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/MPC Models/MPC Model with Spacing Control/Adaptive Model with Spacing Control'],'-isa','Stateflow.EMChart');
    ch.Script = Code;
    % -----------------------------------------
    % set MPC Model without Spacing Control variant
    Code = [...
        "function [A,B,C,D] = getAdaptiveMPCModel(Ts,Vx, Ag, Bg, Cg, lag)"
        "% Get discrete state-space model for Adaptive MPC with sample time Ts."
        "% Called by the 'Path Following Control System' Block."
        ""
        "% Inputs:"
        "%   Ts:               mask variable for sample time"
        "%   Vx:               mask inport for longitudinal velocity"
        "%   Ag,Bg,Cg:         continuous or discrete vehicle model with sample time Ts"
        "% Outputs:"
        "%   A,B,C,D:          discrete time model for adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = pfcblock_utilGetDTModelNoLagForMPCNoSpacingCtrl(Ts,Vx,Ag,Bg,Cg);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/MPC Models/MPC Model/Adaptive Model'],'-isa','Stateflow.EMChart');
    ch.Script = Code;
end
