% This is a helper function and may be changed or removed without notice.

%   Copyright 2016-2020 The MathWorks, Inc.

% Update the bird's-eye plot with lane boundaries and detections
%
% bep:      Bird's-eye plot to be updated
% egoCar:   Actor object assigned as the ego vehicle
% sensor:   Sensor object used by egoCar to generate detections
% dets:     Detections generated by sensor that will be used to update the
%           bird's-eye plot
function helperUpdateSensorDemoDisplay(bep, egoCar, sensor, dets)

% If parent no longer exists, do nothing
if ~ishghandle(bep.Parent)
    return
end

% Get road and detection plotters for bird's eye plot
roadPlotter = findPlotter(bep, 'DisplayName', 'road');
if isa(sensor,'drivingRadarDataGenerator')
    detPlotter = findPlotter(bep, 'DisplayName', 'radar');
else
    detPlotter = findPlotter(bep, 'DisplayName', 'object detections');
end

% Update road boundary display
if ~isempty(roadPlotter)
   laneBounds = roadBoundaries(egoCar);
   plotLaneBoundary(roadPlotter,laneBounds);
end

% Update ground truth information
outlinePlotter = findPlotter(bep, 'Tag', 'Ground truth');
[position, yaw, length, width, originOffset, color] = targetOutlines(egoCar);
plotOutline(outlinePlotter, position, yaw, length, width, 'OriginOffset', originOffset, 'Color', color);

% Prepare detections and update display
numDets = numel(dets);
detPos = zeros(numDets,2);
detCov = zeros(2,2,numDets);
for i = 1:numDets
    detPos(i,:) = dets{i}.Measurement(1:2)';
    detCov(:,:,i) = dets{i}.MeasurementNoise(1:2,1:2);
end
plotTrack(detPlotter, detPos, 4*detCov); % Plot the 2-sigma ellipses (4*detCov)

% When publishing, keep bird's eye plots zoomed in on targets for snapshots
hasSnapshot = ~isempty(snapnow('get'));
if hasSnapshot && numDets>0
    tgtIds = cellfun(@(d)d.ObjectAttributes{1}.TargetIndex,dets);
    isFalse = tgtIds<0;
    if any(~isFalse)
        stepSize = 10;
        xVals = detPos(~isFalse,1);
        xMin = floor(min([xVals;0])/stepSize)*stepSize;
        xMax = ceil(max(xVals+10)/stepSize)*stepSize;
        axis(bep.Parent,'equal');
        xlim(bep.Parent,[xMin xMax]);
    end
end
end
