function [prob atlas cseg flush] = add_chart(atlas, prob, cseg)
%ADD_CHART   Add chart to point list.
%
% Add successully located chart to curve segment point list and increment
% counter. Designate end point by 'EP' point type and flush. Terminate if
% successive charts do not qualify as "neighbors".

% This file is part of the atlas_kd toolbox, copyright (C) Michael
% Henderson, Frank Schilder, Harry Dankowicz, Erika Fotsch, of the package
% COCO (http://sourceforge.net/projects/cocotools).

chart         = cseg.curr_chart;
chart.pt      = atlas.next_pt;
atlas.next_pt = atlas.next_pt+1; % Increment chart counter
chart.id      = chart.pt+1;
chart.dim     = atlas.dim;

if chart.pt>=atlas.cont.PtMX
  chart.pt_type = 'EP';
  chart.ep_flag = 1;
end
[prob cseg] = cseg.add_chart(prob, chart);
flush       = true;

%% step size control: update step size

chart1   = cseg.ptlist{1};
chart2   = cseg.ptlist{2};
arc_beta = subspace(chart1.TS, chart2.TS);

cont     = atlas.cont;
h        = chart1.h;

% remesh atlas if step size has been reduced
if h < chart1.R
  chart1.R = h;
  atlas.charts{chart1.id} = chart1;
  atlas.tree.root = change_radius(atlas.tree.root, numel(chart1.x), ...
    chart1, 1, h, false);
  atlas    = atlas.update_atlas(chart1.id);
end

% This if-statement takes care of the case arc_beta==0, which produced an
% error in some versions of Matlab.
if cont.h_fac_max^2*arc_beta < cont.almax
  h_fac = cont.h_fac_max;
else
  h_fac = max(cont.h_fac_min, min(cont.h_fac_max, ...
    cont.almax/(sqrt(cont.h_fac_max)*arc_beta)));
end

h = max( min(cont.ga*h_fac*h, cont.h_max), cont.h_min);

% check residuum condition for vertices of new hypercube
res = chart_hc_res(prob, chart2, h);
while (res>cont.MaxRes) && (h>cont.h_min)
	h   = max(cont.h_min, cont.h_fac_min*h);
  res = chart_hc_res(prob, chart2, h);
end

%% Assign radius and build cube

R  = chart1.R;
ca = cos(cont.almax);
d  = ca*(chart1.t' * (chart2.x-chart1.x));
h  = max( min([h cont.h_max sqrt(R*R+d*d)]), cont.h_min);
if h<sqrt(R*R-d*d)
  chart1.h = max(cont.h_fac_min*chart1.h, cont.h_min);
  atlas.charts{chart1.id} = chart1;
  atlas.next_pt = atlas.next_pt - 1;
  flush    = false;
elseif ~atlas.isneighbor(chart1,chart2)
  if  chart1.h<=cont.h_min
    cseg.ptlist{end}.pt_type = 'GAP';
    cseg.ptlist{end}.ep_flag = 2;
    cseg.Status              = cseg.CurveSegmentCorrupted;
  else
    chart1.h = max(cont.h_min, chart1.h/2);
    atlas.charts{chart1.id} = chart1;
    atlas.next_pt = atlas.next_pt - 1;
    flush    = false;
  end
else
  chart2.R = h;
  chart2.h = h;
  chart2   = create_polyhedron(chart2, cont.Rmarg);
  chart2.loc     = [];
  chart2.emptyN  = []; 
  chart2.intsct  = [];
  cseg.ptlist{2} = chart2;
end

end

function res = chart_hc_res(prob, chart, h)

% Ignore residuum of arc-length constraint
prob.xfunc.mode = 2;

% Compute residuum on boundary box
xx    = chart.TS*chart.s;
res   = 0;
for i=1:size(xx,2)
  [prob chart f] = prob.efunc.F(prob, chart, chart.x+h*xx(:,i));
  res = max(res, norm(f));
end

end
