function exampleHelperInitializeVisualization()
% exampleHelperInitializeVisualization is a helper function for plotting the
% scenario of the urban driving example

% This is a helper function for example purposes and may be removed or
% modified in the future.
%

% Copyright 2019 The MathWorks, Inc.

%% Setting the Visualization window.
% Closing all the previously opened windows
close all;

% Figure creates a new figure window
hFigure = figure('units', 'normalized', 'outerposition', [0 0 1 1]);

% uipanel creates a panel in the current figure and
% returns the Panel object
hPanel1 = uipanel(hFigure, 'Units', 'Normalized', 'Position', ...
    [0 0 1/2 1], 'Title','OccupancyMap With Trajectory Plot');

hPanel2 = uipanel(hFigure, 'Units', 'Normalized', 'Position', ...
    [1/2 0 1/2 1], 'Title', 'Ego and Other Vehicles Plot');

% Axes creates the default Cartesian axes in the current figure
% and makes it the current axes.
% hAxes1 is the current axes for OccupancyMap With Trajectory Plot
hAxes1 = axes('Parent', hPanel1);

% hAxes2 is the current axes for Ego and other Vehicles Plot
hAxes2 = axes('Parent', hPanel2);

% Add title to the Axeses
title('Trajectory Execution in Scenario','Parent',hAxes2);

% Export axes handle for the figure to base workspace
assignin('base','hAxes1',hAxes1);
assignin('base','hAxes2',hAxes2);

% Load laneBoundaries from base workspace
laneBoundaries = evalin('base', 'laneBoundaries');

% Number of roads in the scenario
numRoads = 3;

% Number of lanes available in the segments.
% From point A to point B number of roads are 2
% From point B to point D number of roads are 4
% From point D to point goal point number of roads are 2
numLanes = {2, 4, 2};

% Set Ego vehicle initial position
egoInitPose     = [5.1, -1.8, 0];
egoInitYaw      = -0.165;

% Load otherVehicles from base workspace
otherVehicles = evalin('base', 'otherVehicles');

% Plot the roads
for i=1:numRoads
    for j=1:numLanes{i}+1
        hold on;
        plot(laneBoundaries{i}{j}(:, 1), laneBoundaries{i}{j}(:, 2),...
            'black', 'Parent', hAxes2);
    end
end
axis(hAxes2, 'equal');

% Create and update Ego vehicle polygon in visualization with green color
egoCenter = [egoInitPose(1, 1) egoInitPose(1, 2)];
egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(egoInitYaw), egoCenter);

% Fill Ego vehicle polygon with green color and assign the object handle to
% axes2EgoHandle for updating the visualization.
hold on;
axes2EgoHandle = fill(egoPolygon(1, :), egoPolygon(2, :), 'g', 'Parent', hAxes2);

for i=1:numel(otherVehicles)
    vehicleCenter = [otherVehicles(i).Position(1,1) ...
        otherVehicles(i).Position(1,2)];
    vehiclePolygon = exampleHelperTransformPointtoPolygon(otherVehicles(i).Yaw(1), vehicleCenter);
    
    hold on;
    % Fill other vehicle polygon with blue color and assign the handle to
    % targetVehicleHandle for updating the visualization.
    otherVehicleHandle(i) = fill(vehiclePolygon(1, :), vehiclePolygon(2, :), 'b', 'Parent', hAxes2);
end

% Export the figure handles to base workspace
assignin('base','axes2EgoHandle',axes2EgoHandle);
assignin('base','otherVehicleHandle',otherVehicleHandle);

end