classdef uavOrbitFollower < uav.internal.follower.OrbitFollowerBase
%uavOrbitFollower Orbit location of interest using a UAV
%   The orbit follower is a 3-D path follower based on the lookahead
%   distance concept for following a circular path. Given the circle
%   center and radius, the orbit follower computes a lookahead point,
%   desired course, desired yaw, cross-track error and number of turns
%   respectively.
%
%   ORBIT = uavOrbitFollower returns an orbit follower system object,
%   ORBIT, with default property values.
%
%   ORBIT = uavOrbitFollower('PropertyName', PropertyValue, ...)
%   returns a orbit follower object, ORBIT, with each specified property
%   set to the specified value.
%
%   Step method syntax:
%
%   [LOOKAHEADPOINT, DESIREDCOURSE, DESIREDYAW, ORBITRADIUSFLAG, LOOKAHEADFLAG, CROSSTRACKERROR, NUMTURNS] = step(ORBIT,POSE,LOOKAHEADDISTANCE)
%   finds the lookahead point, LOOKAHEADPOINT, the desired course,
%   DESIREDCOURSE, the desired yaw, DESIREDYAW, the orbit radius flag, 
%   ORBITRADIUSFLAG, the lookahead flag, LOOKAHEADFLAG, the cross-track 
%   error, CROSSTRACKERROR, and the number of turns, NUMTURNS for a 4-by-1 
%   input POSE, and a scalar input LOOKAHEADDISTANCE. The POSE is the 
%   current position with course angle [x y z courseAngle] of the UAV. The 
%   lookahead point is the [x y z] location on the circular path. The 
%   lookahead point is used to compute the desired course direction, which
%   is the desired direction of the UAV velocity vector. The desired yaw is
%   the direction along which UAV body axis has to align. The lookahead 
%   flag indicates if the input lookahead distance is saturated to the 
%   minimum lookahead distance. The orbit radius flag indicates if the 
%   input orbit radius is saturated to the minimum orbit radius.
%
%   System objects may be called directly like a function instead of using
%   the step method. For example, y = step(obj, x) and y = obj(x) are
%   equivalent.
%
%   uavOrbitFollower methods:
%
%   step        - Compute lookahead point, desired course, desired
%                 yaw, cross-track error and number of turns
%   release     - Allow property value changes
%
%   uavOrbitFollower properties:
%
%   UAVType                   - Type of UAV (default: 'fixed-wing')
%   OrbitCenter               - Location of the center of orbit
%   OrbitRadius               - Radius of the orbit
%   TurnDirection             - Direction of orbit (positive value is clockwise, negative value is counterclockwise, 0 is auto)
%
%
%   Example:
%
%       % Create an orbit follower object
%       orbFollower = uavOrbitFollower;
%
%       % Assign center, radius, turn direction
%       orbFollower.OrbitCenter = [1;1;5];
%       orbFollower.OrbitRadius = 2.5;
%       orbFollower.TurnDirection = 1;
%
%       % Assign pose and lookahead distance
%       pose = [0;0;5;0];
%       lookaheadDistance = 2;
%
%       % Compute outputs for pose and lookaheadDistance
%       [lookaheadPoint,desiredCourse,desiredYaw,orbitRadiusFlag,lookaheadFlag,crossTrackError,numTurns] = orbFollower(pose,lookaheadDistance);
%
%   See also uavWaypointFollower, uavPathManager, fixedwing, multirotor

%   References:
%
%   [1] Sanghyuk Park, John Deyst, and Jonathan P. How.
%       "A New Nonlinear Guidance Logic for Trajectory Tracking," in
%       AIAA Guidance, Navigation, and Control Conference and Exhibit,
%       Providence, RI, 2004

%   Copyright 2018-2022 The MathWorks, Inc.

%#codegen

    properties

        %OrbitCenter Location of the center of orbit
        OrbitCenter = [0;0;0]

        %OrbitRadius Radius of the orbit
        OrbitRadius = 10

        %TurnDirection Direction of orbit (positive value is clockwise,
        %negative value is counterclockwise, 0 is auto)
        %The direction of orbit specified as positive, 0, or negative value.
        %A positive value indicates a clockwise direction as viewed from
        %above. A value of 0 automatically determines the direction based
        %on the UAV pose.
        TurnDirection = 0

    end

    methods
        function set.OrbitCenter(obj, orbitCenter)
        %set.OrbitCenter Setter for orbit center property

            validateattributes(orbitCenter, {'double', 'single'},...
                               {'real','finite','nrows',3}, ...
                               'uavOrbitFollower', 'Orbit center');
            obj.OrbitCenter = orbitCenter;
        end

        function set.OrbitRadius(obj, orbitRadius)
        %set.OrbitRadius Setter for orbit radius property

            validateattributes(orbitRadius, {'double', 'single'},...
                               {'real','positive','nonnan','finite','nonempty'}, ...
                               'uavOrbitFollower', 'Orbit radius');
            obj.OrbitRadius = orbitRadius;
        end

        function set.TurnDirection(obj, turnDirection)
        %set.TurnDirection Setter for turn radius property

            validateattributes(turnDirection, {'double', 'single'},...
                               {'real','finite','nonempty'}, ...
                               'uavOrbitFollower', 'Turn direction');
            obj.TurnDirection = turnDirection;
        end

        function obj = uavOrbitFollower(varargin)
        %uavOrbitFollower Constructor
            setProperties(obj,nargin,varargin{:},'UAVType','OrbitCenter','OrbitRadius', ...
                              'TurnDirection');
        end
    end

    methods (Access = protected)
        function setupImpl(obj,currentPose)
        % Perform one-time calculations, such as computing constants

        %initialization required for codegen
            obj.PrevPosition = cast([0;0;0],'like',currentPose);
            obj.setupInternal(obj.OrbitCenter,obj.OrbitRadius);

        end

        function [lookaheadPoint,desiredHeading,desiredYaw,orbitRadiusFlag,lookaheadDistFlag,crossTrackError,numTurns] = stepImpl(obj,currentPose,lookaheadDistance)

            %removed the 'positive' attribute check to address g2153017    
            validateattributes(lookaheadDistance,{'double', 'single'},{'real', 'scalar', ...
                                'nonempty', 'nonnan','finite'},'step','lookahead distance');

            validateattributes(currentPose, {'double', 'single'}, ...
                               {'nonempty','finite', 'real', 'vector','nrows', 4}, ...
                               'step','pose');

            %data type of current pose, center, radius and lookahead
            %distance must be same
            isDataTypeEqual = isequal(class(currentPose), class(obj.OrbitCenter), class(obj.OrbitRadius), class(lookaheadDistance));
            coder.internal.errorIf(~isDataTypeEqual, ...
                                   'uav:robotuav:orbitfollowerml:OrbitFollowerDataTypeMismatch');

            %step once using the internal step method of the orbit follower base class
            [lookaheadPoint,desiredHeading,desiredYaw,orbitRadiusFlag,lookaheadDistFlag,crossTrackError,numTurns] = obj.stepInternal(currentPose,...
                                                              obj.OrbitCenter,obj.OrbitRadius,obj.TurnDirection,lookaheadDistance);

        end

        function num = getNumInputsImpl(~)
        %getNumInputsImpl return number of inputs

            num = 2;

        end

        function num = getNumOutputsImpl(~)
        %getNumOutputsImpl return number of outputs

            num = 7;

        end

        function loadObjectImpl(obj,svObj,wasLocked)
        % Set properties in object obj to values in structure s

            obj.OrbitCenterInternal = svObj.OrbitCenterInternal;
            obj.OrbitRadiusInternal = svObj.OrbitRadiusInternal;
            obj.NumCircles = svObj.NumCircles;
            obj.PrevPosition = svObj.PrevPosition;
            obj.NumCirclesFlag = svObj.NumCirclesFlag;
            obj.StartFlag = svObj.StartFlag;
            obj.LookaheadDistance = svObj.LookaheadDistance;
            obj.SelectTurnDirectionFlag = svObj.SelectTurnDirectionFlag;
            obj.TurnDirectionInternal = svObj.TurnDirectionInternal;
            
            %added two new properties (g2153017)
            if isfield(svObj,'OrbitRadiusFlag')
                obj.OrbitRadiusFlag = svObj.OrbitRadiusFlag;
            else
                obj.OrbitRadiusFlag = uint8(0);
            end
            
            if isfield(svObj,'LookaheadFlag')
                obj.LookaheadDistFlag = svObj.LookaheadDistFlag;
            else
                obj.LookaheadDistFlag = uint8(0);
            end

            % Set public properties and states
            loadObjectImpl@uav.internal.follower.OrbitFollowerBase(obj,svObj,wasLocked);
        end

        function s = saveObjectImpl(obj)
        %saveObjectImpl Custom save implementation
            s = saveObjectImpl@uav.internal.follower.OrbitFollowerBase(obj);

        end

        function resetImpl(obj)
        % Initialize / reset discrete-state properties
            obj.OrbitCenterInternal = 0*obj.OrbitCenterInternal;
            obj.OrbitRadiusInternal = 0*obj.OrbitRadiusInternal;
            obj.NumCircles = 0*obj.NumCircles;
            obj.PrevPosition = 0*obj.PrevPosition;
            obj.NumCirclesFlag = false;
            obj.StartFlag = true;
            obj.SelectTurnDirectionFlag = true;
            obj.TurnDirectionInternal = 1;
            
            %added two new properties (g2153017)
            obj.OrbitRadiusFlag = uint8(0);
            obj.LookaheadDistFlag = uint8(0);

        end

    end
end
