%% Generate Stop-and-Go Driving Scenario
% Create a driving scenario consisting of two, two-lane roads that intersect at a right angle.

scenario = drivingScenario('StopTime',2.75);
roadCenters = [50 1 0; 2 0.9 0];
laneSpecification = lanespec(2,'Width',4);
road(scenario,roadCenters,'Lanes',laneSpecification);
roadCenters = [27 24 0; 27 -21 0];
road(scenario,roadCenters,'Lanes',laneSpecification);

% Add an ego vehicle to the scenario. Specify the waypoints and the speed values for the vehicle at each waypoint. Set a wait time for the vehicle at the second waypoint. Generate a trajectory in which the ego vehicle travels through the specified waypoints at the specified speed.

egoVehicle = vehicle(scenario,'ClassID',1,'Position',[5 -1 0]);
waypoints = [5 -1 0; 16 -1 0; 40 -1 0];
speed = [30; 0; 30];
waittime = [0; 0.3; 0];
trajectory(egoVehicle,waypoints,speed,waittime);

% Add a car to the scenario. Specify the waypoints and the speed values for the car at each waypoint. Set a wait time for the car at the second waypoint. Generate a trajectory in which the car travels through the specified waypoints at the specified speed.

car = vehicle(scenario,'ClassID',1,'Position',[48 4 0],'PlotColor',[0.494 0.184 0.556], 'Name','Car');
waypoints = [47 3 0; 38 3 0; 10 3 0];
speed = [30; 0; 30];
waittime = [0; 0.3; 0];
trajectory(car,waypoints,speed,waittime);

% Add an ambulance to the scenario. Generate a trajectory in which the ambulance travels through the specified waypoints at a constant speed.

ambulance = vehicle(scenario,'ClassID',6,'Position',[25 22 0],'PlotColor',[0.466 0.674 0.188],'Name','Ambulance');
waypoints = [25 22 0; 25 13 0; 25 6 0; 26 2 0; 33 -1 0; 45 -1 0];
speed = 25;
trajectory(ambulance,waypoints,speed);

% Create a custom figure window to plot the scenario.

fig = figure;
set(fig,'Position',[0,0,800,600]);
movegui(fig,'center');
hViewPnl = uipanel(fig,'Position',[0 0 1 1],'Title','Stop-and-Go Scenario');
hPlt = axes(hViewPnl);

% Plot the scenario and run the simulation. The ego vehicle and the car pause for their specified wait times to avoid collision with the ambulance.

plot(scenario,'Waypoints','on','RoadCenters','on','Parent',hPlt)
while advance(scenario)
    pause(0.1)
end

% Copyright 2019 The MathWorks, Inc.