function cineq = vppblock_IneqConFcn(stage, x, u, du, paras)
%#codegen
% Inequality constraints for Vehicle Path Planner (VPP) block using
% multistage nonlinear mpc.
%
% To customize the Vehicle Path Planner (VPP) block, specify an inequality
% constraint function for the nonlinear mpc object (nlobj). For example,
% use the specified inequality function at stage k:
%
%       cineq = myIneqConFcn(stage, x, u, du, paras)
%       nlobj.Stages(k).IneqConFcn = "myIneqConFcn";
%
% To enforce collision avoidance, specify the collision checking method and
% collision-free distances in the inequality constraint function.
%
% see also: vppblock_CreateNonlinearMPC, vppblock_CheckCollision

% Copyright 2021-2022 The MathWorks, Inc.

%%
% Update ego positions
distToCenter = paras(3);
xPos = x(1) + distToCenter*cos(x(3));
yPos = x(2) + distToCenter*sin(x(3));
ego = [xPos,yPos, x(3), paras(1),paras(2)];

% Get obstacles positions
obsParas = paras(5:end);
obsMat = reshape(obsParas,5,[]);
obsMat = obsMat';

% Calculate distances from ego to obstacles
numObstacles = size(obsMat,1);
distances = zeros(numObstacles,1);
minDist = paras(4);
for ct = 1:numObstacles
    distances(ct) = vppblock_CheckCollision(ego,obsMat(ct,:));
end

% Enforce constraints
cineq = -min(distances) + minDist;
end

