function exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState)
% exampleHelperUpdateOccupancyMap A helper function to update occupancy 
% map with otherVehicles information.

% Inputs
%   -otherVehicles:
%    - Structure array containing
%      (Position, Yaw, Velocity, SimulationTime) of each vehicle in the
%      scenario other than ego vehicle.
%      in the scenario
%   -simStep:
%       Simulation step
%   -currentEgoState:
%       currentEgoState that contains information of ego position and
%       orientation,velocity etc.

% 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 
% Load egoMap from base workspace
egoMap = evalin('base', 'egoMap');

% Loading the predefined vehicle dimensions to be applied to 
% ego vehicle and other vehicles in the scenario.
carPolygon = evalin('base', 'carPolygon');

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

%% Initialize the required variables
% Update ego information
egoPose = [currentEgoState(1) currentEgoState(2)];

% Mesh grid
[gridX, gridY] = meshgrid(1:egoMap.GridSize(1), 1:egoMap.GridSize(2));

% Setting initial values
values = true(numel(gridX), 1);

% Setting occupancy map as fully occupied
egoMap.setOccupancy([gridX(:) gridY(:)], values, 'grid');

% Moving the egoMap to current position of ego vehicle
move(egoMap, [egoPose(1) egoPose(2)]);

% Sync egoMap with static map
syncWith(egoMap, staticOccupancyMap);

% Initializing the obstacle info arrays as empty
obstaclePoints = [];

%% Iterate over all the otherVehicles in the scenario and set them in the occupancy map
for i = 1:numel(otherVehicles)
    % Clear the obstacle info every time
    obstacleData = [];
    
    % Filling actor info
    actorPose = otherVehicles(i).Position(simStep, :);
    actorYaw = otherVehicles(i).Yaw(simStep);
    % Check whether any targetVehicle (other than ego) are started at same place
    % and ignores that tagetVehicle. i.e. it won't be added to map
    if(actorPose(1) == egoPose(1) && ...
            actorPose(2) == egoPose(2))
        disp('Warning: Ego actor and other actor', ...
            'are at same location,', ...
            'ignoring the actor(#', otherVehicles(i).id, ')');
        continue;
    end
    
    % Check for the targetVehicle pose, to find whether it can be fit into
    % current occupancy map using egoMap world limits
    % Obstacles refers to otherVehicles
    if(actorPose(1) > (egoMap.XWorldLimits(1) + 6) && ...
            actorPose(1) < (egoMap.XWorldLimits(2) - 6) ...
            && actorPose(2) > (egoMap.YWorldLimits(1) + 6)...
            && actorPose(2) < (egoMap.YWorldLimits(2) - 6))
        % Constructing each actor's polygon(dimensions) by using
        % with its current position and ego info
        vehicleCenter = [actorPose(1), actorPose(2)];
        pointPose = repmat(vehicleCenter, 4, 1)';
        rotationMat = [cosd(actorYaw) -sind(actorYaw);...
            sind(actorYaw) cosd(actorYaw)];
        obstaclePolygon = vehicleCenter' + carPolygon;
        obstaclePolygon = rotationMat * ...
            (obstaclePolygon - pointPose) + pointPose;
        obstacleData = [obstacleData; obstaclePolygon, ...
            obstaclePolygon(:, 1), [NaN; NaN]];
        
        % Filtering the obstacles
        % Check whether which points can be added to current egoMap
        if(~isempty(obstacleData))
            [xPoints , yPoints] = meshgrid(0:0.5:100);
            xPoints = xPoints(:) + egoMap.XWorldLimits(1);
            yPoints = yPoints(:) + egoMap.YWorldLimits(1);
            [in, on] = inpolygon(xPoints, yPoints, ...
                obstacleData(1, :), obstacleData(2, :));
            obstaclePoints = [obstaclePoints; xPoints(in), ...
                yPoints(in); xPoints(on), yPoints(on)];
        end
        
        % Setting egoMap with detected obstacle points as
        % occupied space
        setOccupancy(egoMap, obstaclePoints, 1);
    end
end
% Inflate the Map
inflate(egoMap, 0.5);

end