function [reachedGoal, currentEgoState] = exampleHelperExecuteAndVisualize(currentEgoState, simstep, ...
    trajectory, previousReplanTime)
% exampleHelperExecuteAndVisualize is a helper function that advances the
% simulation and updates the visualization.

% Inputs:
%   -currentEgoState:
%       currentEgoState that contains information of ego position and
%       orientation,velocity etc.
%   -simStep:
%      simulation step
%   -trajectory:
%      Generated Trajectory.
%   -previousReplanTime:
%      Previos replan time.

% Outputs:
%   -reachedGoal:
%      Flag to determine whether the goal point is reached.
%   -currentEgoState:
%      Updated ego state.

% Example of Calling this function:
%
%     [isGoalReached, currentEgoState] = ...
%         exampleHelperExecuteAndVisualize(currentEgoState, simStep,...
%         trajectory, replanPeriod, previousReplanTime);

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

% Copyright 2019 The MathWorks, Inc.

%% Get the required data from base workspace
% Loads the globalPlanPoints from the base workspace
globalPlanPoints = evalin('base', 'globalPlanPoints');

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

% Loads hAxes1 from base workspace
hAxes1 = evalin('base', 'hAxes1');

% Loads hAxes2 from base workspace
hAxes2 = evalin('base', 'hAxes2');

%% Initialize the required variables
% Log ego vehicle poses to base workspace to playback in Driving scenario. 
egoPoses = evalin('base', 'egoPoses');

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

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

% Get goal point/end of reference path
goalPoint = globalPlanPoints(end, 1:2);

% Variable to check if the goal point is reached
reachedGoal = false;

% Initializing the helper variables
hAxes1.CameraUpVector = [1 0 0];

% Time resolution of the planner
timeResolution = 0.01;

%% Check if the ego vehicle has reached the goal point.
% Distance threshold (in meters) used to stop the ego vehicle before the goal point
% Avoiding no trajectory error by not planning till the end of global plan.
distanceThresholdToGoal = 22;

% Calculates distance between the current position of ego vehicle and
% the goal point.
distToGoal = sqrt((goalPoint(1,1)-currentEgoState(1,1))^2 + ...
    (goalPoint(1,2)-currentEgoState(1,2))^2);

% Check if the ego vehicle's current distance to goal point is less than
% distanceThresholdToGoal
if(distToGoal < distanceThresholdToGoal)
    reachedGoal = true;
end

%% Advance the Simulation and plot the results
% Updates the current state of the vehicle if there is a valid trajectory
if (~isempty(trajectory))
    % Get the index of trajectory that needs to be applied to
    % the ego vehicle
    [~, idx] = min(abs((trajectory(:,7)) - simstep*timeResolution + previousReplanTime));
    
    % Updating the current state of the vehicle
    currentEgoState = [trajectory(idx, 1), ...
        trajectory(idx, 2), ...
        trajectory(idx, 3), ...
        trajectory(idx, 4), ...
        trajectory(idx, 5), ...
        trajectory(idx, 6), ...
        0];
    egoPoses(simstep, :) = [currentEgoState(1:3), simstep*timeResolution];
end

% Export the ego poses to base work space.
assignin('base','egoPoses',egoPoses);

% Setting up the position and orientation of ego vehicle according to
% the given path
egoCenter = [currentEgoState(1, 1) currentEgoState(1, 2)];
egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);

% Update Ego vehicle polygon on execution by updating the Vertices of the 
% figure handle
set(axes2EgoHandle,'Vertices',[egoPolygon(1, :); egoPolygon(2, :)]');

% Setting up the position and orientation of other actors according to
% the given path
for i=1:numel(otherVehicles)
    vehicleCenter = [otherVehicles(i).Position(simstep,1) ...
        otherVehicles(i).Position(simstep,2)];
    vehiclePolygon = exampleHelperTransformPointtoPolygon(...
                            otherVehicles(i).Yaw(simstep), vehicleCenter);
    
    % Update other vehicle polygon in the figure by updating the Vertices 
    % of the figure handle
    set(otherVehicleHandle(i),'Vertices',[vehiclePolygon(1, :);...
                                           vehiclePolygon(2, :)]');
end

% The camera up vector specifies the direction that is oriented up in the
% plot
hAxes2.CameraUpVector = [1 0 0];

hold off;

end