function  [msc] = helperModifyScenario(scenario,egoNewSpeed,egoData,targetData,modifications,method)
%   This script applies computed modifications to the seed scenario

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

%   Copyright 2021 The MathWorks, Inc.

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

for i = 1 : numel(scenario.Actors)
    ac = scenario.Actors(1,i);
    if ac.ActorID == egoData.id
        waypoints = egoData.waypoints;
        if isfield(egoData,'yaw')
            % egoData has yaw information
            yaw = egoData.yaw;
        else
            % egoData has no yaw information
            yaw = [];
        end
        if isfield(targetData,'waittime')
            % egoData has waittime information
            waitTime = egoData.waittime;
        else
            % egoData has no waittime information
            waitTime = [];
        end
    elseif ac.ActorID == targetData.id
        waypoints = targetData.waypoints;
        if isfield(egoData,'yaw')
            % targetData has yaw information
            yaw = targetData.yaw;
        else
            % targetData has no yaw information
            yaw = [];
        end
        if isfield(targetData,'waittime')
            % targetData has waittime information
            waitTime = targetData.waittime;
        else
            % targetData has no waittime information
            waitTime = [];
        end
    end

    if ac.ActorID == egoData.id || ac.ActorID == targetData.id
        newSpeed = ones(size(waypoints,1),1);
        if ac.ActorID == egoData.id
            % copy ego's new speed
            newSpeed = newSpeed * egoNewSpeed;
        else
            newSpeed = newSpeed * targetData.speed;
        end

        if strcmp(method,'WaitTime')
            % For waittime method assign speed zero to first waypoint and
            % the calculated waittime value
            if ac.ActorID == egoData.id && modifications.egoCollisionTimeDiff > 0
                if ~isempty(waitTime)
                    newWaitTime = zeros(size(waypoints,1),1)+ waitTime;
                else
                    newWaitTime = zeros(size(waypoints,1),1);
                end
                newSpeed(1) = 0;
                newWaitTime(1) = modifications.egoCollisionTimeDiff;
                waitTime = newWaitTime;
            end
            if ac.ActorID == targetData.id && modifications.targetCollisionTimeDiff > 0
                if ~isempty(waitTime)
                    newWaitTime = zeros(size(waypoints,1),1)+ waitTime;
                else
                    newWaitTime = zeros(size(waypoints,1),1);
                end
                newSpeed(1) = 0;
                newWaitTime(1) = modifications.targetCollisionTimeDiff;
                waitTime = newWaitTime;
            end
        elseif strcmp(method,'EntryTime')
            % Modify actor's EntryTime property
            if ac.ActorID == egoData.id && modifications.egoCollisionTimeDiff > 0
                ac.EntryTime = modifications.egoCollisionTimeDiff;
            end
            if ac.ActorID == targetData.id && modifications.targetCollisionTimeDiff > 0
                ac.EntryTime =  modifications.targetCollisionTimeDiff;
            end
        elseif strcmp(method,'StartingPosition')
            % Modify waypoints,speed,yaw and waittime attribute of actors tarjectory
           
            if ac.ActorID == targetData.id
                waypoints = modifications.waypoints;
                newsize = size(waypoints,1);
                if size(waitTime,1) > 1 && size(waitTime,1)>newsize
                    % Delete values from waittime attribute as new waypoints
                    % for target actor has less size than original scenario
                    len = size(waitTime,1);
                    last = waitTime(len-newsize+1);
                    diff = len-newsize;
                    waitTime(1:diff) = [];
                    waitTime(1) = last;
                end
                if size(newSpeed,1) > 1 && size(newSpeed,1)>newsize
                    % Delete values from speed attribute as new waypoints
                    % for target actor has less size than original scenario
                    len = size(newSpeed,1);
                    last = newSpeed(len-newsize+1);
                    diff = len-newsize;
                    newSpeed(1:diff) = [];
                    newSpeed(1) = last;
                end
                if size(yaw,1) > 1 && size(yaw,1)>newsize
                    % Delete values from yaw attribute as new waypoints
                    % for target actor has less size than original scenario
                    len = size(yaw,1);
                    last = yaw(len-newsize+1);
                    diff = len-newsize;
                    yaw(1:diff) = [];
                    yaw(1) = last;
                end
            end
        end
        makeTrajectory(newSpeed,waypoints,yaw,waitTime,ac)
    end
end
msc = scenario;
end


function makeTrajectory(speed,waypoints,yaw,waittime,newactor)
% This function makes calls to trajectory function
if isempty(waittime)
    if isempty(yaw)
        trajectory(newactor,waypoints,speed);
    else
        trajectory(newactor,waypoints,speed,'Yaw',yaw);
    end
else
    if  isempty(yaw)
        trajectory(newactor,waypoints,speed,waittime);
    else
        trajectory(newactor,waypoints,speed,waittime,'Yaw',yaw);
    end
end
end