classdef WaypointFollowerBase < matlab.System & uav.internal.InternalAccess
    %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 waypoint
    %follower in MATLAB and Simulink
    
    %WaypointFollowerBase Follow a set of waypoints using a UAV.
    %   The waypoint follower is a 3-D 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 = uav.internal.follower.WaypointFollowerBase returns a
    %   waypoint follower system object, WF, that computes the lookahead
    %   distance, desired course and desired yaw for a fixed-wing or
    %   multirotor UAV.
    %
    %   [LOOKAHEADPOINT, DESIREDCOURSE, DESIREDYAW,CROSSTRACKERROR,STATUS] = stepInternal(WF,POSE,WAYPOINTS,LOOKAHEADDISTANCE)
    %   finds the lookahead point, LOOKAHEADPOINT, the desired course,
    %   DESIREDCOURSE, and the desired yaw, DESIREDYAW, the cross track
    %   error, CROSSTRACKERROR, status,STATUS for a 4-by-1 input POSE,
    %   Nx3 or Nx4 or Nx5 input WAYPOINTS 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.
    %
    %   WaypointFollowerBase methods:
    %
    %   stepInternal - Compute lookahead point, desired course, desired
    %                  yaw, cross track error and status
    %   release      - Allow property value changes
    %
    %   WaypointFollowerBase properties:
    %
    %   UAVType                   - Type of UAV
    %   StartFrom                 - Starting behaviour of the UAV
    %   TransitionRadiusSource    - How the transition radius is
    %                               specified
    %   TransitionRadius          - Value of transition radius
    %   YawInput                  - Yaw angle specified at the waypoints
    %
    %
    %   Example
    %
    %       % Create a waypoint follower object
    %       wf = uav.internal.follower.WaypointFollowerBase;
    %
    %       % Assign pose, a sequence of waypoints and lookahead distance
    %       pose = [0;0;5;0];
    %       waypoints = [0 0 5;8 0 5;8 8 5];
    %       lookaheadDistance = 2;
    %
    %       % Compute outputs for pose, waypoints and lookaheadDistance
    %       [lookaheadPoint,desiredCourse,desiredYaw,crossTrackError,status] = wf(pose,waypoints,lookaheadDistance);
    %
    %   See also uavWaypointFollower, robotics.sluav.internal.follower.WaypointFollower.
    %
    %   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-2024 The MathWorks, Inc.
    
    %#codegen
    
    properties(Access = {?uav.internal.InternalAccess})
        
        %LookaheadDistance Lookahead Distance
        %   The lookahead distance changes the behaviour of path 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 = 1;
        
        %WaypointIndex The index of the start waypoint
        WaypointIndex = 1;
        
        %InitYaw Initial yaw angle in radians
        %   The yaw angle specified at the start waypoint. This valued will
        %   be used for calculating the interpolated yaw value for multirotor
        %   UAV.
        InitYaw = 0;
        
        %FinalYaw Final yaw angle in radians
        %   The yaw angle specified at the end waypoint. This valued will
        %   be used for calculating the interpolated yaw value for multirotor
        %   UAV.
        FinalYaw = 0;
        
        %NumWaypoints Number of waypoints in the waypoints input
        %   The number of waypoints specified in the waypoints input.
        NumWaypoints = 0;
        
        %WaypointsInternal Waypoints input
        %   Internal property to store the waypoints. This is needed to
        %   check if the waypoints input change
        WaypointsInternal
        
        %LastWaypointFlag Indicates if the last waypoint is reached
        %   A boolean that indicates if the last waypoint in the sequence
        %   is reached.
        LastWaypointFlag = false;
        
        %StartFlag Boolean start flag
        %   When this flag is true, the current pose of the UAV is stored
        %   in the InitialPose property.
        StartFlag = true;
        
        %InitialPose Initial pose of the UAV x, y, z and the course angle
        %   This is the initial pose of the UAV at start of simulation.
        %   This is needed to handle single waypoint input.
        InitialPose
        
        %LookaheadFactor Lookahead adjustment factor
        %   If projected distance is greater than lookahead distance,
        %   change lookahead distance to LookaheadFactor*projected
        %   distance. this guarantees that a lookahead point always exits
        %   on the line segment.
        LookaheadFactor = 1.01;
        
        %SearchFlag Boolean search flag
        %   Search for the closest path segment when flag is true
        SearchFlag = true;
        
        %LookaheadDistFlag Lookahead distance flag. Property added to
        %address g2153017.
        LookaheadDistFlag = uint8(0);
        
    end
    
    properties(Nontunable)
        
        %UAV type
        %   Specify type of UAV as either 'fixed-wing' or 'multirotor'.
        UAVType = message('uav:robotsluav:waypointfollower:UAVTypeFixedWing').getString
        
        %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 = message('uav:robotsluav:waypointfollower:StartFromFirst').getString
        
        %Transition radius source
        %   Specify how the transition radius is input as either 'internal'
        %   or 'external'
        TransitionRadiusSource = message('uav:robotsluav:waypointfollower:TransitionRadiusSourceInternal').getString
        
        %Transition radius (r)
        TransitionRadius = 10;
        
        %Minimum lookahead distance
        %   Specify the minimum value of the lookahead distance. Property
        %   added to address g2153017.
        MinLookaheadDistance = 0.1;
    
        
        %Show Yaw input variable
        YawInput (1, 1) logical = false
    end
    
    properties(Constant, Hidden)
        
        %StartFromSet
        StartFromSet = matlab.system.StringSet({message('uav:robotsluav:waypointfollower:StartFromFirst').getString,...
            message('uav:robotsluav:waypointfollower:StartFromClosest').getString})
        
        %UAVTypeset
        UAVTypeSet = matlab.system.StringSet({message('uav:robotsluav:waypointfollower:UAVTypeFixedWing').getString,...
            message('uav:robotsluav:waypointfollower:UAVTypeMultirotor').getString})
        
        %TransitionRadiusSet
        TransitionRadiusSourceSet = matlab.system.StringSet({message('uav:robotsluav:waypointfollower:TransitionRadiusSourceInternal').getString,...
            message('uav:robotsluav:waypointfollower:TransitionRadiusSourceExternal').getString})
        
    end
    
    methods
        
        function set.TransitionRadius(obj, radius)
            %set.TransitionRadius Setter for TransitionRadius property
            
            validateattributes(radius,{'double', 'single'},{'nonnan', 'real', 'scalar', ...
                'positive', 'nonempty', 'finite'},'WaypointFollower','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'},'WaypointFollower','Minimum lookahead distance');
            obj.MinLookaheadDistance = distance;
        end
        
    end
    
    methods (Access = protected)
        function setupImpl(obj,currentPose,waypointsIn,~)
            
            % this function created to facilitate setupImpl of MATLAB API
            % for the waypoint follower
            setupInternal(obj,currentPose,waypointsIn);
            
        end
    end
    
    %added access to the matlab API
    methods(Access = {?uav.internal.InternalAccess,?uavWaypointFollower})
        
        function setupInternal(obj,currentPose,waypointsIn)
            %setupInternal internal setup function
            
            if obj.getExecPlatformIndex()
                % In Simulink initialize Waypoints with max-size of the
                % var-size signal. First argument decides the datatype.
                %inType = propagatedInputDataType(obj,1);
                sz = propagatedInputSize(obj,2);
                obj.WaypointsInternal = zeros(sz, 'like', currentPose);
                obj.NumWaypoints = 0;
            else
                % For MATLAB assign Waypoints based on input size
                %this line changed from pure pursuit implementation
                obj.WaypointsInternal = zeros(size(waypointsIn),'like',currentPose);
                obj.NumWaypoints = 0;
            end
        end
        
        function [lookaheadPoint,desiredHeading,desiredYaw,lookaheadDistFlag,crossTrackError,status] = stepInternal(obj,currentPose,waypointsIn,lookaheadDist)
            %stepInternal Compute outputs
            
            %saturate the lookahead distance. Lines added to
            %address g2153017.
            lookaheadDist = obj.limitLookaheadDistance(lookaheadDist,currentPose);
            lookaheadDistFlag = obj.LookaheadDistFlag;
            
            startWaypoint = cast(zeros(3,1),'like',currentPose);
            endWaypoint = cast(ones(3,1),'like',currentPose);
            obj.InitialPose = cast(zeros(4,1),'like',currentPose);
            %check if waypoints have changed. From pure pursuit
            paddedWaypts = zeros(size(obj.WaypointsInternal), 'like', currentPose);
            paddedWaypts(1:size(waypointsIn,1), :) = waypointsIn;
            obj.NumWaypoints = size(waypointsIn,1);
            
            %check if waypoints changed during run time
            waypointsChanged(obj,paddedWaypts);
            
            %remove duplicate waypoints. Added to address geck g2096156
            waypointsIn = getDistinctWpts(obj,waypointsIn,currentPose);
            
            %copy lookaheadDist into LookaheadDistance property
            %cast for codegen
            obj.LookaheadDistance = cast(lookaheadDist,'like',currentPose);
            
            %check for empty waypoints input
            if isempty(waypointsIn)
                
                %get the lookahead point, desired heading, desired yaw,
                %cross track error and status output
                
                [lookaheadPoint,desiredHeading,desiredYaw,crossTrackError,status] = emptyWaypoints(obj,currentPose,lookaheadDist);
                
                return
            end
            
            %compute outputs for single waypoints input case
            if size(waypointsIn,1)==1
                
                if obj.StartFlag
                    %store the current pose
                    obj.InitialPose = currentPose;
                end
                
                %initial pose at the single waypoint location. Treat this
                %as empty waypoints input case
                if norm(waypointsIn(1:3)'-currentPose(1:3))<sqrt(eps)
                    [lookaheadPoint,desiredHeading,desiredYaw,crossTrackError,status] = emptyWaypoints(obj,currentPose,lookaheadDist);
                    obj.StartFlag = false;
                    return;
                end
                
                %create a virtual waypoint to add to the single waypoint
                virtualWaypoint = createWaypoint(obj,waypointsIn);
                appendedWaypoints = [virtualWaypoint; waypointsIn];
                
                %update number of rows of the appended new waypoints input
                obj.NumWaypoints = 2;
                waypoints = appendedWaypoints;
                
            else
                %more than one waypoints input
                waypoints = waypointsIn;
            end
            
            %switch according to start from first or start from closest
            switch(obj.StartFrom)
                
                case 'first'
                    
                    [startWaypoint,endWaypoint,transitionRadius] = getWaypoints(obj,waypoints);
                    
                case 'closest'
                    
                    %search for closest path when search flag is true
                    if obj.SearchFlag
                        %search for closest path segment
                        searchClosestPath(obj,waypoints,currentPose);
                    end
                    
                    %get start and end waypoints of the closest path to
                    %start path following from the closest path
                    [startWaypoint,endWaypoint,transitionRadius] = getWaypoints(obj,waypoints);
                    
            end
            
            %if end waypoint reached, update the start and end waypoints
            %or else start and end waypoints remain unchanged
            [startWaypoint,endWaypoint] = endWaypointReached(obj,waypoints, startWaypoint,endWaypoint,currentPose,transitionRadius);
            
            %distance to the path between start and end waypoint
            dist = pointToLine(obj,startWaypoint,endWaypoint,currentPose(1:3));
            
            %to handle last waypoint
            if obj.LastWaypointFlag
                dist = projectToLine(obj,startWaypoint,endWaypoint,currentPose(1:3));
            end
            
            %adjust lookahead distance to ensure existence of
            %lookahead point
            adjustLookaheadDistance(obj,dist);
            
            lookaheadPoint = getLookahead(obj,startWaypoint,endWaypoint,currentPose(1:3));
            
            %compute desired heading
            desiredHeading = atan2(lookaheadPoint(2)-currentPose(2),lookaheadPoint(1)-currentPose(1));
            desiredYaw = desiredHeading;
            
            %yaw interpolation
            if obj.YawInput
                desiredYaw = yawInterpolation(obj,startWaypoint,endWaypoint,currentPose(1:3),desiredHeading);
            end
            [crossTrackError,status] = getOptionalOutputs(obj,dist,currentPose);
            
        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 distinctWpts = getDistinctWpts(~,waypoints,pose)
            
            if isempty(waypoints)
                distinctWpts = [];
                return
            end
            
            wptsShifted = waypoints(2:end, 1:3);
            distinctWptsIdx = true(1, size(waypoints, 1));
            distinctWptsIdx(1:end-1) = any(waypoints(1:end-1,1:3)~=wptsShifted, 2);
            distinctWpts = cast(waypoints(distinctWptsIdx, :), 'like', pose);

        end
        
        function searchClosestPath(obj,waypoints,currentPose)
            %searchClosestPath Search the closest path segment
            
            %initialize variable to hold the projected distance to
            %all the paths
            d = zeros(1,size(waypoints,1)-1);
            
            %loop over number of paths
            for k = 1:obj.NumWaypoints-1
                
                %get distance to each of the paths
                [tempStartWaypoint,tempEndWaypoint] = getWaypointsIndex(obj,waypoints,k);
                dist = pointToLine(obj,tempStartWaypoint,tempEndWaypoint,currentPose(1:3));
                d(k) = dist;
                
            end
            
            %find the index of closest path
            [~,kk] = min(d);
            
            %set index of the start waypoint
            obj.WaypointIndex = kk;
            
            %reset search flag
            obj.SearchFlag = false;
            
        end
        
        function waypointsChanged(obj,paddedWaypts)
            %waypointsChanged reset waypoint index if the waypoints change
            %during run time
            
            if obj.NumWaypoints~=size(obj.WaypointsInternal, 1) || ~isequal(obj.WaypointsInternal(1:obj.NumWaypoints, :), paddedWaypts(1:obj.NumWaypoints, :))
                % Reset computation if waypoints have changed.
                obj.WaypointsInternal = paddedWaypts;
                obj.WaypointIndex = 1;
                obj.SearchFlag = true;
            end
        end
        
        
        function [startWaypoint,endWaypoint] = endWaypointReached(obj,waypoints,curStartWaypoint,...
                curEndWaypoint,currentPose,transitionRadius)
            
            if norm(currentPose(1:3)-curEndWaypoint) <= transitionRadius || ...
                    hyperTransition(obj,curStartWaypoint,curEndWaypoint,currentPose(1:3))>=0
                %increment waypoint index if end waypoint is
                %reached
                obj.WaypointIndex = obj.WaypointIndex + 1;
                
                %get new start and end waypoint
                [startWaypoint,endWaypoint] = getWaypoints(obj,waypoints);
            else
                
                %start and end waypoint are same as the current
                %start and end waypoint
                startWaypoint = curStartWaypoint;
                endWaypoint = curEndWaypoint;
                
            end
            
        end
        
        function [dist,point] = pointToLine(~,p1,p2,p)
            %pointToLine calculate the closest point and projected distance
            %from point p to the line specified by waypoints p1 and p2
            
            lambda = ((p-p1)'*(p2-p1))/((p2-p1)'*(p2-p1));
            
            if lambda <0
                point = p1;
                dist = norm(p-p1);
            elseif lambda >1
                point = p2;
                dist = norm(p-p2);
            else
                point = p1 + lambda*(p2-p1);
                dist = norm(p-point);
            end
        end
        
        function lookaheadPoint = getLookahead(obj,startWaypoint,endWaypoint,currentPosition)
            %getLookahead calculate the lookahead point given the start waypoint,
            %the end waypoint and the current position
            
            %lookahead point on the line segment from start waypoint to
            %end waypoint. Distance from current position to lookahead point
            %will be the lookahead distance
            L = obj.LookaheadDistance;
            
            %coefficients of the quadratic equation ax^2+b^x+c for find the
            %two lookahead points
            % coefficient a computation modified for performance
            a = (endWaypoint-startWaypoint)'*(endWaypoint-startWaypoint);
            
            b = 2*dot(endWaypoint-startWaypoint,startWaypoint-currentPosition);
            
            % coefficient c computation modified for performance
            c = (startWaypoint-currentPosition)'*(startWaypoint-currentPosition) - L^2;
            
            dis = b^2 - 4*a*c;
            
            %solution of the quadratic equation giving the two lookahead
            %points
            y1 = (-b + sqrt(dis))/2/a;
            y2 = (-b - sqrt(dis))/2/a;
            
            %select correct lookahead point equal to y*v + (1-y)*u
            %select the lookahead point with larger y (beyond the
            %projection point)
            y = max(y1,y2);
            
            r = y*endWaypoint + (1-y)*startWaypoint;
            
            lookaheadPoint = r;
            
        end
        
        function desiredYaw = yawInterpolation(obj,u,v,p,desiredHeading)
            %yawInterpolation Interpolated yaw at the projection point
            switch(obj.UAVType)
                case 'multirotor'
                    
                    [~,projPoint] = pointToLine(obj,u,v,p);
                    lam = norm(projPoint-u)/norm(u-v);
                    
                    % restrict desired yaw to (-pi/pi]
                    desiredYaw = robotics.internal.wrapToPi(obj.InitYaw + lam*angdiff(obj.InitYaw,obj.FinalYaw));
                    
                    
                case 'fixed-wing'
                    
                    desiredYaw = desiredHeading;
                    
            end
            %desired yaw restricted to (-pi,pi]
            if abs(desiredYaw-(-pi))<sqrt(eps)
                desiredYaw = cast(pi,'like',desiredYaw);
            end
            
        end
        
        function [lookaheadPoint,desiredHeading,desiredYaw,crossTrackError,status] = emptyWaypoints(obj,currentPose,lookaheadDist)
            %emptyWaypoints Compute lookahead point when waypoints input is
            %empty. Lookahead point is current position for multirotor and
            %lookahead point is along the current heading direction for
            %fixed wing UAV. CrossTrackError is 0 and Status is 1
            
            
            switch(obj.UAVType)
                case 'fixed-wing'
                    %lookahead point along the current heading direction
                    lookaheadPoint = currentPose(1:3) + lookaheadDist*[cos(currentPose(4));sin(currentPose(4));0];
                    desiredHeading = currentPose(4);
                    desiredYaw = desiredHeading;
                    
                    
                case 'multirotor'
                    %lookahead point is the current position
                    lookaheadPoint = currentPose(1:3);
                    desiredHeading = currentPose(4);
                    desiredYaw = desiredHeading;
                    
            end
            crossTrackError = cast(0,'like',currentPose);
            status = uint8(1);            
        end
        
        function [dist,point] = projectToLine(~,startWaypoint,endWaypoint,currentPosition)
            %projectToLine Calculate projection point and projected distance to
            %line segment between startWaypoint and endWaypoint from
            %current position. This is slightly different from pointToLine
            %method as this function always gives the projected point and
            %distance even when the currentPosition is outside the start and
            %end waypoints
            
            lambda = ((currentPosition-startWaypoint)'*(endWaypoint-startWaypoint))/((endWaypoint-startWaypoint)'*(endWaypoint-startWaypoint));
            point = startWaypoint + lambda*(endWaypoint-startWaypoint);
            dist = norm(currentPosition-point);
            
            
        end
        
        function out = hyperTransition(~,startWaypoint,endWaypoint,currentPosition)
            %hyperTransition Check if the UAV current position has passed
            %the end waypoint v. u and v should not be same. End waypoint
            %is reached only when the hyperplane (perpendicular to the line
            %joining u to v) is crossed, 0 when on the hyperplane
            
            %compute unit vector from startWaypoint to endWaypoint
            unitVectorUtoV = (endWaypoint-startWaypoint)/norm(endWaypoint-startWaypoint);
            
            %compute unit vector from end waypoint to current Position
            unitVectorVtoP = (currentPosition-endWaypoint)/norm(currentPosition-endWaypoint);
            
            %output is 1 if the UAV has crossed the hyperplane
            %0 if on the hyperplane and -1 otherwise
            out = sign(unitVectorUtoV'*unitVectorVtoP);
            
        end
        
        function adjustLookaheadDistance(obj,projectedDist)
            %adjustLookaheadDistance Change lookahead distance to
            %LookaheadFactor times the projected distance. This is done
            %when the projected distance is larger than the lookahead
            %distance. This guarantees that a lookahead point always exists
            
            %line modified to address geck g1848252, tolerance added
            %Due to limit in numerical precision, the relational condition
            %may return wrong values. To remedy this situation, a tolerance
            %level is added.
            if obj.LookaheadDistance <= projectedDist + max(sqrt(eps(projectedDist)),5*eps(projectedDist))
                obj.LookaheadDistance = obj.LookaheadFactor*projectedDist;
            end
        end
        
        function out = getSystemValidator(obj)
            %getSystemValidator Get the correct class corresponding to the
            %properties YawInput and TransitionRadiusSource
            
            if(obj.YawInput && isequal(obj.TransitionRadiusSource,'external'))
                out = uav.internal.follower.YawTrueRadiusExternal;
            end
            if(~obj.YawInput && isequal(obj.TransitionRadiusSource,'external'))
                out = uav.internal.follower.YawFalseRadiusExternal;
            end
            if(obj.YawInput && isequal(obj.TransitionRadiusSource,'internal'))
                out = uav.internal.follower.YawTrueRadiusInternal;
            end
            if(~obj.YawInput && isequal(obj.TransitionRadiusSource,'internal'))
                out = uav.internal.follower.YawFalseRadiusInternal;
            end
            
        end
        
        function [startWaypoint,endWaypoint,transitionRadius] = getWaypoints(obj,waypoints)
            %getWaypoints Get the start waypoint at obj.WaypointIndex,
            %end waypoint at obj.WaypointIndex+1 and transitionRadius of
            %the end waypoint. Minimum two waypoints are needed. This is
            %used for both fixed wing and multirotor UAV
            
            if isequal(obj.WaypointIndex,obj.NumWaypoints)
                
                %last waypoint reached
                obj.LastWaypointFlag = true;
                
                %decrement waypoint index so that UAV continues to fly
                %along the last path segment
                obj.WaypointIndex = obj.WaypointIndex - 1;
                
            end
            s = getSystemValidator(obj);
            [startWaypoint,endWaypoint,transitionRadius] = s.getWaypoints(obj,waypoints);
            
        end
        
        function [startWaypoint,endWaypoint] = getWaypointsIndex(~,waypoints,index)
            %getWaypointsIndex Get the start waypoint and
            %end waypoint v at of line segment specified by index.
            %The number of line segments is number of waypoints minus one
            %Minimum two waypoints are needed
            
            startWaypoint = waypoints(index,1:3)';
            endWaypoint = waypoints(index+1,1:3)';
            
        end
        
        function virtualWaypoint = createWaypoint(obj,waypoints)
            %virtualWaypoint create a virtual waypoint to handle single
            %waypoints input case
            
            s = getSystemValidator(obj);
            virtualWaypoint = s.createWaypoint(obj,waypoints);
            obj.StartFlag = false;
        end
        
        function [crossTrackError,status] = getOptionalOutputs(obj,dist,pose)
            %getOptionalOutputs Compute the optional outputs cross track
            %error and status
            status = uint8(0);
            crossTrackError = cast(dist,'like',pose);
            if isequal(obj.LastWaypointFlag,true)
                status = uint8(1);
            end
            obj.LastWaypointFlag = false;
        end
        
        function validateInputs(obj,waypoints,lookaheadDist)
            %validateInputs Validate the waypoints and
            %lookahead distance inputs. Pose input is validated in the
            %validatePose function
            
            %not validating for attribute positive due to g2153017
            validateattributes(lookaheadDist,{'double', 'single'},{'nonnan', 'real', 'scalar', ...
                'nonempty', 'finite'},'step','lookahead distance');        
            
            if size(waypoints,1) >= 1         
                obj.validateWaypoints(waypoints,'step','waypoints');
            end
            
            
        end
        
        function validatePose(~,curPose,fcnName,argName)
            validateattributes(curPose, {'double', 'single'}, ...
                {'nonempty', 'nonnan', 'finite', 'real', 'vector','nrows', 4}, ...
                fcnName,argName);
        end
        
        
        function validateWaypoints(obj,waypts,fcnName,argName)
            %validateWaypoints Validate waypoints
            
            s = getSystemValidator(obj);
            s.validateWaypoints(waypts,fcnName,argName);
            
        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,'TransitionRadius')
                flag = ~isequal(obj.TransitionRadiusSource,'internal');
            end
            
        end
        
        function flag = isInputSizeMutableImpl(~,index)
            %isInputSizeMutableImpl Return false if input size is not allowed
            % to change while system is running
            
            % Pose and lookahead input are fixed size, waypoints are variable size
            if index == 2
                flag = true;
            else
                flag = false;
            end
        end
        
        function loadObjectImpl(obj,svObj,wasLocked)
            % Set properties in object obj to values in structure s
            
            obj.LookaheadDistance = svObj.LookaheadDistance;
            obj.WaypointIndex = svObj.WaypointIndex;
            obj.InitYaw = svObj.InitYaw;
            obj.FinalYaw = svObj.FinalYaw;
            obj.NumWaypoints = svObj.NumWaypoints;
            obj.WaypointsInternal = svObj.WaypointsInternal;
            obj.LastWaypointFlag = svObj.LastWaypointFlag;
            obj.StartFlag = svObj.StartFlag;
            obj.InitialPose = svObj.InitialPose;
            obj.LookaheadFactor = svObj.LookaheadFactor;
            obj.SearchFlag = svObj.SearchFlag;
            
            %new property added to address g2153017
            if isfield(svObj,'LookaheadDistFlag')
                obj.LookaheadDistFlag = svObj.LookaheadDistFlag;
            else
                obj.LookaheadDistFlag = uint8(0);
            end
            
            % Set public properties and states
            loadObjectImpl@matlab.System(obj,svObj,wasLocked);
        end
        
        function s = saveObjectImpl(obj)
            %saveObjectImpl Custom save implementation
            s = saveObjectImpl@matlab.System(obj);
            
            s.LookaheadDistance = obj.LookaheadDistance;
            s.WaypointIndex = obj.WaypointIndex;
            s.InitYaw = obj.InitYaw;
            s.FinalYaw = obj.FinalYaw;
            s.NumWaypoints = obj.NumWaypoints;
            s.WaypointsInternal = obj.WaypointsInternal;
            s.LastWaypointFlag = obj.LastWaypointFlag;
            s.StartFlag = obj.StartFlag;
            s.InitialPose = obj.InitialPose;
            s.LookaheadFactor = obj.LookaheadFactor;
            s.SearchFlag = obj.SearchFlag;
            
            %new property added to address g2153017
            s.LookaheadDistFlag = obj.LookaheadDistFlag;
        end
        
    end
    
end



