classdef TrajectoryPlotter < fusion.theaterPlot.TheaterPlotter
%TrajectoryPlotter Create a new trajectory plotter
%
%   See:  <a href="matlab:help theaterPlot/trajectoryPlotter">theaterPlot/trajectoryPlotter</a> 

%   Copyright 2018 The MathWorks, Inc.

    properties (Dependent)
        %Color        Color of the trajectory
        %   Color is a 'color' string or an [R, G, B] vector defining a color.
        %   Default: 'gray'        
        Color
        
        %LineStyle    Line style
        %   specified as one of the line styles below:
        %      '-'   Solid line
        %      '--'  Dashed line
        %      ':'   Dotted line
        %      '-.'  Dash-dotted line
        %   Default: ':'   
        LineStyle
        
        %LineWidth Line width
        %   specified as a positive value in points.
        %   Default: 0.5 
        LineWidth
    end
    
    properties (Access = private)
        Line = []
    end

    methods
        function obj = TrajectoryPlotter(tp, varargin)
            obj@fusion.theaterPlot.TheaterPlotter(tp, varargin{:});
        end
        
        function plotTrajectory(obj, boundaryCoordList)
            %plotTrajectory Plot trajectory in a theater plot
            %    plotTrajectory(trajPlotter, trajCoordList) specifies all the 
            %    trajectories to show in the plot.  trajCoordList is a cell array
            %    of N elements, where N corresponds to the number of trajectories
            %    to plot.  The i-th trajectory coordinates, trajCoordList{i}, is
            %    represented by a three-column matrix of an arbitrary number of
            %    rows, where each row corresponds to a single waypoint coordinate
            %    along the trajectory curve, and each column corresponds to the
            %    x-, y- and z- positions of the coordinates, respectively.
            %
            %    % Example
            %    % -------
            %    % Create a trackingScenario and add waypoints for a trajectory
            %    ts = trackingScenario;
            %    height = 100; 
            %    d = 1;
            %    wayPoints = [ ...
            %      -30   -25   height; 
            %      -30    25-d height; 
            %      -30+d  25   height; 
            %      -10-d  25   height; 
            %      -10    25-d height; 
            %      -10   -25+d height; 
            %      -10+d -25   height; 
            %       10-d -25   height; 
            %       10   -25+d height; 
            %       10    25-d height; 
            %       10+d  25   height; 
            %       30-d  25   height; 
            %       30    25-d height; 
            %       30   -25+d height; 
            %       30   -25   height];
            %
            %    % Specify time for each waypoint
            %    elapsedTime = linspace(0,10,size(wayPoints,1));
            %
            %    % Create a platform in the tracking scenario and add trajectory
            %    % information using the trajectory method
            %    target = platform(ts);
            %    traj = waypointTrajectory('Waypoints',wayPoints,'TimeOfArrival',elapsedTime);
            %    target.Trajectory = traj;
            %
            %    % Record the tracking scenario to retrieve platform's trajectory
            %    r = record(ts);
            %    pposes = [r(:).Poses];
            %    pposition = vertcat(pposes.Position);
            %
            %    % Create a theater plot to display the recorded trajectory
            %    tp = theaterPlot('XLim',[-40 40],'YLim',[-40 40]);    
            %    trajPlotter = trajectoryPlotter(tp,'DisplayName','Trajectory');
            %    plotTrajectory(trajPlotter,{pposition})
            %
            %    % Animate using the platformPlotter
            %    restart(ts);
            %    platPlotter = platformPlotter(tp,'DisplayName','Platform');
            %
            %    while advance(ts)
            %       p = pose(target,'true');
            %       plotPlatform(platPlotter, p.Position);
            %       pause(0.1)
            %    end            
            
            validateattributes(boundaryCoordList,{'cell'},{'vector'},'trajectoryPlotter','boundaryCoordList');
            drawTrajectories(obj, boundaryCoordList);
            drawnow limitrate
        end
        
        function clearData(obj)
            %clearData remove all data from a plotter
            %   clearData(trajPlotter) removes all trajectories associated with the
            %   plotter, trajPlotter.  
            drawTrajectories(obj, {});
            drawnow limitrate
        end
        
        function value = get.Color(obj)
            hLine = obj.Line;
            value = hLine.Color;
        end
        
        function value = get.LineStyle(obj)
            hLine = obj.Line;
            value = hLine.LineStyle;
        end
        
        function value = get.LineWidth(obj)
            hLine = obj.Line;
            value = hLine.LineWidth;
        end
        
        function set.Color(obj,value)
            hLine = obj.Line;
            if ishghandle(hLine)
                hLine.Color = value;
            end
            drawnow limitrate
        end
        
        function set.LineStyle(obj,value)
            hLine = obj.Line;
            if ishghandle(hLine)
                hLine.LineStyle = value;
            end
            drawnow limitrate
        end
        
        function set.LineWidth(obj,value)
            hLine = obj.Line;
            if ishghandle(hLine)
                hLine.LineWidth = value;
            end
            drawnow limitrate
        end
    end
    
    methods (Access = protected)
        function initParser(obj, p)
            initParser@fusion.theaterPlot.TheaterPlotter(obj, p);
            addParameter(p,'Color',[0.5 0.5 0.5]);
            addParameter(p,'LineStyle',':');
            addParameter(p,'LineWidth',0.5);
        end

        function value = getPrimaryObject(obj)
            value = obj.Line;
        end
        
        function createGraphics(obj, hAxes, p)
            pr = p.Results;
            if ~isempty(pr.DisplayName)
                hLine = line(hAxes, 0, 0, 0, ...
                             'DisplayName',pr.DisplayName, ...
                             'Color',pr.Color, ...
                             'LineStyle', pr.LineStyle, ...
                             'LineWidth', pr.LineWidth, ...
                             'Tag', ['tpTrajectory_' pr.DisplayName]);
            else
                hLine = line(hAxes, 0, 0, 0, ...
                             'HandleVisibility','off', ...
                             'Color',pr.Color, ...
                             'LineStyle', pr.LineStyle, ...
                             'LineWidth', pr.LineWidth, ...
                             'Tag', 'tpTrajectory');
            end
            obj.Line = hLine;
        end
        
        function deleteGraphics(obj)
            delete(obj.Line);
        end
    end
    
    methods (Access = private)
        function drawTrajectories(obj,markerCoordList)

            xpLineData = concatenateLines(obj,markerCoordList);

            hLine = obj.Line;
            if ishghandle(hLine)
                set(hLine,'XData',xpLineData(:,1),'YData',xpLineData(:,2),'ZData',xpLineData(:,3));
            end
        end
    
        function lineData = concatenateLines(~,markerCoordList)
            lineData = zeros(0,3);
            nLines = numel(markerCoordList);

            for i=1:nLines-1
                entry = markerCoordList{i};
                if ~isreal(entry) || ~ismatrix(entry) || size(entry,2)~=3
                    error(message('shared_radarfusion:theaterPlot:InvalidMarkerCoordListEntry',i));
                end
                lineData = [lineData; entry; NaN(1,3)]; %#ok<AGROW>
            end

            if nLines>0
                entry = markerCoordList{nLines};
                if ~isreal(entry) || ~ismatrix(entry) || size(entry,2)~=3
                    error(message('shared_radarfusion:theaterPlot:InvalidMarkerCoordListEntry',nLines));
                end
                lineData = [lineData; entry];
            end
        end
    end
end