function hFigure = helperPlotEgoAndTargetVehicleProfiles(varargin)
% helperPlotEgoAndTargetVehicleProfiles function plots the profiles of ego
% and target vehicles after the simulation.
%
%   helperPlotEgoAndTargetVehicleProfiles(logsout, lineColor) plots the
%   orientation and velocity profiles of target vehicle and velocity
%   profile of ego vehicle from logged simulation data.
%
%   The function assumes that the example outputs the Simulink log,
%   logsout, containing the following elements to be plotted.
%
%   This is a helper function for example purposes and may be removed or
%   modified in the future.
%
%   Copyright 2020 The MathWorks, Inc.
%

%% Plot the spacing control results

% Create a figure handle
hFigure = figure('Name','Spacing Control Performance','position',[100 100 720 600]);

% Define default line color
defaultColor = 'b';

% Check number of arguments and update the color if required
minArgs = 1;
maxArgs = 2;
narginchk(minArgs,maxArgs)
if nargin > 2
    error('Usage:: helperPlotEgoAndTargetVehicleProfiles(logsout, color)');
elseif nargin == 2
    logsout   = varargin{1};
    plotColor = varargin{2};
else
    logsout   = varargin{1};
    plotColor = defaultColor;
end
     
%% Get the data from simulation
ego_long_velocity  = logsout.getElement('ego_velocity');   % longitudinal velocity of ego car
yaw_red_sedan      = logsout.getElement('<Yaw>');          % yaw angle of target vehicle (truth)
velocity_red_sedan = logsout.getElement('<Velocity>');     % velocity of target vehicle(truth)

% Extract each velocity component
velocity_red_sedan_x = reshape(velocity_red_sedan.Values(1).Data(:,1,:), size(velocity_red_sedan.Values(1).Data(:,1,:),3), 1);
velocity_red_sedan_y = reshape(velocity_red_sedan.Values(1).Data(:,2,:), size(velocity_red_sedan.Values(1).Data(:,1,:),3), 1);
velocity_red_sedan_z = reshape(velocity_red_sedan.Values(1).Data(:,3,:), size(velocity_red_sedan.Values(1).Data(:,1,:),3), 1);

% Construct the absolute_velocity_red_sedan vector
absolute_velocity_red_sedan = [velocity_red_sedan_x, velocity_red_sedan_y, velocity_red_sedan_z];
tmax = ego_long_velocity.Values.time(end); % simulation time

% Red Sedan Steering Angle
subplot(3,1,1)
hold on

% Plot orientation of the first target vehicle 
plot(yaw_red_sedan.Values(1).Time,yaw_red_sedan.Values(1).Data, 'Color', plotColor)

% Set plot limits in x and y axises
xlim([0,tmax])
ylim([min(yaw_red_sedan.Values(1).Data)-2, max(yaw_red_sedan.Values(1).Data)+2])
grid on

% Set the plot title and axis labels
title('Yaw Angle of Target Vehicle (Red sedan)')
xlabel('time (sec)')
ylabel('$degrees$','Interpreter','latex')

% relative velocity
subplot(3,1,2)
hold on

% Plot orientation of the first target vehicle 
plot(velocity_red_sedan.Values(1).Time,vecnorm(absolute_velocity_red_sedan, 2, 2), 'Color', plotColor)

% Set plot limits in y axis
ylim([-2 15])
grid on

% Set the plot title and axis labels
title('Absolute Velocity of Target Vehicle (Red sedan)')
xlabel('time (sec)')
ylabel('$m/s$','Interpreter','latex')


% ego velocity
subplot(3,1,3)
hold on

% Plot orientation of the first target vehicle 
plot(ego_long_velocity.Values.Time,ego_long_velocity.Values.Data, 'Color', plotColor)

% Set plot limits in x and y axises
xlim([0,tmax])
ylim([-2 15])
grid on

% Set the plot title and axis labels
title('Absolute Velocity of Ego Vehicle (Blue sedan)')
xlabel('time (sec)')
ylabel('$m/s$','Interpreter','latex')

end

