function [p,ndxnew] = project2poly(p,bnd,edgexy,ndx,lim)

% Project the points in P(BND) onto the closest edge of the polygon defined
% by the edge segments in EDGEXY. NDX is an optional argument defining
% the edge to project onto: P(BND) is projected onto EDGEXY(NDX(BND)).

% Uses (something like?) a double sweep-line approach to reduce the number
% of edges that are required to be tested in order to determine the closest
% edge for each point. On average only size(EDGEXY)/4 comparisons need to
% be made for each point.

if nargin<5 || isempty(lim)
   lim = inf*ones(size(p,1),1);
end
if nargin<4 || isempty(ndx)
   ndx = zeros(size(p,1),1);
end
ndxnew = zeros(size(p,1),1);
todo = true(size(bnd));

% Check NDX first
for k = 1:length(bnd)
   cn = bnd(k);
   if ndx(cn)>0
      j = ndx(cn);

      x1 = edgexy(j,1); x2mx1 = edgexy(j,3)-x1;
      y1 = edgexy(j,2); y2my1 = edgexy(j,4)-y1;

      r = ((p(cn,1)-x1)*x2mx1+(p(cn,2)-y1)*y2my1)/(x2mx1^2+y2my1^2);
      if (r>0) && (r<1)
         todo(k) = false;
         p(cn,1) = x1+r*x2mx1;
         p(cn,2) = y1+r*y2my1;
         ndxnew(cn) = j;
      end

   end
end

% Do a full search for points not already projected
if any(todo)

   bnd = bnd(todo);

   % Choose the direction with the biggest range as the "y-coordinate" for the
   % test. This should ensure that the sorting is done along the best
   % direction for long and skinny problems wrt either the x or y axes.
   dxy = max(p)-min(p);
   if dxy(1)>dxy(2)
      % Flip co-ords if x range is bigger
      p = p(:,[2,1]);
      edgexy  = edgexy(:,[2,1,4,3]);
      flip = true;
   else
      flip = false;
   end

   % Ensure edgexy(:,[1,2]) contains the lower y value
   swap = edgexy(:,4)<edgexy(:,2);
   edgexy(swap,:) = edgexy(swap,[3,4,1,2]);

   % Sort edges
   [ilower,ilower] = sort(edgexy(:,2));                                    % Sort edges by lower y value
   edgexy_lower = edgexy(ilower,:);
   [iupper,iupper] = sort(edgexy(:,4));                                    % Sort edges by upper y value
   edgexy_upper = edgexy(iupper,:);

   % Mean edge y value
   ne = size(edgexy,1);
   ymean = 0.5*( sum(sum(edgexy(:,[2,4]))) )/ne;

   % Loop through points
   for k = 1:length(bnd)

      cn = bnd(k);
      x = p(cn,1);
      y = p(cn,2);
      d = lim(cn);

      if y<ymean

         % Loop through edges bottom up
         for j = 1:ne
            y2 = edgexy_lower(j,4);
            if y2>=(y-d)
               y1 = edgexy_lower(j,2);
               if y1<=(y+d)

                  x1 = edgexy_lower(j,1);
                  x2 = edgexy_lower(j,3);

                  if x1<x2
                     xmin = x1;
                     xmax = x2;
                  else
                     xmin = x2;
                     xmax = x1;
                  end

                  if xmin<=(x+d) && xmax>=(x-d)
                     % Calculate the distance along the normal projection from [x,y] to the jth edge
                     x2mx1 = x2-x1;
                     y2my1 = y2-y1;

                     r = ((x-x1)*x2mx1+(y-y1)*y2my1)/(x2mx1^2+y2my1^2);
                     if r>1                                                   % Limit to wall endpoints
                        r = 1;
                     elseif r<0
                        r = 0;
                     end
                     xn = x1+r*x2mx1;
                     yn = y1+r*y2my1;

                     dj = (xn-x)^2+(yn-y)^2;
                     if ( dj<d^2 )
                        d = sqrt(dj);
                        p(cn,1) = xn;
                        p(cn,2) = yn;
                        ndxnew(cn) = ilower(j);
                     end

                  end

               else
                  break
               end
            end
         end

      else

         % Loop through edges top down
         for j = ne:-1:1
            y1 = edgexy_upper(j,2);
            if y1<=(y+d)
               y2 = edgexy_upper(j,4);
               if y2>=(y-d)
                  
                  x1 = edgexy_upper(j,1);
                  x2 = edgexy_upper(j,3);

                  if x1<x2
                     xmin = x1;
                     xmax = x2;
                  else
                     xmin = x2;
                     xmax = x1;
                  end

                  if xmin<=(x+d) && xmax>=(x-d)
                     % Calculate the distance along the normal projection from [x,y] to the jth edge
                     x2mx1 = x2-x1;
                     y2my1 = y2-y1;

                     r = ((x-x1)*x2mx1+(y-y1)*y2my1)/(x2mx1^2+y2my1^2);
                     if r>1                                                   % Limit to wall endpoints
                        r = 1;
                     elseif r<0
                        r = 0;
                     end
                     xn = x1+r*x2mx1;
                     yn = y1+r*y2my1;

                     dj = (xn-x)^2+(yn-y)^2;
                     if ( dj<d^2 )
                        d = sqrt(dj);
                        p(cn,1) = xn;
                        p(cn,2) = yn;
                        ndxnew(cn) = iupper(j);
                     end

                  end

               else
                  break
               end
            end
         end

      end

   end

   if flip
      p = p(:,[2,1]);
   end

end
