classdef TrackPlotter < fusion.theaterPlot.TheaterPlotter
%TrackPlotter Create a new track plotter
%
%   See:  <a href="matlab:help theaterPlot/trackPlotter">theaterPlot/trackPlotter</a> 

%   Copyright 2018 The MathWorks, Inc.

    properties
        %HistoryDepth   Number of previous updates to display
        %   This property specifies the number of previous
        %   updates to render in the display.  It must be a
        %   value between 0 and 10,000.  If set to 0, then no
        %   previous updates are shown.  The default is 0.
        HistoryDepth
    end
    
    properties (Dependent)
        %Marker    Marker symbol
        %   Specify Marker as one of the following values:
        %      .   point    x  x-mark       ^ triangle (up)
        %      o   circle   d  diamond      v triangle (down)
        %      +   plus     p  pentagram    < triangle (left)
        %      *   star     h  hexagram     > triangle (right)
        %      s   square (default)
        Marker
        
        %MarkerSize  Marker size
        %   A positive integer specifying the size of the marker.
        %
        %   The default is 10.
        MarkerSize
        
        % MarkerEdgeColor  Marker outline color
        %   Specify MarkerEdgeColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.  
        %
        %   The default is taken from the current theme.
        MarkerEdgeColor
        
        %MarkerFaceColor  Marker fill color  
        %   Specify MarkerFaceColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.   
        %
        %   The default is 'none'.
        MarkerFaceColor
        
        %FontSize   Font size for detection labels
        %   Specify FontSize as used for labeling detections.  
        %
        %   The default is 10 points.
        FontSize
        
        %LabelOffset  Space between label and its position.
        %   LabelOffset is constant offset used to specify a gap between a 
        %   label and the positional point it annotates. Specify the offset
        %   as a three-element row vector containing the x and y offset 
        %   (in meters).
        %
        %   The default is [0 0 0].
        LabelOffset
        
        %VelocityScaling  Velocity scale factor
        %   VelocityScaling is a positive scale factor used to scale the
        %   magnitude length of velocity vectors.  For example, if set to
        %   a value, K, then a velocity with a magnitude of V meters per
        %   second will render as a line of length K*V meters.  
        %
        %   The default is 1.
        VelocityScaling
    end

    properties (Constant, Access = private)
        NumEllipsoidPoints = 20
        StandardDeviationWidth = 1
    end
    
    properties (Access = private)
        PositionsLine = []
        VelocitiesLine = []
        CovariancesPatches = []
        HistoryPositions = {}
        HistoryTotal = 0
        LabelText = []
        ParentAxes = []
        LabelFontSize = 10
        LabelTextOffset = [0 0 0]
        ScaleFactor = 1
        ExtentPatch = matlab.graphics.primitive.Patch.empty
    end
    
    properties (Hidden)
        HistoryLabels = {}
        HistoryLine = []
    end
    
    properties (SetAccess = private)
        ConnectHistory matlab.lang.OnOffSwitchState = false;
        ColorizeHistory matlab.lang.OnOffSwitchState = false;
    end
    
    
    methods
        function obj = TrackPlotter(tp, varargin)
            obj@fusion.theaterPlot.TheaterPlotter(tp,varargin{:});
        end
       
        function value = get.Marker(obj)
            pLine = obj.PositionsLine;
            value = pLine.Marker;
        end
        
        function value = get.MarkerSize(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerSize;
        end
        
        function value = get.MarkerFaceColor(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerFaceColor;
        end
        
        function value = get.MarkerEdgeColor(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerEdgeColor;
        end
        
        function value = get.FontSize(obj)
            value = obj.LabelFontSize;
        end
        
        function value = get.VelocityScaling(obj)
            value = obj.ScaleFactor;
        end
        
        function value = get.LabelOffset(obj)
            value = obj.LabelTextOffset;
        end
        
        function set.Marker(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.Marker = value;
            end
            hLine = obj.HistoryLine;
            if ishghandle(hLine)
                hLine.Marker = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerSize(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerSize = value;
            end
            hLine = obj.HistoryLine;
            if ishghandle(hLine)
              hLine.MarkerSize = max(1,round(value/2));
            end            
            drawnow limitrate
        end
        
        function set.MarkerFaceColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerFaceColor = value;
            end
            % use colorspec from pLine to interpret color strings
            hLine = obj.HistoryLine;
            if ishghandle(hLine) && ishghandle(pLine)
                hLine.MarkerFaceColor = historyColor(obj,pLine.MarkerFaceColor);
            end
            
            hPatch = obj.ExtentPatch;
            set(hPatch(ishghandle(hPatch)),'FaceColor',value);
            
            drawnow limitrate
        end
        
        function set.MarkerEdgeColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerEdgeColor = value;
            end
            
            % use colorspec from pLine to interpret color strings
            hLine = obj.HistoryLine;
            if ~obj.ColorizeHistory && isscalar(hLine) && ishghandle(hLine)
                hLine.Color = value;
                hLine.MarkerEdgeColor = historyColor(obj,pLine.MarkerEdgeColor);
            end
            
            vLine = obj.VelocitiesLine;
            if ishghandle(vLine)
                vLine.Color = value;
            end
            
            hPatch = obj.ExtentPatch;
            set(hPatch(ishghandle(hPatch)),'EdgeColor',value);
            
            drawnow limitrate
        end
        
        function set.FontSize(obj,value)
            validateattributes(value,{'numeric'},{'positive','finite','real','scalar'});
            set(obj.LabelText(ishghandle(obj.LabelText)),'FontSize',value);
            obj.LabelFontSize = value;
            drawnow limitrate
        end
        
        function set.VelocityScaling(obj,value)
            validateattributes(value,{'numeric'},{'positive','finite','real','scalar'});
            rescaleVelocities(obj, obj.ScaleFactor, value);
            obj.ScaleFactor = value;
            drawnow limitrate
        end
        
        function set.LabelOffset(obj,value)
            validateattributes(value,{'numeric'},{'finite','real','size',[1 3]});
            repositionLabels(obj, obj.LabelTextOffset, value);
            obj.LabelTextOffset = value;
            drawnow limitrate;
        end
        
        function clearData(obj)
            %clearData removes all annotations and labels associated with this plotter.             
            %     clearData(plotter) removes all annotations and labels
            %     created by the plotter.
            drawPositions(obj, zeros(0,3));
            drawVelocities(obj, zeros(0,3), zeros(0,3));
            drawCovariances(obj, zeros(3,0), zeros(3,3,0));
            drawExtents(obj, zeros(3,0), [], []);
            drawLabels(obj, zeros(0,2), {});
            clearHistory(obj);
            drawnow limitrate
        end
        
        function set.HistoryDepth(obj,value)
            validateattributes(value,{'numeric'},{'scalar','integer','nonnegative','<',10000});
            clearHistory(obj);
            obj.HistoryDepth = value;
            drawnow limitrate
        end

        function plotTrack(obj, positions, varargin)
            %plotTrack plot a set of tracks in a theater track plotter.
            %   plotTrack(tPlotter, positions) specifies positions of M tracked
            %       objects whose positions are to be shown for the tracks plotter,
            %       tPlotter. Specify the positions as an M-by-3 matrix, where each
            %       column of the matrix corresponds to the X, Y, and Z coordinates 
            %       of the objects' locations, respectively.
            % 
            %    plotTrack(tPlotter, positions, velocities) additionally specifies
            %       the corresponding velocities of the tracks.  Velocities are
            %       plotted as line vectors emanating from the positions of
            %       the tracks.  If specified, velocities must have the same
            %       dimensions as positions. If unspecified, no velocity information
            %       will be plotted.
            % 
            %    plotTrack(tPlotter, positions, ..., covariances) additionally
            %       specifies the covariances of the M track uncertainties, where the
            %       covariances are a 3-by-3-by-M matrix of covariances that are
            %       centered at the track positions.  These uncertainties are plotted
            %       as an ellipsoid.
            % 
            %    plotTrack(tPlotter, positions, ..., labels) additionally specifies
            %       the labels and positions of M objects whose positions are
            %       estimated by a tracker, where labels are an Mx1 cell array of
            %       character vectors that correspond to the M detections specified
            %       in the positions matrix.  If omitted, no labels are plotted.
            % 
            %    plotTrack(tPlotter, positions, ..., labels, trackIDs) additionally 
            %       specifies the unique track identifiers for each track when the
            %       'ConnectHistory' property is set to 'on'.  Specify the track
            %       identifiers as an Mx1 array of unique integer values, an Mx1
            %       array of strings, or an Mx1 cell array of unique character
            %       vectors.  If trackIDs is omitted when 'ConnectHistory' is 'on',
            %       then the track identifiers are derived from the labels input
            %       instead.  The trackIDs input is ignored when 'ConnectHistory' is
            %       'off'.
            % 
            %    plotTrack(tPlotter, positions, ..., dim, orientation) additionally
            %       specifies the dimensions and orientation of the extent of each
            %       tracked object.  The dimensions, dim, is an M-by-1 dimensions
            %       structure containing the Length, Width, Height and OriginOffset
            %       for each object.  The orientation, orient, is expressed as either
            %       a 3-by-3-by-M rotation matrix or a quaternion vector of length M.
            %       If omitted, no extents are plotted.            
            %
            %   % Example
            %   % -------
            %   % Create a theater plot and update it with three tracks labeled
            %   % 'T1', 'T2', and 'T3'; with start positions in units of meters all 
            %   % starting at (30,5,1); with constant velocities (in m/s)
            %   % of (3,0,1), (3,2,2) and (3,-3,5), respectively.
            %   % Update the tracks with the velocities for ten trials, showing seven
            %   % history values
            %   tp = theaterPlot('XLim',[0,90],'YLim',[-35,35]);
            %   tPlotter = trackPlotter(tp,'DisplayName','Tracks','HistoryDepth',7);
            %   positions = [30, 5, 1; 30, 5, 1; 30, 5, 1];
            %   velocities = [3, 0, 1; 3, 2, 2; 3, -3, 5];
            %   labels = {'T1','T2','T3'};
            %   for i=1:10
            %      plotTrack(tPlotter, positions, velocities, labels);
            %      positions = positions + velocities;
            %   end
            %
            %   See also theaterPlot/trackPlotter.
            if nargin > 2
                varargin = convertStringsToCellChars(varargin);
            end
            positions = parsePositions(positions, 'plotTrack');
            drawPositions(obj, positions);
            
            [velocities,varargin] = parseVelocities(positions, varargin);
            drawVelocities(obj, positions, velocities);
            
            [labels,varargin] = parseLabels(positions,varargin);
            drawLabels(obj, positions, labels);
            
            [dims,orientations,varargin] = fusion.theaterPlot.TrackPlotter.parseExtents(positions,varargin);
            drawExtents(obj, positions, dims, orientations);
            
            [covariances,varargin] = parseCovariances(positions, varargin);
            drawCovariances(obj, positions.', covariances);

            if isempty(varargin)
                trackIDs = getTrackIDsFromLabels(obj, positions, labels);
            else
                [trackIDs,varargin] = parseTrackIDs(positions,varargin);
            end
            
            if ~isempty(varargin)
                error(message('shared_radarfusion:theaterPlot:UnrecognizedInput'));
            end
            
            if obj.ConnectHistory
                addConnectedHistory(obj, positions, trackIDs);
                drawHistory(obj);
            else
                drawHistory(obj);
                addUnconnectedHistory(obj, positions);
            end
            
            drawnow limitrate
        end
    end
    
    methods (Access = protected)
        function initParser(obj, p)
            initParser@fusion.theaterPlot.TheaterPlotter(obj, p)
            addParameter(p,'HistoryDepth',0);
            addParameter(p,'Marker','s');
            addParameter(p,'MarkerSize',10);
            addParameter(p,'MarkerEdgeColor',[0 0 0]);
            addParameter(p,'MarkerFaceColor','none');
            addParameter(p,'FontSize',10);
            addParameter(p,'VelocityScaling',1);
            addParameter(p,'LabelOffset',[0 0 0]);
            addParameter(p,'ConnectHistory','off')
            addParameter(p,'ColorizeHistory','off')
        end

        function value = getPrimaryObject(obj)
            value = obj.PositionsLine;
        end
        
        function createGraphics(obj, hAxes, p)
            pr = p.Results;
            % positions line
            pLine = line(hAxes, 0, 0, 0, ...
                         'Marker',pr.Marker, ...
                         'MarkerSize',pr.MarkerSize, ...
                         'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                         'MarkerFaceColor',pr.MarkerFaceColor, ...
                         'LineStyle','none', ...
                         'Tag', ['tpTrackPositions_' pr.DisplayName]);

            % velocities line
            vLine = line(hAxes, 0, 0, 0, ...
                         'HandleVisibility','off', ...
                         'Color', pr.MarkerEdgeColor);

            if ismember('MarkerEdgeColor',p.UsingDefaults)
                matlab.graphics.internal.themes.specifyThemePropertyMappings( ...
                    pLine, 'MarkerEdgeColor', "--mw-graphics-borderColor-axes-primary");
                matlab.graphics.internal.themes.specifyThemePropertyMappings( ...
                    vLine, 'Color', "--mw-graphics-borderColor-axes-primary");
            end

            
            if ~isempty(pr.DisplayName)
                set(pLine, ...
                    'DisplayName',pr.DisplayName, ...
                    'Tag', ['tpTrackPositions_' pr.DisplayName]);
                set(vLine, ...
                    'Tag', ['tpTrackVelocities_' pr.DisplayName]);
            else
                set(pLine,...
                    'HandleVisibility','off', ...
                    'Tag', 'tpTrackPositions');
                set(vLine,...
                    'Tag', 'tpTrackVelocities');
            end
            
            obj.ConnectHistory = strcmp(pr.ConnectHistory,'on');
            if obj.ConnectHistory
                histLineStyle = '-';
                histMarkerStyle = 'none';
            else
                histLineStyle = 'none';
                histMarkerStyle = pr.Marker;
            end

            obj.ColorizeHistory = strcmp(pr.ColorizeHistory,'on');
            if obj.ColorizeHistory
                % use default color
                histColor = pLine.Color;
            else
                % use specified color
                histColor = pr.MarkerEdgeColor;
            end
            
            % history line
            hLine = line(hAxes, 0, 0, 0, ...
                         'MarkerEdgeColor',historyColor(obj,pLine.MarkerEdgeColor), ...
                         'MarkerFaceColor',historyColor(obj,pLine.MarkerFaceColor), ...
                         'Marker',histMarkerStyle, ...
                         'MarkerSize', max(1,round(pr.MarkerSize/2)), ...
                         'LineStyle',histLineStyle, ...
                         'LineWidth', 2, ...
                         'Color', histColor);
            
            if ~isempty(pr.DisplayName) && (pr.HistoryDepth ~= 0 || obj.ConnectHistory)
                set(hLine, ...
                    'DisplayName','(history)', ...
                    'Tag',['tpTrackHistory_' pr.DisplayName]);
            else
                set(hLine, ...
                    'HandleVisibility','off', ...
                    'Tag','tpTrackHistory');
            end
            
            set(pLine,'XData',[],'YData',[],'ZData',[]);
            set(vLine,'XData',[],'YData',[],'ZData',[]);
            set(hLine,'XData',[],'YData',[],'ZData',[]);

            obj.PositionsLine = pLine;
            obj.VelocitiesLine = vLine;
            obj.ParentAxes = hAxes;
            obj.HistoryLine = hLine;            
            obj.HistoryDepth = pr.HistoryDepth;
            obj.LabelFontSize = pr.FontSize;
            obj.LabelOffset = pr.LabelOffset;
            obj.VelocityScaling = pr.VelocityScaling;
        end
        
        function deleteGraphics(obj)
            delete(obj.PositionsLine);
            delete(obj.VelocitiesLine);
            delete(obj.CovariancesPatches);
            delete(obj.HistoryLine);
        end
    end
    
    methods (Access = private)
        function color = historyColor(~, colorspec)
            % return faded color
            if isequal(colorspec,'none')
                color = 'none';
            else
                white = [1 1 1];
                color = white - (white - colorspec)/3;
            end
        end
        
        function drawPositions(obj, positions)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                xData = positions(:,1);
                yData = positions(:,2);
                zData = positions(:,3);
                set(pLine,'XData',xData,'YData',yData,'ZData',zData);
            end
        end
        
        function rescaleVelocities(obj, oldScale, newScale)
            vLine = obj.VelocitiesLine;
            if ishghandle(vLine)
                lineXData = vLine.XData;
                lineYData = vLine.YData;
                lineZData = vLine.ZData;
                lineXData(2:3:end) = lineXData(1:3:end) + newScale/oldScale .* (lineXData(2:3:end) - lineXData(1:3:end));
                lineYData(2:3:end) = lineYData(1:3:end) + newScale/oldScale .* (lineYData(2:3:end) - lineYData(1:3:end));
                lineZData(2:3:end) = lineZData(1:3:end) + newScale/oldScale .* (lineZData(2:3:end) - lineZData(1:3:end));
                set(vLine, ...
                    'XData', lineXData(:), ...
                    'YData', lineYData(:), ...
                    'ZData', lineZData(:));
            end
        end
        
        function drawVelocities(obj, positions, velocities)
            if ~isempty(velocities)
                % fencepost velocity vectors with NaN
                % to render all as a single line
                lineXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.ScaleFactor.*velocities(:,1))', ...
                                  NaN(1,size(positions,1)));
                lineYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.ScaleFactor.*velocities(:,2))', ...
                                  NaN(1,size(positions,1)));
                lineZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.ScaleFactor.*velocities(:,3))', ...
                                  NaN(1,size(positions,1)));
            else
                lineXData = [];
                lineYData = [];
                lineZData = [];
            end
            
            vLine = obj.VelocitiesLine;
            if ishghandle(vLine)
                set(vLine, ...
                    'XData', lineXData(:), ...
                    'YData', lineYData(:), ...
                    'ZData', lineZData(:));
            end
        end
        
        function repositionLabels(obj, oldOffset, newOffset)
            hText = obj.LabelText;
            for i=1:numel(hText)
                if ishghandle(hText(i))
                    set(hText(i),'Position', get(hText(i),'Position') + newOffset - oldOffset);
                end
            end
        end
        
        function drawLabels(obj, positions, labels)

            hText = obj.LabelText;
            hAxes = obj.ParentAxes;
            fontSize = obj.LabelFontSize;

            if ishghandle(obj.PositionsLine)
               dispName = obj.DisplayName;
                for i=1:numel(labels)
                    if i<=numel(hText) && ishghandle(hText(i))
                        set(hText(i),'Position',positions(i,:)+obj.LabelTextOffset,'String',labels{i});
                    else
                        hText(i) = text(hAxes, positions(i,1),positions(i,2),positions(i,3),labels{i}, ...
                            'VerticalAlignment','bottom', ...
                            'Clipping','on', ...
                            'FontSize',fontSize, ...
                            'Color', 1 - hAxes.Color, ...
                            'Tag',sprintf('tpTrackLabels_%s',dispName));
                    end
                end
                
                % delete unused text objects
                hUnusedText = hText(numel(labels)+1:end);
                delete(hUnusedText(ishghandle(hUnusedText)));
                hText(numel(labels)+1:end) = [];

                obj.LabelText = hText;
            end
        end
        
        function drawExtents(obj, positions, dims, orientations)
            hAxes = obj.ParentAxes;
            if ishghandle(obj.PositionsLine)
                n = numel(dims);
                xdata = zeros(4,6*n);
                ydata = zeros(4,6*n);
                zdata = zeros(4,6*n);
                for i=1:n
                    [xdata(:,(i-1)*6+(1:6)), ...
                     ydata(:,(i-1)*6+(1:6)), ...
                     zdata(:,(i-1)*6+(1:6))] = fusion.theaterPlot.TrackPlotter.cuboidPatchData(positions(i,:), dims(i), orientations(:,:,i));
                end
                
                if isempty(obj.ExtentPatch) || ~ishghandle(obj.ExtentPatch)
                    color = obj.PositionsLine(1).MarkerFaceColor;
                    if isequal(color,'none')
                        color = obj.PositionsLine(1).MarkerEdgeColor;
                    end
                    obj.ExtentPatch = patch(hAxes,xdata,ydata,zdata, color, ...
                        'EdgeColor',obj.PositionsLine(1).MarkerEdgeColor,'FaceAlpha',0.05,'HandleVisibility','off','Tag',sprintf('tpTrackExtent'));
                else
                    set(obj.ExtentPatch,'XData',xdata,'YData',ydata,'ZData',zdata);
                end
            end                        
        end
        
        function drawCovariances(obj, positions, covariances)
            hAxes = obj.ParentAxes;
            if ishghandle(hAxes) && ishghandle(obj.PositionsLine)
                if isempty(covariances)
                    positions = zeros(3,0);
                end

                npts = obj.NumEllipsoidPoints;
                sdwidth = obj.StandardDeviationWidth;

                cSurf = obj.CovariancesPatches;
                for i=1:size(positions,2)
                   [x,y,z] = makeEllipsoid(obj,positions(:,i),covariances(:,:,i),npts,sdwidth);
                   fvc = surf2patch(x,y,z);
                   if i<=numel(cSurf) && ishghandle(cSurf(i))
                      set(cSurf(i),fvc);
                   else
                      cSurf(i) = patch(hAxes,fvc,...
                                    'FaceColor',obj.MarkerFaceColor, ...
                                    'FaceAlpha',0.1, ...
                                    'EdgeColor',obj.MarkerEdgeColor, ...
                                    'EdgeAlpha',0.1, ...
                                    'Tag', sprintf('tpCovariancePatch%i',i),...
                                    'HandleVisibility','off');
                   end
                end
                idx = size(positions,2)+1:numel(cSurf);
                delete(cSurf(idx(ishghandle(cSurf(idx)))));
                cSurf(idx) = [];
                obj.CovariancesPatches = cSurf;
            end
        end
        
        function [x,y,z] = makeEllipsoid(~, positions, covariances, npts, sdwidth)
            % plot the gaussian fits
            [v,d]=eig(covariances);
            % Inf or nan eigen values will result in a degenerated ellipse of null radius
            
            % should be real, but sometimes there is a small imaginary part
            v = real(v);
            d = real(d);
            
            % convert variance to sdwidth*sd
            d = sdwidth * sqrt(max(diag(d),0)); 
            
            % create covariance ellipse
            [x,y,z] = ellipsoid(0,0,0,d(1),d(2),d(3),npts);
            ellip = [x(:) y(:) z(:)]';
            
            % rotate and translate into coordinate frame
            ellip = v*ellip;
            ellip = bsxfun(@plus,ellip,positions);
            
            x = reshape(ellip(1,:),size(x));
            y = reshape(ellip(2,:),size(y));
            z = reshape(ellip(3,:),size(z));
        end
        
        function clearHistory(obj)
            obj.HistoryPositions = {};
            obj.HistoryLabels = {};
            
            if obj.ColorizeHistory
                idx = 2:numel(obj.HistoryLine);
                delete(obj.HistoryLine(idx(ishghandle(obj.HistoryLine(idx)))));
                obj.HistoryLine(idx) = [];
            end
            
            if ishghandle(obj.HistoryLine(1))
                set(obj.HistoryLine(1),'XData',[],'YData',[],'ZData',[]);
            end
        end

        function drawHistory(obj)
            if ~obj.ColorizeHistory
                lineData = concatenateHistory(obj);
                hLine = obj.HistoryLine;
                if ishghandle(hLine)
                    set(hLine,'XData',lineData(:,1),'YData',lineData(:,2),'ZData',lineData(:,3));
                end
            elseif ishghandle(obj.HistoryLine(1))
                colors = get(obj.ParentAxes,'DefaultAxesColorOrder');
                for i=1:numel(obj.HistoryLabels)
                    lineData = obj.HistoryPositions{i};
                    if numel(obj.HistoryLine) < i
                        obj.HistoryLine(i) = line(obj.ParentAxes, ...
                            lineData(:,1), ...
                            lineData(:,2), ...
                            lineData(:,3), ...
                            'LineStyle','-', ...
                            'LineWidth', 2, ...
                            'Color',colors(mod(i,size(colors,1))+1,:), ...
                            'HandleVisibility','off', ...
                            'Tag',['tpTrackHistory_T' num2str(i)]);
                    else
                        set(obj.HistoryLine(i),'XData',lineData(:,1),'YData',lineData(:,2),'ZData',lineData(:,3));
                    end
                end
            end
        end

        function lineData = concatenateHistory(obj)
            if obj.ConnectHistory
                lineData = concatenateConnectedHistory(obj);
            else
                lineData = concatenateUnconnectedHistory(obj);
            end
        end
            
        function lineData = concatenateUnconnectedHistory(obj)
            lineCells = obj.HistoryPositions;
            lineData = vertcat(zeros(0,3),lineCells{:});
        end

        
        function lineData = concatenateConnectedHistory(obj)
            lineCells = obj.HistoryPositions;
            q = size(obj.HistoryLabels,1)+obj.HistoryTotal;
            lineData = zeros(q,3);
            n = 0;
            for i=1:numel(lineCells)
                positions = lineCells{i};
                m = size(positions,1);
                lineData(n+1:n+m,:) = positions;
                lineData(n+m+1,:) = nan(1,3);
                n = n+m+1;
            end
        end
                
        function addUnconnectedHistory(obj, positions)
            if obj.HistoryDepth > 0
                if numel(obj.HistoryPositions) == obj.HistoryDepth
                    obj.HistoryPositions(1) = [];
                end
                obj.HistoryPositions{end+1} = positions;
            end
        end
        
        function addConnectedHistory(obj, positions, trackIDs)
            
            n = size(positions,1);
            
            obj.HistoryTotal = obj.HistoryTotal + n;
            if isempty(obj.HistoryLabels)
                obj.HistoryLabels = trackIDs;
                if ~isempty(trackIDs)
                   obj.HistoryPositions = mat2cell(positions,ones(n,1),3);
                end
            else
                [~, iPrev, iNext] = intersect(obj.HistoryLabels, trackIDs);
                for i=1:numel(iPrev)
                    obj.HistoryPositions{iPrev(i)} = vertcat(obj.HistoryPositions{iPrev(i)},...
                                                             positions(iNext(i),:));
                end
            end
            [~,~,iNew] = setxor(obj.HistoryLabels, trackIDs);
            obj.HistoryLabels = [obj.HistoryLabels; trackIDs(iNew)];
            for i=1:numel(iNew)
               obj.HistoryPositions{end+1} = positions(iNew(i),:);
            end
        end
        
        function trackIDs = getTrackIDsFromLabels(obj,positions,labels)
            trackIDs = labels(:);

            % some users generate multi-line labels via a cell array of cell array
            % of character vectors.  As a convenience, allow parsing to work when 
            % just each contained cell array is scalar.
            if ~isempty(labels) && iscell(labels) && iscell(labels{1})
                trackIDs = vertcat(labels{:});
            end

            if obj.ConnectHistory && numel(unique(trackIDs)) ~= numel(trackIDs)
                error(message('shared_radarfusion:theaterPlot:LabelsMustBeUnique'))
            end

            if obj.ConnectHistory && isempty(labels) && size(positions,1)>0
                error(message('shared_radarfusion:theaterPlot:LabelsMustBeSpecified'))
            end        
        end
    end
        
    methods (Static, Access = {?matlab.unittest.TestCase})
        function [xdata, ydata, zdata] = cuboidPatchData(position, dim, orientation)
            %cuboidPatchData vertices for cuboid position, dimension and orientation

            L = dim.Length;
            W = dim.Width;
            H = dim.Height;
            offset = dim.OriginOffset;

            % vertices ordered counterclockwise observed from the outside
            % forward, left, up, backward, right, down
            f = [ 1  1  1; 1 -1  1; 1 -1 -1; 1  1 -1] .* [L W H];
            l = [ 1  1  1; 1  1 -1;-1  1 -1;-1  1  1] .* [L W H];
            u = [ 1  1  1;-1  1  1;-1 -1  1; 1 -1  1] .* [L W H];
            b = [-1  1  1;-1  1 -1;-1 -1 -1;-1 -1  1] .* [L W H];
            r = [-1 -1  1;-1 -1 -1; 1 -1 -1; 1 -1  1] .* [L W H];
            d = [ 1  1 -1; 1 -1 -1;-1 -1 -1;-1  1 -1] .* [L W H];

            faces = [f; l; u; b; r; d]/2;

            % adjust faces to center of rotation
            faces = faces - offset;

            % rotate and translate to desired position
            R = orientation;
            faces = faces * R + position;

            % return (x,y,z) positions of 4 corners of 6 sides of cube
            faces = reshape(faces',3,4,[]); 
            xdata = squeeze(faces(1,:,:));
            ydata = squeeze(faces(2,:,:));
            zdata = squeeze(faces(3,:,:));
        end
        
        function [dims, orientations, arglist] = parseExtents(positions,arglist)
            
            n = find(cellfun(@isstruct, arglist));
            
            if ~isscalar(n) || ~isequal(fieldnames(arglist{n}),{'Length';'Width';'Height';'OriginOffset'})
                % return empty output
                dims = [];
                orientations = [];
            elseif ~isvector(arglist{n})
                % dim not a vector
                error(message('shared_radarfusion:theaterPlot:DimensionStructureMustBeVector'));
            elseif numel(arglist{n}) ~= size(positions,1)
                % number of dims does not match 
                error(message('shared_radarfusion:theaterPlot:PositionDimensionsMismatch'));
            elseif numel(arglist) == n
                % dim provided without orientation
                error(message('shared_radarfusion:theaterPlot:MissingOrientation'));
            elseif isa(arglist{n+1},'quaternion') && ~isvector(arglist{n+1})
                % dim with non-vector orientation quaternion
                error(message('shared_radarfusion:theaterPlot:OrientationQuaternionMustBeVector'));
            elseif isa(arglist{n+1},'quaternion') && numel(arglist{n+1}) ~= size(positions,1)
                % dim with mismatched orientation quaternion
                error(message('shared_radarfusion:theaterPlot:PositionOrientationQuaternionMismatch'))
            elseif size(arglist{n+1},1)==3 && size(arglist{n+1},2)==3 && size(arglist{n+1},3) ~= size(positions,1)
                % dim with mismatched orientation matrix
                error(message('shared_radarfusion:theaterPlot:PositionOrientationMatrixMismatch'))
            elseif isa(arglist{n+1},'quaternion')
                % fetch dimensions and quaternions (translating to orientation matrices)
                dims = arglist{n};
                orientations = arglist{n+1};
                orientations = rotmat(orientations(:),'frame');
                arglist = arglist([(1:n-1) (n+2:end)]);
            elseif size(arglist{n+1},1)==3 && size(arglist{n+1},2)==3
                % fetch dimensions and orientation matrices
                dims = arglist{n};
                orientations = arglist{n+1};
                arglist = arglist([(1:n-1) (n+2:end)]);
            else
                % dims correctly parsed, but not the orientation
                error(message('shared_radarfusion:theaterPlot:UnrecognizedOrientationQuaternionOrMatrix'))
            end
        end
    end
end

function args = convertStringsToCellChars(args)
   % error on lone character vectors
   if any(cellfun(@ischar, args))
      error(message('shared_radarfusion:theaterPlot:UnrecognizedInput'));
   end
   
   % convert strings to cell array of character vectors
   [args{:}] = convertStringsToChars(args{:});

   % to be consistent with the behavoir of string vectors, package
   % a character vector inside a cell array
   idx = find(cellfun(@ischar, args));
   for i=1:numel(idx)
       args{idx} = args(idx);
   end
end
