function [p,t,h] = quadtree(edgexy,hdata,dhmax,wbar)

%  QUADTREE: 2D quadtree decomposition.
%
% A quadtree decomposition of the linear geometry input is performed. The 
% bounding box is recursively subdivided until the dimension of each box 
% matches the local geometry feature size. The geometry feature size is 
% based on the minimum distance between linear geometry segments.
%
% A size function is obtained at the quadtree vertices based on the minimum
% neighbouring box dimension at each vertex. This size function is gradient
% limited to produce a smooth function.
%
% The quadtree is triangulated for use as a background mesh to store the
% size function.
%
%   edgexy  : [x1,y1,x2,y2] edge endpoints
%   hdata   : User defined size function structure
%   dhmax   : Maximum allowalble relative gradient in the size function
%
%   p       : Background mesh nodes
%   t       : Background mesh triangles
%   h       : Size function value at p

%   Darren Engwirda : 2007
%   Email           : d_engwirda@hotmail.com
%   Last updated    : 20/07/2007 with MATLAB 7.0

% LOCAL FEATURE SIZE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
waitbar(0,wbar,'Estimating local geometric feature size');

% Get size function data
[hmax,edgeh,fun,args] = gethdata(hdata);

% Insert test points along the boundaries at which the LFS can be
% approximated.
wm = 0.5*(edgexy(:,[1,2])+edgexy(:,[3,4]));                                % Use the edge midpoints as a first pass
len = sqrt(sum((edgexy(:,[3,4])-edgexy(:,[1,2])).^2,2));                   % Edge length
L = 2*dist2poly(wm,edgexy,2*len);                                          % Estimate the LFS at these points by calculating
                                                                           % the distance to the closest edge segment                           
% In cases where edges are separated by less than their length
% we will need to add more points to capture the LFS in these
% regions. This allows us to pick up "long and skinny" geometry
% features
r = 2*len./L;                                                              % Compare L (LFS approximation at wm) to the edge lengths
r = round((r-2)/2);                                                        % Amount of points that need to be added
add = find(r);                                                             % at each edge
if ~isempty(add)
   num = 2*sum(r(add));                                                    % Total number of points to be added
   start = size(wm,1)+1;
   wm = [wm; zeros(num,2)];                                                % Alloc space
   L = [L; zeros(num,1)];
   next = start;
   for j = 1:length(add)                                                   % Loop through edges to be subdivided
      
      ce = add(j);                                                         % Current edge
      num = r(ce);
      tmp = (1:num)'/(num+1);                                              % Subdivision increments
      num = next+2*num-1;

      x1 = edgexy(ce,1); x2 = edgexy(ce,3); xm = wm(ce,1);                 % Edge values
      y1 = edgexy(ce,2); y2 = edgexy(ce,4); ym = wm(ce,2);

      wm(next:num,:) = [x1+tmp*(xm-x1), y1+tmp*(ym-y1)                     % Add to list
                        xm+tmp*(x2-xm), ym+tmp*(y2-ym)];
      
      L(next:num) = len(ce);                                               % Upper bound on LFS
                     
      next = num+1;
   
   end
   L(start:end) = dist2poly(wm(start:end,:),edgexy,L(start:end));          % Estimate LFS at the new points
end

% Compute the required size along the edges for any boundary layer size
% functions and add additional points where necessary.
if ~isempty(edgeh)
   h0 = edgeh(:,2);
   i = edgeh(:,1);
   for j = 1:length(i)
      if L(i(j))>h0
        
         cw = i(j);
         r = 2*len(cw)/h0(j);
         r = round((r-2)/2);                                               % Number of points to be added
         tmp = (0:r)'/(r);

         x1 = edgexy(cw,1); x2 = edgexy(cw,3); xm = wm(cw,1);              % Edge values
         y1 = edgexy(cw,2); y2 = edgexy(cw,4); ym = wm(cw,2);

         wm = [wm; x1+tmp*(xm-x1), y1+tmp*(ym-y1);                         % Add to list
                   xm+tmp*(x2-xm), ym+tmp*(y2-ym)];

         L(cw) = h0(j);                                                    % Update LFS
         L = [L; h0(j)*ones(2*(r+1),1)];
      
      end
   end
end

% To speed the point location in the quadtree decomposition
% sort the LFS points based on y-value
[i,i] = sort(wm(:,2));
wm = wm(i,:);
L = L(i);
nw = size(wm,1);

% UNBALANCED QUADTREE DECOMPOSITION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
waitbar(0.2,wbar,'Quadtree decomposition');

xymin = min([edgexy(:,[1,2]); edgexy(:,[3,4])]);                           % Bounding box
xymax = max([edgexy(:,[1,2]); edgexy(:,[3,4])]);

dim = max(xymax-xymin);                                                    % Bbox dimensions
xm = 0.5*(xymin(1)+xymax(1));
ym = 0.5*(xymin(2)+xymax(2));

% Setup boxes with a consistent CCW node order
%  b(k,1) = n1 : bottom left
%  b(k,2) = n2 : bottom right
%  b(k,3) = n3 : top right
%  b(k,4) = n4 : top left

if hmax==inf
   % Start with bbox
   p = [xm-0.5*dim, ym-0.5*dim
        xm+0.5*dim, ym-0.5*dim
        xm+0.5*dim, ym+0.5*dim
        xm-0.5*dim, ym+0.5*dim]; 
   b = [1,2,3,4];
else
   % Discretisation at HMAX
   n = ceil(dim/hmax)+1;
   d = dim/(n-1);
   x = ((xm-0.5*dim):d:(xm+0.5*dim))';
   y = ((ym-0.5*dim):d:(ym+0.5*dim))';
   % Alloc space
   p = zeros(n^2,2);
   b = zeros((n-1)^2,4);
   % Build mesh
   p(1:n,1) = x;
   p(1:n,2) = y(1);
   for k = 1:n-1
      % Indexing
      vec1 = (1:n)' + k*n;
      vec2 = (1:n-1)' + (k-1)*n;
      % Nodes
      p(vec1,1) = x;
      p(vec1,2) = y(k+1);
      % Boxes
      b((1:n-1)'+(k-1)*(n-1),:) = [vec2, vec2+1, vec2+n+1, vec2+n];
   end
end
h = userhfun(p(:,1),p(:,2),fun,args,hmax,xymin,xymax);

pblock = 5*nw;                                                             % Alloc memory in blocks
bblock = pblock;

np = size(p,1);
nb = size(b,1);
test = true(nb,1);
while true                                                           
   
   vec = find(test(1:nb));                                                 % Boxes to be checked at this step
   if isempty(vec)
      break
   end

   N = np;
   for k = 1:length(vec)                                                   % Loop through boxes to be checked for subdivision
      
      m  = vec(k);                                                         % Current box

      n1 = b(m,1);   n2 = b(m,2);                                          % Corner nodes
      n3 = b(m,3);   n4 = b(m,4);
      x1 = p(n1,1);  y1 = p(n1,2);                                         % Corner xy
      x2 = p(n2,1);  y4 = p(n4,2);

      % Binary search to find first wm with y>=ymin for current box
      if wm(1,2)>=y1
         start = 1;
      elseif wm(nw,2)<y1
         start = nw+1;
      else
         lower = 1;
         upper = nw;
         for i = 1:nw
            start = round(0.5*(lower+upper));
            if wm(start,2)<y1
               lower = start;
            elseif wm(start-1,2)<y1
               break;
            else
               upper = start;
            end
         end
      end
      
      % Init LFS as the min of corner user defined size function values
      LFS = 4*h(n1);
      if 4*h(n2)<LFS, LFS = 4*h(n2); end
      if 4*h(n3)<LFS, LFS = 4*h(n3); end
      if 4*h(n4)<LFS, LFS = 4*h(n4); end

      % Loop through all WM in box and take min LFS
      for i = start:nw                                                     % Loop through points (acending y-value order)
         if wm(i,2)<=y4                                                    % Check box bounds and current min
            if wm(i,1)>=x1 && wm(i,1)<=x2 && L(i)<LFS
               LFS = L(i);                                                 % New min found - reset
            end
         else                                                              % Due to the sorting
            break;
         end
      end

      % Split box into 4
      if (x2-x1)>LFS 
         
         if (np+5)>=size(p,1)                                              % Alloc memory on demand
            p = [p; zeros(pblock,2)];
            pblock = 2*pblock;
         end
         if (nb+3)>=size(b,1)
            b = [b; zeros(bblock,4)];
            test = [test; true(bblock,1)];
            bblock = 2*bblock;
         end

         xm = x1+0.5*(x2-x1);                                              % Current midpoints
         ym = y1+0.5*(y4-y1);

         % New nodes
         p(np+1,1) = xm;   p(np+1,2) = ym;
         p(np+2,1) = xm;   p(np+2,2) = y1;
         p(np+3,1) = x2;   p(np+3,2) = ym;
         p(np+4,1) = xm;   p(np+4,2) = y4;
         p(np+5,1) = x1;   p(np+5,2) = ym;   

         % New boxes
         b(m,1)      = n1;               % Box 1
         b(m,2)      = np+2;
         b(m,3)      = np+1;
         b(m,4)      = np+5;
         b(nb+1,1)   = np+2;             % Box 2
         b(nb+1,2)   = n2;
         b(nb+1,3)   = np+3;
         b(nb+1,4)   = np+1;
         b(nb+2,1)   = np+1;             % Box 3
         b(nb+2,2)   = np+3;
         b(nb+2,3)   = n3;
         b(nb+2,4)   = np+4;
         b(nb+3,1)   = np+5;             % Box 4
         b(nb+3,2)   = np+1;
         b(nb+3,3)   = np+4;
         b(nb+3,4)   = n4;

         nb = nb+3;
         np = np+5;
      else
         test(m) = false;
      end
   end

   % User defined size function at new nodes
   h = [h; userhfun(p(N+1:np,1),p(N+1:np,2),fun,args,hmax,xymin,xymax)];

end
p = p(1:np,:);
b = b(1:nb,:);

% Remove duplicate nodes
[p,i,j] = unique(p,'rows');                               
h = h(i);
b = reshape(j(b),size(b));


% FORM SIZE FUNCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
waitbar(0.4,wbar,'Forming element size function')

e = [b(:,[1,2]); b(:,[2,3]); b(:,[3,4]); b(:,[4,1])];                      % Edges
e = unique(sort(e,2),'rows');
L = sqrt(sum((p(e(:,1),:)-p(e(:,2),:)).^2,2));                             % Edge length

ne = size(e,1);
for k = 1:ne                                                               % Initial h - minimum neighbouring edge length
   Lk = L(k);
   if Lk<h(e(k,1)), h(e(k,1)) = Lk; end             
   if Lk<h(e(k,2)), h(e(k,2)) = Lk; end
end

% Gradient limiting
tol = 1e-4;
while true                                                                 % Loop over the edges of the background mesh ensuring
   h_old = h;                                                              % that dh satisfies the dhmax tolerance
   for k = 1:ne                                                            % Loop over edges
      n1 = e(k,1);
      n2 = e(k,2);
      Lk = L(k);
      if h(n1)>h(n2)                                                       % Ensure grad(h)<=dhmax
         dh = (h(n1)-h(n2))/Lk;
         if dh>dhmax
            h(n1) = h(n2) + dhmax*Lk;
         end
      else
         dh = (h(n2)-h(n1))/Lk;
         if dh>dhmax
            h(n2) = h(n1) + dhmax*Lk;
         end
      end
   end
   if norm((h-h_old)./h,'inf')<tol                                         % Test convergence
      break
   end
end

% TRIANGULATE QUADTREE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
waitbar(0.6,wbar,'Triangulating quadtree');

if size(b,1)==1
   % Split box diagonally into 2 tri's
   t = [b(1),b(2),b(3); b(1),b(3),b(4)];
else

   % Get node-to-node connectivity
   % First column is column count per row
   % Max neighbours is 8 due to quadtree setup
   n2n = zeros(size(p,1),9);
   for k = 1:size(e,1)
      % Nodes in kth edge
      n1 = e(k,1);
      n2 = e(k,2);
      % Indexing
      n2n(n1,1) = n2n(n1,1)+1;                                             % Node 1
      n2n(n1,n2n(n1,1)+1) = n2;
      n2n(n2,1) = n2n(n2,1)+1;                                             % Node 2
      n2n(n2,n2n(n2,1)+1) = n1;
   end

   % Check for regular boxes with no mid-side nodes
   num = n2n(:,1)<=4;
   reg = all(num(b),2);

   % Split regular boxes diagonally into 2 tri's
   t = [b(reg,[1,2,3]); b(reg,[1,3,4])];
  
   if ~all(reg)
      
      % Use the N2N connectivity to directly triangulate the quadtree
      % nodes. Some additional nodes may be added at the centroids of some
      % boxes to facilitate triangulation. The triangluation is not
      % necessarily Delaunay, but should always be high quality and
      % symmetric where possible.

      b = b(~reg,:);                                                       % Boxes that still need to be dealt with
      nb = size(b,1);
      nt = size(t,1);
      
      % Alloc space
      t = [t; zeros(5*nb,3)];                                              % Has to be a least 5 times as many tri's as boxes
      nlist = zeros(512,1);                                                % Wont ever be exceeded!

      lim = 0.5*nb;
      for k = 1:nb
         
         if k>lim                                                          % Halfway!
            waitbar(0.8,wbar,'Triangulating quadtree');
            lim = nb+1;
         end

         % Corner nodes
         n1 = b(k,1); n2 = b(k,2);
         n3 = b(k,3); n4 = b(k,4);

         % Assemble node list for kth box in CCW order
         nlist(1) = n1;
         count = 1;
         next = 2;
         while true
            
            cn = nlist(next-1);

            % Find the closest node to CN travelling CCW around box
            old = inf;
            for j = 1:n2n(cn,1)
               nn = n2n(cn,j+1);
               dx = p(nn,1)-p(cn,1);
               dy = p(nn,2)-p(cn,2);
               if count==1                         % Edge 12
                  if (dx>0) && (dx<old)
                     old = dx;
                     tmp = nn;
                  end
               elseif count==2                     % Edge 23
                  if (dy>0) && (dy<old)
                     old = dy;
                     tmp = nn;
                  end
               elseif count==3                     % Edge 34
                  if (dx<0) && (abs(dx)<old)
                     old = abs(dx);
                     tmp = nn;
                  end
               else                                % Edge 41
                  if (dy<0) && (abs(dy)<old)
                     old = abs(dy);
                     tmp = nn;
                  end
               end

            end
            
            if tmp==n1                                                     % Back to start - Done!
               break
            elseif (count<4) && (tmp==b(k,count+1))                        % New edge
               count = count+1;
            end
            nlist(next) = tmp;
            next = next+1;
            
         end
         nnode = next-1;

         if (nt+nnode)>=size(t,1)                                          % Realloc memory on demand
            t = [t; zeros(nb,3)];
         end
         if (np+1)>=size(p,1)
            p = [p; zeros(nb,2)];
            h = [h; zeros(nb,1)];
         end
         
         % Triangulate box
         if nnode==4                                                       % Special treatment if no mid-side nodes
                                                                           % Split box diagonally into 2 tri's
            % New tri's
            t(nt+1,1) = n1;                     % t1
            t(nt+1,2) = n2;
            t(nt+1,3) = n3;
            t(nt+2,1) = n1;                     % t2
            t(nt+2,2) = n3;
            t(nt+2,3) = n4;
            
            % Update count
            nt = nt+2;
            
         elseif nnode==5                                                   % Special treatment if only 1 mid-side node
                                                                           % Split box into 3 tri's centred at mid-side node
            % Find the mid-side node
            j = 2;
            while j<=4
               if nlist(j)~=b(k,j)
                  break
               end
               j = j+1;
            end
           
            % Permute indexing so that the split is always between n1,n2
            if j==3
               n1 = b(k,2);   n2 = b(k,3);
               n3 = b(k,4);   n4 = b(k,1);
            elseif j==4
               n1 = b(k,3);   n2 = b(k,4);
               n3 = b(k,1);   n4 = b(k,2);
            elseif j==5
               n1 = b(k,4);   n2 = b(k,1);
               n3 = b(k,2);   n4 = b(k,3);
            end
            
            % New tri's
            t(nt+1,1) = n1;                     % t1
            t(nt+1,2) = nlist(j);
            t(nt+1,3) = n4;
            t(nt+2,1) = nlist(j);               % t2
            t(nt+2,2) = n2;
            t(nt+2,3) = n3;
            t(nt+3,1) = n4;                     % t3
            t(nt+3,2) = nlist(j);
            t(nt+3,3) = n3;

            % Update count
            nt = nt+3;
            
         else                                                              % Connect all mid-side nodes to an additional node
                                                                           % introduced at the centroid
            % New tri's
            xave = 0;
            yave = 0;
            have = 0;
            for j = 1:nnode-1
               jj = nlist(j);
               % New tri's
               t(nt+j,1) = jj;
               t(nt+j,2) = np+1;
               t(nt+j,3) = nlist(j+1);
               % Averaging
               xave = xave+p(jj,1);
               yave = yave+p(jj,2);
               have = have+h(jj);
            end
            jj = nlist(nnode);
            % Last tri
            t(nt+nnode,1) = jj;
            t(nt+nnode,2) = np+1;
            t(nt+nnode,3) = nlist(1);
            % New node
            p(np+1,1)   = (xave+p(jj,1)) /nnode;
            p(np+1,2)   = (yave+p(jj,2)) /nnode;
            h(np+1)     = (have+h(jj))   /nnode;

            % Update count
            nt = nt+nnode;
            np = np+1;

         end

      end
      p = p(1:np,:);
      h = h(1:np);
      t = t(1:nt,:);

   end

end


%% SUB-FUNCTIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = userhfun(x,y,fun,args,hmax,xymin,xymax)

% Evaluate user defined size function.

if ~isempty(fun)
   h = feval(fun,x,y,args{:});
   if size(h)~=size(x)
      error('Incorrect user defined size function. SIZE(H) must equal SIZE(X).');
   end
else
   h = inf*ones(size(x));
end
h = min(h,hmax);

% Limit to domain
out = (x>xymax(1))|(x<xymin(1))|(y>xymax(2))|(y<xymin(2));
h(out) = inf;

if any(h<=0)
   error('Incorrect user defined size function. H must be positive.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [hmax,edgeh,fun,args] = gethdata(hdata)

% Check the user defined size functions

if ~isempty(hdata)
   if ~isstruct(hdata)
      error('HDATA must be a structure');
   end
   if numel(hdata)~=1
      error('HDATA cannot be an array of structures');
   end
   fields = fieldnames(hdata);
   names = {'hmax','edgeh','fun','args'};
   for k = 1:length(fields)
      if ~any(strcmp(fields{k},names))
         error('Invalid field in HDATA');
      end
   end
   if isfield(hdata,'hmax')
      if (numel(hdata.hmax)~=1) || (hdata.hmax<=0)
         error('HDATA.HMAX must be a positive scalar');
      else
         hmax = hdata.hmax;
      end
   else
      hmax = inf;
   end
   if isfield(hdata,'edgeh')
      if (numel(hdata.edgeh)~=2*size(hdata.edgeh,1)) || any(hdata.edgeh(:)<0)
         error('HDATA.EDGEH must be a positive Kx2 array');
      else
         edgeh = hdata.edgeh;
      end
   else
      edgeh = [];
   end
   if isfield(hdata,'fun')
      if ~ischar(hdata.fun) && ~isa(hdata.fun,'function_handle')
         error('HDATA.FUN must be a function name or a function handle');
      else
         fun = hdata.fun;
      end
   else
      fun = '';
   end
   if isfield(hdata,'args')
      if ~iscell(hdata.args)
         error(['HDATA.ARGS must be a cell array of additional' ...
            'inputs for HDATA.FUN']);
      else
         args = hdata.args;
      end
   else
      args = {};
   end
else
   hmax = inf;
   edgeh = [];
   fun = '';
   args = {};
end
