%% Simulate Vehicle Parking Maneuver in Driving Scenario
% This example shows how to simulate a parking maneuver and generate sensor
% detections in a large parking lot using a cuboid driving scenario
% environment. This scenario and sensor simulation environment enables you
% to generate rare and potentially dangerous events on which to test your
% autonomous vehicle algorithms.
%
%   Copyright 2021 The MathWorks, Inc.

%% Create Scenario
% The scenario used in this example contains a road, parking lot, moving
% vehicles, stationary vehicles, and pedestrians. The ego vehicle in this
% scenario contains a radar sensor, which generates synthetic
% detections for other actors in the scene.
%
% Define an empty scenario.
scenario = drivingScenario;
scenario.SampleTime = 0.2;

%% 
% Add a two-lane, 70-meter road to the scenario.
roadCenters = [69.2 11.7 0;
              -1.1 11.5 0];
marking = [laneMarking("Solid")
           laneMarking("DoubleSolid",Color=[1 0.9 0])
           laneMarking("Solid")];
laneSpecification = lanespec(2,Width=5.925,Marking=marking);
road(scenario,roadCenters,Lanes=laneSpecification,Name="Road");

%%
% Add a second, shorter road to serve as an entrance to the parking lot.
roadCenters = [12.4 7.7 0;
               12.4 -15.8 0];
road(scenario,roadCenters,Name="Road1");

%%
% Use the <docid:driving_ref#mw_f20bd7d2-0d79-4e16-9d3b-e244f5da0d59
% parkingLot> function to create a parking lot with four grids of parking
% spaces: one each along the top and bottom edge of the lot, and two in the
% middle of the lot. The parking grids along the edges contain regular
% parking spaces while the parking grids in the middle contain a
% combination of regular and accessible parking spaces. The parking lot
% also contains a fire lane along the right edge.
lot = parkingLot(scenario,[3 -5; 60 -5; 60 -48; 3 -48]);

% Create the parking spaces.
cars = parkingSpace;
accessible = parkingSpace(Type="Accessible");
accessibleLane = parkingSpace(Type="NoParking",MarkingColor=[1 1 1],Width=1.5);
fireLane = parkingSpace(Type="NoParking",Length=2,Width=40);

% Insert the parking spaces.
insertParkingSpaces(lot,cars,Edge=2); % Top edge
insertParkingSpaces(lot,cars,Edge=4); % Bottom edge
insertParkingSpaces(lot, ...
    [cars accessibleLane accessible accessibleLane accessible], ...
    [7 1 1 1 1],Rows=2,Position=[42 -12]);
insertParkingSpaces(lot, ...
    [cars accessibleLane accessible accessibleLane accessible], ...
    [7 1 1 1 1],Rows=2,Position=[23 -12]);
insertParkingSpaces(lot,fireLane,1,Edge=3,Offset=8); % Right edge

% Plot the scenario.
plot(scenario)

%% 
% Add actors to the driving scenario. The positions and trajectories of the
% actors are defined in the |helperAddActors| supporting file.

% Add actors.
scenario = helperAddActors(scenario);

% Define ego vehicle.
ego = scenario.Actors(1);

%% Define Radar Sensor
% In this example, you simulate an ego vehicle that has one long-range
% radar sensor located on the front bumper of the vehicle. The sensor has a
% maximum range of 50 meters, an azimuth field of view of 60 degrees, and
% an elevation field of view of 5 degrees. The figure in the next section
% displays the sensor coverage.

sensors = cell(1,1);
% Front-facing long-range radar sensor at the center of the front bumper of the car.
sensors{1} = drivingRadarDataGenerator(SensorIndex=1,RangeLimits=[0 50], ... 
    MountingLocation=[(ego.Wheelbase + ego.FrontOverhang) 0 0.2],FieldOfView=[60 5]);

% Register actor profiles with the sensors.
profiles = actorProfiles(scenario);
for m = 1:numel(sensors)
    sensors{m}.Profiles = profiles;
end

%% Visualize Scenario
% Create a display for the scenario. This display visualizes the scenario
% from the perspective of the ego vehicle from the top down and from behind
% the ego vehicle. It also displays the sensor coverage on a bird's-eye
% plot and returns a handle to this plot to call during simulation.
BEP = createDemoDisplay(ego,sensors);

%% Simulate the Scenario
% Use a loop to move the vehicles and call the sensor simulation.
% 
% Note that the scenario generation and sensor simulation can have
% different time steps. Specifying different time steps for the scenario
% and sensors enables you to decouple the scenario simulation from the
% sensor simulation. This technique is useful for modeling actor motion
% with high accuracy independently from the measurement rate of the sensor.
% 
% In this example, the scenario has a time step of 0.01 seconds, while the
% sensor detects every 0.1 seconds. The sensor returns a logical flag,
% |isValidTime|, that is |true| if the sensor generates detections.

while advance(scenario) && ishghandle(BEP.Parent)

    % Get scenario time
    time = scenario.SimulationTime;
    
    % Get position of other vehicle in ego vehicle coordinates
    poses = targetPoses(ego);
    
    % Simulate sensors
    detections = {};
    isValidTime = false; %#ok<NASGU> 
    
    [sensorDets,numValidDets,isValidTime] = sensors{1}(poses,time);
    if numValidDets
        detections = [detections; sensorDets]; %#ok<AGROW> 
    end    
    
    % Update bird's-eye plot
    if any(isValidTime) 
        updateBEP(BEP,ego,detections);
    end
end

%% Summary
% This example showed how to generate a scenario containing a parking lot, 
% simulate sensor detections, and use these detections to detect vehicles
% and pedestrians in the parking lot. 
% 
% You can try to modify the parking lot or add or remove vehicles. You 
% can also try to add, remove, or modify the sensors on the ego vehicle.

%% Supporting Functions

%%% 
% *|createDemoDisplay|*
% 
% This function creates a three-pane display:
% 
% # Top-left pane &mdash; A top view that follows the ego vehicle.
% # Bottom-left pane &mdash; A chase-camera view that follows the ego vehicle.
% # Right pane &mdash; A |<docid:driving_ref#bvi0yqz-1 birdsEyePlot>| display.
function BEP = createDemoDisplay(egoCar,sensors)
    % Make a figure
    hFigure = figure(Position=[0 0 1200 640],Name="Parking Maneuver Scenario");
    movegui(hFigure,[0 -1]); % Moves figure left and a little down from the top    

    % Add a car plot that follows the ego vehicle from behind
    hCarViewPanel = uipanel(hFigure,Position=[0 0 0.5 0.5],Title="Chase Camera View");
    hCarPlot = axes(hCarViewPanel);
    chasePlot(egoCar,Parent=hCarPlot,Meshes="on");

    % Add a car plot that follows the ego vehicle from a top view
    hTopViewPanel = uipanel(hFigure,Position=[0 0.5 0.5 0.5],Title="Top View");
    hCarPlot = axes(hTopViewPanel);
    chasePlot(egoCar,Parent=hCarPlot,ViewHeight=130,ViewLocation=[0 0],ViewPitch=90);
    
    % Add pane for bird's-eye plot
    hBEVPanel = uipanel(hFigure,Position=[0.5 0 0.5 1],Title="Bird's-Eye Plot");
    
    % Create bird's-eye plot for the ego vehicle and sensor coverage
    hBEVPlot = axes(hBEVPanel);
    frontBackLim = 60;
    BEP = birdsEyePlot(Parent=hBEVPlot,Xlimits=[-frontBackLim frontBackLim],Ylimits=[-35 35]);
    
    % Plot the coverage area for radar
    cap = coverageAreaPlotter(BEP,FaceColor="red",EdgeColor="red");
    plotCoverageArea(cap,sensors{1}.MountingLocation(1:2), ...
        sensors{1}.RangeLimits(2),sensors{1}.MountingAngles(1),sensors{1}.FieldOfView(1));
    
    % Combine all radar detections into one entry and store it for later update
    detectionPlotter(BEP,DisplayName="Radar",MarkerEdgeColor="red");
    
    % Add road borders to plot
    laneBoundaryPlotter(BEP,DisplayName="Road boundaries");

    % Add lane markings to plot
    laneMarkingPlotter(BEP,DisplayName="Lane markings");
    
    axis(BEP.Parent,"equal");
    xlim(BEP.Parent,[-frontBackLim frontBackLim]);
    ylim(BEP.Parent,[-40 40]);
    
    % Add an outline plotter for ground truth
    outlinePlotter(BEP,Tag="Ground truth");
end

%%% 
% *|updateBEP|*
% 
% This function updates the bird's-eye plot with road boundaries,
% detections, and tracks.
function updateBEP(BEP,ego,detections)
    % Update road boundaries and their display
    [lmv,lmf] = laneMarkingVertices(ego);
    plotLaneMarking(findPlotter(BEP,DisplayName="Lane markings"),lmv,lmf);

    % Update parking lot boundaries and markings and their display
    [plmv,plmf] = parkingLaneMarkingVertices(ego);
    plotParkingLaneMarking(findPlotter(BEP,DisplayName="Lane markings"),plmv,plmf);
    
    % Update ground truth data
    [position,yaw,length,width,originOffset,color] = targetOutlines(ego);
    plotOutline(findPlotter(BEP,Tag="Ground truth"),position,yaw,length,width, ...
        OriginOffset=originOffset,Color=color);

    % Update road boundaries
    rbs = roadBoundaries(ego);
    plotLaneBoundary(findPlotter(BEP,DisplayName="Road boundaries"),rbs);
    
    % Prepare and update detections display
    N = numel(detections);
    detPos = zeros(N,2);    
    isRadar = true(N,1);
    for i = 1:N
        detPos(i,:) = detections{i}.Measurement(1:2)';        
    end
    plotDetection(findPlotter(BEP,DisplayName="Radar"), detPos(isRadar,:));    
end
