function [waypoints] = helperComputeStartPosition(egoNewSpeed,time,egoData,targetData)
%   This script computes modifications because of the ego actor's new speed
%   in terms of modified waypoints of target

%   This method is applicable only when ego arrives before on the collision
%   point

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

%   Copyright 2021 The MathWorks, Inc.

egoOldSpeed = egoData.speed;
egoNewTime = (egoOldSpeed*time)/egoNewSpeed;
if egoNewTime == time
    waypoints = targetData.waypoints;
    return;
end
if time < egoNewTime
    error("StartingPosition method is valid only when the ego arrives" + ...
        " before the collision point");
end
egoTimeDiff = time - egoNewTime;
% Amount of distance to move the target
distance = targetData.speed*egoTimeDiff;
% Store all target positions
pathpoints = targetData.points;
waypoints = targetData.waypoints;
% Store new starting position in structure "newStartPosition"
newStartPosition = struct('x',-1,'y',-1,'z',-1);
% Error Limit
errorLimit = 0.1;
% Find a point from pathpoints that is required distance away from the first waypoint of the target 
% Store it as a new starting position
temp = abs(sqrt((waypoints(1,1)-pathpoints(:,1)).^2 ...
    + (waypoints(1,2)-pathpoints(:,2)).^2) - distance);
point = find(temp < errorLimit,1);
if ~isempty(point)
    newStartPosition.x = pathpoints(point,1);
    newStartPosition.y = pathpoints(point,2);
    newStartPosition.z = pathpoints(point,3);
end
% Unable to find a new starting position
if newStartPosition.x == -1 || newStartPosition.y == -1
    error('Precision error, try another method');
end
% Below codes check if the new starting position is the same as one of the existing waypoints
index = find((abs(newStartPosition.x - waypoints(:,1)) < errorLimit)...
    & (abs(newStartPosition.y - waypoints(:,2)) < errorLimit),1);
if ~isempty(index)
    index = index-1;
    % No waypoint to be deleted
    if index == 0
        return;
    end
end

% Below code checks if the new starting position lies between existing waypoints
% Delete all waypoints before the new starting position
if isempty(index)
    for i =1:size(waypoints,1)-1
        minx = min(waypoints(i,1),waypoints(i+1,1));
        maxx = max(waypoints(i,1),waypoints(i+1,1));
        miny = min(waypoints(i,2),waypoints(i+1,2));
        maxy = max(waypoints(i,2),waypoints(i+1,2));
        if (newStartPosition.x >= minx || abs(newStartPosition.x-minx)< errorLimit)...
                && (newStartPosition.x <= maxx|| abs(newStartPosition.x-maxx)< errorLimit)...
                &&(newStartPosition.y >= miny || abs(newStartPosition.y-miny)< errorLimit)...
                && (newStartPosition.y <= maxy || abs(newStartPosition.y-maxy)< errorLimit)
            index = i;
            break;
        end
    end
end
if isempty(index)
    error('Precision error, try another method');
end

% Delete waypoints till index position
if index >= 1
    waypoints(1:index,:) = [];
end
waypoints = [newStartPosition.x newStartPosition.y newStartPosition.z;waypoints];