%% Simulate Vehicle Backing into Parking Spot
% Simulate a driving scenario in which a car drives in reverse to back into
% a parking spot.

% Copyright 2020 The MathWorks, Inc.

%%
% Create a driving scenario. Add road segments to define a parking lot. The
% first road segment defines the parking spaces. The second road segment
% defines the driving lane and overlays the first road segment.
scenario = drivingScenario;

roadCentersParking = [6 0; 24 0];
lmParking = [laneMarking('Unmarked') ...
            repmat(laneMarking('Solid'),1,5) ...
            laneMarking('Unmarked')];
lspecParking = lanespec(6,'Width',3,'Marking',lmParking);
road(scenario,roadCentersParking,'Lanes',lspecParking);

roadCentersDriving = [12 0; 18 0];
lmDriving = [laneMarking('Unmarked') laneMarking('Unmarked')];
lspecDriving = lanespec(1,'Width',18,'Marking',lmDriving);
road(scenario,roadCentersDriving,'Lanes',lspecDriving);
 
%%
% Add a vehicle to the driving scenario.
car = vehicle(scenario,'ClassID',1,'Position',[15 -6 0],'Yaw',90);

%%
% Define the trajectory of the vehicle. The vehicle drives forward, stops,
% and then drives in reverse until it backs into the parking spot. As the
% vehicle enters the parking spot, it has a yaw orientation angle that is
% 90 degrees counterclockwise from where it started.
waypoints = [15 -6; 15 5; 12 -1.5; 7.3 -1.5];
speed = [4.5; 0; -2; 0];
trajectory(car,waypoints,speed,'Yaw',[90 90 180 180]);

%%
% Plot the driving scenario and display the waypoints of the trajectory.
plot(scenario,'Waypoints','on')
while advance(scenario)
    pause(0.001)
end