function [distanceToLeadVehicle,...
          velocityOfLeadVehicle,...
          leadVehicleID] = helperFindLeadVehicle(targetVehicles,...
                                                 egoVehicle, laneWidth)
%helperFindLeadVehicle checks for the lead vehicle and computes distance
%between lead vehicle and ego.
%
% The helperFindLeadVehicle identifies the lead vehicle based on following
% conditions:
% * Within the ego car's lane.
% * In front of the ego car.
% * Closest to the ego car.
%
%   This is a helper function for example purposes and may be removed or
%   modified in the future.
%
% Copyright 2020 The MathWorks, Inc.

%% Initialize outputs and parameters
maxX = cast(200, 'like', egoVehicle.Position(1));  % Far enough forward so that no track is expected to exceed this distance
mioTrackIndex = 0;

% The curvature of the lane is computed using the ego velocity and angular
% velocity as this example requires lanes for each vehicle in the scenario.
% Finding lanes for each vehicle result in more complex system.
kappa = deg2rad(egoVehicle.AngularVelocity(3))/norm(egoVehicle.Velocity);

% Construct lane boundary information from the curvature
lb = [0, kappa, 0, laneWidth/2];
rb = [0, kappa, 0, -laneWidth/2];

% Iterate over all the vehicles to find the lead vehicle
% This follows following steps:
% 1) Check if the x-position of the vehicle is less than maxX and grater
% than 0
% 2) Get lateral position of left lane boundary and right lane boundary to
% check if the vehicle is with in that lane.
% 3) If the vehicle is with in that lane update MaxX and update
% mioTrackIndex.
% 4) Repeat the same process over all the vehicles.

for i = 1:numel(targetVehicles)
    vehicle = targetVehicles(i);
    if ~isequal(egoVehicle.ActorID, vehicle.ActorID)
        
        if ~isempty(vehicle.Position)
            posX = vehicle.Position(1); % Longitudinal position of the track relative to ego
            y = vehicle.Position(2); % Lateral position of the track relative to ego
        else
            disp('No Position from actor');
        end
        
        if posX < maxX && posX > 0 % No point checking otherwise
            
            yleftLane  = polyval(lb,posX); % lateral position of left lane
            yrightLane = polyval(rb,posX); % lateral position of right lane
            
            % Find a new lead car
            if (yrightLane <= y) && (y <= yleftLane)
                maxX = posX;
                mioTrackIndex(:) = i;
            end
        end
    end
end

% Check if there is any lead vehicle
if mioTrackIndex>0
    longitudinalDistance = targetVehicles(mioTrackIndex).Position(1,1);
    vehVelocity = norm(targetVehicles(mioTrackIndex).Velocity);
    leadVehicleID = targetVehicles(mioTrackIndex).ActorID;
else
    longitudinalDistance = inf;
    vehVelocity = inf;
    leadVehicleID = inf;
end

% Assign output
distanceToLeadVehicle = longitudinalDistance;    % Longitudinal position of the lead car
velocityOfLeadVehicle = vehVelocity;   % Longitudinal velocity of the lead car
end