function [Fl,rl,zl] = flcs(Fx,FN,rA,zA,L)
% FLCS Cubic spline interpolation of flux on limiter and identification of local extremas
% The limiter is described using piecewise cubic polynomials.
%
% [FL,RL,ZL] = FLCS(FX,FN,RA,ZA,L)
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

doplot = false;

Ml = L.Ml;
taul = L.taul;
breaks = L.taul([4,5:3:end-4,end-3]); % Assume knots with multiplicity 3
nb = numel(breaks);
rl = L.G.rl;
zl = L.G.zl;
nl = L.G.nl;

% psitbxfun internals
npar = L.csnthreads;
rk = L.taur;
zk = L.tauz;

% spline coefficients
c = L.Mr*(L.Mz*Fx).';

[Fl,drFl,dzFl] = bsp2deval(rk,zk,c,rl,zl,[0,1],npar);

sl = (0:nl-1).'/nl;

if doplot
  ax  = gca(figure(1));
  ax2 = gca(figure(2));

  sinf = 0:1e-5:1;
  xinf = bspsum(taul,Ml,sinf,0,0,npar);
  rinf = xinf(:,1).';
  zinf = xinf(:,2).';
  Finf = bspsum(zk,bspsum(rk,c,rinf,0,0,npar).',zinf,0,1,npar);

  cla(ax2);
  hold(ax2,'all');
  h=plot(ax2,sinf,Finf);
  plot(ax2,sl,Fl,'.','Color',h.Color);
end

% Eliminate non-extrema and ...
dFl = diff(Fl([end 1:end]));
Fl(dFl .* dFl([2:end 1]) > 0 | (dFl - dFl([2:end 1])) * FN > 0) = FN;
dAFl = (drFl.*(rA-rl) + dzFl.*(zA-zl));
Fl(dAFl*FN > 0) = FN;
il = find(Fl ~= FN);

if doplot
  plot(ax2,sl(il),Fl(il),'o','Color',h.Color);
  h = plot(ax2,NaN,NaN,'o','MarkerEdgeColor','k','MarkerFaceColor','r');
end

rs = rl(il);
zs = zl(il);
ss = mod(sl(il),1);

niter = L.P.itercs;
tol = L.P.tolcs;
dsmax = 1/L.G.nl;

if doplot
  cla(ax);
  hold(ax,'all');
  axis(ax,'equal');
  contourf(ax,L.G.rx,L.G.zx,Fx,21);
  plot(ax,rl,zl,'-c','Marker','.');
  plot(ax,rs,zs,'oc');
  hline = plot(ax,NaN(2,numel(ss)),NaN(2,numel(ss)),'-k');
  hprev = plot(ax,NaN(1,numel(ss)),NaN(1,numel(ss)),'ok','MarkerFaceColor','k');
  hcurr = plot(ax,NaN(1,numel(ss)),NaN(1,numel(ss)),'ok','MarkerFaceColor','r');
end

spike = false(numel(il),1);

for kiter = 1 : niter

  if isempty(il), continue; end

  ib = ifloor(breaks,ss);
  smin = breaks(ib);
  smax = breaks(mod(ib,nb)+1);

  % Compute flux gradient and hessian
  [~,drFs,dzFs,dr2Fs,drzFs,dz2Fs] = bsp2deval(rk,zk,c,rs,zs,[1,2],npar);

  % Compute limiter tangent
  dsxs = bspsum(taul,Ml,ss,1,0,npar);
  dsrs = dsxs(:,1);
  dszs = dsxs(:,2);
  ds2xs = bspsum(taul,Ml,ss,2,0,npar);
  ds2rs = ds2xs(:,1);
  ds2zs = ds2xs(:,2);

  % Compute flux derivatives along limiter
  dsFs = drFs.*dsrs + dzFs.*dszs;
  ds2Fs = dr2Fs.*(dsrs).^2 + 2*drzFs.*(dsrs.*dszs) + dz2Fs.*(dszs).^2 + ...
    drFs.*ds2rs + dzFs.*ds2zs;
  dss = - dsFs./ds2Fs;
  dss(spike) = 0;
  mask = ds2Fs.*FN<0;
  dss(mask) = -sign(dss(mask))*dsmax;
  dss = min(max(dss,-dsmax),dsmax);

  % Check for extremas when limiter contour has a slope discontinuity (e.g. baffle tip).
  snew = min(max(ss + dss,smin),smax);
  mask = ~spike & (snew == smin | snew == smax);
  for ii = 1:numel(il)
    if ~mask(ii), continue;
    elseif snew(ii) == smin(ii)
      sx = mod(snew(ii) + eps*[1,-1],1);
    elseif snew(ii) == smax(ii)
      sx = mod(snew(ii) + eps*[-1,1],1);
    end
    xx = bspsum(taul,Ml,sx,0,0,npar);
    rx = xx(:,1).';
    zx = xx(:,2).';
    % Compute flux gradient and hessian
    [~,drFx,dzFx,dr2Fx,drzFx,dz2Fx] = bsp2deval(rk,zk,c,rx,zx,[1,2],npar);
    % Compute limiter tangent
    dsxs = bspsum(taul,Ml,sx,1,0,npar);
    dsrx = dsxs(:,1);
    dszx = dsxs(:,2);
    ds2xs = bspsum(taul,Ml,sx,2,0,npar);
    ds2rx = ds2xs(:,1);
    ds2zx = ds2xs(:,2);
    % Compute flux derivatives along limiter
    dsFx = drFx.*dsrx + dzFx.*dszx;
    ds2Fx = dr2Fx.*(dsrx).^2 + 2*drzFx.*(dsrx.*dszx) + dz2Fx.*(dszx).^2 + ...
      drFx.*ds2rx + dzFx.*ds2zx;
    dsx = -dsFx./ds2Fx;
    maskx = ds2Fx*FN<0;
    dsx(maskx)=-sign(dsx(maskx))*dsmax;
    if (dsx(1)*dss(ii)<0)
      % Extremum is in this segment
      dss(ii) = 0.5*(snew(ii) - ss(ii));
    elseif (dsx(2)*dss(ii)<0)
      % Extremum is at segment joint
      dss(ii) = snew(ii) - ss(ii);
      spike(ii) = true;
    else
      % Extremum is in next segment
      dss(ii) = sx(2) - ss(ii);
      % Avoid large dss values when going from first to last segment or vice versa
      if     (ib(ii) ==    1 && dss(ii) < 0), dss(ii) = dss(ii) - 1;
      elseif (ib(ii) == nb-1 && dss(ii) > 0), dss(ii) = dss(ii) + 1;
      end
    end
  end

  ss = mod(ss + dss,1);

  xs = bspsum(taul,Ml,ss,0,0,npar);
  if doplot
    xsprev = [rs,zs];
    for ii = 1:numel(il)
      hline(ii).XData = [xsprev(ii,1),xs(ii,1)];
      hline(ii).YData = [xsprev(ii,2),xs(ii,2)];
    end
    hprev.XData = xsprev(:,1);
    hprev.YData = xsprev(:,2);
    hcurr.XData = xs(:,1);
    hcurr.YData = xs(:,2);
    drawnow;
  end

  % Maximum change in real space coordinates (R,Z) 
  dl = max(abs([xs(:,1)-rs;xs(:,2)-zs]));

  rs = xs(:,1);
  zs = xs(:,2);

%   fprintf('kiter=%02d/%02d, max(abs(dss))=%g\n',kiter,niter,max(abs(dss)));
  if dl < tol && ~any(mask), break; end
end

Fl(il) = bsp2deval(rk,zk,c,rs,zs,0,npar);

if doplot
  h.XData = ss;
  h.YData = Fl(il);
  drawnow;
  pause(0.5);
end

% Outputs
rl(il) = rs;
zl(il) = zs;

end
