classdef PlatformPlotter < fusion.theaterPlot.TheaterPlotter
%PlatformPlotter Create a new platform plotter
%
%   See:  <a href="matlab:help theaterPlot/PlatformPlotter">theaterPlot/platformPlotter</a> 

%   Copyright 2018 The MathWorks, Inc.
    
    properties (Dependent)
        %Marker    Marker symbol
        %   Specify Marker as one of the following values:
        %      .   point    s  square       o circle
        %      x   x-mark   d  diamond      v triangle (down)
        %      +   plus     p  pentagram    < triangle (left)
        %      *   star     h  hexagram     > triangle (right)
        %      ^ triangle (up) (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
        
        % ExtentAlpha Alpha value for the extent edges and faces
        %   Specify FaceAlpha and EdgeAlpha for plotting extent.
        % 
        %   The default is 1.
        %
        ExtentAlpha
        
        %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 (Access = private)
        PositionsLine = []
        VelocitiesLine = []
        LabelText = []
        ParentAxes = []
        LabelFontSize = 10
        LabelTextOffset = [0 0 0]
        ScaleFactor = 1
        ExtentFaceAlpha = 1;
        ExtentEdgeAlpha = 1;
        ExtentPatch = matlab.graphics.primitive.Patch.empty
    end
    
    methods
        function obj = PlatformPlotter(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 value = get.ExtentAlpha(obj)
            value = obj.ExtentFaceAlpha;
        end
        
        function set.Marker(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.Marker = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerSize(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerSize = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerFaceColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerFaceColor = value;
            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
            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 set.ExtentAlpha(obj, value)
            validateattributes(value,{'numeric'},{'finite','real','scalar','>=',0,'<=',1});
            set(obj.ExtentPatch(ishghandle(obj.ExtentPatch)),'FaceAlpha',value);
            set(obj.ExtentPatch(ishghandle(obj.ExtentPatch)),'EdgeAlpha',value);
            obj.ExtentFaceAlpha = value;
            obj.ExtentEdgeAlpha = 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));
            drawLabels(obj, zeros(0,3), {});
            drawExtents(obj, zeros(0,3), [], []);
            drawnow limitrate
        end
        
        function plotPlatform(obj, positions, varargin)
            %plotPlatform plot a set of platforms in a platform plotter.
            %      plotPlatform(plotter, positions) specifies positions of M
            %         platforms whose positions are to be plotted by the platform
            %         plotter, plotter. Specify the positions as an M-by-3 matrix,
            %         where the each column of the matrix corresponds to the X, Y, and
            %         Z coordinates of the platforms' locations.
            %
            %      plotPlatform(plotter, positions, velocities) additionally
            %         specifies the corresponding velocities of the platforms.
            %         Velocities are plotted as line vectors emanating from the
            %         positions of the platforms.  If specified, velocities must have
            %         the same dimensions as positions.
            %
            %      plotPlatform(plotter, positions, ..., labels) additionally
            %         specifies a cell vector of length M whose elements contain the
            %         text labels corresponding to the M platforms specified in the
            %         positions matrix.  If omitted, no labels are plotted.
            % 
            %      plotPlatform(plotter, positions, ..., dim, orient) additionally
            %         specifies a cuboid extent of the platform.  The dimensions, dim,
            %         is an M-by-1 dimensions structure containing the length, width,
            %         height and origin offset 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.
            %       
            %     plotPlatform(plotter, positions, ..., mesh, orient) specifies the
            %         extent of the platform using a mesh. The mesh is a M-by-1 array
            %         of extendedObjectMesh objects. The orientation, orient, is
            %         expressed as either a 3-by-3-by-M rotation matrix or a quaternion
            %         vector of length M.
            %      
            %
            %   % Example
            %   % -------
            %   % Create a theater plot and update it with three platforms labeled
            %   % 'P1', 'P2', and 'P3'; 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]);
            %   plotter = platformPlotter(tp,'DisplayName','Platforms');
            %
            %   positions = [30, 5, 4; 30, -10, 2; 30, 15, 1];
            %   velocities = [-10, 0, 2; -10, 3, 1; -10, -4, 1];
            %   labels = {'P1','P2','P3'};
            %   plotPlatform(plotter, positions, velocities, labels);
            %
            %   See also theaterPlot/platformPlotter.
            if nargin > 2
                [varargin{:}] = convertStringsToChars(varargin{:});
            end
            positions = parsePositions(positions, 'plotPlatform');
            drawPositions(obj, positions);
            
            [velocities,varargin] = parseVelocities(positions, varargin);
            drawVelocities(obj, positions, velocities);
            
            [labels,varargin] = parseLabels(positions,varargin);
            drawLabels(obj, positions, labels);
            
            [geometry,orientations,varargin] = fusion.theaterPlot.PlatformPlotter.parseExtents(positions,varargin);
            
            drawExtents(obj, positions, geometry, orientations);
            
            if ~isempty(varargin)
                error(message('shared_radarfusion:theaterPlot:UnrecognizedInput'));
            end
            
            drawnow limitrate;
        end
    end
    
    methods (Access = protected)
        function initParser(obj, p)
            initParser@fusion.theaterPlot.TheaterPlotter(obj, p)
            addParameter(p,'Marker','^');
            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]);
            addParameter(p,'ExtentAlpha',1);
        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', ['tpPlatformPositions_' pr.DisplayName]);
                vLine = line(hAxes, 0, 0, ...
                             'Color', pr.MarkerEdgeColor, ...
                             'HandleVisibility','off', ...
                             'Tag', ['tpPlatformVelocities_' pr.DisplayName]);
            else
                pLine = line(hAxes, 0, 0, ...
                             'HandleVisibility','off', ...
                             'Marker',pr.Marker, ...
                             'MarkerSize',pr.MarkerSize, ...
                             'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                             'MarkerFaceColor',pr.MarkerFaceColor, ...
                             'LineStyle','none', ...
                             'Tag', 'tpPlatformPositions');
                vLine = line(hAxes, 0, 0, ...
                             'Color', pr.MarkerEdgeColor, ...
                             'HandleVisibility','off', ...
                             'Tag', 'tpPlatformVelocities');
            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
                     
            set(pLine,'XData',[],'YData',[],'ZData',[]);
            set(vLine,'XData',[],'YData',[],'ZData',[]);
            
            obj.PositionsLine = pLine;
            obj.VelocitiesLine = vLine;
            obj.ParentAxes = hAxes;
            
            obj.LabelFontSize = pr.FontSize;
            obj.LabelOffset = pr.LabelOffset;
            obj.VelocityScaling = pr.VelocityScaling;
            obj.ExtentFaceAlpha = pr.ExtentAlpha;
            obj.ExtentEdgeAlpha = pr.ExtentAlpha;
        end
        
        function deleteGraphics(obj)
            delete(obj.PositionsLine);
            delete(obj.VelocitiesLine);
        end
    end
        
    methods (Access = private)
        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('tpPlatformLabels_%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, geometry, orientations)
            hAxes = obj.ParentAxes;
            if ishghandle(obj.PositionsLine)
                if isa(geometry,'extendedObjectMesh')
                    [v,f] = fusion.theaterPlot.PlatformPlotter.meshesPatchData(positions, geometry, orientations);
                else
                    [v,f] = fusion.theaterPlot.PlatformPlotter.cuboidPatchesFVData(positions, geometry, orientations);
                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,'Faces',f,'Vertices',v,'FaceColor',color, ...
                        'EdgeColor',obj.PositionsLine(1).MarkerEdgeColor,'FaceAlpha',obj.ExtentFaceAlpha,...
                        'EdgeAlpha',obj.ExtentEdgeAlpha,'HandleVisibility','off','Tag',sprintf('tpPlatformExtent'));
                else
                    set(obj.ExtentPatch,'Faces',f,'Vertices',v);
                end
            end                        
        end

    end
    
    methods (Static, Access = {?matlab.unittest.TestCase})
        function [v,f] = cuboidPatchesFVData(positions, dims, orientations)
            n = numel(dims);
            v = zeros(24*n,3);
            f = zeros(6*n,4);
            for i=1:n
                [x,y,z] = fusion.theaterPlot.PlatformPlotter.cuboidPatchData(positions(i,:), dims(i), orientations(:,:,i));
                v((i-1)*24 + (1:24),:) = [x(:) y(:) z(:)];
                f((i-1)*6 + (1:6),:) = reshape((i-1)*24 + (1:24),4,6)';
            end
        end
        
        function [v, f] = meshesPatchData(positions, meshes, orientations)
            if isempty(meshes)
                v = zeros(0,3);
                f = zeros(0,3);
            else
                joinedMesh = transform(meshes(1),positions(1,:),orientations(:,:,1));
                for i = 2:numel(meshes)
                    meshi = transform(meshes(i),positions(i,:),orientations(:,:,i));
                    joinedMesh = join(joinedMesh,meshi);
                end
                v = joinedMesh.Vertices;
                f = joinedMesh.Faces;
            end
        end
        
        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 [geom, orientations, arglist] = parseExtents(positions,arglist)
            if isempty(arglist) || (~isstruct(arglist{1}) || ~isequal(fieldnames(arglist{1}),{'Length';'Width';'Height';'OriginOffset'})) && ~isa(arglist{1},'extendedObjectMesh')
                % return empty output
                geom = [];
                orientations = [];
            elseif ~isvector(arglist{1})
                % dim not a vector
                error(message('shared_radarfusion:theaterPlot:DimensionStructureMustBeVector'));
            elseif numel(arglist{1}) ~= size(positions,1)
                % number of dims does not match 
                error(message('shared_radarfusion:theaterPlot:PositionDimensionsMismatch'));
            elseif numel(arglist) < 2
                % dim provided without orientation
                error(message('shared_radarfusion:theaterPlot:MissingOrientation'));
            elseif isa(arglist{2},'quaternion') && ~isvector(arglist{2})
                % dim with non-vector orientation quaternion
                error(message('shared_radarfusion:theaterPlot:OrientationQuaternionMustBeVector'));
            elseif isa(arglist{2},'quaternion') && numel(arglist{2}) ~= size(positions,1)
                % dim with mismatched orientation quaternion
                error(message('shared_radarfusion:theaterPlot:PositionOrientationQuaternionMismatch'))
            elseif size(arglist{2},1)==3 && size(arglist{2},2)==3 && size(arglist{2},3) ~= size(positions,1)
                % dim with mismatched orientation matrix
                error(message('shared_radarfusion:theaterPlot:PositionOrientationMatrixMismatch'))
            elseif isa(arglist{2},'quaternion')
                % fetch dimensions and quaternions (translating to orientation matrices)
                geom = arglist{1};
                orientations = arglist{2};
                orientations = rotmat(orientations(:),'frame');
                arglist = arglist(3:end);
            elseif size(arglist{2},1)==3 && size(arglist{2},2)==3
                % fetch dimensions and orientation matrices
                geom = arglist{1};
                orientations = arglist{2};
                arglist = arglist(3:end);
            else
                % dims correctly parsed, but not the orientation
                error(message('shared_radarfusion:theaterPlot:UnrecognizedOrientationQuaternionOrMatrix'))
            end
        end
    end
end