function nlobj = helperCreateNLmpcObjLC(PredictionHorizon,ControlHorizon)
% Part of set up script for the Lane Change Example
%
% This function designs a nonlinear MPC controller.
%
%   This is a helper for example purposes and may be removed or
%   modified in the future.

%   Copyright 2019 The MathWorks, Inc.

%%
% Create a nonlinear MPC controller object with 5 states, 5 outputs, and 1
% input.
nlobj = nlmpc(5,5,1);

%%
% Specify the controller sample time, prediction horizon, and control
% horizon.
nlobj.Ts = 0.1;
nlobj.PredictionHorizon = PredictionHorizon;
nlobj.ControlHorizon = ControlHorizon;

%%
% Specify the state function for the nonlinear plant model and its
% Jacobian.
nlobj.Model.StateFcn = @(x,u) vehicleStateFcnLC(x,u);
nlobj.Jacobian.StateFcn = @(x,u) vehicleStateJacFcnLC(x,u);

%%
% Specify the output function for the nonlinear plant model and its
% Jacobian. The states are the output variables.
nlobj.Model.OutputFcn = @(x,u) x;
nlobj.Jacobian.OutputFcn = @(x,u) eye(5);

%%
% Set the constraints for manipulated variables.
nlobj.MV(1).Min = -1.13;   % Minimum steering angle -65
nlobj.MV(1).Max = 1.13;    % Maximum steering angle 65

%%
% Set the scale factors.
nlobj.OV(3).ScaleFactor = 200;  % position X
nlobj.OV(4).ScaleFactor = 15;   % position Y
nlobj.MV(1).ScaleFactor = 1;    % Range of steering angle

%%
% Specify the weights in the standard MPC cost function. The first, second,
% and fifth outputs are allowed to float, with and emphasis given more to
% tracking the lateral position (Y).
nlobj.Weights.OutputVariables = [0 0 1 5 0];

%%
% To produce a smooth driiving experience, penalize steering angle changes
% more.
nlobj.Weights.ManipulatedVariablesRate = 1;

%%
% Validate the prediction model functions at an arbitrary operating point
% using the validateFcns command
x0 = [0.1 0.5 200 -1.8 0.5];
u0 = 0.4;
ref0 = [0 0 200 -2 0];
validateFcns(nlobj,x0,u0,{},{},ref0);
