%% Helper function : exampleHelperCreateStraightRoadScenario
% This helper function is to create straight road scenario with 
% 1. Four lanes road constructed with given road center points
% 2. Creation of ego vehicle
% 3. Specify the trajectory of each vehicle through 
%    given N waypoints and speed at each waypoint

% This function takes nothing and ouputs scenario handle and ego vehicle
% handle

% Copyright 2019 The MathWorks, Inc.

function [scenario, egoVehicle] = exampleHelperCreateStraightRoadScenario()
% Generated by MATLAB(R) 9.7 (R2019b) and Automated Driving Toolbox 3.0
% (R2019b). Generated on: 09-Jul-2019 19:15:14

% Construct a drivingScenario object.
scenario = drivingScenario;

% Setting up sample time to match with sensor update rate
scenario.SampleTime = 0.1;

% Setting up simulation stop time
scenario.StopTime   = Inf;

% Add all road segments
roadCenters = [-200 400 0; 50 400 0];

% Create a lane specification
% Here 4 represents number of lanes to create
laneSpecification = lanespec(4);

% Create a road segment with given road centers and lane spec information
road(scenario, roadCenters, 'Lanes', laneSpecification);

% Add the ego vehicle. Define waypoints to follow and speed of the vehicle,
% based on that trajectory will be generated for the ego vehicle.
egoVehicle = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-193.19 398.34 0.01]);
waypoints = [-193.19 398.34 0.01;
    -176.8 398.34 0.01;
    -151.92 401.69 0.01;
    -138.58 401.6 0.01;
    -126.08 401.47 0.01;
    -103.63 401.22 0.01;
    -30 401.5 0.01];
speed = 10; % Constant speed at all waypoints
trajectory(egoVehicle, waypoints, speed);

% Add the target vehicle1. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle1.
targetVehicle1 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-194 394.97 0.01]);
waypoints = [-194 394.97 0.01;
    -139.7 394.97 0;
    -52.35 394.97 0;
    19.1 394.97 0;
    44.4 394.97 0];
speed = [5;6;0.001;0.1;0];
trajectory(targetVehicle1, waypoints, speed);

% Add the target vehicle2. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle2.
targetVehicle2 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-192.8 405 0]);
waypoints = [-192.8 405 0;
    -138.1 405 0;
    -66 405 0;
    -42.1 405 0;
    43.2 405 0];
speed = [5;0;0.1;1;0];
trajectory(targetVehicle2, waypoints, speed);

% Add the target vehicle3. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle3.
targetVehicle3 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-117.99 398.5 0.01]);
waypoints = [-117.99 398.5 0.01;
    -75.2 398.5 0;
    -67.44 398.5 0;
    -37.1 398.5 0;
    0 398.5 0;
    30.4 398.5 0;
    47.1 398.5 0];
speed = [5;2;3;8;4;1;0];
trajectory(targetVehicle3, waypoints, speed);

% Add the target vehicle4. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle4.
targetVehicle4 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-44.83 394.91 0.01]);
waypoints = [-44.83 394.91 0.01;
    -14.59 394.91 0.0001;
    9.86 394.91 0;
    28 394.91 0;
    44.6 394.91 0];
speed = [5;1;3;0.001;0];
trajectory(targetVehicle4, waypoints, speed);

% Add the target vehicle5. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle5.
targetVehicle5 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [-20 398.5 0]);
waypoints = [-20 398.5 0;
    4 398.5 0;
    32.11 398.5 0;
    43.36 398.5 0.01];
speed = [5;0.5005;2;0];
trajectory(targetVehicle5, waypoints, speed);

% Add the target vehicle6. Define waypoints to follow and speed of the
% vehicle, based on that trajectory will be generated for the target
% vehicle6.
targetVehicle6 = vehicle(scenario, ...
    'ClassID', 1, ...
    'Position', [15 401.5 0.01]);
waypoints = [-149.39 398.41 0.01;
    -139.34 398.4 0.01;
    -127.5 398.4 0];
speed = [5;0.001;0.001];
trajectory(targetVehicle6, waypoints, speed);



