function empcblock_InitFcn
% EMPC_INIT Explicit MPC Block Initialization Function

%   Author: A. Bemporad
%   Copyright 1986-2011 The MathWorks, Inc.

%% Get parameters from EMPC block
blk=gcb;
if ~any(strcmp('is_multiple',get_param(blk,'MaskNames')))
    % old block withdisabled or broken link
    ctrlMsgUtils.error('MPC:utility:ObsoleteMPCblock',gcb);
end

%% Process initial states
empcobjname=strtrim(get_param(blk,'empcobj'));
InitialState=strtrim(get_param(blk,'x0'));
if isempty(empcobjname)
    empcobjname='[]';
end
if isempty(InitialState)
    InitialState='[]';
end
% The object name is obtained from the mask as a string. It must be evaluated.
% This may change empcobj if InitialState is "mpcstate(empcobj)"
try
    xmpc0 = slResolve(InitialState, blk);
catch ME
    throw(ME);
end
if ~isempty(xmpc0) && ~isa(xmpc0,'mpcstate')
    ctrlMsgUtils.error('MPC:designtool:InvalidMPCStateObject',blk);
end

%% Process EMPC object
try
    empcobj = slResolve(empcobjname,blk);
catch ME
    throw(ME);
end
if ~isa(empcobj,'explicitMPC')
    ctrlMsgUtils.error('MPC:object:EMPCInvalidObject',blk);
end

%% Obtain EMPCData
EMPCData=getExplicitMPCData(empcobj);
nmv=EMPCData.nmv;
nmd=EMPCData.nv-1;

%% Process MMPC information
gc = gcbh;
% The flag "is_multiple" distinguishes between stand-alone MPC blocks and
% blocks that belong to a multiple MPC block set. 
is_multiple = str2double(strtrim(get_param(gc,'is_multiple')));
% Signal enabling the optimization. QP is NOT formed and solved when
% the switching signal is enabled and connected to the MPC block and
% the switching signal is not equal to the enable_value
% This is used with multiple MPC blocks, to enable one block at the time.
enable_value = str2double(strtrim(get_param(gc,'enable_value')));

%% Inports connectivity
if is_multiple
    gcp=get_param(get_param(gc,'Parent'),'Handle');
    ports=get_param(gcp,'PortConnectivity');
    port = 1;
    % SWITCH
    switch_inport = 'on';
    no_switch = ~(ports(port).SrcBlock>=0);
    port = port + 1;
    % MO
    no_ym = ~(ports(port).SrcBlock>=0);
    port = port + 1;
    % REF
    no_ref = ~(ports(port).SrcBlock>=0);
    port = port + 1;
    % MD
    md_inport = get_param(gcp,'md_inport_multiple');
    no_md = ~(strcmp(md_inport,'on') && ports(port).SrcBlock>=0);
    port = port + strcmp(md_inport,'on');
    % When MD port is enabled and connected but MPC object does not have MD signal, error out
    if (~no_md) && nmd==0
        ctrlMsgUtils.error('MPC:designtool:MDSignalInSLButNotInModel',gcb);
    end
    % EXT.MV
    mv_inport = 'on';
    no_mv = false;
    port = port + strcmp(get_param(gcp,'mv_inport_multiple'),'on'); %#ok<NASGU>
else
    ports=get_param(blk,'PortConnectivity');
    port = 1;
    % MO
    no_ym = ~(ports(port).SrcBlock>=0);
    port = port + 1;
    % REF
    no_ref = ~(ports(port).SrcBlock>=0);
    port = port + 1;
    % MD
    md_inport = get_param(blk,'md_inport');
    no_md = ~(strcmp(md_inport,'on') && ports(port).SrcBlock>=0);
    port = port + strcmp(md_inport,'on'); % port number for next signal
    % When MD port is enabled and connected but MPC object does not have MD signal, error out
    if (~no_md) && nmd==0
        ctrlMsgUtils.error('MPC:designtool:MDSignalInSLButNotInModel',gcb);
    end
    % EXT.MV
    mv_inport = get_param(blk,'mv_inport');
    no_mv = ~(strcmp(mv_inport,'on') && ports(port).SrcBlock>=0);

    % SWITCH
    switch_inport = get_param(gc,'switch_inport');
    no_switch = ~(strcmp(switch_inport,'on') && ports(port).SrcBlock>=0);
    port = port + strcmp(switch_inport,'on'); %#ok<NASGU> % port number for next signal
end

% Update initial state and initial input
nxp=EMPCData.nxp;
xpoff=EMPCData.xpoff;

if isempty(xmpc0)
    xmpc0=mpcstate(empcobj);
end
xp0=xmpc0.Plant;
xd0=xmpc0.Disturbance;
xn0=xmpc0.Noise;

try
    xp0=mpc_chkx0u1(xp0,nxp,xpoff,...
        ctrlMsgUtils.message('MPC:utility:InitialStatePlantModel')); % Check consistency of xp0
    xp0=xp0-xpoff; % Initial condition for the linearized plant
    
    xd0=mpc_chkx0u1(xd0,EMPCData.nxdist,zeros(EMPCData.nxdist,1),...
        ctrlMsgUtils.message('MPC:utility:InitialStateUnmeasuredDisturbanceModel'));
    xn0=mpc_chkx0u1(xn0,EMPCData.nxnoise,zeros(EMPCData.nxnoise,1),...
        ctrlMsgUtils.message('MPC:utility:InitialStateNoiseModel'));
    
    % Form extended state vector
    x0=[xp0;xd0;xn0];
    
    u1=xmpc0.LastMove./EMPCData.Uscale(EMPCData.mvindex);
    mvoff=EMPCData.mvoff;
    u1=mpc_chkx0u1(u1,nmv,mvoff,...
        ctrlMsgUtils.message('MPC:utility:InitialInput'));% Check consistency of u1
    u1=u1-mvoff; % Initial condition for linearized input
catch ME
    throw(ME);
end

EMPCData.lastx=x0;
EMPCData.lastu=u1;

% Scaling info
Uscale = EMPCData.Uscale;
Yscale = EMPCData.Yscale;


%% Update MPCstruct
EMPCData.no_ref=double(no_ref); % Simulink Coder does like non-numerical values !!
EMPCData.no_md=double(no_md);
EMPCData.no_ym=double(no_ym);
EMPCData.no_mv=double(no_mv);
EMPCData.no_switch=double(no_switch);

% External signals (1='on', 0='off')
EMPCData.md_inport=double(strcmp(md_inport,'on'));
EMPCData.mv_inport=double(strcmp(mv_inport,'on'));

return_status = get_param(blk,'return_status');
EMPCData.return_status=double(strcmp(return_status,'on'));
return_region= get_param(blk,'return_region');
EMPCData.return_region=double(strcmp(return_region,'on'));

EMPCData.switch_inport=double(strcmp(switch_inport,'on'));
EMPCData.enable_value=enable_value; 
EMPCData.is_multiple=is_multiple; 

EMPCData.MVscale = Uscale(EMPCData.mvindex);
if ~no_md
    EMPCData.MDscale = Uscale(EMPCData.mdindex);
else
    EMPCData.MDscale = 1;
end

EMPCData.RMYscale = 1./Yscale(EMPCData.myindex);
EMPCData.RMVscale = 1./Uscale(EMPCData.mvindex);
if ~no_md
    EMPCData.RMDscale = 1./Uscale(EMPCData.mdindex);
else
    EMPCData.RMDscale = 1;
end
EMPCData.RYscale = 1./Yscale;

EMPCData.CustomEstimation = strcmp(get_param(blk,'state_inport'),'on');

%% Update save EMPC data
set_param(blk,'Userdata',EMPCData);
