function lkablock_callbackCreateSubsystemButton(blk)
% Callback function of the "Create LKA subsystem" button in the Lane
% Keeping Assist 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('lkasubsystem', Exclusion);
while true
    filename = evalin('base',['which(''-all'',''' modelname, ''');']);
    if isempty(filename)
        break;
    else
        Exclusion = union(Exclusion, modelname);
        modelname = matlab.lang.makeUniqueStrings('lkasubsystem', Exclusion); 
    end
end
% create new model
new_system(modelname);

%% add ACC subsystem block
name = 'LKA 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] = lkablock_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 acc subsystem
setACCParam(blk,subsystemname,structname);

%% open acc subsystem
open_system(modelname);




%% local function: set ACC parameters using the mpcacc struct
function setACCParam(blk,subsystemname,structname)
%% 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',...
               'Sample time constant',...
               'Transport lag constant'
               };
% parameters for model        
mdlblk = [subsystemname  '/' 'Model for Adaptive MPC'];         
set_param([mdlblk '/' blocknames{7}],'Value',[structname '.Ts']);
set_param([mdlblk '/' blocknames{8}],'Value',[structname '.TransportLag']);

% parameters for vehicle dynamics
dynblk = [mdlblk '/' 'Vehicle Dynamics from Parameters'];
set_param([dynblk '/' blocknames{1}],'Value',[structname '.VehicleMass']);
set_param([dynblk '/' blocknames{2}],'Value',[structname '.VehicleYawInertia']);
set_param([dynblk '/' blocknames{3}],'Value',[structname '.LengthToFront']);
set_param([dynblk '/' blocknames{4}],'Value',[structname '.LengthToRear']);
set_param([dynblk '/' blocknames{5}],'Value',[structname '.FrontTireStiffness']);
set_param([dynblk '/' blocknames{6}],'Value',[structname '.RearTireStiffness']);

%% 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

%% dimension for X and DX
set_param([mdlblk '/' 'X Constant'],'Value', ['zeros(' [structname '.NumEgoStates'] '+2*(1+' [structname '.hasLag'] ')' ',1)']);
set_param([mdlblk '/' 'DX Constant'],'Value',['zeros(' [structname '.NumEgoStates'] '+2*(1+' [structname '.hasLag'] ')' ',1)']);

% dimension for dynamics ports (A,B,C)
if strcmp(get_param(blk,'ModelType'),slsvInternal('slsvGetEnStringFromCatalog','MPC:adasblocks:promptLKASpecifyVehicleModel'))
    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'] ',' '1' ']']);
    set_param(PortCName,'PortDimensions',['[' '2' ',' [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

% data type 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

% data type 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

% data type in the "MPCDataTypeAutoConversion" 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

% data type 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
ctrlblk = [subsystemname  '/' 'Adaptive MPC for LKA'];         
set_param([ctrlblk '/' 'Adaptive MPC Controller'],'mpcobj',[structname '.Controller']);

%% Local function: update Matlab function block
function updateMLfcnblk(subsystemname,hasLag)
if hasLag
    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 '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 adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = lkablock_utilGetDTModelHasLagForMPC(Ts,Vx,Ag,Bg,Cg,lag);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/Model for Adaptive MPC/Adaptive Model'],'-isa','Stateflow.EMChart');
    ch.Script = Code;
else
    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 '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 adaptive MPC"
        ""
        "%   Author: Meng Xia, Rong Chen"
        "%   Copyright 2017-2018 The MathWorks, Inc."
        ""
        "% Discrete MPC model"
        "[A,B,C,D] = lkablock_utilGetDTModelNoLagForMPC(Ts,Vx,Ag,Bg,Cg);"];
    Code = sprintf('%s\n',Code);
    % Write M code to block
    S = sfroot;
    ch = S.find('Path',[subsystemname '/Model for Adaptive MPC/Adaptive Model'],'-isa','Stateflow.EMChart');
    ch.Script = Code;
end
