function isValid = checkPathValidity(refPathOrPoses, costmap)
%checkPathValidity Check if a vehicle path is valid.
%   checkPathValidity is used to check if the path returned by a path
%   planner is valid.
%
%   A path is valid if the following conditions are true:
%   - The path has at least one path segment (non-empty).
%   - The path is collision-free and within the limits of the costmap.
%
%   isValid = checkPathValidity(refPath, costmap) checks the validity of
%   the vehicle path refPath against costmap . refPath is a driving.Path
%   object and is returned by the plan method of the pathPlannerRRT.
%   costmap is a vehicleCostmap object describing the environment around
%   the vehicle.
%
%   isValid = checkPathValidity(refPoses, costmap) checks the validity of a
%   sequence of poses refPoses against costmap. refPoses is an  M-by-3
%   matrix of [x, y, theta] poses. x, y must be in the same world units
%   used by the costmap and theta is in degrees. costmap is a
%   vehicleCostmap object describing the environment around the vehicle.
%
%
%   Notes
%   -----
%   - checkPathValidity checks if a vehicle path refPath is valid by
%     discretizing the path and checking that the discretized poses are
%     collision-free. This is subject to the resolution at which
%     discretization is performed. For a different discretization, use
%     refPoses instead.
%
%
%   Example: Plan a path and check if it is valid
%   ----------------------------------------------
%   % Load a costmap for a parking lot
%   data = load('parkingLotCostmap.mat');
%   parkMap = data.parkingLotCostmap;
% 
%   % Define a start and goal pose as [x,y,theta]
%   startPose = [ 4, 4, 90];   % [meters, meters, degrees]
%   goalPose  = [30, 13, 0];
% 
%   % Create an RRT planner
%   planner = pathPlannerRRT(parkMap);
% 
%   % Plan a route from start to goal pose
%   refPath = plan(planner, startPose, goalPose);
% 
%   % Check if the path is valid
%   disp('Is path valid (before adding obstacle)?')
%   isPathValidBefore = checkPathValidity(refPath, parkMap)
% 
%   % Display path on map
%   figure, plot(parkMap)
%   hold on
%   plot(refPath)
%   title('Valid path')
% 
%   % Add an obstacle on the map blocking the path
%   [xObstacle, yObstacle] = meshgrid(0:0.5:12, 6:0.5:8);
%   setCosts(parkMap, [xObstacle(:) yObstacle(:)], 1);
% 
%   % Check if path is valid after adding obstacle
%   disp('Is path valid after adding obstacle?')
%   isPathValidAfter = checkPathValidity(refPath, parkMap)
% 
%   % Display path on new map
%   figure, plot(parkMap)
%   hold on
%   plot(refPath)
%   title('Invalid path')
%
%
%   See also driving.Path, pathPlannerRRT/plan, vehicleCostmap.

% Copyright 2017-2018 The MathWorks, Inc.

%#codegen

isPathObj = validateInputs(refPathOrPoses, costmap);

if isPathObj
    isValid = checkPathObjectValidity(refPathOrPoses, costmap);
else
    isValid = checkPathPosesValidity(refPathOrPoses, costmap);
end
end

%--------------------------------------------------------------------------
function isPathObj = validateInputs(pathOrPoses, costmap)

% Check refPath or refPoses
isPathObj = isa(pathOrPoses, 'driving.Path');
if isPathObj
    validateattributes(pathOrPoses, {'driving.Path'}, {'scalar'}, ...
        'checkPathValidity', 'refPath');
else
    matlabshared.planning.internal.validation.checkPose(...
        pathOrPoses, 3, 'refPoses', 'checkPathValidity', 1);
end

% Check costmap
validateattributes(costmap, {'vehicleCostmap'}, {'scalar'}, ...
    'checkPathValidity', 'costmap');
end

%--------------------------------------------------------------------------
function isValid = checkPathObjectValidity(refPath, costmap)

if isempty(refPath.PathSegments)
    % An empty path is not valid
    isValid = false;
else
    % Compute samples along which to interpolate
    samples = computeSamples(refPath, costmap);
    
    % Interpolate along path
    refPoses = interpolate(refPath, samples);
    
    isValid = checkPathPosesValidity(refPoses, costmap);
end
end

%--------------------------------------------------------------------------
function isValid = checkPathPosesValidity(refPoses, costmap)

% Convert theta to radians
refPoses(:,3) = deg2rad(refPoses(:,3));

% Check that all poses are valid
throwError = false;
isValid = ~isempty(refPoses) && all(checkFreePoses(costmap, refPoses, throwError));
end

%--------------------------------------------------------------------------
function pathSamples = computeSamples(path, costmap)
% Compute resolution to use for interpolation. We have to pick an
% appropriate resolution here. Find the longest segment in the path.
% Use this to pick a resolution like pathPlannerRRT.

segmentLengths = [path.PathSegments.Length];
longestSegment   = max(segmentLengths);
resolutionFactor = 5;

numSteps = resolutionFactor * max(3, ceil(longestSegment/costmap.CellSize));

accumLengths = cumsum([0; segmentLengths(:)]);
numSegments  = numel(path.PathSegments);

pathSamples  = 0;
for n = 2 : numSegments+1
    % Compute step size for this sample
    step = (accumLengths(n)-accumLengths(n-1)) / numSteps;
    
    % Compute samples
    thisSegmentSamples = (accumLengths(n-1)+step) : step : accumLengths(n);
    
    % Concatenate
    pathSamples = [pathSamples, thisSegmentSamples]; %#ok<AGROW>
end

% Make sure requested samples are unique
pathSamples = unique(pathSamples, 'stable');
end

% LocalWords: RRT