%% Driving Scenario for Changing Lanes and Passing Vehicles
% This example shows how to create a driving scenario for maneuvers such as changing lanes and passing other vehicles. You create roads with passing zones and add vehicles to the scenario. Then, define the trajectories for these vehicles to simulate vehicle lane change in passing zones.
%% Create Road with Passing Zones by Using Composite Lane Marking
% Create a driving scenario. Specify the road centers and the number of lanes to add a two-way, two-lane straight road of 54 meters with draw direction from top-to-bottom.

scenario = drivingScenario('StopTime',10);
roadCenters = [50 0; -4 0];
numLanes = [1 1];

% Typically, the number of lane markings is equal to number of lanes plus one.  A two-way, two-lane road has 3 lane markings and the outermost lane markings at both the edges are solid white lines.
% Create a solid marking object of marking width 0.25 meters, to constitute the outermost lane markings for the two-way road.

outerLM = laneMarking('Solid','Width',0.25);

% Create a lane marking array of SolidMarking and DashedMarking objects that contain the properties for solid and dashed double yellow lines.

lmArray = [laneMarking('DoubleSolid','Color','Yellow','Width',0.25)
           laneMarking('DashedSolid','Color','Yellow','Length',1,'Space',1.5,'Width',0.25)
           laneMarking('DoubleSolid','Color','Yellow','Width',0.25)
           laneMarking('SolidDashed','Color','Yellow','Length',1,'Space',1.5,'Width',0.25)];

% Create a composite lane marking object for the center lane marking by using the lane marking array. Specify the normalized length for each marking object.

centerLM = laneMarking(lmArray,'SegmentRange',[0.1 0.25 0.2 0.35]);

% Create a vector of the outermost and the center lane marking objects. Pass the vector as input to the lanespec function in order to define the lane specifications of the road.

marking = [outerLM centerLM outerLM];
ls = lanespec(numLanes,'Width',7,'Marking',marking);

% Add the road to the driving scenario. Plot the driving scenario. Since the draw direction of the road is from top-to-bottom, the marking types in the composite lane marking also occur in top-to-bottom order.

road(scenario,roadCenters,'Lanes',ls);
figMark = figure;
set(figMark,'Position',[0 0 600 600]);
hPlot = axes(figMark);
plot(scenario,'Parent',hPlot);
title('Composite Marking: Road with Passing Zones')

%
%% Simulate Vehicle Lane Change in Passing Zones
% Add a slow moving vehicle (SMV) to the scenario. Specify the waypoints and speed value to set the trajectory for the SMV.

slowVehicle = vehicle(scenario,'ClassID',1,'Position',[37 -3 0]);
waypoints = [37 -3;12 -3];
speed = 2;
smoothTrajectory(slowVehicle,waypoints,speed);

% Add another vehicle to the scenario. Set the trajectory for the vehicle in such a way that it passes the SMV in front of it by changing lanes at the passing zones.

passingVehicle = vehicle(scenario,'ClassID',1,'Position',[49 -3 0]);
waypoints = [49 -3; 45 -3; 40 -3; 35 0;
             30 3; 26 3; 22 3; 18 3;
             8 0; 5 -2; 2 -3; 1 -3];
speed = 6;
smoothTrajectory(passingVehicle,waypoints,speed);

% Create a custom figure window and plot the scenario.

close all;
figScene = figure;
set(figScene,'Position',[0 0 600 600]);
hPanel = uipanel(figScene);
hPlot = axes(hPanel);
plot(scenario,'Parent',hPlot);
title('Passing Zone: Change Lane and Pass Other Vehicle')

% Run the simulation
while advance(scenario)
    pause(0.01)
end

% Copyright 2019 The MathWorks, Inc.