classdef uavDubinsPathSegment
%uavDubinsPathSegment Dubins 3-D path segment connecting two UAV poses
%   The uavDubinsPathSegment stores properties and motion data connecting
%   a start and a goal pose for a UAV in North-East-Down coordinate system.
%   A path segment is a sequence of motions, each which can be:
%   - Straight                            "S"
%   - Left turn (counter-clockwise)       "L"
%   - Right turn (clockwise)              "R"
%   - Helix left turn (counter-clockwise) "Hl"
%   - Helix right turn (clockwise)        "Hr"
%   - No Action                           "N"
%
%   Turn direction is defined when viewed from above the UAV. Helical
%   motions are used to ascend or descend in altitude. The
%   no action segment "N" is only used as a filler at the end when
%   only three path segments are needed.
%
%   To generate a path segment between two poses, call the "connect"
%   function on a uavDubinsConnection object:
%   [PATHSEGOBJ, ~] = connect(CONNECTIONOBJ, STARTPOSE, GOALPOSE)
%
%   PATHSEGOBJ = uavDubinsPathSegment(CONNECTIONOBJ, STARTPOSE, GOALPOSE)
%   creates a UAV Dubins path segment that connects a given start and
%   goal pose for a UAV. The min turning radius and flight path angle
%   are obtained from the uavDubinsConnection object. It internally
%   computes the optimal path and assigns to PATHSEGOBJ.
%
%   PATHSEGOBJ = uavDubinsPathSegment(CONNECTIONOBJ, STARTPOSE, ...
%   GOALPOSE, MOTIONTYPE) create a UAV Dubins path segment with given 
%   MOTIONTYPES and sets the corresponding property. MOTIONTYPE is a 
%   4-element cell array of (e.g. {'L','S','R', 'Hr'}).
%
%   PATHSEGOBJ = uavDubinsPathSegment(STARTPOSE, GOALPOSE, ...
%   FLIGHTPATHANGLE, AIRSPEED, MINTURNINGRADIUS, HELIXRADIUS, ...
%   MOTIONTYPE, MOTIONLENGTH) explicitly specifies all the parameters 
%   needed for a UAV Dubins path segment.
%   The corresponding properties are set on the object.
%
%   uavDubinsPathSegment properties:
%
%   StartPose           - Initial pose (read-only)
%   GoalPose            - Desired final pose (read-only)
%   MinTurningRadius    - Minimum turning radius (read-only)
%   HelixRadius         - Radius for Helical Path (read-only)
%   FlightPathAngle     - Angle of flight path (read only)
%   AirSpeed            - UAV airspeed (read-only)
%   MotionLengths       - Length of each motion (read-only)
%   MotionTypes         - Type of each motion (read-only)
%   Length              - Length of path segment (read-only)
%
%   uavDubinsPathSegment methods:
%
%   interpolate         - Interpolate poses along path segment
%   show                - Visualization of path segment
%
%   Example 1: Create uavDubinsPathSegment and obtain the poses
%
%       % Create an object uavDubinsConnection
%       connectionObj = uavDubinsConnection;
%
%       % startPose and goalPose
%       startPose = [0 0 0 pi];
%       goalPose = [1 0 10 0];
%
%       % Create an object of uavDubinsPathSegment
%       pathSegObj = connect(connectionObj,startPose, goalPose)
%
%       % Sample the poses
%       [poses] = interpolate(pathSegObj{1},pathSegObj{1}.Length);
%
%       % Visualize the path, start, goal and headings
%       show(pathSegObj{1});
%
%   Example 2: Create uavDubinsPathSegment for a specific motion type
%
%       % startPose and goalPose
%       startPose = [0 5 0 pi/4];
%       goalPose = [1 0 10 0];
%
%       connectionObj = uavDubinsConnection;
%       pathSegObj = uavDubinsPathSegment(connectionObj, startPose, ...
%                    goalPose, {'L' 'R' 'L' 'N'});
%
%       show(pathSegObj);
%
%   Example 3: Create uavDubinsPathSegment with explicit parameter setting
%       
%       pathSegObj = uavDubinsPathSegment([0 0 0 0], [0 1 3 pi], 0.5,...
%         10, 18, 0, {'R' 'S' 'R' 'N'}, [4 5 6 0]);
%
%   See also dubinsPathSegment, reedsSheppPathSegment

%   Copyright 2019-2023 The MathWorks, Inc.

%#codegen

    properties (Access = private, Constant)
        %Colors to plot the path

        LightBlue   = [0.301 0.745 0.933];
        DarkGray    = [0.6, 0.6, 0.6];
        DarkBlue    = [0.15 0.25 0.8];
        Orange      = [0.85 0.325 0.098];
        Magenta     = [1 0 1];
        Red         = [1 0 0];
        Green       = [0.001 0.745 0.233];
        Black       = [0.3 0.245 0.233];
    end

    properties (SetAccess = {?private,?uavDubinsConnection})
        %StartPose Initial pose (read-only)
        %   The start pose specified as a [x,y,z,heading angle] vector.
        %   [x,y,z] is the position in meters and heading angle in radians.
        StartPose

        %GoalPose Desired final pose (read-only)
        %   The goal pose specified as a [x,y,z,heading angle] vector.
        %   [x,y,z] is the position in meters and heading angle in radians.
        GoalPose

        %FlightPathAngle Angle of the flight path (read only)
        %   The uav's flight path angle to reach goal's altitude
        FlightPathAngle

        %AirSpeed UAV airpseed
        %   The UAV's airspeed in meters per second
        AirSpeed

        %MinTurningRadius Minimum turning radius (read-only)
        %   Minimum turning radius, specified in meters. This
        %   corresponds to the turning radius of the circle at maximum
        %   bank angle.
        MinTurningRadius

        %HelixRadius Radius for Helical Path (read-only)
        %   The uav's helical path radius in meters
        HelixRadius

        %MotionType Type of each motion (read-only)
        %   A path segment is a sequence of motions, each which can be:
        %   - Straight                            "S"
        %   - Left turn (counter-clockwise)       "L"
        %   - Right turn (clockwise)              "R"
        %   - Helix left turn (counter-clockwise) "Hl"
        %   - Helix right turn (clockwise)        "Hr"
        %   - No Action                           "N"
        MotionTypes

        %MotionLengths Length of each motion type
        %   specified as a 4-element vector of positive scalars in meters.
        MotionLengths

        %Length Length of path segment
        %   The length of the flight path in meters
        Length

    end

    properties (Constant, Access = private)
        %Epsilon
        %   comparison tolerance
        Epsilon = 0.000001;

        %STLfile
        % name of the stl file
        STLfilename = 'fixedwing.stl';

        %StepSize
        % default step size for plotting
        StepSize = 0.05;

        %NumPoints
        % minimum number of points for plotting.
        MinNumPoint = 100;
    end

    methods
        %------------------------------------------------------------------
        function this = uavDubinsPathSegment(varargin)

        % Check the number of inputs.
            if (nargin == 3 || nargin == 4)

                uavDubinsConnObj = varargin{1};

                validateattributes(uavDubinsConnObj, ...
                                   {'uavDubinsConnection'}, ...
                                   {'scalar'}, 'uavDubinsPathSegment', 'ConnectionMethod');

                this.StartPose = varargin{2};
                this.GoalPose = varargin{3};

                disabledPathTypes = uavDubinsConnObj.DisabledPathTypes;
                tempObj = uavDubinsConnection('AirSpeed', uavDubinsConnObj.AirSpeed, ...
                                              'MaxRollAngle', uavDubinsConnObj.MaxRollAngle,...
                                              'FlightPathAngleLimit', uavDubinsConnObj.FlightPathAngleLimit, ...
                                              'DisabledPathTypes' , disabledPathTypes);
                if( nargin == 4)
                    motionType = varargin{4};
                    this.MotionTypes = motionType;

                    % set disabled path type
                    % find the index of the required motionType in AllPathTypes
                    I = 1:length(uavDubinsConnObj.AllPathTypes);
                    for i =1:length(uavDubinsConnObj.AllPathTypes)
                        S = strcmpi(uavDubinsConnObj.MotionTypes{i},motionType);
                        if(~any(S == 0))
                            I(i) = [];

                            allPath = uavDubinsConnObj.AllPathTypes;
                            disabledPath = cell(1,27);
                            for j = 1:27
                                disabledPath{j} = allPath{I(j)};
                            end
                            tempObj = uavDubinsConnection('AirSpeed', uavDubinsConnObj.AirSpeed,...
                                                          'MaxRollAngle', uavDubinsConnObj.MaxRollAngle, ...
                                                          'FlightPathAngleLimit', uavDubinsConnObj.FlightPathAngleLimit,...
                                                          'DisabledPathTypes', disabledPath);
                            break;
                        end
                    end
                end
                % compute optimal path
                A = tempObj.connect(this.StartPose, this.GoalPose);

                % set the properties
                this.MinTurningRadius = A{1}.MinTurningRadius;
                this.StartPose = A{1}.StartPose;
                this.GoalPose = A{1}.GoalPose;
                this.AirSpeed = A{1}.AirSpeed;
                this.HelixRadius = A{1}.HelixRadius;

                this.FlightPathAngle = A{1}.FlightPathAngle;
                this.MotionLengths = A{1}.MotionLengths;
                this.MotionTypes = A{1}.MotionTypes;
                this.Length = A{1}.Length;

            elseif(nargin==8)
                % set the property values

                this.MinTurningRadius = varargin{5};
                this.StartPose = varargin{1};
                this.GoalPose = varargin{2};
                this.AirSpeed = varargin{4};
                this.HelixRadius = varargin{6};

                this.FlightPathAngle= varargin{3};
                this.MotionLengths = varargin{8};
                this.MotionTypes = varargin{7};
                this.Length = sum(this.MotionLengths);

            else
                coder.internal.error('uav:robotuav:uavdubinspathsegment:InvalidNumArgs');

            end
        end

        %------------------------------------------------------------------
        function this = set.MotionLengths(this, motionLengths)
        %set.MotionLengths Setter for MotionLengths property
            validateattributes(motionLengths, {'double'}, ...
                               {'real', 'nonnegative', 'nonsparse', 'row', 'numel', 4}, ...
                               '', 'MotionLengths');
            if(any(isinf(motionLengths)))
                coder.internal.error('uav:robotuav:uavdubinspathsegment:ErrorInf');
            end
            this.MotionLengths = double(motionLengths);
        end

        %------------------------------------------------------------------
        function this = set.Length(this, mlength)
        %set.Length Setter for Length property
            validateattributes(mlength, {'single', 'double'}, ...
                               {'real', 'nonsparse', 'nonnegative' 'scalar'});

            this.Length = double(mlength);
        end

        %------------------------------------------------------------------
        function this = set.AirSpeed(this, airspeed)
        %set.AirSpeed Setter for AirSpeed property
            validateattributes(airspeed, {'single', 'double'}, ...
                               {'real', 'nonsparse', 'positive', 'finite', 'scalar'});

            this.AirSpeed = double(airspeed);
        end

        %------------------------------------------------------------------
        function this = set.MinTurningRadius(this, radius)
        %set.MinTurningRadius Setter for MinTurningRadius property
            validateattributes(radius, {'single', 'double'}, ...
                               {'real', 'nonsparse', 'nonnegative', 'finite', 'scalar', '>',0});

            this.MinTurningRadius = double(radius);
        end

        %------------------------------------------------------------------
        function this = set.HelixRadius(this, hradius)
        %set.HelixRadius Setter for HelixRadius property
            validateattributes(hradius, {'single', 'double'}, ...
                               {'real', 'nonsparse', 'nonnegative', 'finite', 'scalar'});

            this.HelixRadius = double(hradius);
        end

        %------------------------------------------------------------------
        function this = set.FlightPathAngle(this, fpa)
        %set.FlightPathAngle Setter for FlightPathAngle property
            validateattributes(fpa, {'single', 'double'}, ...
                               {'real', 'nonsparse',  'finite', 'scalar', '<',pi/2,'>',-pi/2});

            this.FlightPathAngle = double(fpa);
        end

        %------------------------------------------------------------------
        function this = set.MotionTypes(this, motionTypes)
        %set.MotionTypes Setter for MotionTypes property

            validateattributes(motionTypes, {'cell','string'}, {'row', 'numel', 4});
            % '' means path types has disabled.
            allPathTypes = uavDubinsConnection.AllPathTypes;

            validatestring([motionTypes{:}], allPathTypes, '', 'MotionTypes');

            for i = 1:numel(motionTypes)
                motionTypes{i} = upper(motionTypes{i});
            end

            if(strcmpi(motionTypes{1},'HL'))
                motionTypes{1} = 'Hl';
            elseif(strcmpi(motionTypes{1},'HR'))
                motionTypes{1} = 'Hr';
            elseif(strcmpi(motionTypes{4},'HR'))
                motionTypes{4} = 'Hr';
            elseif(strcmpi(motionTypes{4},'HL'))
                motionTypes{4} = 'Hl';
            end
            this.MotionTypes = motionTypes;
        end

        %------------------------------------------------------------------
        function this = set.StartPose(this, startPose)
        %set.StartPose Setter for StartPose property
            validateattributes(startPose, {'single', 'double'}, ...
                               {'real', 'nrows' , 1, 'ncols', 4, 'finite'}, '', 'startPose');

            this.StartPose = startPose;
            this.StartPose(4) = ...
                robotics.internal.wrapTo2Pi(this.StartPose(4));
            this.StartPose(4) = ...
                robotics.internal.wrapToPi(this.StartPose(4));
        end

        %------------------------------------------------------------------
        function this = set.GoalPose(this, goalPose)
        %set.GoalPose Setter for GoalPose property
            validateattributes(goalPose, {'single', 'double'}, ...
                               {'real', 'nrows' , 1, 'ncols', 4, 'finite'}, '', 'goalPose');
            this.GoalPose = goalPose;
            this.GoalPose(4) = ...
                robotics.internal.wrapTo2Pi(this.GoalPose(4));
            this.GoalPose(4) = ...
                robotics.internal.wrapToPi(this.GoalPose(4));
        end

        %------------------------------------------------------------------
        function [poses] = interpolate(this,samples)
        %INTERPOLATE Interpolate poses along path segment
        %   Provides the interpolated position and orientation along the
        %   path segment at the specified path LENGTHS. LENGTHS is 1xn 
        %   vector of distances in ascending order in the range 
        %   [0 PATHSEGOBJ.LENGTH].
        %
        %   [POSES] = INTERPOLATE(PATHSEGOBJ, LENGTHS) interpolates
        %   along the path segment at the specified path lengths.
        %
        %   The output POSES is a 6-element vector, where the first 
        %   3-elements are positions (x, y, z) and the next 3-elements are 
        %   orientations (heading angle, flight path angle, roll angle) 
        %   along the path as a nx6 vector.
        %
        %   Example:
        %       % Define a start and goal pose
        %       startPose = [4 4 0 0];
        %       goalPose  = [3 5 15 pi/4];
        %
        %       % Create a DubinsConnection object
        %       connectionObj = uavDubinsConnection;
        %
        %       % Find the optimal path
        %       [pathSegObj, pathCost] = connect(connectionObj, ...
        %                               startPose, goalPose)
        %
        %       stepSize = pathSegObj{1}.Length/10;
        %
        %       lengths = 0:stepSize:pathSegObj{1}.Length;
        %
        %       % Interpolate at the lengths along the path segment
        %       [poses] = interpolate(pathSegObj{1}, lengths)
        %
        %       % Visualize the path segment with start, goal and 
        %       % transition poses
        %       show(pathSegObj{1});
        %       % Obtain poses at motion transition
        %       lengths = cumsum(pathSegObj{1}.MotionLengths);
        %       lengths(end) = [];
        %
        %       [poses] = interpolate(pathSegObj{1}, lengths);
        %
        %   See also  dubinsPathSegment, reedsSheppPathSegment

            narginchk(2,2);

            % process when the length is not a NaN
            if(~isnan(this.Length) )
                %  process when the length of samples is not empty
                if(~isempty(samples))
                    totalLength = this.Length;

                    validateattributes(samples, {'single', 'double'}, ...
                                       {'vector', 'finite', 'real', 'nonsparse', 'nonnegative',...
                                        'nondecreasing', '<=', totalLength}, 'interpolate', 'samples');

                    tempStartPose = this.StartPose;
                    tempGoalPose = this.GoalPose;
                    tempMotionType = this.MotionTypes;

                    tempStartPose(1) = this.StartPose(1);
                    tempStartPose(2) = -this.StartPose(2);
                    tempStartPose(3) = -this.StartPose(3);
                    tempStartPose(4) = -this.StartPose(4);

                    tempGoalPose(1) = this.GoalPose(1);
                    tempGoalPose(2) = -this.GoalPose(2);
                    tempGoalPose(3) = -this.GoalPose(3);
                    tempGoalPose(4) = -this.GoalPose(4);

                    % pre-allocate the output assuming user provides the length
                    poses   = zeros(length(samples),6);
                    samples = samples * cos(abs(this.FlightPathAngle));

                    tempMotionLength = this.MotionLengths * cos(abs(this.FlightPathAngle));

                    if(any(strcmp(tempMotionType, 'N')))
                        tempMotionLength(end) = [];
                    end

                    % cumulative transition length
                    transLength = cumsum(tempMotionLength);

                    % sort the samples in ascending order
                    samples = sort(samples);

                    intermediateLength = 0;
                    startIndex = 1;
                    state = zeros(1,6);
                    state(1:4) = tempStartPose;

                    % loop through the motion segments
                    for i = 1:numel(tempMotionLength)
                        %temporary samples
                        if(i == 1)
                            tempSamplesIndex = samples >= intermediateLength & samples <= ((intermediateLength + tempMotionLength(i)) + this.Epsilon);
                        else
                            tempSamplesIndex = samples > intermediateLength & samples <= ((intermediateLength + tempMotionLength(i)) + this.Epsilon);
                        end

                        I = find(tempSamplesIndex == 1);

                        % get the current motion type
                        MT = tempMotionType{i};

                        if(~strcmp(MT,'N'))
                            radius = this.MinTurningRadius;

                            if(strcmp(MT,'Hl') || strcmp(MT,'Hr'))
                                radius = this.HelixRadius;
                            end

                            count = 0; % Count variable to track the number of elements in the variable.
                            tempSamples = zeros(1,1);

                            if(~isempty(I))
                                tempSamples = zeros(numel(samples(I))+1,1);
                                tempSamples(1:(end-1),1) = samples(I);
                                count = numel(samples(I));
                                samples(I) = -1;
                            end
                            tempSamples(count + 1, 1) = transLength(i);
                            S = ((tempSamples - intermediateLength)/radius);

                            theta = state(4);
                            ns = zeros(length(S), 6);
                            z = tempStartPose(3) + tempSamples;

                            TransformMatrix = [cos(theta) sin(theta); -sin(theta) cos(theta)];

                            startAltitude = tempStartPose(3);
                            goalAltitude = tempGoalPose(3);

                            if(this.Length ~= 0)
                                z = tempStartPose(3) + (tempSamples/(this.Length * cos(abs(this.FlightPathAngle)))) * (goalAltitude - startAltitude);
                            end

                            flightAngle = ones(length(S),1) * this.FlightPathAngle;

                            if(strcmp(MT,'Hl') || strcmp(MT,'L'))

                                x = sin(S);
                                y = 1 - cos(S);
                                p = [x y] * TransformMatrix;

                                rollAngle =  ones(length(S),1) * -atan2((this.AirSpeed * this.AirSpeed), (uavDubinsConnection.Gravity * radius));
                                ns        = [state(1) + p(:, 1) * radius, state(2) + p(:, 2) * radius,  z, state(4) + S, flightAngle, rollAngle];

                            elseif(strcmp(MT,'Hr') || strcmp(MT,'R'))

                                x = sin(S);
                                y = cos(S) - 1;
                                p = [x y] * TransformMatrix;

                                rollAngle =  ones(length(S),1) * atan2((this.AirSpeed * this.AirSpeed), (uavDubinsConnection.Gravity * radius));
                                ns = [state(1) + p(:, 1) * radius, state(2) + p(:, 2) * radius, z, state(4) - S, flightAngle, rollAngle];

                            elseif(strcmp(MT,'S'))

                                x = S;
                                y = zeros(length(S),1);
                                p = [x y] * TransformMatrix;
                                ns = [state(1) + p(:, 1) * radius, state(2) + p(:, 2) * radius, z, state(4) + y, flightAngle, y];

                            end

                            if(~isempty(I))

                                poses(startIndex:startIndex + length(I)-1, 1:6) =  ns(1:end-1, 1:6);
                                poses(startIndex:startIndex + length(I)-1, 1) = ns(1:end-1, 1);
                                poses(startIndex:startIndex + length(I)-1, 2) = -ns(1:end-1, 2);
                                poses(startIndex:startIndex + length(I)-1, 3) = -ns(1:end-1, 3);
                                poses(startIndex:startIndex + length(I)-1, 4) = -ns(1:end-1, 4);

                            end

                            state = ns(end,:);
                            startIndex = startIndex + length(I);

                        end

                        intermediateLength = intermediateLength + tempMotionLength(i);

                    end

                    poses(:,4) = robotics.internal.wrapToPi(poses(:,4));
                else
                    poses = zeros(0,6);
                end
            else
                poses = nan(1,6);
            end
        end

        %------------------------------------------------------------------
        function axHandle = show(this, varargin)
        %SHOW Visualize UAV Dubins path segment
        %
        %   AXHANDLE = SHOW(PATHSEGOBJ) plots the path segment with
        %   icons to show the poses at start, goal and at the transition
        %   between motion types. Only the position and the yaw angle are 
        %   used for plotting of the icon.
        %
        %   AXHANDLE = SHOW(PATHSEGOBJ,NAME,VALUE) specifies
        %   additional name-value pair arguments
        %
        %   "Parent"            Handle to an axes on which to display 
        %                       the path.
        %   "Positions"         Display specific positions in the path 
        %                       segment, specified as a cell array of 
        %                       strings, or character or as vector of 
        %                       strings with any combination of start, 
        %                       goal, and transitions. For disabling the 
        %                       display of all the positions provide empty 
        %                       cell array {} or empty vector [].
        %                       Default: {"start","goal","transitions"}
        %
        %   Example:
        %       % Define a start and goal pose
        %       startPose = [4 4 3 0];
        %       goalPose  = [3 5 9 pi/4];
        %
        %       % Create a uavDubinsConnection object
        %       connectionObj = uavDubinsConnection;
        %
        %       % Find the optimal path
        %       [pathSegObj, pathCost] = connect(connectionObj, ...
        %              startPose, goalPose,"PathSegment","all");
        %
        %       % Visualize the path segment with transition poses
        %       show(pathSegObj{1});
        %
        %       % Visualize the path segment with poses at start and
        %       % goal while disabling the transition positions.
        %       axHandle = show(pathSegObj{1}, 'Positions', ...
        %                  {"start", "goal"});
        %
        %       % Visualize path segment on an existing figure by
        %       % passing the axes handle.
        %       show(pathSegObj{5}, 'Parent', axHandle);
        %
        %   See also  dubinsPathSegment, reedsSheppPathSegment,
        %   plotTransform

            parser = inputParser;

            addParameter(parser, 'Parent', [], ...
                         @(x)robotics.internal.validation.validateAxesUIAxesHandle(x));

            % Parse the Poses
            PositionDefaultValue = ["start", "goal", "transitions"];
            addParameter(parser, 'Positions', PositionDefaultValue);

            parser.parse(varargin{:});

            tempaxHandle = parser.Results.Parent;

            if(~isnan(this.Length))
                tempPosition = parser.Results.Positions;


                tempPosition=convertCharsToStrings(tempPosition);

                position = cell(1,1);

                for i=1:numel(tempPosition)
                    position{i} = validatestring(tempPosition{i}, {'start' 'goal' 'transitions'}, 'show', 'Positions');
                end

                stlfilename = this.STLfilename;

                samples = 0:this.StepSize:this.Length;
                if(length(samples) < this.MinNumPoint)
                    tempStepSize = this.Length/this.MinNumPoint;
                    samples = 0:tempStepSize:this.Length;
                end

                [poses] = this.interpolate(samples);

                if isempty(tempaxHandle)
                    tempaxHandle = newplot;
                end

                % Get the hold status for given axes
                holdStatus = ishold(tempaxHandle);

                if(holdStatus)
                    hold(tempaxHandle, 'on');
                end

                xlabel(tempaxHandle,'North');
                hold on;

                ylabel(tempaxHandle,'East');
                zlabel(tempaxHandle,'Down');

                themeUtil = robotics.utils.internal.ThemeColorUtil;
                hpath = plot3(tempaxHandle, poses(:, 1), poses(:, 2), poses(:, 3));
                themeUtil.setThemeProperty(hpath, 'Color', themeUtil.Red);
                
                %Filter legends
                legendHandles = [];
                legendNames   = {};
                counter = 1;

                legendHandles(counter) = hpath;
                legendNames{counter} = "Path";


                counter = counter + 1;

                set(tempaxHandle, 'ZDir','reverse')
                set(tempaxHandle, 'YDir','reverse')

                if(~isempty(position))
                    lengths = cumsum(this.MotionLengths);
                    lengths = [0 lengths];

                    [poses] = this.interpolate(lengths);

                    % plot transition first
                    for i = 1:numel(position)
                        if(strcmp(position{i}, 'transitions'))
                            if(size(poses, 1) > 2)
                                posesTransition = [poses(2:end-1, 1) -poses(2:end-1, 2) -poses(2:end-1, 3)];

                                L = size(poses, 1) - 2;
                                htransition = plot3(tempaxHandle,nan,nan,nan,'.');
                                themeUtil.setThemeProperty(htransition, 'Color', themeUtil.Blue);

                                before = findobj(tempaxHandle, 'type', 'patch');
                                plotTransforms(posesTransition, quaternion(eul2quat([-poses(2:end-1, 4), zeros(L,1), zeros(L, 1)+pi], 'ZYX')), 'MeshFilePath', stlfilename, 'Parent', tempaxHandle,'InertialZDirection','down');
                                after = findobj(tempaxHandle, 'type', 'patch');
                                new = setdiff(after, before);
                                for idx = 1:numel(new)
                                    themeUtil.setThemeProperty(new(idx), 'FaceColor', themeUtil.Blue);
                                end

                                legendHandles(counter) = htransition;
                                legendNames{counter} = "Transition Position";
                                counter = counter + 1;
                            end
                        end
                    end

                    % plot start and goal after plotting transition
                    for i = 1:numel(position)
                        if(strcmp(position{i}, 'start'))
                            posesStart = [poses(1, 1) -poses(1, 2) -poses(1, 3)];

                            hstart = plot3(tempaxHandle,nan,nan,nan,'.');
                            themeUtil.setThemeProperty(hstart, 'Color', themeUtil.Green);

                            before = findobj(tempaxHandle, 'type', 'patch');
                            plotTransforms(posesStart, quaternion(eul2quat([-poses(1, 4), 0, 0+pi], 'ZYX')), 'MeshFilePath', stlfilename, 'Parent', tempaxHandle','InertialZDirection','down');
                            after = findobj(tempaxHandle, 'type', 'patch');
                            new = setdiff(after, before);
                            for idx = 1:numel(new)
                                themeUtil.setThemeProperty(new(idx), 'FaceColor', themeUtil.Green);
                            end
                            legendHandles(counter) = hstart;
                            legendNames{counter} = "Start Position";
                            counter = counter + 1;
                        end

                        if(strcmp(position{i}, 'goal'))
                            posesGoal = [poses(end, 1) -poses(end, 2) -poses(end, 3)];

                            hgoal = plot3(tempaxHandle,nan,nan,nan,'.');
                            themeUtil.setThemeProperty(hgoal, 'Color', themeUtil.Red);

                            before = findobj(tempaxHandle, 'type', 'patch');
                            plotTransforms(posesGoal, quaternion(eul2quat([-poses(end, 4) 0 0+pi], 'ZYX')), 'MeshFilePath', stlfilename, 'Parent', tempaxHandle,'InertialZDirection','down');
                            after = findobj(tempaxHandle, 'type', 'patch');
                            new = setdiff(after, before);
                            for idx = 1:numel(new)
                                themeUtil.setThemeProperty(new(idx), 'FaceColor', themeUtil.Red);
                            end
                            legendHandles(counter) = hgoal;
                            legendNames{counter} = "Goal Position";
                        end
                    end
                end

                % set zoom in and out without axes clipping
                tempaxHandle.Clipping = 'off';

                % set the grid on
                grid on;

                % Legends
                legend(tempaxHandle, legendHandles,...
                       legendNames);

                % Restore the hold status of the original figure
                if ~holdStatus
                    hold(tempaxHandle, 'off');
                end

            end

            % Only return handle if user requested it
            if nargout > 0
                axHandle = tempaxHandle;
            end
        end

        %------------------------------------------------------------------
        function cpObj = copy(obj)
        %copy Create a deep copy of uavDubinsPathSegment object.
            cpObj = uavDubinsPathSegment(...
                obj.StartPose,obj.GoalPose,...
                obj.FlightPathAngle, obj.AirSpeed,....
                obj.MinTurningRadius,obj.HelixRadius,...
                obj.MotionTypes, obj.MotionLengths);
        end
    end

end
