function [rl,zl,Fl] = fl4pinterp(Fx,kl,cl,klh,clh,iFl,FN,rl,zl,rA,zA)
% Fl4pinterp - 3-point interpolation on limiter contour to find more accurate flux extremum
% [rl,zl,Fl] = fl4pinterp(Fx,kl,cl,klh,clh,iFl,FN,rl,zl,rA,zA)
%
% First seeks candidate limiter points using same methods as fl4pmex
% Then, for each candidate limiter point (c), uses left (l) and right (r)
% point, as well as intermediate points (*) and does a 3-point
% interpolation of the flux to find a more accurate extremum.
% l -- * -- c -- * -- r
% Substitute rl,zl,Fl values to match these extrema.
% Fl==FN is set for non-viable limiter points.
%
% Inputs:
%   Fx: flux map
%   kl,cl,klh,clh: limiter and half(h) point interpolation coefficients contained in L.
%   iFl: A priori selection of which limiter points to consider
%   FN: Value to set for invalid limiter point flux
%   rl,zl: r,z coordinates of limiter points
%   rA,zA: magnetic axis
% Outputs:
%   rl, zl, Fl: arrays with candidate limiter points modified to contain local extremum values
%   Fl will contain entries equal to FN for non-candidate limiter points
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

%% Candidate points
Fl  = bintmex(Fx,int32(kl-1),cl); % interpolate flux on limiter values

% exclude points that are not local extrema of the correct sign
% some duplication here w.r.t. fl4pmex
dFl = diff(Fl([end 1:end]));
iFl = iFl & ~(dFl .* dFl([2:end 1]) > 0 | (dFl - dFl([2:end 1])) * FN > 0);

% also exclude points with wrong flux gradient (private flux region)
% some duplication here w.r.t. fl4pmex
nz = size(Fx,1);
dRFl = (Fx(kl+nz+1) - Fx(kl   +1)).*(cl(2,:)+cl(1,:))' + ...
       (Fx(kl+nz+2) - Fx(kl   +2)).*(cl(4,:)+cl(3,:))';
dZFl = (Fx(kl   +2) - Fx(kl   +1)).*(cl(4,:)+cl(1,:))' + ...
       (Fx(kl+nz+2) - Fx(kl+nz+1)).*(cl(3,:)+cl(2,:))';
dAFl = (dRFl.*(rA-rl) + dZFl.*(zA-zl));
iFl = iFl & ~(dAFl*FN > 0);

if ~any(iFl)
  return
end

% indices of remaining candidates
iiFl = find(iFl); nFl = numel(iiFl);

%% Intepolation

[rle,zle,Fle] = deal(zeros(numel(iiFl,1)));
for ilca=1:nFl % for each limiter candidate
  ic = iiFl(ilca); % central point
  % left and right points
  if ic == numel(rl)
    ir = 1; il = ic-1; % wrap
  elseif ic==1
    ir = 2; il = numel(rl); % wrap
  else
    ir = ic+1; il = ic-1;
  end
  
  ilc = [il,ic]; icr = [ic,ir];
  Flh = bintmex(Fx,int32(klh(ilc)-1),clh(:,ilc)); % Flux on intermediate points used for interpolation
  sIp = -sign(FN); % Ip sign
  % 3 point interpolation between left and right points
  [rlel,zlel,Flel] = Flextremum(rl(ilc),zl(ilc),Fl(ilc),Flh(1),sIp); % left
  [rler,zler,Fler] = Flextremum(rl(icr),zl(icr),Fl(icr),Flh(2),sIp); % right
  
  if (Flel-Fler)*sIp > 0
    rle(ilca) = rlel; zle(ilca)=zlel; Fle(ilca)=Flel;
  else
    rle(ilca) = rler; zle(ilca)=zler; Fle(ilca)=Fler;
  end
  rl(ic) = rle(ilca);
  zl(ic) = zle(ilca);
  Fl(ic) = Fle(ilca);
end

Fl(~iFl) = FN;

end

function [rel,zel,Fel] = Flextremum(rl,zl,Fl,Flh,sIp)
% Find local flux extremum between candidate limiter points i1,i2

  % fluxes at lower, cental upper limiting points, multiply by sign
  Flc = sIp*Flh;
  Fll = sIp*Fl(1); 
  Flu = sIp*Fl(2); 
  
  [xel,Fel] = max3p(Fll,Flc,Flu); % Find maximum
  rel = rl(1) + 0.5*(xel+1)*(rl(2)-rl(1)); % find corresponding r,z
  zel = zl(1) + 0.5*(xel+1)*(zl(2)-zl(1));
  Fel = sIp*Fel; % Re-correct flux sign
end

function [xe,ye] = max3p(yl,yc,yu)
% Parabolic interpolation to find local maximum
% given x,y on 3 points: left, center right
% Local maximum may be on edge of domain
xl = -1; xc = 0; xu = 1;

% second/first derivatives
hd2 = 0.5*(yu+yl)-yc; %d2/2
hd1 = 0.5*(yu-yc-hd2);

if hd2>=0 % convex or flat parabola: take one of the points
  if yl>yu
    xe = xl; ye = yl;
  elseif yl<yu 
    xe = xu; ye = yu;
  else % all equal
    xe = xc; ye = yc; % central
  end
elseif hd2<0 % concave parabola, extremum may be maximum
  xe = xc - hd1/hd2;
  if xe>xu
    xe=xu; ye=yu;
  elseif xe<xl
    xe=xl; ye=yl;
  else
    ye = yc + hd1*(xe-xc); % interpolated maximum
  end
end

%%% debugging plot, uncomment if desired
% if doplot
%   x = linspace(xl,xu,101);
%   y = (d2/2)*(x-xc).^2 + d1*(x-xc) + yc;
%   plot([xl,xc,xu],[yl,yc,yu],'x',xe,ye,'*r',x,y)
% end
end
