function nlobj = vppblock_CreateNonlinearMPC(p,Ts,v_range,steer_range,...
    numStateParas,hasObs,numStageParas)
% This is a utility function to create multistage nonlinear MPC object for
% the Vehicle Path Planner (VPP) System block.
%
% Inputs 
%                p: prediction horizon
%               Ts: sample time
%          v_range: velocity range (vector 1-by-2)
%      steer_range: steering angle range (vector 1-by-2)
%    numStateParas: parameter length for prediction model
%           hasObs: if there are any obstacles when planning the path (True
%                   or False)
%    numStageParas: parameter length for stage cost/constraint functions.
%                   Use stage parameters to provide obstacle information by
%                   the "Stacked stage parameters" inport of the
%                   "Multistage Nonlinear MPC Controller" block.
% Outputs
%            nlobj: multistage nonlinear MPC object

%   Copyright 2017-2022 The MathWorks, Inc.

%%
% Prediction horizon p, sample time Ts, # of states 3, and # of controls 2
nlobj = nlmpcMultistage(p,3,2);
nlobj.Ts = Ts;
nlobj.UseMVRate = true;
% Model
nlobj.Model.StateFcn = "vppblock_StateFcn";
nlobj.Model.StateJacFcn = "vppblock_StateJacFcn";
nlobj.Model.ParameterLength = numStateParas;
% Constraints on MVs
nlobj.MV(1).Min = v_range(1);    % Minimum velocity
nlobj.MV(1).Max = v_range(2);    % Maximum velocity
nlobj.MV(2).Min = steer_range(1);% Minimum steering angle
nlobj.MV(2).Max = steer_range(2);% Maximum steering angle
nlobj.MV(1).RateMin = -2;        % Minimum acceleration
nlobj.MV(1).RateMax = 2;         % Maximum acceleration
nlobj.MV(2).RateMin = -0.6;      % Minimum steering angle rate
nlobj.MV(2).RateMax = 0.6;       % Maximum steering angle rate
% Cost (minimize control effort)
for ct = 1:p
    nlobj.Stages(ct).CostFcn = "vppblock_CostFcn";
    nlobj.Stages(ct).CostJacFcn = "vppblock_CostJacFcn";
    nlobj.Stages(ct).ParameterLength = numStageParas;
end
% Inequality constraints (collision avoidance)
if hasObs
    for ct = 2:p
        nlobj.Stages(ct).IneqConFcn = "vppblock_IneqConFcn";
    end
end
% Terminal states (to be enforced online)
nlobj.Model.TerminalState = zeros(3,1);
