function [mpcobj,MinVelocity,DataType] = accblock_MaskInitFcn(blk,G,L0,vmax,amax,amin,vel0,psteps,Ts,maxiter,weightratio)
% Mask initialization function for the Adaptive Cruise Control System block.
%
% Inputs:
%   G:                 linear model from longitudinal acceleration to velocity
%   amax, amin:        maximum acceleration, minimum acceleration
%   vel0:              initial ego longitudinal velocity
%   psteps:            prediction horizon (# of sampling steps)
%   Ts:                sample time
%   L0:                default spacing
%   maxiter:           maximum iteration number for MPC optimization
%   weightratio:        balancing factor for controller behavior (between 0 and 1, 0.5 for balanced)
%   vmax:              maximum velocity
% Outputs:
%   mpcobj:            MPC object under the mask
%   MinVelocity:       minimum velocity
%   DataType:          data type

%   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:promptACCWeightTextLeft')),blanks(12),...
                getString(message('MPC:adasblocks:promptACCWeightTextRight')),blanks(12)];

%% Check parameter validity
accblock_utilParameterCheck(blk);

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

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

%% generate MPC controller under the mask
[mpcobj,MinVelocity] = generateMPC(G,amax,amin,vel0,psteps,Ts,L0,weightratio,vmax);

%% 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([blk '/MPC'],'mv_inport','on');
    connectMPCextmv(blk);
else
    disconnectMPCextmv(blk);
    set_param([blk '/MPC'],'mv_inport','off');
end


%% Local function: generate MPC controller
function  [mpc1,vmin] = generateMPC(G,amax,amin,vel0,psteps,Ts,L0,weightratio,vmax)

status = mpcverbosity('off');
%% Initialize parameters
vmin = 0;       % ego car min speed
% MPC Structure
%
% * Two measured outputs (MO): the relative distance between two cars and
% host car velocity.  The distance should be greater than the safe distance
% that is a function of host car velocity.  Meanwhile host car velocity
% should track the driver-set velocity whenever possible.
% * One manipulated variable (MV): Host car acceleration constrained by [-3,2].
% * One measured disturbance (MD): Lead car velocity, which allows MPC to
% predict the relative distance.

% Convert to ss model from idParametric model
if isa(G,'idParametric')
    G = ss(G);
end
s = tf('s');
z = tf('z',Ts);
% Specify MPC internal plant model.
if isct(G)
    sys = ss([-G/s 1/s;G 0],'min');
else 
    G = d2d(G,Ts);
    sys = ss([-G*(Ts/(z-1)) Ts/(z-1);G 0],'min');
end

% 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 the nominal values based on the simulation initial conditions.
mpc1.Model.Nominal.Y(1) = 1.4*vel0+L0; % use default time gap = 1.4 second
mpc1.Model.Nominal.Y(2) = vel0;
mpc1.Model.Nominal.U(1) = 0;
mpc1.Model.Nominal.U(2) = vel0;
% Specify MV constraints: max/min acceleraion.
mpc1.MV.Min = amin;
mpc1.MV.Max = amax;
% Specify scale factors based on the operating ranges of the variables.
mpc1.MV.ScaleFactor = amax - amin;
mpc1.DV.ScaleFactor = vmax;
mpc1.OV(1).ScaleFactor = vmax; 
mpc1.OV(2).ScaleFactor = vmax;
% Specify OV constraints.
mpc1.OV(1).Min = L0;
mpc1.OV(2).Min = vmin;
mpc1.OV(2).Max = vmax;
% Specify weights. 
alpha = getWeightParam(weightratio);
mpc1.Weights.MVRate = 0.1*alpha;
mpc1.Weights.OV = [0 0.1/alpha];
mpc1.Weights.ECR = 1e6/alpha; % more weights on the constraint violation

mpcverbosity(status);


%% Local function: update mask inports/outports
function updateOptionalPorts(blk,DataType)
% Update optional inports and outports
inport_names = {'Minimum longitudinal acceleration',...
                'Maximum longitudinal acceleration',...
                '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,'optmode'),'on'),...
    strcmp(get_param(blk,'trackmode'),'on'), ...
    ];
% Update Inport
port = 6;
port = adasblocks_utilUpdateInport(blk,'DataTypeConversion_amin',AvailableBlocks,type_in_mask,type_in_block,1,inport_names{1},port,1,'MinAcceleration',DataType);
port = adasblocks_utilUpdateInport(blk,'DataTypeConversion_amax',AvailableBlocks,type_in_mask,type_in_block,2,inport_names{2},port,1,'MaxAcceleration',DataType);
port = adasblocks_utilUpdateInport(blk,'DataTypeConversion_optsgn',AvailableBlocks,type_in_mask,type_in_block,3,inport_names{3},port,1,'1',DataType);
port = adasblocks_utilUpdateInport(blk,'DataTypeConversion_atrack',AvailableBlocks,type_in_mask,type_in_block,4,inport_names{4},port,1,'0',DataType);

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


%% Local function: disconnect ext.mv for MPC
function disconnectMPCextmv(blk)
if strcmp(get_param([blk '/MPC'],'mv_inport'),'on')
    % delete line to MPC block
    delete_line(blk,['DataTypeConversion_atrack' '/1'],['MPC' '/4']);
    % add terminator block
    name = 'Track u terminator';
    termblk = [blk '/' name];
    add_block('built-in/Terminator',termblk);
    % connect to ext.mv port
    add_line(blk,['DataTypeConversion_atrack' '/1'],[name '/1']);
    % set position of the terminator block
    pos = get_param([blk '/' 'DataTypeConversion_atrack'],'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 = 10;      % must be greater than one
alpha = 1/weightrange^(beta-0.5);


