classdef OrbitFollowerBase < matlab.System
    %This class is for internal use only. It may be removed in the future.
    
    %This is a base class and the derived classes expose the orbit follower
    %in MATLAB and Simulink  
    
    
    %   Copyright 2018-2022 The MathWorks, Inc.
    
    
    %#codegen
    
    properties(Nontunable)
        %UAV type
        %   Specify type of UAV as either 'fixed-wing' or 'multirotor'.
        UAVType = 'multirotor';
        
        %Minimum orbit radius limit
        %   Specify minimum orbit radius limit. Property added to address 
        %   g2153017.
        MinOrbitRadius = 1;
        
        %Minimum lookahead distance
        %   Specify the minimum value of the lookahead distance. Property
        %   added to address g2153017.
        MinLookaheadDistance = 0.1;
    end
    
    properties(Constant, Hidden)
        
        %UAVTypeset
        UAVTypeSet = matlab.system.StringSet({'fixed-wing','multirotor'});
    end
    
    methods
        
        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'},'OrbitFollower','Minimum lookahead distance');
            obj.MinLookaheadDistance = distance;
        end
        
        function set.MinOrbitRadius(obj, radius)
            %set.MinOrbitRadius Setter for MinOrbitRadius property.
            %Lines added to address g2153017.
                        
            validateattributes(radius,{'double', 'single'},{'nonnan', 'real', 'scalar'...
                'positive', 'nonempty', 'finite'},'OrbitFollower','Minimum orbit radius');
            obj.MinOrbitRadius = radius;
        end
        
    end
    
    properties(Access = {?uav.internal.follower.OrbitFollowerBase, ?matlab.unittest.TestCase})
        
        %NumCircles Number of encirclements done by the UAV
        NumCircles = 0;
        
        %PrevPosition Previous position of the UAV
        %   This is the previous position of the UAV. This is needed to
        %   start counting the number of encirclements done by the UAV
        %   around the desired orbit
        PrevPosition
        
        %NumCirclesFlag Flag to enable counting number of encirclements
        NumCirclesFlag = false;
        
        %StartFlag Flag to start counting number of encirclements
        %   When true, current position of the UAV is stored in the
        %   PrevPosition property
        StartFlag = true;
        
        %LookaheadDistance Lookahead distance
        %   The lookahead distance changes the behaviour of orbit following.
        %   A higher lookahead distance produces smoother paths, but the
        %   UAV takes larger turns at the corners. Smaller lookahead
        %   distances closely follow the path, but may produce oscillations
        %   in the path.
        LookaheadDistance
        
        %SelectTurnDirectionFlag Select turn direction flag
        %   This flag is used to select the correct turn direction when
        %   the turn direction option is selected as 0
        SelectTurnDirectionFlag = true;        
        
        %TurnDirectionInternal Internal turn direction property
        %   This property sets the direction of turn when turn direction
        %   option is selected as 0. The default value is set as clockwise
        TurnDirectionInternal = 1;
        
        %OrbitCenterInternal Internal orbit center property
        %   This property is used to check for a change in the position of
        %   the orbit center. This is used for the inter reset method.
        OrbitCenterInternal
        
        %OrbitRadiusInternal Internal orbit radius property
        %   This property is used to check for a change in the radius of
        %   the orbit. This is used for the inter reset method.
        OrbitRadiusInternal
        
        %OrbitRadiusFlag Orbit radius flag. Property added to address 
        %g2153017.
        OrbitRadiusFlag = uint8(0);
        
        %LookaheadDistFlag Lookahead distance flag. Property added to
        %address g2153017.
        LookaheadDistFlag = uint8(0);
        
    end
    
    methods (Access = {?uav.internal.follower.OrbitFollowerBase, ?matlab.unittest.TestCase})
        
        function [lookaheadPoint,desiredHeading,desiredYaw,orbitRadiusFlag,lookaheadDistFlag,crossTrackError,numTurns]...
                = stepInternal(obj,currentPose,center,radius,turnDirection,lookaheadDistance)
            
            %saturate the orbit radius and lookahead distance. Lines added to
            %address g2153017.
            radius = obj.limitOrbitRadius(radius,currentPose);
            lookaheadDistance = obj.limitLookaheadDistance(lookaheadDistance,currentPose);
            orbitRadiusFlag = obj.OrbitRadiusFlag;
            lookaheadDistFlag = obj.LookaheadDistFlag;
            
            %currentPose is on the orbit center
            if norm(currentPose(1:2)-center(1:2))<5*eps(0)
                [lookaheadPoint,desiredHeading,desiredYaw,crossTrackError,numTurns] = poseOnCenter(obj,currentPose,center,radius);
                return
            end
            
            %check if orbit center or radius changed during run time
            internalReset(obj,center,radius);
            
            %adjust lookahead distance if greater than radius of orbit
            adjustLookaheadDistance(obj,lookaheadDistance,radius);
            
            %x,y coordinates of the UAV current position
            xyPose = [currentPose(1);currentPose(2)];
            
            %x,y coordinates of the orbit center
            xyCenter = [center(1);center(2)];
            
            %compute distance of UAV to orbit center in the x-y plane
            distToCenter = sqrt((xyPose - xyCenter)'*(xyPose - xyCenter));
            
            %compute cross-track error. how far the UAV is from the orbit
            crossTrackError = abs(distToCenter - radius);
            
            %number of orbit encirclements is zero when outside the
            %lookahead region
            
            
            %UAV is outside the annular lookahead region.
            %desired lookahead point lies on the orbit along the radial
            %direction from the current UAV position in the x-y plane.
            %Tolerance added to take care of boundary conditions of the
            %lookahead region
            if distToCenter >= radius + obj.LookaheadDistance - 5*eps(radius + obj.LookaheadDistance)...
                    || distToCenter <= radius - obj.LookaheadDistance + 5*eps(radius + obj.LookaheadDistance)
                %unit vector from UAV x-y position to orbit x-y center
                unitVector = xyPose - xyCenter;
                unitVector = unitVector/norm(unitVector);
                
                %lookahead point on the orbit along radial direction
                xyLookaheadPoint = xyCenter + radius*unitVector;
                
                %computer desired yaw. Points towards the lookahead point
                %and is equal to the desired heading when outside the
                %lookahead region.
                desiredYaw = atan2(xyLookaheadPoint(2)-xyPose(2),xyLookaheadPoint(1)-xyPose(1));
                
                %Number of turns remains at previous value
                numTurns = cast(obj.NumCircles,'double');
                
            else
                
                %entered lookahead region
                if obj.StartFlag
                    %store current pose. This is used to compute
                    %incremental change in UAV position while computing the
                    %number of encirclements
                    obj.PrevPosition = currentPose(1:3);
                    %reset start flag
                    obj.StartFlag = false;
                end
                             
                %compute the lookahead points on the desired orbit
                [xout,yout] = circIntersection(obj,xyPose(1),xyPose(2),obj.LookaheadDistance,xyCenter(1),xyCenter(2),radius);
                
                %turn direction is the input turn direction for clockwise
                %and counter clockwise but it is selected automatically for
                %turn direction option 0
                if turnDirection == 0 && ~obj.SelectTurnDirectionFlag 
                    turnInternal = cast(obj.TurnDirectionInternal,'like',currentPose);
                else
                    turnInternal = cast(turnDirection,'like',currentPose);
                end
                
                %compute number of encirclements done
                numTurns = getNumCircles(obj,turnInternal,currentPose,center);
                
                %previous pose is now the current pose
                obj.PrevPosition = currentPose(1:3);
                
                %select the correct lookahead point from the given turn
                %direction and current pose
                xyLookaheadPoint = selectLookaheadPoint(obj,turnInternal,xout,yout,xyCenter,currentPose);
                
                switch(obj.UAVType)
                    case 'fixed-wing'
                        %desired yaw same as desired heading
                        desiredYaw = atan2(xyLookaheadPoint(2)-xyPose(2),xyLookaheadPoint(1)-xyPose(1));
                    case 'multirotor'
                        %desired yaw towards orbit center when inside the
                        %lookahead region
                        desiredYaw = atan2(xyCenter(2)-xyPose(2),xyCenter(1)-xyPose(1));
                end
                
            end
            
            %The correct lookahead point in 3D
            lookaheadPoint = [xyLookaheadPoint;center(3)];
            %desired heading direction
            desiredHeading = atan2(xyLookaheadPoint(2)-xyPose(2),xyLookaheadPoint(1)-xyPose(1));
            
        end
        
        function satLookaheadDistance = limitLookaheadDistance(obj,lookaheadDistance,currentPose)
            %limitLookaheadDistance Saturate the lookahead distance to
            %minimum lookahead distance . This feature added to address 
            %g2153017
            
            satLookaheadDistance = lookaheadDistance;
            obj.LookaheadDistFlag = uint8(0);
            %saturate to minimum lookahead distance specified
            if lookaheadDistance < obj.MinLookaheadDistance
                satLookaheadDistance = cast(obj.MinLookaheadDistance,'like',currentPose);
                obj.LookaheadDistFlag = uint8(1);            
            end            
        end
        
        function satOrbitRadius = limitOrbitRadius(obj,radius,currentPose)
            %limitOrbitRadius Saturate orbit radius to minimum orbit radius
            %if input orbit radius is less than minimum orbit radius
            %property. Lines added to address g2153017
            
            satOrbitRadius = radius;
            obj.OrbitRadiusFlag = uint8(0);
            %saturate to minimum orbit radius specified
            if radius <= obj.MinOrbitRadius
                satOrbitRadius = cast(obj.MinOrbitRadius,'like',currentPose);
                obj.OrbitRadiusFlag = uint8(1);
            end
        end
        
        function internalReset(obj,center,radius)
            %internalReset Reset status output index if the orbit center or
            %radius changes during run time
            
            %Reset number of encirclements if orbit center or radius
            %have changed
            if ~isequal(obj.OrbitCenterInternal, center) || ~isequal(obj.OrbitRadiusInternal, radius)
                
                obj.NumCircles = cast(0,'like',obj.NumCircles); 
                
                %store current center and radius in the internal property
                %variables
                obj.OrbitCenterInternal = center;
                obj.OrbitRadiusInternal = radius;
                obj.SelectTurnDirectionFlag = true;                
            end
            
        end
        
        function [lookaheadPoint,desiredHeading,desiredYaw,crossTrackError,numTurns] = poseOnCenter(obj,currentPose,center,radius)
            %poseOnCenter Exclude the case when current pose is on the
            %orbit center
            
            %lookahead point is along the current heading direction from
            %the current position
            xyLookaheadPoint = currentPose(1:2) + radius*[cos(currentPose(4));sin(currentPose(4))];
            
            lookaheadPoint = [xyLookaheadPoint;center(3)];
            
            %desired heading is in the current heading direction
            desiredHeading = currentPose(4);
            
            %desired yaw is same as desired heading
            desiredYaw = desiredHeading;
            crossTrackError = radius;
            
            %numTurns is previous number of circles as the UAV is outside 
            %the lookahead region
            numTurns = obj.NumCircles;
            
        end
        
        function [xout,yout] = circIntersection(~,curPoseX,curPoseY,lookaheadDist,orbitCenterX,orbitCenterY,orbitRadius)
            
            %circIntersection Compute intersection point between the two
            %circles in X-Y plane. Circles at positions x0,y0 and x1,y1 with
            %radii r1 and r2 respectively. This algorithm is taken from the
            %reference http://paulbourke.net/geometry/circlesphere/
            
            %distance between the circle centers
            d = norm([curPoseX;curPoseY]-[orbitCenterX;orbitCenterY]);
            
            a = (lookaheadDist^2 - orbitRadius^2 + d^2)/(2*d);
            
            x2 = curPoseX + a*(orbitCenterX-curPoseX)/d;
            y2 = curPoseY + a*(orbitCenterY-curPoseY)/d;
            
            h = sqrt(lookaheadDist^2 - a^2);
            
            x31 = x2 - h*(orbitCenterY-curPoseY)/d;
            y31 = y2 + h*(orbitCenterX-curPoseX)/d;
            
            x32 = x2 + h*(orbitCenterY-curPoseY)/d;
            y32 = y2 - h*(orbitCenterX-curPoseX)/d;
            
            %x coordinates of the intersection point
            xout = [x31; x32];
            %y coordinates of the intersection points
            yout = [y31; y32];
        end
        
        function xyLookaheadPoint = selectLookaheadPoint(obj,turnInternal,xout,yout,xyCenter,currentPose)
            %xyLookaheadPoint Select the correct lookahead point from the
            %given turn direction
            
            %radial direction from UAV position to orbit center in x-y
            %plane
            radialDirection = xyCenter - currentPose(1:2);
            
            %cross product between the lookahead vector and the radial
            %direction. Lookahead vector is the vector from UAV
            %position to the first lookahead point.
            turnVector = cross([xout(1) - currentPose(1);yout(1)-currentPose(2);0],[radialDirection;0]);
            
            switch sign(turnInternal)
                
                %clockwise turn direction
                case 1
                    %turn vector direction is downward. Changed turn
                    %direction to address g2153013 for NED frame. Direction
                    %of turn is clockwise when looking from top-down in NED
                    %frame
                    if turnVector(3) > 0
                        xyLookaheadPoint = [xout(1);yout(1)];
                    else
                        %the other lookahead point gives the correct
                        %turn direction
                        xyLookaheadPoint = [xout(2);yout(2)];
                    end
                    
                case -1
                    %turn vector direction is upward
                    if turnVector(3) < 0
                        xyLookaheadPoint = [xout(1);yout(1)];
                    else
                        xyLookaheadPoint = [xout(2);yout(2)];
                    end
                    
                otherwise                    
                    %find the lookahead point closest to the heading
                    %direction.
                    %angle made by first lookahead point
                    lam1 = atan2(yout(1)-currentPose(2),xout(1)-currentPose(1));
                    %angle made by second lookahead point
                    lam2 = atan2(yout(2)-currentPose(2),xout(2)-currentPose(1));
                    
                    %computed lookahead point closest to the current
                    %heading
                    if abs(angdiff(lam1,currentPose(4)))<abs(angdiff(lam2,currentPose(4)))
                        xyLookaheadPoint = [xout(1);yout(1)];
                    else
                        xyLookaheadPoint = [xout(2);yout(2)];
                    end
                    
                    %compute turn direction of the chosen lookahead point
                    %Note that the turn direction is changed due to g2153013
                    newTurnVector = cross([xyLookaheadPoint(1) - currentPose(1);xyLookaheadPoint(2)-currentPose(2);0],[radialDirection;0]);
                    if newTurnVector(3) > 0
                        obj.TurnDirectionInternal = 1;
                    else
                        obj.TurnDirectionInternal = -1;
                    end
                    obj.SelectTurnDirectionFlag = false;                
            end
        end
        
        function setupInternal(obj,center,radius)
            %setupInternal Internal setup method
            
            % setting both values to 0 will guarantee the reset from user
            % input, since orbit radius used in stepping will be always
            % positive.
            obj.OrbitCenterInternal = zeros(3,1, 'like', center);
            obj.OrbitRadiusInternal = zeros(1, 'like', radius);            
            obj.NumCircles = 0;          
        end        
        
        function adjustLookaheadDistance(obj,lookaheadDistance,radius)
            %adjustLookaheadDistance Change lookahead distance when the
            %lookahead distance is larger than the orbit radius.
            
            if lookaheadDistance >= radius
                obj.LookaheadDistance = 0.9*radius;
            else
                obj.LookaheadDistance = lookaheadDistance;
            end
        end
        
        function y = getNumCircles(obj,turnInternal,currentPose,center)
            %getNumCircles Compute the number of encirclements done around
            %the orbit.
            
            %unit vectors from previous pose and current pose to orbit
            %center. Note that these vectors are never zero since the
            %condition of current pose exactly on the orbit center never
            %occurs. The case where current pose and orbit center are
            %exactly same is handled by the function poseOnCenter
            u = [obj.PrevPosition(1:2)-center(1:2);0]; v = [currentPose(1:2)- center(1:2);0];
            
            %Note that the turn direction is changed due to g2153013
            if turnInternal < 0
                u = [currentPose(1:2)- center(1:2);0];
                v = [obj.PrevPosition(1:2)-center(1:2);0];
            end
                    
            u = u/norm(u); v = v/norm(v);
            
            %incremental angular change between previous and current UAV
            %position in x-y plane
            %reference link
            %https://in.mathworks.com/matlabcentral/answers/101590-how-can-i-determine-the-angle-between-two-vectors-in-matlab
            %slightly modified to take sign of increment into account
            deltaAngle = atan2((cross(u,v)),dot(u,v));
            %sum the incremental changes
            obj.NumCircles = obj.NumCircles + cast(deltaAngle(3)/2/pi,'double');
                       
            y = obj.NumCircles;
                  
        end
    end
           
        methods (Access = protected)
            function s = saveObjectImpl(obj)
                %saveObjectImpl Custom save implementation
                
                s = saveObjectImpl@matlab.System(obj);
                
                %save base class private properties
                s.OrbitCenterInternal = obj.OrbitCenterInternal;
                s.OrbitRadiusInternal = obj.OrbitRadiusInternal;
                s.NumCircles = obj.NumCircles;
                s.PrevPosition = obj.PrevPosition;
                s.NumCirclesFlag = obj.NumCirclesFlag;
                s.StartFlag = obj.StartFlag;
                s.LookaheadDistance = obj.LookaheadDistance;
                s.SelectTurnDirectionFlag = obj.SelectTurnDirectionFlag;
                s.TurnDirectionInternal = obj.TurnDirectionInternal;
                
                %added two new properties (g2153017)
                s.OrbitRadiusFlag = obj.OrbitRadiusFlag;
                s.LookaheadDistFlag = obj.LookaheadDistFlag;
                
            end
        end
end
