function [time,egoID,targetID,collision] = helperComputeCollisionTimeInstant(scenario,varargin)
%   This function computes collision time instant of ego and target actors

%   This is a helper script for example purposes and may be modified in the future.

%   Copyright 2021 The MathWorks, Inc.

% Validation checks on input arguments
close all
narginchk(1,5);
nargoutchk(0,4);
[egoID,targetID] = helperInputValidation(scenario,varargin{:});

% Error cases
if egoID == targetID
    error('EgoID and TargetID passed are equal');
end
try
    restart(scenario);
catch
    error('Error in restart function: Check for open figure windows');
end
ego = scenario.Actors(1,egoID);
target = scenario.Actors(1,targetID);

% Steps to find target ActorID in target poses among all actors
targetPosesEgoCoords = targetPoses(ego);
index = 0;
for i=1:length(targetPosesEgoCoords)
    if targetPosesEgoCoords(i).ActorID == targetID
        index = i;
        break;
    end
end

% Constant to be used in further equations based on actor's dimensions
if isa(ego,'driving.scenario.Vehicle')
    if targetPosesEgoCoords(index).Velocity(1) < 0
        egoxdim = ego.Wheelbase + ego.FrontOverhang;
    else
        egoxdim = ego.RearOverhang;
    end
    egoydim = ego.Width/2;
else
    egoxdim = ego.Width/2;
    egoydim = ego.Length/2;
end
if isa(target,'driving.scenario.Vehicle')
    targetxdim = target.Wheelbase + target.FrontOverhang;
    targetydim = target.Width/2;
else
    targetxdim = target.Width/2;
    targetydim = target.Length/2;
end
yconst = targetydim + egoydim;
xconst = egoxdim + targetxdim;

% Variable to store instant of (First) collision  between ego and target
time=0;
% Variable to indicate collision occurred or not
collision = false;
while advance(scenario)
    % Position of pedestrian according to Ego vehicle
    targetPosesEgoCoords = targetPoses(ego);
    targetXCoord = targetPosesEgoCoords(index).Position(1,1);
    targetYCoord = targetPosesEgoCoords(index).Position(1,2);
    % Equations to check collision
    if abs(targetXCoord) <= xconst && abs(targetYCoord) <= yconst
        time = scenario.SimulationTime;
        collision = true;
        break;
    end
end

%Restart scenario so that, the scenario can be visualized using plot
try
    restart(scenario);
catch
    error('Error in restart function: Check for open figure windows');
end
if ~collision
    error('Ego and Target are not colliding. Check for ActorIDs passed');
end

end

function [egoID,targetID] = helperInputValidation(scenario,varargin)
% This function performs validation checks on the input arguments.
% If ego or target actor ActorIDs are not passed, it searches for
% most suitable ego or target actor and uses their ActorIDs

validateattributes(scenario, {'drivingScenario'}, {'nonempty'});
numActors = length(scenario.Actors);
if numActors < 2
    error(message('Less than two actors in the scenario'));
end
parser = inputParser;
parser.addOptional('EgoID',0,@(EgoID)validateattributes(EgoID,{'double'},...
    {'nonempty','positive','integer','scalar','finite','>',0,'<=',numActors}));
parser.addOptional('TargetID',0,@(TargetID)validateattributes(TargetID,{'double'},...
    {'nonempty','positive','integer','scalar','finite','>',0,'<=',numActors}));
parser.parse(varargin{:});
results = parser.Results;
egoID =  0;
targetID = 0;
actors = scenario.Actors;

if results.EgoID == 0
    % Search for a suitable ego
    for i=1:length(actors)
        ac = actors(1,i);
        if contains(ac.Name,'ego','IgnoreCase',true)||contains(ac.Name,'test','IgnoreCase',true)           
            egoID = ac.ActorID;
            break;
        end
    end
    if egoID == 0
        egoID = 1;
    end
else
    egoID = results.EgoID;
end

if results.TargetID == 0
    % Search for a suitable target
    for i=1:length(actors)
        ac = actors(1,i);
        if contains(ac.Name,'target','IgnoreCase',true)
            targetID = ac.ActorID;
            break;
        end
    end
    if targetID == 0
        targetID = 2;
    end
else
    targetID = results.TargetID;
end
end