classdef PathManagerBase < matlab.System
    %This class is for internal use only. It may be removed in the future.
    
    %   Copyright 2019-2020 The MathWorks, Inc.
    
    %This is a base class and the derived classes expose the path
    %manager in MATLAB and Simulink
    
    
    %#codegen
    
    properties(Access = {?uav.internal.follower.PathManagerBase, ?matlab.unittest.TestCase})
        
        %MissionPointCounter Mission points counter
        MissionPointCounter = 1
        
        %MissionParams Mission params output
        %   Property to store the mission params output
        MissionParams
        
        %PrevMissionPoint The previous mission point that was executed
        %   The previous mission point in the mission data that is output
        %   in the mission params
        PrevMissionPoint
        
        %MissionStart Mission start flag
        %   Flag used to populate previous mission point at start of
        %   simulation
        MissionStart = true
        
        %PrevModeStatus Mode status flag of previous mode completed
        %   Flag used to check isModeDone input changes from 0 to 1
        PrevModeStatus = false
        
        %PoseHoldFlag Boolean pose hold flag
        %   Flag to store the current pose for the hold signal
        PoseHoldFlag = true
        
        %PoseRTLFlag Boolean flag for RTL command
        %   Flag to store pose for RTL command
        PoseRTLFlag = true
        
        %HoldPose Pose to hold
        %   Property used to store the current pose when hold signal is
        %   given as input
        HoldPose       
        
        %EnableRepeat Enable repeat flag only after reaching last mission
        %point
        EnableRepeat = false
        
        %FinalMissionPoint Final mission point reached flag
        FinalMissionPoint = false  
        
    end
    
    properties(Nontunable)
        
        %UAV type
        %   Specify type of UAV as either 'fixed-wing' or 'multirotor'.
        UAVType = 'multirotor'
        
        %Loiter radius
        %   Specify loiter radius for fixed-wing UAV
        LoiterRadius = 25
        
    end
    
    properties(Constant, Hidden)
        
        %UAVTypeset
        UAVTypeSet = matlab.system.StringSet({'fixed-wing','multirotor'})
        
    end

    methods        
        function set.LoiterRadius(obj, radius)
            %set.LoiterRadius Setter for LoiterRadius property
        
            validateattributes(radius,{'double', 'single'},{'nonnan', 'real', 'scalar', ...
                'positive', 'nonempty', 'finite'},'uavPathManager','loiter radius');
            obj.LoiterRadius = radius;
        end        
    end
    
    methods(Access = {?uav.internal.follower.PathManagerBase, ?matlab.unittest.TestCase})
        
        function setupInternal(obj,pose)
            % Perform one-time calculations, such as computing constants
            
            obj.MissionStart = true;
            
            %initialization for codegen
            obj.MissionParams = struct('mode',{uav.internal.pathmanager.Mode.Armed;uav.internal.pathmanager.Mode.TakeOff},...
                'position',{cast([0 0 0]','like',pose);cast([0 0 0]','like',pose)},...
                'params',{cast([0 0 0 0]','like',pose);cast([0 0 0 0]','like',pose)});
            obj.PrevMissionPoint = struct('mode',uav.internal.pathmanager.Mode.Armed,'position',cast([0 0 0]','like',pose),...
                'params',cast([0 0 0 0]','like',pose));
            obj.HoldPose = cast([0;0;0;0],'like',pose);
        end
        
        function missionParams = stepInternal(obj,pose,missionData,isModeDone,missionCmd,home)
            % Implement algorithm. Calculate y as a function of input u and
            % discrete states.
            
            missionParams = obj.MissionParams; % for code generation
            
            %get number of mission points in mission data
            numMissionPoints = size(missionData,1);
            missionMode = missionData(obj.MissionPointCounter).mode;
            
            if obj.MissionStart
                
                %set previous mission point values at start of simulation
                %mode is set as Mode.Armed
                obj.PrevMissionPoint = struct('mode',uav.internal.pathmanager.Mode.Armed,'position',pose(1:3),'params',cast(-1*ones(1,4)','like',pose));
                
                obj.MissionStart = false;
            end
            
            %increment to next mission data point only when isModeDone
            %changes from 0 to 1 (rising signal) and mode is not HOLD
            if isModeDone ~= obj.PrevModeStatus && isModeDone == true && missionMode ~= uav.internal.pathmanager.Mode.Hold...
               && missionCmd ~= uav.internal.pathmanager.Command.Hold && missionCmd ~= uav.internal.pathmanager.Command.RTL   % added to address geck g2122825. Don't increment if
                                                                                  % in hold or RTL command
                
                obj.PrevMissionPoint = missionData(obj.MissionPointCounter);
                
                %path manager behaviour at final mission point
                if obj.MissionPointCounter == numMissionPoints
                    
                    %reached final mission point
                    obj.FinalMissionPoint = true;
                    %set final mission point behaviour
                    obj.finalMissionPoint(missionData,home,pose);
                    missionParams = obj.MissionParams;
                    
                    %enable repeat only after reaching last mission point
                    obj.EnableRepeat = true;
                    
                    %maintain correct counter value
                    obj.MissionPointCounter = obj.MissionPointCounter - 1;
                end
                
                %increment mission point counter
                obj.MissionPointCounter = obj.MissionPointCounter + 1;
                
            end
            
            % block behaviour to Mission command input
            switch missionCmd
                case uav.internal.pathmanager.Command.Default
                    %reset hold flag
                    obj.PoseHoldFlag = true;
                    %reset RTL flag
                    obj.PoseRTLFlag = true;
                    
                case uav.internal.pathmanager.Command.Hold % when in hold command, the output will be hold mode
                    if obj.PoseHoldFlag
                        %pose when hold signal is sent
                        obj.HoldPose = pose;
                        obj.PoseHoldFlag = false;
                    end
                    %set output for internal hold mode
                    setHoldParams(obj,obj.HoldPose);
                    missionParams = obj.MissionParams;
                    %store previous mode status since this is skipped by
                    %return statement
                    obj.PrevModeStatus = isModeDone;
                    %reset RTL flag
                    obj.PoseRTLFlag = true;
                    return
                case uav.internal.pathmanager.Command.Repeat
                    if obj.EnableRepeat
                        obj.MissionPointCounter = 1;
                        obj.PrevModeStatus = isModeDone;
                        %reset hold flag
                        obj.PoseHoldFlag = true;
                        %reset RTL flag
                        obj.PoseRTLFlag = true;
                        %Reset enable repeat flag
                        obj.EnableRepeat = false;
                        %repeat the mission
                        obj.FinalMissionPoint = false;
                    end
                    
                case uav.internal.pathmanager.Command.RTL     % the output will be RTL mode, the mission params won't iterate if isModeDone is used (g2122825)              
                    if obj.PoseRTLFlag
                        %pose when RTL signal is sent
                        obj.HoldPose = pose;
                        obj.PoseRTLFlag = false;
                        obj.PrevMissionPoint.mode = uav.internal.pathmanager.Mode.RTL;
                        obj.PrevMissionPoint.position = pose(1:3);
                        obj.PrevMissionPoint.params = cast([-1 -1 -1 -1]','like',pose);
                    end
                    %set output for RTL mode for RTL command signal
                    setRTLParams(obj,pose,home)
                    missionParams = obj.MissionParams;
                    %reset hold flag
                    obj.PoseHoldFlag = true;
                    %store previous mode status since this is skipped by
                    %return statement
                    obj.PrevModeStatus = isModeDone;
                    return
            end
            
            % compute output for different modes if not reached final
            % mission point
            if ~obj.FinalMissionPoint
                missionMode = missionData(obj.MissionPointCounter).mode;
                
                switch missionMode
                    
                    case uav.internal.pathmanager.Mode.Custom    % Custom
                        
                        %set output for custom mode
                        setCustomModeParams(obj,missionData);
                        missionParams = obj.MissionParams;
                        
                    case uav.internal.pathmanager.Mode.TakeOff   % TakeOff
                        
                        %set output for take-off mode
                        setTakeOffParams(obj, missionData);
                        missionParams = obj.MissionParams;
                        
                    case uav.internal.pathmanager.Mode.Waypoint  % Waypoint
                        
                        %set output for waypoint mode
                        setWaypointParams(obj, missionData,pose);
                        missionParams = obj.MissionParams;
                        
                    case uav.internal.pathmanager.Mode.Orbit     % Orbit
                        
                        %set output for orbit mode
                        setOrbitParams(obj, missionData);
                        missionParams = obj.MissionParams;
                        
                    case uav.internal.pathmanager.Mode.Land      % Land
                        setLandParams(obj,missionData);
                        missionParams = obj.MissionParams;
                        
                    case uav.internal.pathmanager.Mode.RTL      % RTL, the output will be RTL mode, the mission params will iterate if isModeDone is used (g2122825) 
                        
                        %set output for RTL mode
                        setRTLParams(obj,pose,home);
                        missionParams = obj.MissionParams;
                        
                    otherwise
                        % no warning, output the corresponding unsupported
                        % mode and previous mode
                        setCommonMissionParams(obj,missionData);
                        missionParams = obj.MissionParams;
                end
            end
            
            obj.PrevModeStatus = isModeDone;   %store previous mode status
            
        end
        
        function setCommonMissionParams(obj,missionPoints)
            %setCommonMissionParams Set mission params for the modes that
            %have same output format. The modes are take-ff, orbit, land
            %and custom mode. Output is array of two buses. First bus is
            %desired mission point and second bus is previous mission
            %point
            obj.MissionParams(1) = missionPoints(obj.MissionPointCounter);
            obj.MissionParams(2) = obj.PrevMissionPoint;
        end
        
        function setCustomModeParams(obj, missionPoints)
            %setCustomModeParams Set the mission params output for the
            %custom mode
            
            setCommonMissionParams(obj,missionPoints);
            
        end
        
        function setTakeOffParams(obj, missionPoints)
            %setTakeOffParams Set the mission params output for the
            %take-off mode
            
            setCommonMissionParams(obj,missionPoints);
            
        end
        
        function setWaypointParams(obj, missionPoints,pose)
            %setWaypointParams Set the mission params for the waypoint mode
            
            if isequal(obj.PrevMissionPoint.mode,uav.internal.pathmanager.Mode.Waypoint)
                %if previous mission point is of same mode (waypoint)
                obj.MissionParams(2) = obj.PrevMissionPoint;
                
            else
                %if previous mission point is not waypoint mode, compute
                %the heading angle
                startX = obj.PrevMissionPoint.position(1); startY = obj.PrevMissionPoint.position(2);
                goalX =  missionPoints(obj.MissionPointCounter).position(1); goalY = missionPoints(obj.MissionPointCounter).position(2);
                heading = atan2(goalY - startY,goalX - startX);
                
                obj.MissionParams(2).mode = obj.PrevMissionPoint.mode;
                obj.MissionParams(2).position = obj.PrevMissionPoint.position;
                obj.MissionParams(2).params = cast([heading 25 -1 -1]','like',pose); % transition radius 25 is arbitrary as this won't be used but needs to be specified.
                
            end
            
            %first mission param output is the desired mission point
            obj.MissionParams(1) = missionPoints(obj.MissionPointCounter);
            
        end
        
        function setOrbitParams(obj, missionPoints)
            %setOrbitParams Set mission params for the orbit mode
            
            setCommonMissionParams(obj,missionPoints);
            
        end
        
        function setLandParams(obj,missionPoints)
            %setLandParams Set mission params for the land mode
            
            setCommonMissionParams(obj,missionPoints);
        end
        
        function setRTLParams(obj,pose,home)
            %setRTLParams Set mission params for RTL mode. For RTL mode
            %given as mission data, the mission params first output is with
            %mode RTL and position as home location. The second mission
            %params output is 
            
            obj.MissionParams(1).mode = uav.internal.pathmanager.Mode.RTL;
            obj.MissionParams(1).position = home;
            obj.MissionParams(1).params = cast([-1 -1 -1 -1]','like',pose);
            obj.MissionParams(2) = obj.PrevMissionPoint;
            
        end
        
        function setHoldParams(obj,pose)
            %setHoldParams Set mission params for the HOLD mode. this is an
            %internal mode. The mission params output is set to HOLD mode
            %when the hold command is active
            
            obj.MissionParams(1).mode = uav.internal.pathmanager.Mode.Hold;
            obj.MissionParams(1).position = pose(1:3);
            
            % set orbit with radius = loiter radius,
            % direction of turn = 0 (automatic) for fixed-wing UAV
            if isequal(obj.UAVType,'fixed-wing')
                obj.MissionParams(1).params = cast([obj.LoiterRadius 0 -1 -1]','like',pose);
            else
                obj.MissionParams(1).params = cast([-1 -1 -1 -1]','like',pose);
            end
            obj.MissionParams(2) = obj.PrevMissionPoint;
            
        end
        
        function setFinalMissionPointParams(obj,missionPoints,pose)
            %setFinalMissionPointParams set the mission parameters when
            %final mission point is reached
            
            if isequal(obj.UAVType,'fixed-wing')
                obj.MissionParams(1).mode = uav.internal.pathmanager.Mode.Orbit;
                %set orbit center as the position as specified in the
                %mission point. Orbit around final mission point
                obj.MissionParams(1).position = missionPoints(end).position;
                % set orbit with radius = loiter radius,
                % direction of turn = 0 (automatic)
                obj.MissionParams(1).params = cast([obj.LoiterRadius 0 -1 -1]','like',pose);
                obj.MissionParams(2) = missionPoints(end); %obj.PrevMissionPoint;
            else
                % hold mode for multirotor UAV
                setHoldParams(obj,pose);
            end
            
        end
        
        function finalMissionPoint(obj,missionPoints,home,pose)
            %finalMissionPoint Set behaviour of UAV after reaching the
            %final mission point. The behavior is same for the modes
            %take-off, waypoint, orbit and custom. For these modes, the UAV
            %will loiter with center at the position specified and radius
            %equal to loiter radius for fixed-wing UAV and will hover/hold
            %for multirotor UAV
            
            switch missionPoints(end).mode
                
                %loiter for these modes for fixed-wing UAV and hold for
                %multirotor
                case {uav.internal.pathmanager.Mode.TakeOff, uav.internal.pathmanager.Mode.Waypoint, uav.internal.pathmanager.Mode.Orbit, uav.internal.pathmanager.Mode.Custom}
                    setFinalMissionPointParams(obj,missionPoints,pose);
                    %when final mission point is of mode land/RTL, the mission
                    %params output will remain same as the corresponding modes
                case uav.internal.pathmanager.Mode.Land
                    setLandParams(obj,missionPoints);
                case uav.internal.pathmanager.Mode.RTL
                    setRTLParams(obj,pose,home);
            end
            
        end
        
    end
    
    methods(Access = protected)
        function flag = isInactivePropertyImpl(obj,prop)
            % Return false if property is visible based on object
            % configuration, for the command line and System block dialog
            flag = false;
            if strcmp(prop,'LoiterRadius')
                flag = ~isequal(obj.UAVType,'fixed-wing');
            end
            
        end
        
        function s = saveObjectImpl(obj)
            %saveObjectImpl Custom save implementation
            
            s = saveObjectImpl@matlab.System(obj);
            
            %save base class private properties
            s.MissionPointCounter = obj.MissionPointCounter;
            s.MissionParams = obj.MissionParams;
            s.PrevMissionPoint = obj.PrevMissionPoint;
            s.MissionStart = obj.MissionStart;
            s.PrevModeStatus = obj.PrevModeStatus;
            s.PoseHoldFlag = obj.PoseHoldFlag;
            s.HoldPose = obj.HoldPose;
            s.EnableRepeat = obj.EnableRepeat;
            s.FinalMissionPoint = obj.FinalMissionPoint;
            
        end
        
        function loadObjectImpl(obj,svObj,wasLocked)
            % Set properties in object obj to values in structure s
            
            obj.MissionPointCounter = svObj.MissionPointCounter;
            obj.MissionParams = svObj.MissionParams;
            obj.PrevMissionPoint = svObj.PrevMissionPoint;
            obj.MissionStart = svObj.MissionStart;
            obj.PrevModeStatus = svObj.PrevModeStatus;
            obj.PoseHoldFlag = svObj.PoseHoldFlag;
            obj.HoldPose = svObj.HoldPose;
            obj.EnableRepeat = svObj.EnableRepeat;
            obj.FinalMissionPoint = svObj.FinalMissionPoint;            
            
            % Set public properties and states
            loadObjectImpl@matlab.System(obj,svObj,wasLocked);
        end
        
        
    end
    
end

