function [res,info] = vppblock_utilValidatePlanner(vppData,x0,xt,obsMat)
% This is a function to test if settings of Vehicle Path Planner (VPP) are
% correct. It also runs the planner with nlmpcmove. This function is
% intended for internal use and may be changed or removed in the future.

% Copyright 2022 The MathWorks, Inc.

%%
% get data from struct
if isfield(vppData,'ObstaclesMatrix')
    obsMat = vppData.ObstaclesMatrix;
end
numObs = vppData.NumberOfObstacles;
if numObs>0
    obsParas = reshape(obsMat',[],1);
else
    obsParas = [];
end
egoParas = vppData.EgoParameters;
nlobj = vppData.Planner;
wheelbase = vppData.EgoWheelbase;
stageParameters = [egoParas;obsParas];
% simdata
simdata = getSimulationData(nlobj,'TerminalState');
simdata.StateFcnParameter = wheelbase;
simdata.StageParameter = repmat(stageParameters,nlobj.p,1);
simdata.TerminalState = zeros(3,1);

%% validate fcns
xTest = [-5 5 0]';uTest = [0.22 -3]';
try 
    validateFcns(nlobj,xTest,uTest,simdata);
catch
    error(message('MPC:adasblocks:errCPPMPC'))
end

%% run planner with nlmpcmove
u0 = [0 0]'; % initial control
simdata.TerminalState = xt;
[~,~,info] = nlmpcmove(nlobj,x0,u0,simdata);
res = info.ExitFlag;