function nmpcblock_InitFcn()
% Nonlinear MPC block uses two functions in the following order to initialize:
%
%   InitFcn: validate object, cross check signal consistency and
%   connection. Create core data from object and save a structure to block
%   "UserData", to be used by "MaskInit". 
%
%   MaskInitFcn: when "UserData" is empty (i.e. model opens or no @nlmpc),
%   initialize the block with dummy dimensions.  Otherwise, create mask
%   variables from "UserData".  Modify the block I/O based on dialog
%   settings.  Each constant block (when an optional inport is off) is
%   initialized with correct dimension.

%   Authors: N. L. Ricker, R. Chen
%   Copyright 1986-2018 The MathWorks, Inc.

blk = gcb;
%% check NLMPC object
% when NLMPC object name is missing, error out.
nlmpcobjname = strtrim(get_param(blk,'nlmpcobj'));
if isempty(nlmpcobjname)
    ctrlMsgUtils.error('MPC:nlmpc:BlockInvalidNLMPCObject',blk);
end
% when NLMPC object does not exist in workspace, error out.
try
    nlobj = slResolve(nlmpcobjname,blk);
catch ME
    throw(ME);
end
% when NLMPC object is not valid, error out.
if ~isa(nlobj,'nlmpc')
    ctrlMsgUtils.error('MPC:nlmpc:BlockInvalidNLMPCObject',blk);
end
%% check optimization toolbox when custom solver is not used
if isempty(nlobj.Optimization.CustomSolverFcn)
    if ~mpcchecktoolboxinstalled('optim')
        ctrlMsgUtils.error('MPC:nlmpc:MissingOptim');
    end
end
%% check mexname
% when NLMPC object name is missing, error out.
if strcmp(get_param(blk,'UseMEX'),'off')
    mexname = '';
else
    mexnamestr = strtrim(get_param(blk,'mexname'));
    if isempty(mexnamestr)
        % error out if UseMEX is checked
        ctrlMsgUtils.error('MPC:nlmpc:BlockMissingMEX');
    else
        try
            mexname = slResolve(mexnamestr,blk);
        catch
            mexname = mexnamestr;
        end
    end
    validateattributes(mexname,{'string','char'},{'scalartext','nonempty'},'','"MEX function name"');    
    % error out if MEX cannot be located
    if exist(mexname,'file')~=3
        ctrlMsgUtils.error('MPC:nlmpc:BlockMEXNotFound',mexname);
    end
end
%% Create core data structure
coredata = getCoreData(nlobj);
%% process overridden block sample time
% User can run block at a different sample time "TsControl" than nlobj.Ts.
% "TsControl" can be -1 to support triggered and function-call subsystem.
% "TsControl" is sample time of the ML Function block and the unit delay
% block inside the "router" subsystem.
if strcmp(get_param(blk,'UseObjectTs'),'on')
    coredata.TsControl = coredata.Ts;
else
    tsstr = strtrim(get_param(blk,'TsControl'));
    try
        coredata.TsControl = slResolve(tsstr,blk);
        validateattributes(coredata.TsControl,{'double'},{'scalar','real','finite'},'Nonlinear MPC Block','Controller Sample Time');
        if coredata.TsControl<=0 && coredata.TsControl~=-1
            ctrlMsgUtils.error('MPC:nlmpc:errorSampleTime',blk);
        end
    catch ME
        throw(ME);
    end
end
%% Add block information to coredata
coredata.nlmpcobj = nlmpcobjname;
coredata.usemex = strcmp(get_param(blk,'UseMEX'),'on');
coredata.mexname = char(mexname);
coredata.blk = blk;
coredata.md_enabled = strcmp(get_param(blk,'md_enabled'), 'on');
coredata.mvtarget_enabled = strcmp(get_param(blk,'mvtarget_enabled'), 'on');
coredata.ov_min = strcmp(get_param(blk,'ov_min'), 'on');
coredata.ov_max = strcmp(get_param(blk,'ov_max'), 'on');
coredata.mv_min = strcmp(get_param(blk,'mv_min'), 'on');
coredata.mv_max = strcmp(get_param(blk,'mv_max'), 'on');
coredata.mvrate_min = strcmp(get_param(blk,'mvrate_min'), 'on');
coredata.mvrate_max = strcmp(get_param(blk,'mvrate_max'), 'on');
coredata.state_min = strcmp(get_param(blk,'state_min'), 'on');
coredata.state_max = strcmp(get_param(blk,'state_max'), 'on');
coredata.ov_weight = strcmp(get_param(blk,'ov_weight'), 'on');
coredata.mv_weight = strcmp(get_param(blk,'mv_weight'), 'on');
coredata.mvrate_weight = strcmp(get_param(blk,'mvrate_weight'), 'on');
coredata.ecr_weight = strcmp(get_param(blk,'ecr_weight'), 'on');
coredata.param_enabled = strcmp(get_param(blk,'param_enabled'), 'on');
coredata.nlp_initialize = strcmp(get_param(blk,'nlp_initialize'), 'on');
coredata.cost_enabled = strcmp(get_param(blk,'cost_enabled'), 'on');
coredata.mvseq_enabled = strcmp(get_param(blk,'mvseq_enabled'), 'on');
coredata.stateseq_enabled = strcmp(get_param(blk,'stateseq_enabled'), 'on');
coredata.ovseq_enabled = strcmp(get_param(blk,'ovseq_enabled'), 'on');
coredata.slack_enabled = strcmp(get_param(blk,'slack_enabled'), 'on');
coredata.status_enabled = strcmp(get_param(blk,'status_enabled'), 'on');
%% check MD consistency
% when MD port is enabled and NLMPC does not have MD channel, error out
if coredata.md_enabled && (coredata.nmd==0)
    ctrlMsgUtils.error('MPC:nlmpc:ExtraMDPort',blk);
end
% when MD port is disabled and NLMPC has MD channel, error out
if ~coredata.md_enabled && (coredata.nmd>0)
    ctrlMsgUtils.error('MPC:nlmpc:MissingMDPort',blk);
end
%% check Parameter consistency
% when Parameter port is enabled and NLMPC does not have parameter, error out
if coredata.npara < 0 && coredata.param_enabled
    ctrlMsgUtils.error('MPC:nlmpc:ExtraParameterInport',blk);
end
% when Parameter port is disabled and NLMPC has parameter, error out
if coredata.npara > 0 && ~coredata.param_enabled
    ctrlMsgUtils.error('MPC:nlmpc:MissingParameterInport',blk);
end
%% check state and output functions
% if state fcn is not present, error out
if isempty(nlobj.Model.StateFcn)
    ctrlMsgUtils.error('MPC:nlmpc:MissingStateFcn');
end
% if output fcn is not present, error out when nx~=ny
if isempty(nlobj.Model.OutputFcn) && (coredata.nx ~= coredata.ny)
    ctrlMsgUtils.error('MPC:nlmpc:MissingOutputFcn');
end
%% every enabled inport should be connected
ports = get_param(blk,'PortConnectivity');
NoConnection = nmpcblock_getInportStatus(ports, coredata);
InportNames = {'x','ref','last_mv','md','mv.target','y.min','y.max','mv.min','mv.max','dmv.min','dmv.max','x.min','x.max','y.wt','mv.wt','dmv.wt','ecr.wt','params','mv.init','x.init','e.init'};
for ct=1:length(NoConnection)
    if NoConnection(ct)
        ctrlMsgUtils.error('MPC:nlmpc:MissingConnection',InportNames{ct});
    end
end
%% special check for the bus creator block when "params" port is enabled
if coredata.param_enabled
    % "params" port must be connected to a bus creator block
    Connections = get_param(blk, 'PortConnectivity');
    Port = str2double(get_param([blk,'/params'],'Port'));
    BusCreator = Connections(Port).SrcBlock;
    if isempty(BusCreator) || BusCreator==-1
        ctrlMsgUtils.error('MPC:nlmpc:BusMissingBusCreator')
    end
    bcName = get_param(BusCreator,'Name');
    BlockType = get_param(BusCreator, 'BlockType');
    if ~strcmpi(BlockType, 'BusCreator')
        ctrlMsgUtils.error('MPC:nlmpc:BusInvalidBusCreator',bcName,blk);
    end
    % bus inputs must equal number of parameters
    NumIn = str2double(get_param(BusCreator, 'Inputs'));
    if NumIn ~= coredata.npara
        ctrlMsgUtils.error('MPC:nlmpc:BusWrongBusNumber',blk,coredata.npara,bcName,NumIn);
    end
    % must contains a bus object
    if ~contains(get_param(BusCreator,'OutDataTypeStr'),'Bus:')
        ctrlMsgUtils.error('MPC:nlmpc:BusWrongObject',bcName,blk);
    end
end
%% Save structure
set_param(blk,'Userdata',coredata);

%% Helper functions
function NoConnection = nmpcblock_getInportStatus(ports, coredata)
port = 1;
% 1: x (permanent)
NoConnection(1) = ~(ports(port).SrcBlock>=0);
port = port + 1;
% 2: ref (permanent)
NoConnection(2) = ~(ports(port).SrcBlock>=0);
port = port + 1;
% 3: last_mv (permanent)
NoConnection(3) = ~(ports(port).SrcBlock>=0);
port = port + 1;
% 4: md
NoConnection(4) = coredata.md_enabled && ~(ports(port).SrcBlock>=0);
port = port + coredata.md_enabled;
% 5: mv.target
NoConnection(5) = coredata.mvtarget_enabled && ~(ports(port).SrcBlock>=0);
port = port + coredata.mvtarget_enabled;
% 6: ymin
NoConnection(6) = coredata.ov_min && ~(ports(port).SrcBlock>=0);
port = port + coredata.ov_min;
% 7: ymax
NoConnection(7) = coredata.ov_max && ~(ports(port).SrcBlock>=0);
port = port + coredata.ov_max;
% 8: umin
NoConnection(8) = coredata.mv_min && ~(ports(port).SrcBlock>=0);
port = port + coredata.mv_min;
% 9: umax
NoConnection(9) = coredata.mv_max && ~(ports(port).SrcBlock>=0);
port = port + coredata.mv_max;
% 10: dumin
NoConnection(10) = coredata.mvrate_min && ~(ports(port).SrcBlock>=0);
port = port + coredata.mvrate_min;
% 11: dumax
NoConnection(11) = coredata.mvrate_max && ~(ports(port).SrcBlock>=0);
port = port + coredata.mvrate_max;
% 12: xmin
NoConnection(12) = coredata.state_min && ~(ports(port).SrcBlock>=0);
port = port + coredata.state_min;
% 13: xmax
NoConnection(13) = coredata.state_max && ~(ports(port).SrcBlock>=0);
port = port + coredata.state_max;
% 14: y.wt
NoConnection(14) = coredata.ov_weight && ~(ports(port).SrcBlock>=0);
port = port + coredata.ov_weight;
% 15: u.wt
NoConnection(15) = coredata.mv_weight && ~(ports(port).SrcBlock>=0);
port = port + coredata.mv_weight;
% 16: du.wt
NoConnection(16) = coredata.mvrate_weight && ~(ports(port).SrcBlock>=0);
port = port + coredata.mvrate_weight;
% 17: ecr.wt
NoConnection(17) = coredata.ecr_weight && ~(ports(port).SrcBlock>=0);
port = port + coredata.ecr_weight;
% 18: params
NoConnection(18) = coredata.param_enabled && ~(ports(port).SrcBlock>=0);
port = port + coredata.param_enabled;
% 19: mv.init
NoConnection(19) = coredata.nlp_initialize && ~(ports(port).SrcBlock>=0);
port = port + coredata.nlp_initialize;
% 20: x.init
NoConnection(20) = coredata.nlp_initialize && ~(ports(port).SrcBlock>=0);
port = port + coredata.nlp_initialize;
% 21: e.init
NoConnection(21) = coredata.nlp_initialize && ~(ports(port).SrcBlock>=0);
%port = port + coredata.nlp_initialize;
