function modifications = helperComputeArrivalTimeDiff(scenario,egoNewSpeed,time,egoData,targetData,method)
%   This script computes modifications depending upon the method
%   WaitTime, EntryTime or StartingPosition

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

%   Copyright 2021 The MathWorks, Inc.

% If a method is not passed as input
if nargin == 5
    method = 'WaitTime';
end

% Store complete Pose and Sample Times information of actors
data = record(scenario);
restart(scenario);
pose = [data(:).ActorPoses];
sampleTimes = [data(:).SimulationTime]';

% Store ego actor's Positions
idx = ([pose(:,1).ActorID] == egoData.id);
egoPoints = reshape([pose(idx,:).Position]',3,size(pose,2))';
% Check for stationary ego
if size(unique(egoPoints,'rows'),1) == 1
    error('Ego is stationary');
end

% Variable 'egoArrivalTimes' stores the simulation time instant at ego's
%waypoints
errorLimit = 0.05;
egoArrivalTimes = ones(size(egoData.waypoints,1),1)*-1;
index = 1;
% Below loop finds the time instants when ego reaches its waypoints
for i = 1:size(egoPoints,1)
    if (abs(egoPoints(i,1) - egoData.waypoints(index,1)) < errorLimit)...
            && (abs(egoPoints(i,2) - egoData.waypoints(index,2)) < errorLimit)
        egoArrivalTimes(index) = sampleTimes(i);
        index = index+1;
    end
    if index > size(egoData.waypoints,1)
        break;
    end
end
% Simulation time instant, when ego reaches second waypoint (value of egoArrivalTimes(2)), is used in the method -"WaitTime"
% Below code finds approximate value if no value was found before
if egoArrivalTimes(2) == -1
    egoArrivalTimes(2) = norm(egoData.waypoints(1,:,:) ...
        - egoData.waypoints(2,:,:))/egoData.speed;
end

% Store ego's arrival times and ego's position in struct 'egoData'
egoData.arrivaltimes = egoArrivalTimes;
egoData.points = egoPoints;

% Store target actor's Positions
idx = ([pose(:,1).ActorID] == targetData.id);
targetPoints = reshape([pose(idx,:).Position]',3,size(pose,2))';
if size(unique(targetPoints),1) == 1
    error('Target is stationary');
end

% Variable 'targetArrivalTimes' stores the simulation time instant at target's waypoints
targetArrivalTimes = ones(size(targetData.waypoints,1),1)*-1;
index =1;
% Below loop finds the time instant when the target reaches its waypoints
for i = 1:size(targetPoints,1)
    if (abs(targetPoints(i,1) - targetData.waypoints(index,1)) < errorLimit)...
            && (abs(targetPoints(i,2) - targetData.waypoints(index,2)) < errorLimit)
        targetArrivalTimes(index) = sampleTimes(i);
        index = index+1;
    end
    if index > size(targetData.waypoints,1)
        break;
    end
end

% Simulation time instant, when the target reaches the second waypoint (value of targetArrivalTimes(2)), is used in the method -"WaitTime"
% Below code finds approximate value if no value was found before
if targetArrivalTimes(2) == -1
    targetArrivalTimes(2) = norm(targetData.waypoints(1,:,:) ...
        - targetData.waypoints(2,:,:))/targetData.speed;
end

% Store target's arrival times and target's position in struct targetData'
targetData.arrivaltimes = targetArrivalTimes;
targetData.points = targetPoints;
% Struct egoData and targetData hold data required by helper functions


% Struct variable 'modifications' store the modifications that need to be made in the scenario object
modifications = struct('egoCollisionTimediff',0,'targetCollisionTimediff',0,'waypoints',[]);
% Below steps calls different functions depending upon the type of method
try
    if strcmp(method,'WaitTime')
        [modifications.egoCollisionTimediff,modifications.targetCollisionTimediff]...
            = helperComputeArrivalTimeDiffWaittime(egoNewSpeed,time,egoData,targetData);
    elseif strcmp(method,'EntryTime')
        [modifications.egoCollisionTimediff,modifications.targetCollisionTimediff]...
            = helperComputeArrivalTimeDiffEntrytime(egoNewSpeed,time,egoData);
    elseif strcmp(method,'StartingPosition')
        % This method works only if ego is arriving before on the collision point
        modifications.waypoints...
            = helperComputeStartPosition(egoNewSpeed,time,egoData,targetData);
    else
        error('Invalid method');
    end
catch ME
    error(ME.message);
end
end