classdef DetectionPlotter < fusion.theaterPlot.TheaterPlotter
%DetectionPlotter Create a new detection plotter
%
%   See:  <a href="matlab:help theaterPlot/detectionPlotter">theaterPlot/detectionPlotter</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    s  square       ^ triangle (up)
        %      x   x-mark   d  diamond      v triangle (down)
        %      +   plus     p  pentagram    < triangle (left)
        %      *   star     h  hexagram     > triangle (right)
        %      o   circle (default)
        Marker
        
        %MarkerSize  Marker size
        %   A positive integer specifying the size of the marker.
        %
        %   The default is 6.
        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 = []
        HistoryLine = []
        HistoryPositions = {}
        LabelText = []
        ParentAxes = []
        LabelFontSize = 10
        LabelTextOffset = [0 0 0]
        ScaleFactor = 1
    end
    
    methods
        function obj = DetectionPlotter(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
            drawnow limitrate
        end
        
        function set.MarkerEdgeColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerEdgeColor = value;
            end
            hLine = obj.HistoryLine;
            if ishghandle(hLine)
                hLine.MarkerEdgeColor = historyColor(obj,pLine.MarkerEdgeColor);
            end
            vLine = obj.VelocitiesLine;
            if ishghandle(vLine)
                vLine.Color = value;
            end
            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));
            drawLabels(obj, zeros(0,3), {});
            clearHistory(obj);
            drawHistory(obj);
            drawnow limitrate
        end
        
        function set.HistoryDepth(obj,value)
            validateattributes(value,{'numeric'},{'scalar','integer','nonnegative'});
            clearHistory(obj);
            obj.HistoryDepth = value;
            drawHistory(obj);
            drawnow limitrate
        end
        
        function plotDetection(obj, positions, varargin)
            %plotDetection plot a set of detections in a theater detection plotter.
            %      plotDetection(detPlotter, positions) specifies positions of M
            %         detected objects whose positions are to be plotted by the
            %         detections plotter, detPlotter. 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 detected locations.
            %
            %      plotDetection(detPlotter, positions, velocities) additionally
            %         specifies the corresponding velocities of the detections.
            %         Velocities are plotted as line vectors emanating from the center
            %         positions of the detections.  If specified, velocities must have
            %         the same dimensions as positions. 
            %
            %      plotDetection(detPlotter, positions, ..., labels) additionally
            %         specifies a cell vector of length M whose elements contain the
            %         text labels corresponding to the M detections specified in the
            %         positions matrix.  If omitted, no labels are plotted.
            %
            %   % Example
            %   % -------
            %   % Create a theater plot and update it with three detections labeled
            %   % 'R1', 'R2', and 'R3'; positioned in units of meters at 
            %   % (30,5,4), (30,-10,2), and (30,15,1); with corresponding velocities
            %   % (in m/s) of (-10,0,2), (-10,3,1) and (-10,-4,1), respectively.
            %
            %   tp = theaterPlot('XLim',[0,90],'YLim',[-35,35],'ZLim',[1 10]);
            %   radarPlotter = detectionPlotter(tp,'DisplayName','Radar Detections');
            %
            %   positions = [30, 5, 4; 30, -10, 2; 30, 15, 1];
            %   velocities = [-10, 0, 2; -10, 3, 1; -10, -4, 1];
            %   labels = {'R1','R2','R3'};
            %   plotDetection(radarPlotter, positions, velocities, labels);
            %
            %   See also theaterPlot/detectionPlotter.
            if nargin > 2
                [varargin{:}] = convertStringsToChars(varargin{:});
            end
            positions = parsePositions(positions, 'plotDetection');
            drawPositions(obj, positions);
            
            [velocities,varargin] = parseVelocities(positions, varargin);
            drawVelocities(obj, positions, velocities);
            
            [covariances,varargin] = parseCovariances(positions, varargin);
            drawCovariances(obj, positions.', covariances);
            
            [labels,varargin] = parseLabels(positions,varargin);
            drawLabels(obj, positions, labels);
            
            if ~isempty(varargin)
                error(message('shared_radarfusion:theaterPlot:UnrecognizedInput'));
            end
            
            drawHistory(obj);
            addHistory(obj,positions);
            drawnow limitrate
        end
    end
    
    methods (Access = protected)
        function initParser(obj, p)
            initParser@fusion.theaterPlot.TheaterPlotter(obj, p)
            addParameter(p,'HistoryDepth',0);
            addParameter(p,'Marker','o');
            addParameter(p,'MarkerSize',6);
            addParameter(p,'MarkerEdgeColor',[0 0 0]);
            addParameter(p,'MarkerFaceColor','none');
            addParameter(p,'FontSize',10);
            addParameter(p,'VelocityScaling',1);
            addParameter(p,'LabelOffset',[0 0 0]);
        end

        
        function value = getPrimaryObject(obj)
            value = obj.PositionsLine;
        end
        
        function createGraphics(obj, hAxes, p)
            pr = p.Results;
            if ~isempty(pr.DisplayName)
                 pLine = line(hAxes, 0, 0, ...
                              'DisplayName',pr.DisplayName, ...
                              'Marker',pr.Marker, ...
                              'MarkerSize',pr.MarkerSize, ...
                              'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                              'MarkerFaceColor',pr.MarkerFaceColor, ...
                              'LineStyle','none', ...
                              'Tag', ['tpDetectionPositions_' pr.DisplayName]);
                vLine = line(hAxes, 0, 0, 0, ...
                             'Color', pr.MarkerEdgeColor, ...
                             'HandleVisibility','off', ...
                             'Tag', ['tpDetectionVelocities_' pr.DisplayName]);
            else
                pLine = line(hAxes, 0, 0, 0, ...
                             'HandleVisibility','off', ...
                             'Marker',pr.Marker, ...
                             'MarkerSize',pr.MarkerSize, ...
                             'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                             'MarkerFaceColor',pr.MarkerFaceColor, ...
                             'LineStyle','none', ...
                             'Tag', 'tpDetectionPositions');
                vLine = line(hAxes, 0, 0, 0, ...
                             'Color', pr.MarkerEdgeColor, ...
                             'HandleVisibility','off', ...
                             'Tag', 'tpDetectionVelocities');
            end
            
            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) && pr.HistoryDepth ~= 0
                hLine = line(hAxes, 0, 0, 0, ...
                             'MarkerEdgeColor',historyColor(obj,pLine.MarkerEdgeColor), ...
                             'MarkerFaceColor',historyColor(obj,pLine.MarkerFaceColor), ...
                             'Marker', pr.Marker, ...
                             'MarkerSize', max(1,round(pr.MarkerSize/2)), ...
                             'LineStyle', 'none', ...
                             'DisplayName','(history)', ...
                             'Tag',['tpDetectionHistory_', pr.DisplayName]);
            else
                hLine = line(hAxes, 0, 0, 0, ...
                             'MarkerEdgeColor',historyColor(obj,pLine.MarkerEdgeColor), ...
                             'MarkerFaceColor',historyColor(obj,pLine.MarkerFaceColor), ...
                             'Marker', pr.Marker, ...
                             'MarkerSize', max(1,round(pr.MarkerSize/2)), ...
                             'LineStyle', 'none', ...
                             'HandleVisibility','off', ...
                             'Tag',['tpTrackHistory_', pr.DisplayName]);
            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('tpDetectionLabels_%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 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));
                   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);
            
            % 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 = {};
        end

        function drawHistory(obj)
            lineData = concatenateHistory(obj);
            hLine = obj.HistoryLine;
            if ishghandle(hLine)
                set(hLine,'XData',lineData(:,1),'YData',lineData(:,2),'ZData',lineData(:,3));
            end
        end
        
        function lineData = concatenateHistory(obj)
            lineCells = obj.HistoryPositions;
            lineData = zeros(0,3);
            for i=1:numel(lineCells)
                lineData = [lineData; lineCells{i}]; %#ok<AGROW>
            end
        end
        
        function addHistory(obj, positions)
            if obj.HistoryDepth > 0
                if numel(obj.HistoryPositions) == obj.HistoryDepth
                    obj.HistoryPositions(1) = [];
                end
                obj.HistoryPositions{end+1} = positions;
            end
        end
    end
end