function rearVehicleIndex = helperFindRearVehicle(actorsInWorld,...
                                                vehicleInWorld,...
                                                vehicleCurrentLaneWidth)
%helperFindRearVehicle checks for the existence of rear vehicle and
%extracts the actorID of the rear vehicle
%
%
% The helperFindRearVehicle identifies the rear vehicle based on following
% conditions:
% * Within the ego car's lane.
% * At rear of the ego car.
% * Closest to the ego car.
% This function uses ground truth actors information
%   
%   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
minX = cast(-200, 'like', vehicleInWorld.Position(1));  % Far enough backward 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(vehicleInWorld.AngularVelocity(3))/norm(vehicleInWorld.Velocity);

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

% Iterate over all the vehicles to find the rear vehicle
% This follows following steps:
% 1) Check if the x-position of the vehicle is greater than minX and less
% 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 minX and update
% mioTrackIndex.
% 4) Repeat the same process over all the vehicles.
for i = 1:numel(actorsInWorld)
    vehicle = actorsInWorld(i);
    if ~isequal(vehicleInWorld.ActorID, vehicle.ActorID)
        

        if ~isempty(vehicle.Position)
            x = 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 x < 0 && x > minX % No point checking otherwise
            
            yleftLane  = polyval(lb,x); % lateral position of left lane
            yrightLane = polyval(rb,x); % lateral position of right lane
            
            % Find a new rear car
            if (yrightLane <= y) && (y <= yleftLane)
                minX = x;
                mioTrackIndex(:) = i;
            end
        end
    end
end
% Check if there is any rear vehicle and assign the ActorID to output.
if mioTrackIndex>0
    rearVehicleIndex = actorsInWorld(mioTrackIndex).ActorID;
else
    rearVehicleIndex = inf;
end
end