classdef uavPathManager < uav.internal.follower.PathManagerBase
    %uavPathManager Execute mission for UAV
    %   The uavPathManager system object computes mission parameters for an
    %   Unmanned Aerial Vehicle (UAV) by sequentially switching between the
    %   mission points specified in the MissionData property. The
    %   MissionCmd property can change the execution order at run-time. The
    %   object supports both fixed-wing and multirotor UAV types.
    %
    %   PM = uavPathManager returns a path
    %   manager object, PM, with default property values.
    %
    %   PM = uavPathManager('PropertyName', PropertyValue, ...)
    %   returns a path manager system object, PM, with each specified 
    %   property set to the specified value.
    %
    %   Step method syntax:
    %
    %   MISSIONPARAMS = step(PM,POSE)
    %   finds the mission parameters, MISSIONPARAMS for a 4-by-1 input
    %   POSE. The POSE is the current position with course angle 
    %   [x y z courseAngle] of the UAV.
    %
    %   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.
    %
    %   uavPathManager methods:
    %
    %   step        - Compute mission parameters
    %   release     - Allow property value changes
    %
    %   uavPathManager properties:
    %
    %   UAVType                   - Type of UAV (default: 'fixed-wing')
    %   MissionData               - UAV mission data
    %   MissionCmd                - Command to change mission at run-time
    %   IsModeDone                - Determine if mission point executed
    %   Home                      - UAV home location
    %
    %   Example
    %
    %       % Create a path manager object
    %       pm = uavPathManager;
    %
    %       % Assign pose, mission data, is mode done, mission command and
    %       % home
    %       pose = [0;0;5;0];
    %       pm.MissionData = [struct('mode',uint8(1),'position',[0;0;100],'params',[-1;-1;-1;-1]);
    %                         struct('mode',uint8(2),'position',[100;0;100],'params',[-1;-1;-1;-1]);
    %                         struct('mode',uint8(3),'position',[100;0;100],'params',[40;1;2;-1])];
    %       pm.IsModeDone = false;
    %       pm.MissionCmd = uint8(0);
    %       pm.Home = [0;0;0];
    %
    %       % Compute output mission params
    %       missionParams = pm(pose);
    %
    %   See also uavWaypointFollower, uavOrbitFollower.
    
    %   Copyright 2019-2020 The MathWorks, Inc.
    
    %#codegen
    
    properties
        
        %MissionData Specify mission data
        MissionData = struct('mode',uint8(0),'position',[0;0;0],'params',[0;0;0;0]);
        
        %MissionCmd Specify uint8 mission command
        MissionCmd = uint8(0)
        
        %IsModeDone Specify boolean property
        IsModeDone = false
        
        %Home Specify UAV home location
        Home = [0;0;0]
        
    end
    
    methods
        function set.MissionData(obj, missionData)
            %set.MissionData Setter for MissionData property  
            
            coder.internal.errorIf(~isstruct(missionData), ...
                'uav:robotuav:pathmanager:InvalidStructure');
            
            coder.internal.errorIf(~((isfield(missionData(1),'params') & isfield(missionData(1),'position') & isfield(missionData(1),'mode'))) , ...
                'uav:robotuav:pathmanager:InvalidStructure');
            
            for k = 1:numel(missionData)
                validateattributes(missionData(k).mode,{'uint8'},{'nonnan', 'real',...
                    'nonempty', 'finite'},'uavPathManager','mode');
                
                validateattributes(missionData(k).position,{'double', 'single'},{'nonnan', 'real',...
                    'nonempty', 'finite','nrows',3},'uavPathManager','position');
                
                validateattributes(missionData(k).params,{'double', 'single'},{'nonnan', 'real',...
                    'nonempty', 'finite','nrows', 4},'uavPathManager','params');
            end
            
            obj.MissionData = missionData;
        end
        
        function set.MissionCmd(obj, missionCmd)
            %set.MissionCmd Setter for MissionCmd property
            
            validateattributes(missionCmd,{'uint8'},{'nonnan', 'real',...
                'nonempty', 'finite','scalar'},'uavPathManager','mission command');
            obj.MissionCmd = missionCmd;
        end
        
        function set.IsModeDone(obj, isModeDone)
            %set.IsModeDone Setter for IsModeDone property
            validateattributes(isModeDone,{'logical'},{'nonnan', 'real',...
                'nonempty', 'finite','scalar'},'uavPathManager','isModeDone');
            
            obj.IsModeDone = isModeDone;
        end
        
        function set.Home(obj, home)
            %set.Home Setter for Home property
            
            validateattributes(home,{'double', 'single'},{'nonnan', 'real',...
                'nonempty', 'finite','vector','nrows', 3},'uavPathManager','home');
            obj.Home = home;
        end
        
        function obj = uavPathManager(varargin)
            %uavPathManager Constructor
            setProperties(obj,nargin,varargin{:},'UAVType','MissionData','MissionCmd', ...
                'LoiterRadius','IsModeDone','Home');
        end
    end
    
    methods (Access = protected)
        function setupImpl(obj,currentPose)
            % Perform one-time calculations, such as computing constants
            
            %initialization required for codegen
            obj.setupInternal(currentPose);
            
        end
        
        function missionParams = stepImpl(obj,currentPose)
            
            validateattributes(currentPose,{'double', 'single'},{'nonnan', 'real',...
                'nonempty', 'finite','vector','nrows', 4},'step','pose');
            
            %data type of current pose, mission data position, mission data
            %params and home must be same
            isDataTypeEqual = isequal(class(currentPose), class(obj.MissionData(1).position), ...
                class(obj.MissionData(1).params), class(obj.Home));
            coder.internal.errorIf(~isDataTypeEqual, ...
                'uav:robotuav:pathmanager:DataTypeMismatch');
            
            %step once using the internal step method of the path manager base class
            missionParams = obj.stepInternal(currentPose,obj.MissionData,obj.IsModeDone,obj.MissionCmd,obj.Home);
            
        end
        
        function num = getNumInputsImpl(~)
            %getNumInputsImpl return number of inputs
            
            num = 1;
            
        end
        
        function num = getNumOutputsImpl(~)
            %getNumOutputsImpl return number of outputs
            
            num = 1;
            
        end
        
        function loadObjectImpl(obj,svObj,wasLocked)
            % Set properties in object obj to values in structure s
            
            % Set public properties and states
            loadObjectImpl@uav.internal.follower.PathManagerBase(obj,svObj,wasLocked);
        end
        
        function s = saveObjectImpl(obj)
            %saveObjectImpl Custom save implementation
            s = saveObjectImpl@uav.internal.follower.PathManagerBase(obj);
            
        end
    end
end
