function cost = exampleHelperCostFunction(funcInput, Map, start, goal, minTurningRadius, safeDistance, weightMinTurningRadius, weightObstacle, weightSmoothness)
%exampleHelperCostFunction Function to evaluate cost of the path based upon
%the curvature and proximity to the obstacles

% Copyright 2021 The MathWorks, Inc.

funcInput(1,:) = start;
funcInput(end,:) = goal;

% Finding position vector between two consecutive poses
dk = zeros(3, size(funcInput,1)-1);
dk(1:2,:) = diff(funcInput(:,1:2))';

% Finding turning angle
deltaBetaK = wrapToPi(diff(funcInput(:,3))');

% For straight motions, turning radius will be inf
rhok = inf(1,size(dk,2));

% Turning radius for circular motions
rhok(deltaBetaK ~= 0) = (vecnorm(dk(1:2,deltaBetaK ~= 0)))./abs(2*sin(deltaBetaK(deltaBetaK ~= 0)/2));
    
% Calculating hK as per paper
constantCurvatureMatrix = [(cos(funcInput(1:end-1,3)) + cos(funcInput(2:end,3)))'; ...
    (sin(funcInput(1:end-1,3)) + sin(funcInput(2:end,3)))'; ...
    zeros(1,size(funcInput,1)-1)];

hK = cross(constantCurvatureMatrix, dk, 1);

% Constraint on minimum turning radius
rCurveK = zeros(1, size(funcInput,1)-1);
rCurveK(deltaBetaK ~= 0) = rhok(deltaBetaK ~= 0) - minTurningRadius;

% Maintaing certain distance from obstacles
minDist = clearance(funcInput(1:end-1,1:2), Map);
oK = minDist - safeDistance;

% Calculating cost
cost = sum(weightMinTurningRadius*vecnorm(hK).^2 + weightMinTurningRadius*min(0,rCurveK).^2 + weightObstacle*min(0,oK).^2 + ...
    weightSmoothness*deltaBetaK.^2); 

end

function [minDist,xyObstacles] = clearance(points, map)

minDist = inf(1,size(points,1));
xyObstacles = nan(size(points,1),2);

[nearestObstacleCellDistance, nearestObstacleCellIDs] = nearestObstacleDistance(map);

% Return distances inf if environment is free.
if isinf(nearestObstacleCellDistance(1,1))
    return;
end

% Convert poses into grid cells.
gridCells = world2grid(map, points(:,1:2));

% Compute distance from obstacles' (world coordinate)
accurate = 1/(2*map.Resolution);
coordiantesAxis = [1, 1; -1, 1; -1, -1; 1, -1];
indexMatrix = repmat(accurate, 4, 2).*coordiantesAxis;

% Compute minimum distances in terms of points (not grid cells)
for k = 1:size(gridCells,1)
    
    % If points(k,:) is on obstacle then move to next point
    if nearestObstacleCellDistance(gridCells(k,1), gridCells(k,2)) == 0
        minDist(k) = 0;
        xyObstacles(k,:) = points(k,:);
        continue;
    end
    
    % Find nearest occupied grid cell index for gridCells(k)
    [I, J] = ind2sub(size(nearestObstacleCellIDs), nearestObstacleCellIDs(gridCells(k,1), gridCells(k,2)));
    
    % Convert the occupied grid into world point.
    obsPoint = grid2world(map,[I,J]);
    
    % Compute corner points of nearest obstacle (obsPoint)
    obsCornerPoints = obsPoint + indexMatrix;
    
    % Find minimum distance from poses(k) to nearest obstacle edges
    for j = 1:size(obsCornerPoints,1)
        s = j+1;
        if mod(j,4) == 0
            s = 1;
        end
        [ptsSegMinDistance, p1] = pointLineSegmentDistance(points(k,1), points(k,2),...
            obsCornerPoints(j,1), obsCornerPoints(j,2), obsCornerPoints(s,1), obsCornerPoints(s,2));
        if (ptsSegMinDistance < minDist(k))
            xyObstacles(k,:) = p1;
            minDist(k) = ptsSegMinDistance;
        end
    end
end
end

function [dist, p1] = pointLineSegmentDistance(px, py, x1, y1, x2, y2)
%pointLineSegmentDistance Compute the minimum distance between a 2D point
%   and a 2D line segement. It also returns nearest point coordinate on
%   line segment. px and py are point coordinate where as x1, y1, x2 and y2
%   are segment's start and end coordinates.

p1 = [x1, y1];

dx = x2-x1;
dy = y2-y1;

% If the segment is just a point
if (dx == 0) && (dy == 0)
    dist = hypot(px-x1, py-y1);
    return;
end

% Calcualte the t that minimizes the distance
t = ((px - x1)*dx + (py-y1)*dy)/(dx*dx + dy*dy);

%See if this represents one of the segments's end points or a point in the
%middle.
if t < 0
    dx = px - x1;
    dy = py - y1;
elseif t > 1
    dx = px - x2;
    dy = py - y2;
    p1 = [x2, y2];
else
    near_x  = x1 + t*dx;
    near_y  = y1 + t*dy;
    dx      = px - near_x;
    dy      = py - near_y;
    p1      = [near_x, near_y];
end

dist = hypot(dx,dy);
end

function [nearestObstacleCellDistance, nearestObstacleCellIDs] = nearestObstacleDistance(map)

%outType = uint32(1);

if isa(map, "binaryOccupancyMap")
    matrix = map.occupancyMatrix;
else
    matrix = occupancyMatrix(map, 'ternary');
    matrix(matrix == -1) = 0;
    matrix = logical(matrix);
end

%Find the minimum distance for each grid cells and closest grid
%cells indices. Size of nearestObstacleDistance and
%nearestObstacleIDs are equal to size(matrix).
[nearestObstacleCellDistance, nearestObstacleCellIDs] = ...
       	imshared.coder.bwdistEDTFT(matrix,single(matrix),uint32(matrix));       
%imageslib.internal.bwdistComputeEDTFT(matrix, outType);
end