classdef HelperSlerpPlotting
%HELPERSLERPPLOTTING helper methods for LowpassSLERPExample
%   This class is for example use only. It may be removed in the future. 

%   Copyright 2018 The MathWorks, Inc.   

    methods (Static)
        
        function plotEulerd(f, q, varargin)
            % Plot quaternion q in Euler angles in degrees in 3 subplots
            set(0, 'CurrentFigure', f);
            
            eul = eulerd(q, 'ZYX', 'frame');
            
            for ii=1:3
                ax = subplot(3,1,ii);
                ax.ColorOrderIndex = ii;
                hold(ax, 'on');
                plot(eul(:,ii), varargin{:});
                hold(ax, 'off');
            end
        end
        
        function addAnnotations(f, hrange, hbias)
            % Annotate euler angle plot
            set(0, 'CurrentFigure', f);
            
            subplot(3,1,1);
            legend('Noisy', 'Truth', 'Filtered');
            title('Z-axis Rotation (Yaw)');
            ylabel('Degrees');
            
            subplot(3,1,2);
            legend('Noisy', 'Truth', 'Filtered');
            title('Y-axis Rotation (Roll)');
            ylabel('Degrees');
            
            subplot(3,1,3);
            legend('Noisy', 'Truth', 'Filtered');
            title('X-axis Rotation (Pitch)');
            ylabel('Degrees');
            
            s = "SLERP Interpolation Parameter : hrange = " + hrange + ", hbias = " ...
                + hbias;
            
            xlabel(s);
        end
        
        function plotAngularVelocities(avSlerp, avLerp)
            % Plot angular velocities for slerp vs lerp
            figure;
            plot(avSlerp);
            ax = gca;
            hold(ax, 'on');
            ax.ColorOrderIndex = 1; % restart colors
            plot(avLerp, '-.');
            title('Angular Velocity - SLERP vs Linear Interpolation')
            ylabel('Angular Velocity (rad/s)');
            
            legend(...
                'X angular velocity (SLERP)', ...
                'Y angular velocity (SLERP)', ...
                'Z angular velocity (SLERP)', ...
                'X angular velocity (part-wise linear)', ...
                'Y angular velocity (part-wise linear)', ...
                'Z angular velocity (part-wise linear)', 'Location', 'best');
            hold(ax, 'off');
        end
        
    end
    
end
