function distanceVector = computeDistanceFromHrep2D(hrep, vrep, pointMatrix)
% compute the signed distance of points from polytope
%
% THIS IS NO USER FUNCTION

% The elk-library: convex geometry applied to crystallization modeling.
%   Copyright (C) 2013 Alexander Reinhold
%
% This program is free software: you can redistribute it and/or modify it 
%   under the terms of the GNU General Public License as published by the 
%   Free Software Foundation, either version 3 of the License, or (at your 
%   option) any later version.
%
% This program is distributed in the hope that it will be useful, but 
%   WITHOUT ANY WARRANTY; without even the implied warranty of 
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
%   General Public License for more details.
%
% You should have received a copy of the GNU General Public License along 
%   with this program.  If not, see <http://www.gnu.org/licenses/>.

%% catch empty hrep (origin is hrep)
if isempty(hrep) || isempty(hrep.A)
    [tmp distanceVector] = convertCartToPolar(pointMatrix(1,:), pointMatrix(2,:));
    return
end

nEdge = size(hrep.A, 1);

distanceMatrix = bsxfun(@minus, hrep.A * pointMatrix, hrep.h);
[distanceVector edgeIndexVector] = max(distanceMatrix, [], 1);
% negative or zero values are already appropriate because they imply that 
%   all points are contained in the polytope. However, if the points are
%   outside the situation is more complex.
% The current distance vector provides only the maximum distance from
%   halfspaces and the corresponding closest point in this halfspace might
%   not be in the polytope.
% We can observe that the closest point of the polytope to a given point 
%   (xi) is
%     1) on the edge with maximum distance, if it is perpendicular above 
%        the edge:
%          xi <is in> gamma*edgeNormal + lam*edgePointOne ...
%                     + (1-lam)*edgePointTwo
%             <with> 0<=gamma, 0<=lam<=1
%     2) on one of the vertices of the edge with maximum distance for which
%        the adjacent edge angle must be considered:
%          xi <is in> edgePoint + gammaOne*edgeNormal ...
%                               + gammaTwo*adjacentEdgeNormal
%             <with> 0<=gammaOne, 0<=gammaTwo
%   It is important that no other edge must be considered. The points (xi)
%   can be no closer to the polytope than the maximum distance that was 
%   already choosen. Therefore, facets with smaller distances are not
%   actively constraining the closest point -- except the two adjacent
%   edges that do define the two adjacent vertices.

%% correlate vertices and edges
% the result of this step is knowing which vertices are situated clockwise
%   to each edge and which anticlockwise to each edge.

[edgeAngleVector ~] = convertCartToPolar(hrep.A(:,1)', hrep.A(:,2)');
[tmp sortedEdgeIndexVector] = sort(edgeAngleVector, 2, 'descend');
% the incidence matrix tells in every column (=vertex) to which edges it
%   belongs to:
incidenceMatrix = abs(bsxfun(@minus, hrep.A * vrep.V', hrep.h)) < elkZerotol;
clockwiseVertexMatrix = nan(2, length(edgeAngleVector));
for iEdge = 1:nEdge
    thisEdgeIndex = sortedEdgeIndexVector(iEdge);
    nextEdgeIndex = sortedEdgeIndexVector(mod(iEdge, nEdge) + 1);
    thisVertexIndex = find(incidenceMatrix(thisEdgeIndex, :));
    if incidenceMatrix(nextEdgeIndex, thisVertexIndex(1))
        clockwiseVertexMatrix(:, thisEdgeIndex) = vrep.V(thisVertexIndex(1), :)';
    elseif incidenceMatrix(nextEdgeIndex, thisVertexIndex(2))
        clockwiseVertexMatrix(:, thisEdgeIndex) = vrep.V(thisVertexIndex(2), :)';
    else
        error('elk:boundary:internalError', ['Subsequent vertex not found.' ...
            'This should not happen.']);
    end
end
counterclockwiseVertexMatrix = circshift(clockwiseVertexMatrix, [0 -1]);

%% clockwise and anticlockwise boundary normals and distances
normalMatrix = hrep.A';
clockwiseNormalMatrix = [0 1;-1 0] * normalMatrix;
counterclockwiseNormalMatrix = -1*clockwiseNormalMatrix;
clockwiseDistanceVector = sum(...
    clockwiseNormalMatrix .* clockwiseVertexMatrix, 1);
counterclockwiseDistanceVector = sum(...
    counterclockwiseNormalMatrix .* counterclockwiseVertexMatrix, 1);

for iSample = find(distanceVector > 0)
    if  clockwiseNormalMatrix(:, edgeIndexVector(iSample))'...
       *pointMatrix(:, iSample) > clockwiseDistanceVector(edgeIndexVector(iSample))'
        % clockwise vertex is closest
        distanceVector(iSample) = norm(pointMatrix(:, iSample) - ...
            clockwiseVertexMatrix(:, edgeIndexVector(iSample)));
    elseif counterclockwiseNormalMatrix(:, edgeIndexVector(iSample))'...
       *pointMatrix(:, iSample) > counterclockwiseDistanceVector(edgeIndexVector(iSample))'
        % anticlockwise vertex is closest
        distanceVector(iSample) = norm(pointMatrix(:, iSample) - ...
            counterclockwiseVertexMatrix(:, edgeIndexVector(iSample)));
    else
        % entered distance is already correct - nothing to do
    end
end