classdef referencePathFrenet < nav.algs.internal.FrenetReferencePath
%referencePathFrenet Smooth reference path fit to waypoints
%
%   The referencePathFrenet object fits a smooth, piecewise continuous curve
%   to the provided [x y] or [x y theta] waypoints. Points along the curve
%   are expressed as [x, y, theta, kappa, dkappa, s], where:
%
%       x,y,theta   - SE2 state relative to global coordinate
%       kappa       - Curvature
%       dkappa      - Derivative of curvature relative to arc length
%       s           - Arclength, or distance along path from path origin
%
%   Use methods on the object to convert trajectories between global and 
%   Frenet coordinate systems, interpolate states along the path based on 
%   arc length, and query the closest point on the path from a global state.
%
%   Frenet states are [S dS ddS L dL ddL], where S is the arc
%   length, L is the deviation perpendicular to the path direction, 
%   derivatives of S are with respect to time and derivatives of L are with
%   respect to arclength, S.
%
%   Global states are [x y theta kappa speed accel], where speed and accel
%   are scalar magnitudes in the direction of theta.
%
%   SYNTAXES:
%
%       refPathObj = referencePathFrenet(WAYPOINTS) fits a piecewise continuous
%       set of curves to provided [x y] or [x y theta] waypoints. Provide
%       theta when reference path must approach waypoints with desired
%       orientation.
%
%       refPathObj = referencePathFrenet(__,Name,Value) also sets properties
%       on the object by specifying the property name and value as name-value
%       pairs. Name must appear inside single quotes (''). You can specify
%       several name-value pair arguments in any order as 
%       Name1,Value1,...,NameN,ValueN:
%
%           'DiscretizationDistance'	- Arclength between interpolated path points
%                                           Default: 0.05 (m)
%
%   referencePathFrenet properties:
%       DiscretizationDistance      - Arclength between interpolated path points
%       Waypoints                   - Presampled points along the path
%
%   referencePathFrenet methods:
%       closestPoint                - Find closest state along path to global point
%       copy                        - Create a deep copy of the object
%       frenet2global               - Convert Frenet states to global states
%       global2frenet               - Convert global states to Frenet states
%       interpolate                 - Interpolate states at provided arc lengths
%       show                        - Show the reference path in a figure
%
%   Example:
%
%       % Create reference waypoints.
%       waypoints = [0 0; 100 50; 400 -50; 800 0];
%
%       % Initialize reference path object using the waypoints and default
%       % discretization interval.
%       refPath = referencePathFrenet(waypoints);
%
%       % Create a low fidelity trajectory in Frenet coordinates
%       dt = 0.5;
%       sInitial = [0 0 0];
%       simpleAccS = [linspace(0,5,10) linspace(5,0,10)]';
%       simpleVelS = cumsum(simpleAccS*dt);
%       simpleS    = cumsum(simpleVelS*dt);
%       latScale = 0.1;
%       simpleAccL = simpleAccS*latScale;
%       simpleVelL = cumsum(simpleAccL*dt);
%       simpleL    = cumsum(simpleVelL*dt);
%       frenetTraj = [simpleS simpleVelS simpleAccS simpleL simpleVelL simpleAccL];
%
%       % Convert trajectory to global coordinates.
%       globalTraj = frenet2global(refPath, frenetTraj);
%
%       % Find the state of the path closest to a desired point.
%       xyDesired = waypoints(3,:) + [20 -50];
%       nearestPathPoint = closestPoint(refPath, xyDesired);
%
%       % Shift the original Frenet trajectory so that it begins at this point.
%       shiftS = nearestPathPoint(1,6);
%       curveTangent = [cos(nearestPathPoint(3)) sin(nearestPathPoint(3))];
%       normalToPoint = xyDesired-nearestPathPoint(1:2);
%       shiftL = norm(normalToPoint)*sign(det([curveTangent' normalToPoint']));
%
%       % Update Frenet trajectory.
%       frenetTrajShifted = frenetTraj + [shiftS 0 0 shiftL 0 0];
%
%       % Convert new trajectory back to global coordinates.
%       globalTrajShifted = frenet2global(refPath, frenetTrajShifted);
%       
%       % Display results
%       show(refPath);
%       axis equal;
%       hold on;
%       plot(globalTraj(:,1),globalTraj(:,2),'-b');
%       plot(xyDesired(1),xyDesired(2),'rX');
%       plot(globalTrajShifted(:,1),globalTrajShifted(:,2),'g-');
%       legend({'Waypoints','refPath','globalTrajShifted','xyDesired','globalTrajShifted'})
%
%       % Get path length
%       lastPathPoint = closestPoint(refPath, refPath.Waypoints(end,:));
%       pathLength = lastPathPoint(6);
%       
%       % Retrieve evenly spaced points along the original path.
%       resampleLengths = linspace(0, pathLength, 5)
%       pathStates = interpolate(refPath, resampleLengths);
%
%       % Create a new path, but force all points to alternate orientation
%       % between pi/2 and -pi/2.
%       waypointsNew = [pathStates(:,1:2), pi/4*((-1).^(0:(numel(resampleLengths)-1)))'];
%       refPathNew = referencePathFrenet(waypointsNew);
%
%       % Display the updated path.
%       show(refPathNew);
%       legend({'Waypoints','refPath','globalTrajShifted','xyDesired','globalTrajShifted','waypointsNew','refPathNew'});
%
%   See also trajectoryOptimalFrenet, trajectoryGeneratorFrenet

%   Copyright 2020 The MathWorks, Inc.

%#codegen
    
    methods
        function obj = referencePathFrenet(varargin)
        %referencePathFrenet Represents waypoints using smooth continuous set of curves
            narginchk(1,3);
            obj = obj@nav.algs.internal.FrenetReferencePath(varargin{:});
        end
        
        function pathPoint = closestPoint(obj, point)
        %closestPoint Find closest point along path to global point
        %
        %   pathPoint = closestPoint(OBJ, POINT) finds the closest point on
        %   the reference path based on the given N-by-2 matrix of [x y] 
        %   points. PATHPOINT describes the path in global coordinates, 
        %   [x y theta kappa dkappa s].
            
            % Validate number of input arguments
            narginchk(2,2);
            validateattributes(point, {'numeric'}, {'nonnan','finite','nonempty','size',[nan 2]},'closestPoint','points');
            pathPoint = obj.closestPoint@nav.algs.internal.FrenetReferencePath(point);
        end
        
        function cObj = copy(obj)
        %copy Create a deep copy of the object
            cObj = referencePathFrenet(obj.Waypoints, ...
                'DiscretizationDistance', obj.DiscretizationDistance);
        end
        
        function pathPoint = interpolate(obj, arcLength)
        %interpolate Interpolate states at provided arc lengths
        %
        %   PATHPOINT = interpolate(OBJ, ARCLENGTH) samples the path at 
        %   the provided arc lengths. ARCLEGNTH is an N-element vector of
        %   distances along the reference path. PATHPOINT is in global
        %   coordinates, [x y theta kappa dkappa s].
        
            % Validate number of input arguments
            narginchk(2,2);
            validateattributes(arcLength,{'numeric'},{'nonempty','nonnan','finite','vector'},'interpolate','arcLength');
            pathPoint = obj.interpolate@nav.algs.internal.FrenetReferencePath(arcLength(:));
        end
        
        function globalState = frenet2global(obj, frenetState)
        %frenet2global Convert Frenet states to global states
        %
        %   GLOBALSTATE = frenet2global(OBJ, FRENETSTATE) converts Frenet
        %   trajectory states to global states.
        %
        %       FRENETSTATE - [S dS ddS L dL ddL]
        %       GLOBALSTATE - [x y theta kappa speed accel]
        %
        %   Derivatives of S are with respect to time and derivatives of L
        %   are with respect to arclength, S.
            
            % Validate number/type of input arguments
            narginchk(2,2);
            validateattributes(frenetState,{'numeric'},{'nonnan','nonempty','finite','size',[nan 6]},'referencePathFrenet','frenet2global');
            
            % Convert frenet states to global coordinate frame
            globalState = obj.frenet2global@nav.algs.internal.FrenetReferencePath(frenetState);
        end
        
        function frenetState = global2frenet(obj, globalState)
        %global2frenet Convert global states to Frenet states
        %
        %   FRENETSTATE = global2frenet(OBJ, GLOBALSTATE) converts global
        %   trajectory states to Frenet states.
        %
        %       GLOBALSTATE - [x y theta kappa speed accel]
        %       FRENETSTATE - [S dS ddS L dL ddL]
        %
        %   Derivatives of S are with respect to time and derivatives of L
        %   are with respect to arclength, S.
            
            % Validate number/type of input arguments
            narginchk(2,2);
            validateattributes(globalState,{'numeric'},{'nonnan','nonempty','finite','size',[nan 6]},'referencePathFrenet','global2frenet');
            
            % Convert frenet states to global coordinate frame
            frenetState = obj.global2frenet@nav.algs.internal.FrenetReferencePath(globalState);
        end
    end
end
