classdef uavWaypointFollower < matlab.System
%uavWaypointFollower Follow a set of waypoints using a UAV.
%   The uav waypoint follower is a 3D path follower based on the lookahead
%   distance concept for following a path. Given a set of waypoints, the
%   waypoint follower computes lookahead point, desired course, desired
%   yaw and the optional outputs cross-track error and status respectively.
%
%   WF = uavWaypointFollower returns a waypoint follower system
%   object, WF, that computes the lookahead distance, desired course
%   and desired yaw for a fixed-wing or multirotor UAV.
%
%   WF = uavWaypointFollower('PropertyName', PropertyValue, ...)
%   returns a waypoint follower object, WF, with each specified property
%   set to the specified value.
%
%   Step method syntax:
%
%   [LOOKAHEADPOINT, DESIREDCOURSE, DESIREDYAW,LOOKAHEADFLAG,CROSSTRACKERROR,STATUS] = step(WF,POSE,LOOKAHEADDISTANCE)
%   finds the lookahead point, LOOKAHEADPOINT, the desired course,
%   DESIREDCOURSE, the desired yaw, DESIREDYAW, the lookahead flag, 
%   LOOKAHEADFLAG, the cross-track error, CROSSTRACKERROR, and the status, 
%   STATUS 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 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. 
%
%   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.
%
%   uavWaypointFollower methods:
%
%   step        - Compute lookahead point, desired course, desired yaw,
%                 cross-track error and status outputs
%   release     - Allow property value changes
%
%   uavWaypointFollower properties:
%
%   UAVType                   - Type of UAV
%   StartFrom                 - Specify the starting behaviour of the UAV
%   Waypoints                 - Specify waypoints to be followed
%   YawAngles                 - Specify yaw angles for the waypoints
%   TransitionRadius          - Specify transition radius for the
%                               waypoints
%
%   Example
%
%       % Create a waypoint follower object
%       wf = uavWaypointFollower;
%
%       % Assign waypoints, yaw angles and transition radius
%       wf.Waypoints = [0 0 5;8 8 5;0 4 6];
%       wf.YawAngles = [pi/3;pi/4;pi/6];
%       wf.TransitionRadius = [5;10;8];
%
%       % Assign pose and lookahead distance
%       pose = [0;0;5;0];
%       lookaheadDistance = 2;
%
%       % Compute outputs for pose, waypoints and lookaheadDistance
%       [lookaheadPoint,desiredCourse,desiredYaw,lookaheadFlag,crossTrackError,status] = wf(pose,lookaheadDistance);
%
%   See also uavOrbitFollower, uavPathManager, fixedwing, multirotor

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

% Copyright 2018-2020 The MathWorks, Inc.

%#codegen

    properties(Access = {?uavWaypointFollower, ?matlab.unittest.TestCase,...
                         ?uav.internal.InternalAccess})

        %WaypointsInternal Waypoints input
        %   Internal property used to convert Waypoints property into the
        %   correct input format for the stepInternal method of
        %   WaypointFollowerBase class
        WaypointsInternal

    end


    properties(Nontunable)

        %UAV Type
        %   Specify the type of UAV as either 'fixed-wing' or 'multirotor'
        UAVType = 'fixed-wing'

        %Start From
        %   Specify the starting behaviour of the UAV as 'first' or
        %   'closest'. For 'first', the UAV starts path following from the
        %   first path. For 'closest', the UAV starts path following
        %   from the closest path and continues to the end.
        StartFrom = 'first'

    end

    properties(Constant, Hidden)

        %StartFromSet Start behaviour of the UAV
        %   This property determines the starting behavior of the UAV. For
        %   start from first, the UAV starts path following from the first
        %   path and for start from closest, the UAV starts path
        %   following from the closest path.
        StartFromSet = matlab.system.StringSet({'first','closest'})

        %UAVTypeset Type of UAV
        %   Specify the type of UAV as either 'fixed-wing' or 'multirotor'.
        UAVTypeSet = matlab.system.StringSet({'fixed-wing','multirotor'})

    end


    properties(Access = private)
        %WaypointFollowerBase The base class used for the matlab API
        WaypointFollowerBase

        %TransitionRadiusInternal Internal transition radius property
        %created to support code generation
        TransitionRadiusInternal

        %YawAnglesInternal Internal yaw angles property
        %created to support code generation
        YawAnglesInternal
    end

    properties(Nontunable)

        %Waypoints The waypoints representing a path to follow
        %Default: []
        Waypoints = [0 0 0];

        %YawAngles The yaw angles in range (-pi,pi] specified at each waypoint
        %Default: [];
        YawAngles = 0;

        %TransitionRadius The transition radius specified for each waypoint
        %Default: Scalar positive value
        TransitionRadius = 10;
        
        %Minimum lookahead distance
        %   Specify the minimum value of the lookahead distance. Property
        %   added to address g2153017.
        MinLookaheadDistance = 0.1;

    end

    methods
        function set.Waypoints(obj, waypts)
        %set.Waypoints Setter for Waypoints property

            validateattributes(waypts, {'double', 'single'},...
                               {'real','finite','ncols',3}, ...
                               'uavWaypointFollower', 'Waypoints');
            obj.Waypoints = waypts;
            %allocate size for transition radius internal
        end

        function set.YawAngles(obj, yawAngles)
        %set.YawAngles Setter for YawAngles property

            validateattributes(yawAngles, {'double', 'single'},...
                               {'real','finite'}, ...
                               'uavWaypointFollower', 'Waypoints');
            obj.YawAngles = yawAngles;
        end

        function set.TransitionRadius(obj, radius)
        %set.TransitionRadius Setter for TransitionRadius property

            validateattributes(radius, {'double', 'single'},...
                               {'nonempty','real','positive','nonnan','finite'}, ...
                               'uavWaypointFollower', 'TransitionRadius');
            obj.TransitionRadius = radius;
        end
        
        function set.MinLookaheadDistance(obj, distance)
            %set.MinLookaheadDistance Setter for MinLookaheadDistance 
            %property. Lines added to address g2153017.
            
            validateattributes(distance,{'double', 'single'},{'nonnan', 'real', 'scalar',...
                'positive', 'nonempty', 'finite'},'uavWaypointFollower','Minimum lookahead distance');
            obj.MinLookaheadDistance = distance;
        end

        function obj = uavWaypointFollower(varargin)
        %uavWaypointFollower Constructor
            setProperties(obj,nargin,varargin{:},'UAVType','StartFrom','Waypoints', ...
                              'YawAngles', 'TransitionRadius','MinLookaheadDistance');
        end
    end


    methods (Access = protected)
        function setupImpl(obj,currentPose,~)
            % Perform one-time calculations, such as computing constants
            
            validateattributes(currentPose, {'numeric'}, ...
                {'nonempty'}, ...
                'step','pose');
            
            %create waypoint follower base object
            obj.WaypointFollowerBase = uav.internal.follower.WaypointFollowerBase;
            
            %set correct property values
            obj.WaypointFollowerBase.UAVType = obj.UAVType;
            obj.WaypointFollowerBase.StartFrom = obj.StartFrom;
            obj.WaypointFollowerBase.MinLookaheadDistance = obj.MinLookaheadDistance;
            
            %get rid of empty waypoints case first
            if isempty(obj.Waypoints)
                obj.WaypointsInternal = [];
                return;
            end
            
            % for the MATLAB API, the transition radius is always set
            % externally
            obj.WaypointFollowerBase.TransitionRadiusSource = 'external';
            
            % resize transition radius to match waypoints input
            if isscalar(obj.TransitionRadius)
                obj.TransitionRadiusInternal = repmat(obj.TransitionRadius,size(obj.Waypoints,1),1);
            else
                %validate transition radius for vector case
                validateattributes(obj.TransitionRadius,{'double', 'single'},...
                    {'nrows',size(obj.Waypoints,1)},'setup','transition radius');
                obj.TransitionRadiusInternal = obj.TransitionRadius;
            end
            
            % resize yaw angles to match waypoints input
            if isscalar(obj.YawAngles)
                obj.YawAnglesInternal = repmat(obj.YawAngles,size(obj.Waypoints,1),1);
            else
                if ~isempty(obj.YawAngles)
                    %validate the yaw angles for vector case
                    validateattributes(obj.YawAngles,{'double', 'single'},...
                        {'nrows',size(obj.Waypoints,1)},'setup','yaw angles');
                end
                
                obj.YawAnglesInternal = obj.YawAngles;
            end
            
            %set yaw interpolation true if yaw angles are specified
            if ~isempty(obj.YawAngles)
                
                obj.WaypointFollowerBase.YawInput = true;
                obj.WaypointsInternal = cast([obj.Waypoints obj.YawAnglesInternal obj.TransitionRadiusInternal],'like',currentPose);
            else
                obj.WaypointsInternal = cast([obj.Waypoints obj.TransitionRadiusInternal],'like',currentPose);
            end
            
            %call the internal setup function of the waypoint follower base
            %class
            obj.WaypointFollowerBase.setupInternal(currentPose,obj.WaypointsInternal);
            
        end

        function [lookaheadPoint,desiredHeading,desiredYaw,lookaheadDistFlag,crossTrackError,status] = 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');

            % create waypoint follower base object
            y = obj.WaypointFollowerBase;

            %step once using the internal step method of the waypoint follower base class
            [lookaheadPoint,desiredHeading,desiredYaw,lookaheadDistFlag,crossTrackError,status] = y.stepInternal(currentPose,obj.WaypointsInternal,lookaheadDistance);

        end

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

            num = 2;

        end

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

            num = 6;

        end

        function s = saveObjectImpl(obj)
        %saveObjectImpl Custom save implementation
            s = saveObjectImpl@matlab.System(obj);
            s.WaypointFollowerBase = obj.WaypointFollowerBase;
            s.WaypointsInternal = obj.WaypointsInternal;
            s.TransitionRadiusInternal = obj.TransitionRadiusInternal;
            s.YawAnglesInternal = obj.YawAnglesInternal;

        end

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

            obj.WaypointFollowerBase = svObj.WaypointFollowerBase;
            obj.WaypointsInternal = svObj.WaypointsInternal;
            obj.TransitionRadiusInternal = svObj.TransitionRadiusInternal;
            obj.YawAnglesInternal = svObj.YawAnglesInternal;
            % Set public properties and states
            loadObjectImpl@matlab.System(obj,svObj,wasLocked);
        end

        function resetImpl(obj)
        %resetImpl Reset the internal state to defaults

            obj.WaypointFollowerBase.UAVType = obj.UAVType;
            obj.WaypointFollowerBase.StartFrom = obj.StartFrom;
            obj.WaypointFollowerBase.TransitionRadiusSource = 'external';     

        end

    end
end
