%% Scenario Variant Generation
% This example shows how to modify a driving scenario object consisting of a collision between two actors, using new ego speed and keeping the collision the same.
% This workflow along with the scenario’s road information can be used to automatically create variants of a driving scenario object with new ego speed and keeping the collision the same. Variants of a driving scenario object can be used when designing and validating automated driving functions especially functionalities related to automated emergency braking systems.
%% Select Scenario and New Ego Speed
% Select a scenario object which has a collision between two actors to use as a seed scenario. Pre-built scenarios like AEB_PedestrainTurning_Nearside_50width can be used as a seed scenario or using the Driving Scenario Designer app export one of the pre-built scenarios as a .m file to create the input driving scenario object. The two colliding actors are called ego and target actors in this example. Select a new speed for ego, which should be a scalar value. Also, in the seed scenario object, the two colliding actors should be traveling at a constant speed.
% A driving scenario object is modified, actor's trajectories are modified such that a collision takes place. The driving scenario object is created using a .m file which was generated from the Driving Scenario Designer app by exporting one of the pre-built scenarios named “AEB_PedestrianChild_Nearside_50width".

scenario = AEB_PedestrianChild_Nearside_50width;

% Store ego and target
targetOld = scenario.Actors(1,1);
egoOld = scenario.Actors(1,2);

% Define new waypoints and speed for target
targetWaypoints = [32 -4 0;32 -3 0;32 0 0;32 4.6 0];
targetSpeed = 2.6;

% Define new waypoints and speed for ego
egoWaypoints = [8.3 0 0;28.8 0 0;40 0 0];
egoSpeed = 14;

% Call to trajectory function will apply new waypoints and speed
trajectory(targetOld,targetWaypoints,targetSpeed);
trajectory(egoOld,egoWaypoints,egoSpeed);

% Visualize the original scenario.

currentFigure = figure('Name', 'Original Scenario', 'Position', [400 50 900 700]);
panel = uipanel(currentFigure);
currentAxes = axes(panel);
chasePlot(egoOld, ...
    'Parent', currentAxes, 'ViewPitch', 90, 'ViewHeight', 120, 'ViewLocation', [0, 0]);
hold(currentAxes, 'on');

while advance(scenario)
    pause(0.01);
end
close all;

%
%  Select new speed for ego. Here, the ego's new speed is selected such that it is late to the collision point as compared to the target actor.

egoNewSpeed = 7;

%
%%  Find Collision Time Instant
% Computes the time instant at which the two actors are colliding in the seed scenario and finds ego or target or both the actors if not specified. Actors under collision, ego or target or both can be specified through their ActorID as a Name-Value pair.

% ActorID for ego actor - egoID 
% ActorID for target actor - targetID
egoID = 2;
targetID = 1;
time = helperComputeCollisionTimeInstant(scenario,'EgoID',egoID,'TargetID',targetID);

%
%  Other valid function calls of this function are:
% [time,egoID,~] = helperComputeCollisionTimeInstant(scenario,'TargetID',1) - Ego's ActorID is not specified so this step will search for an actor with name 'ego' or 'vehicle under test' or 'host' to use as an ego actor. If no actor is found with these names then 1 is selected as a value for ego's ActorID, that is the first actor is selected as an ego.
% [time,~,targetID] = helperComputeCollisionTimeInstant(scenario,'EgoID',2) - Target's ActorID is not specified so this step will search for an actor with name 'target' to use as an target actor. If no actor is found with these names then 2 is selected as a value for the target's ActorID, that is the second actor is selected as a target actor.
% [time,egoID,targetID] = helperComputeCollisionTimeInstant(scenario) - Ego and Target's ActorID are not specified so this step will search for actors with relevant names, as explained in above function calls to use as ego and target actors
% Helper function helperComputeCollisionTimeInstant expects a collision between ego and target, errors out if that's not the case. Variable "time" holds the time instant of collision in seconds. Similarly "egoID" and "targetID", if returned by the helper function will have ActorID for ego and target actors respectively.

time

% For scenarios consisting of a collision between ego and target at merging and junction, Time instant returned in variable "time" may not be completely accurate resulting in not accurate results in subsequent steps
%% Compute Modifications
% Computes the modifications required to be made in the driving scenario object. The helper function "helperComputeArrivalTimeDiff" supports three different methods for computing the modifications, which are passed as an input to this helper function. These methods are:
% a. method = 'WaitTime' :
% An actor can be made to wait on a particular waypoint in its trajectory using the input argument "WaitTime" of the trajectory function. In this method, either the ego or target actor (or both actors) wait on their path. The actor who is arriving before the collision point because of the ego's new speed is made to wait on its trajectory to make sure that both actors arrive at the same time on the collision point. Here, WaitTime is attached to the first waypoint from all the waypoints present in the actor's trajectory. As the "WaitTime" is used, it means one waypoint will have zero speed leading to acceleration as the actor will increase speed from zero to the new speed.
% b. method = 'EntryTime':
% An actor can be made to enter late into the scenario by setting its "EntryTime" attribute. In this method, the "EntryTime" attribute of the actor who is arriving before the collision point because of the ego's new speed is modified to make sure both the actors arrive at the same time on the collision point.
% c. method = 'StartingPosition':
% In this method, the starting position of the target actor is changed to decrease the distance target has to travel to reach the collision point. This makes sure that both actors arrive at the same time on the collision point. This method is applicable only when the ego arrives before the collision point because of its new speed, and errors out if the target arrives before. This method may error out due to precision issues, for such cases it is suggested to use other methods present.

method = 'WaitTime';

% Structures egoData and targetData store speed, waypoints and ActorID
egoData = struct('speed', egoSpeed, 'waypoints', egoWaypoints, 'id', egoID);
targetData = struct('speed', targetSpeed, 'waypoints', targetWaypoints, 'id', targetID);


% Structure modifications stores the modifications to be made in the scenario object
modifications = helperComputeCollisionTimeDiff(scenario,egoNewSpeed,time,egoData,targetData,method);

%
% egoSpeed and targetSpeed passed should be scalar values as ego and target actors are travelling with constant speed. Add yaw information as a field to the respective structure ("egoData" or "targetData" or both), if it is present in the respective trajectory in the seed scenario. Input argument "method" should be the last input argument, if no method is specified 'WaitTime' is used.
% Output structure "modifications" has three fields, values are populated (if any) in these fields depends upon the method selected in the step :
% egoCollisionTimediff: This field is used to store the time difference that needs to be adjusted in the ego's trajectory. It is populated if the method is 'WaitTime' or 'EntryTime'.
% targetCollisionTimediff: This field is used to store the time difference that needs to be adjusted in the target's trajectory. It is populated if the method is 'WaitTime' or 'EntryTime'.
% waypoints: This field is used to store new waypoints for the target actor. It is populated if the method is 'StartingPosition'
%% Modify Scenario
% The scenario object is modified using ego's new speed and modifications structure computed in the last step. Because of the new speed ego is arriving late at the collision point, hence target waits at the collision point.

   helperModifyScenario(scenario,egoNewSpeed,egoData,targetData,modifications,method);

%  Input argument "method" should be the last input argument, if no method is specified 'WaitTime' is used.
%% Visualize Modified Scenario
% Visualize the modified scenario

currentFigure = figure('Name','Modified Scenario','Position',[400 50 900 700]);
panel = uipanel(currentFigure);
currentAxes = axes(panel);
chasePlot(scenario.Actors(1,egoID),'Parent',currentAxes,'ViewPitch', 90,'ViewHeight', 120, 'ViewLocation', [0,0]);
hold(currentAxes,'on');
while advance(scenario)
    pause(0.01);
end

%% Summary
% In this example, ego and target trajectories were modified to keep the collision the same even with new ego speed. This workflow can be used along with scenario road information to automatically create variants.
% Copyright 2021 The MathWorks, Inc.