function [d_min, varargout] = p_poly_dist(xp, yp, xv, yv, varargin)
%p_poly_dist Find minimum distances from points to a polyline or to a 
% closed polygon.
%
% Description:
% Compute the distances from each one of a set of np points p(1), p(2),...
% p(np) on a 2D plane to a polyline or a closed polygon. Polyline is 
% defined as a set of nv-1 segments connecting nv ordered vertices v(1), 
% v(2), ..., v(nv). The polyline can optionally be treated as a closed
% polygon.
% Distance from point j to a segment k is defined as a distance from this
% point to a straight line passing through vertices v(k) and v(k+1), when
% the projection of point j on this line falls INSIDE segment k; and to
% the closest of v(k) or v(k+1) vertices, when the projection falls OUTSIDE
% segment k. Distance from point j to a polyline is defined as a minimum of
% this point's distances to all segments.
% In a case when the projected point fall OUTSIDE of all polyline segments,
% the distance to a closest vertex of the polyline is returned
%
% Input arguments:
% Required:
% [d_min, varargout] = p_poly_dist(xp, yp, xv, yv)
% xp - vector of points X coordinates (1 X np)
% yp - vector of points Y coordinates (1 X np)
% xv - vector of polygon vertices' X coordinates (1 X nv)
% yv - vector of polygon vertices' Y coordinates (1 X nv)
%
% Optional:
% [d_min, varargout] = p_poly_dist(xp, yp, xv, yv, find_in_out)
%
% find_in_out - logical flag. When true, the polyline is treated as a
% closed polygon, and each input point is checked wether it is inside or 
% outside of this polygon. In such case, an open polyline is automatically 
% closed by adding a last point identical to a first one. 
% Note: when this function is called with ver. 1.0 signature, i.e:
% d = p_poly_dist(xp, yp, xv, yv)
% the flag find_in_out is assumed to be true, to keep the functionality
% compatible with a previous version.
%
% Output arguments:
% Required:
% d_min = p_poly_dist(xp, yp, xv, yv, varargin)
%
% d_min - vector of distances (1 X np) from points to polyline. This is 
% defined as either a distance from a point to it's projection on a line 
% that passes through a pair of consecutive polyline vertices, when this 
% projection falls inside a segment; or as a distance from a point to a 
% closest vertex, when the projection falls outside of a segment. When 
% find_in_out input flag is true, the polyline is assumed to be a closed 
% polygon, and distances of points INSIDE this polygon are defined as 
% negative.
% 
% Optional:
% [d_min, x_d_min, y_d_min, is_vertex, xc, yc, idx_c, Cer, Ppr] = 
%       p_poly_dist(xp, yp, xv, yv, varargin);
%
% x_d_min - vector (1 X np) of X coordinates of the closest points of
% polyline. 
%
% y_d_min - vector (1 X np) of Y coordinates of the closest points of
% polyline. 
%
% is_vertex - logical vector (1 X np). If is_vertex(j)==true, the closest
% polyline point to a point (xp(j), yp(j)) is a vertex. Otherwise, this
% point is a projection on polyline's segment.
%
% idx_c - vector (1 X np) of indices of segments that contain the closest
% point. For instance,  idx_c(2) == 4 means that the polyline point closest
% to point 2 belongs to segment 4
%
% xc - an array (np X nv-1) containing X coordinates of all projected
% points. xc(j,k) is an X coordinate of a projection of point j on a
% segment k
%
% yc - an array (np X nv-1) containing Y coordinates of all projected
% points. yc(j,k) is Y coordinate of a projection of point j on a
% segment k
%
% is_in_seg - logical array (np X nv-1). If is_in_seg(j,k) == true, the
% projected point j with coordinates (xc(j,k), yc(j,k)) lies INSIDE the
% segment k
%
% Cer - a 3D array (2 X 2 X nv-1). Each 2 X 2 slice represents a rotation
% matrix from an input Cartesian coordinate system to a system defined
% by polyline segments.
%
% Ppr - 3D array of size 2 X np X (nv-1). Ppr(1,j,k) is an X coordinate
% of point j in coordinate systems defined by segment k. Ppr(2,j,k) is its
% Y coordinate.
%
% Routines: p_poly_dist.m
% Revision history:
% Oct 2, 2015 - version 2.0 (Michael Yoshpe). The original ver. 1.0 
% function was completely redesigned. The main changes introduced in 
% ver. 2.0:
% 1. Distances to polyline (instead of a closed polygon in ver. 1.0) are 
% returned. The polyline can optionally be treated as a closed polygon.
% 2. Distances from multiple points to the same polyline can be found
% 3. The algorithm for finding the closest points is now based on 
% coordinate system transformation. The new algorithm avoids numerical 
% problems that ver. 1.0 algorithm could experience in "ill-conditioned" 
% cases.
% 4. Many optional output variables were added. In particular, the closest
% points on polyline can be returned.
% 5. Added input validity checks
% 7/9/2006  - case when all projections are outside of polygon ribs
% 23/5/2004 - created by Michael Yoshpe 
% Remarks:


find_in_out = false;

if(nargin >= 5)
   find_in_out = varargin{1,1};  
elseif((nargin==4) && (nargout==1)) % mimic ver. 1.0 functionality
   find_in_out = true;
end

% number of points and number of vertices in polyline
nv = length(xv);
np = length(xp);

if(nv < 2)
   error('Polyline must have at least 2 vertices');
end

if((find_in_out == true) && (nv < 3))
   error('Polygon must have at least 3 vertices');
end

% if finding wether the points are inside or outsite the polygon is
% required, make sure the verices represent a closed polygon
if(find_in_out)
   % If (xv,yv) is not closed, close it.
   nv = length(xv);
   if ((xv(1) ~= xv(nv)) || (yv(1) ~= yv(nv)))
      xv = [xv(:)' xv(1)];
      yv = [yv(:)' yv(1)];
      nv = nv + 1;
   end
end

% Cartesian coordinates of the polyline vertices
Pv = [xv(:) yv(:)];

% Cartesian coordinates of the given points
Pp = [xp(:) yp(:)];

% matrix of distances between all points and all vertices
% dpv(j,k) - distance from point j to vertex k
dpv = zeros(np,nv); % required by Maltab Coder
dpv(:,:) = hypot((repmat(Pv(:,1)', [np 1])-repmat(Pp(:,1), [1 nv])),...
                 (repmat(Pv(:,2)', [np 1])-repmat(Pp(:,2), [1 nv])));

% Find the vector of minimum distances to vertices. 
[dpv_min, I_dpv_min] = min(abs(dpv),[],2);

% coordinates of consecutive vertices
P1 = Pv(1:(end-1),:);
P2 = Pv(2:end,:);
dv = P2 - P1;

% vector of distances between each pair of consecutive vertices
vds = hypot(dv(:,1), dv(:,2));

% check for identical points
idx = find(vds < 10*eps);
if(~isempty(idx))
   % Required by Matlab Coder
   fprintf(1, '%s', 'Points ');
   for j=1:length(idx)
      fprintf(1, '%i ', int64(idx(j)));
   end
   fprintf(1, '%s\n', ' of the polyline are identical');
   assert(false);
   
%   error(['Points ' num2str(idx) ' of the polyline are identical']);
end

% check for a case when closed polygon's vertices lie on a stright line, 
% i.e. the distance between the last and first vertices is equal to the sum
% of all segments except the last
if(find_in_out)
   s = cumsum(vds);
   if((s(end-1) - vds(end)) < 10*eps)
      error('Polygon vertices should not lie on a straight line');
   end
end

% Each pair of consecutive vertices P1(j), P2(j) defines a rotated 
% coordinate system with origin at P1(j), and x axis along the vector 
% (P2(j)-P1(j)). 
% Build the rotation matrix Cer from original to rotated system
ctheta = dv(:,1)./vds;
stheta = dv(:,2)./vds;
Cer = zeros(2,2,nv-1);
Cer(1,1,:) = ctheta;
Cer(1,2,:) = stheta;
Cer(2,1,:) = -stheta;
Cer(2,2,:) = ctheta;

% Build the origin translation vector P1r in rotated frame by rotating the
% P1 vector
P1r = [(ctheta.*P1(:,1) + stheta.*P1(:,2)),...
       -stheta.*P1(:,1) + ctheta.*P1(:,2)];

Cer21 = zeros(2, nv-1);
Cer22 = zeros(2, nv-1);

Cer21(:,:) = Cer(1,:,:);
Cer22(:,:) = Cer(2,:,:);

% Ppr is a 3D array of size 2 * np * (nv-1). Ppr(1,j,k) is an X coordinate
% of point j in coordinate systems defined by segment k. Ppr(2,j,k) is its
% Y coordinate.

% Rotation. Doing it manually, since Matlab cannot multiply 2D slices of ND
% arrays.
Ppr = zeros(2,np,nv-1); % required by Matlab Coder
Ppr(1,:,:) = Pp*Cer21;
Ppr(2,:,:) = Pp*Cer22;

% translation
Ppr(1,:,:) = Ppr(1,:,:) - permute(repmat(P1r(:,1), [1 1 np]), [2 3 1]);
Ppr(2,:,:) = Ppr(2,:,:) - permute(repmat(P1r(:,2), [1 1 np]), [2 3 1]);

% Pcr is a 3D array of size 2 * np * (nv-1) that holds the projections of
% points on X axis of rotated coordinate systems. Pcr(1,j,k) is an X
% coordinate of point j in coordinate systems defined by segment k.
% Pcr(2,j,k) is its Y coordinate, which is identically zero for projected
% point.
Pcr = zeros(size(Ppr));
Pcr(1, :, :) = Ppr(1,:,:);
Pcr(2, :, :) = 0;

% Cre is a rotation matrix from rotated to original system
Cre = permute(Cer, [2 1 3]);

% Pce is a 3D array of size 2 * np * (nv-1) that holds the projections of
% points on a segment in original coordinate systems. Pce(1,j,k) is an X
% coordinate of the projection of point j on segment k.
% Pce(2,j,k) is its Y coordinate
Pce = zeros(2,np,(nv-1));
Pce(1,:,:) = Pcr(1,:,:).*repmat(Cre(1,1,:), [1 np 1]) + ...
             Pcr(2,:,:).*repmat(Cre(1,2,:), [1 np 1]);
Pce(2,:,:) = Pcr(1,:,:).*repmat(Cre(2,1,:), [1 np 1]) + ...
             Pcr(2,:,:).*repmat(Cre(2,2,:), [1 np 1]);
          
% Adding the P1 vector
Pce(1,:,:) = Pce(1,:,:) + permute(repmat(P1(:,1), [1 1 np]), [2 3 1]);
Pce(2,:,:) = Pce(2,:,:) + permute(repmat(P1(:,2), [1 1 np]), [2 3 1]);

% x and y coordinates of the projected (cross-over) points in original
% coordinate frame
xc = zeros(np, (nv-1));
yc = zeros(np, (nv-1));
xc(:,:) = Pce(1,:,:);
yc(:,:) = Pce(2,:,:);

r = zeros(np,(nv-1));
cr = zeros(np,(nv-1));
r(:,:) = Ppr(1,:,:);
cr(:,:) = Ppr(2,:,:);

% Find the projections that fall inside the segments
is_in_seg = (r>0) & (r<repmat(vds(:)', [np  1 1]));

% find the minimum distances from points to their projections that fall
% inside the segments (note, that for some points these might not exist,
% which means that closest points are vertices)
B = NaN(np,nv-1);
B(is_in_seg) = cr(is_in_seg);

[cr_min, I_cr_min] = min(abs(B),[],2);

% Build the logical index which is true for members that are vertices,
% false otherwise. Case of NaN in cr_min means that projected point falls
% outside of a segment, so in such case the closest point is vertex.

% point's projection falls outside of ALL segments
cond1 = isnan(cr_min); 

% point's projection falls inside some segments, find segments for which
% the minimum distance to vertex is smaller than to projection
cond2 = ((I_cr_min ~= I_dpv_min) & (cr_min - dpv_min)> 0);

is_vertex = (cond1 | cond2);

% build the minimum distances vector
d_min = cr_min;
d_min(is_vertex) = dpv_min(is_vertex);

% mimic the functionality of ver. 1.0 - make all distances negative for
% points INSIDE the polygon
if(find_in_out)
   in = inpolygon(xp, yp, xv, yv);
   d_min(in) = -d_min(in);
end

% initialize the vectors of minimum distances to the closest vertices
nz = max(np, nv);

vtmp = zeros(nz, 1);
vtmp(1:nv) = xv(:);
x_d_min = vtmp(I_dpv_min);

vtmp = zeros(nz, 1);
vtmp(1:nv) = yv(:);
y_d_min = vtmp(I_dpv_min);

% replace the minimum distances with those to projected points that fall
% inside the segments
idx_pr = sub2ind(size(xc), find(~is_vertex), I_cr_min(~is_vertex));
x_d_min(~is_vertex) = xc(idx_pr);
y_d_min(~is_vertex) = yc(idx_pr);

% find the indices of segments that contain the closest points
% note that I_dpv_min contains indices of closest POINTS. To find the 
% indices of closest SEGMENTS, we have to substract 1
idx_c = I_dpv_min-1; 
[ii,jj] = ind2sub(size(xc), idx_pr);
idx_c(ii) = jj;

% assign optional outputs
switch nargout
   case 0
   case 1
   case 2
      varargout{1} = x_d_min;
   case 3
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
   case 4
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
   case 5
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
   case 6
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
      varargout{5} = xc;
   case 7
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
      varargout{5} = xc;
      varargout{6} = yc;
   case 8
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
      varargout{5} = xc;
      varargout{6} = yc;
      varargout{7} = is_in_seg;
   case 9
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
      varargout{5} = xc;
      varargout{6} = yc;
      varargout{7} = is_in_seg;      
      varargout{8} = Cer;
   case 10
      varargout{1} = x_d_min;
      varargout{2} = y_d_min;
      varargout{3} = is_vertex;
      varargout{4} = idx_c;
      varargout{5} = xc;
      varargout{6} = yc;
      varargout{7} = is_in_seg;            
      varargout{8} = Cer;
      varargout{9} = Ppr;      
   otherwise
      error('Invalid number of output arguments, must be between 1 and 9');
end

end