function [tf, dist] = exampleHelperTargetReachedFunction(q,qTgt)
%exampleHelperTargetReachedFunction Tests whether configurations are sufficiently close

% Copyright 2021 The MathWorks Inc. 

    if isempty(q)
        tf = [];
        dist = [];
    else
        % Thresholds [xyDist^2 thetaError betaError]
        goalThreshold = [0.9^2 0.1 0.1];

        % Calculate errors
        error = [sum((q(:,1:2)-qTgt(1:2)).^2,2) abs(robotics.internal.angdiff(q(:,3:4),qTgt(3:4)))];

        % Convert error to rough distance
        dist = sum(error,2);

        % Check whether states pass or fail the goal check
        tf = all(error <= goalThreshold & sign(q(:,5)) == sign(qTgt(5)),2);
    end
end