function [refPt, refDist, projDist, isIntersecting] = exampleHelperCalculateLookaheadPoint(curPoint,refDist,R,waypoints,minRad)
%exampleHelperCalculateLookaheadPoint Calculate lookahead point along straight line-segment
%
%   Calculates an xy point along a straight-line segment based on the
%   vehicle's current location and lookahead distance. If the circle
%   centered at curPoint intersects with the reference path, then the 
%   intersection nearest the path end is used. The projection of curPoint
%   onto the path is used if no intersection occurs.

% Copyright 2021 The MathWorks Inc.

    % Calculate required vectors
    v1 = (waypoints(:,2)-waypoints(:,1))';
    v2 = curPoint(1:2)-waypoints(:,1)';
    d1 = norm(v1);
    v1Unit = v1/d1;
    projDist = (v1Unit*v2(:));
    isIntersecting = true;
    
    if isnan(projDist)
        % Current point is start of segment
        refDist = max(R,refDist);
    else
        vProj = v1Unit*projDist;
        dPerp = norm(v2-vProj);
        if dPerp > R
            % Lookahead circle does not intersect with refPath
            isIntersecting = false;
            refDist = max(projDist,refDist);
        else
            % Use lookahead point lying beyond target if trajectory is
            % already following the path.
            refDist = max(refDist,projDist+(R^2-dPerp^2)^(0.5));
        end
    end
    refPt = waypoints(:,1)'+v1Unit*refDist;
end