%% Simulate Car Traveling on S-Curve
% Simulate a driving scenario with one car traveling on an S-curve.
% Create and plot the lane boundaries.

%%
% Create the driving scenario with one road having an S-curve.
scenario = drivingScenario('StopTime',3);
roadcenters = [-35 20 0; -20 -20 0; 0 0 0; 20 20 0; 35 -20 0];

%%
% Create the lanes and add them to the road.
lm = [laneMarking('Solid','Color','w'); ...
    laneMarking('Dashed','Color','y'); ...
    laneMarking('Dashed','Color','y'); ...
    laneMarking('Solid','Color','w')];
ls = lanespec(3,'Marking',lm);
road(scenario,roadcenters,'Lanes',ls);

%%
% Add an ego vehicle and specify its trajectory from its waypoints. By
% default, the car travels at a speed of 30 meters per second.
car = vehicle(scenario, ...
    'ClassID',1, ...
    'Position',[-35 20 0]);
waypoints = [-35 20 0; -20 -20 0; 0 0 0; 20 20 0; 35 -20 0];
trajectory(car,waypoints);
%%
% Plot the scenario and corresponding chase plot.
plot(scenario)
%%
%
chasePlot(car)
%%
% Run the simulation loop.
%%
%
% # Initialize a bird's-eye plot and create an outline plotter, left-lane
% and right-lane boundary plotters, and a road boundary plotter.
% # Obtain the road boundaries and rectangular outlines.
% # Obtain the lane boundaries to the left and right of the vehicle.
% # Advance the simulation and update the plotters.
%
bep = birdsEyePlot('XLim',[-40 40],'YLim',[-30 30]);
olPlotter = outlinePlotter(bep);
lblPlotter = laneBoundaryPlotter(bep,'Color','r','LineStyle','-');
lbrPlotter = laneBoundaryPlotter(bep,'Color','g','LineStyle','-');
rbsEdgePlotter = laneBoundaryPlotter(bep);
legend('off');
while advance(scenario)
    rbs = roadBoundaries(car);
    [position,yaw,length,width,originOffset,color] = targetOutlines(car);
    lb = laneBoundaries(car,'XDistance',0:5:30,'LocationType','Center', ...
        'AllBoundaries',false);
    plotLaneBoundary(rbsEdgePlotter,rbs)
    plotLaneBoundary(lblPlotter,{lb(1).Coordinates})
    plotLaneBoundary(lbrPlotter,{lb(2).Coordinates})
    plotOutline(olPlotter,position,yaw,length,width, ...
        'OriginOffset',originOffset,'Color',color)
end


%% 
% Copyright 2012 The MathWorks, Inc.