% ASXYMEX search location and value a 2D function extrema and saddle points
%
% This is the MATLAB equivalent implementation of libmeq/asxy.c.
% A more detailed help is available in ASXYMEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

function [xa,ya,fa,dx2fa,dy2fa,dxyfa,xs,ys,fs,dx2fs,dy2fs,dxyfs,stat] =  asxymexm(f,x,y,dc,dx,dy,idx,idy,l,dimw)
debug = false;

%% ASXYMEX.C
[nx,ny] = size(f);
if (nargin == 10), dima = dimw;
else,              dima = nx*ny;end
dims = dima;
[xa,ya,fa,dx2fa,dy2fa,dxyfa] = deal(zeros(dima,1));
[xs,ys,fs,dx2fs,dy2fs,dxyfs] = deal(zeros(dims,1));
[ws,is,js] = deal(zeros(dims,1));

%% ASXY.C
ls=1;
st1=true;
st2=true;

if (dima)
  xa(1) = x(1);
  ya(1) = y(1);
  [fa(1),dx2fa(1),dy2fa(1),dxyfa(1)] = deal(0);
end

%% search all extrema and saddles

% NOTE: Saddle points along the boundary
%
% We also need to look for saddle points in the boundary cells that
% might have been left out due to a particular choice of orientation.
% We replace missing values by their direct neighbor and we use the
% neighboring cell to perform the interpolation.
% See the redbook for a more detailed explanation.

qidx=idx*idx; qidy=idy*idy; idxy=idx*idy;
i0 = 1;       % 3 4 5 initial and odd columns
i1 = i0 -  1; % 2 0 6
i2 = i0 - nx; %   1
i3 = i2 +  1;
i4 = i0 +  1; %   1   even columns
i5 = i4 + nx; % 2 0 6
i6 = i5 -  1; % 3 4 5
s=-1.0;
% Prepare for 1st column
i2 = i0;
i3 = i4;
dj = +1;
for j = 1:ny
  % Prepare for last column
  if j == ny
    i6 = i0;
    i5 = i4;
    dj = -1;
  end
  % Prepare for 1st row
  if s > 0
    i3 = i2;
    i4 = i0;
    i5 = i6;
  else
    i1 = i0;
  end
  di = +1;
  for i = 1:nx
    % Prepare for last row
    if i == nx
      if s > 0
        i1 = i0;
      else
        i3 = i2;
        i4 = i0;
        i5 = i6;
      end
      di = -1;
    end
    
    % half the number of sign changes from #1 to #1
    d0 = f(i0);      i0 = i0+1;
    d1 = f(i1) - d0; i1 = i1+1;
    d2 = f(i2) - d0; i2 = i2+1;
    d3 = f(i3) - d0; i3 = i3+1;
    d4 = f(i4) - d0; i4 = i4+1;
    d5 = f(i5) - d0; i5 = i5+1;
    d6 = f(i6) - d0; i6 = i6+1;
    df = d1; dl = d1; n = 0;
    % CROSS(d2)
    if (df == 0), df = d2;end
    if (dl == 0), dl = d2;
    elseif (dl*d2 < 0), dl=d2; n = n+1;end
    % CROSS(d3)
    if (df == 0), df = d3;end
    if (dl == 0), dl = d3;
    elseif (dl*d3 < 0), dl=d3; n = n+1;end
    % CROSS(d4)
    if (df == 0), df = d4;end
    if (dl == 0), dl = d4;
    elseif (dl*d4 < 0), dl=d4; n = n+1;end
    % CROSS(d5)
    if (df == 0), df = d5;end
    if (dl == 0), dl = d5;
    elseif (dl*d5 < 0), dl=d5; n = n+1;end
    % CROSS(d6)
    if (df == 0), df = d6;end
    if (dl == 0), dl = d6;
    elseif (dl*d6 < 0), dl=d6; n = n+1;end
    % CROSS(df)
    if (df == 0), df = df;end
    if (dl == 0), dl = df;
    elseif (dl*df < 0), dl=df; n = n+1;end
    n = n/2;
    
    if l(i0-1) && (...
        (~(di || dj) && (n ~= 1)) || ... % inner grid n=0,2,3 saddle or extremum
        ( (di || dj) && (n == 2)) ...    % boundary   n=2     saddle only
       )
      if debug
        fprintf('asxymexm.m ID 1-based     n,i,j=%d,%2d,%2d\n',n,i,j);
        if (s > 0)
          fprintf('ID         %7.4f\n',d1);
          fprintf(  'ID %7.4f %7.4f %7.4f\n',d2,d0,d6);
          fprintf(  'ID %7.4f %7.4f %7.4f\n',d3,d4,d5);
        else
          fprintf(  'ID %7.4f %7.4f %7.4f\n',d3,d4,d5);
          fprintf(  'ID %7.4f %7.4f %7.4f\n',d2,d0,d6);
          fprintf('ID         %7.4f\n',d1);
        end
      end

      st1 = ls < dims;
      if (st1)
        ws   (ls) = n;
        is   (ls) = i;
        js   (ls) = j;
        ls = ls+1;
      end
    end % n != 1
    % Prepare for inner row
    if     i ==  1
      if s > 0
        i3 = i3-1;
        i4 = i4-1;
        i5 = i5-1;
      else
        i1 = i1-1;
      end
      di = 0;
    end
  end % for i
  % Prepare for inner column
  if j == 1
    i2 = i2-nx;
    i3 = i3-nx;
    dj =  0;
  end
  % Prepare for next column
  if s>0
    i3 = i3 + 2;
    i4 = i4 + 2;
    i5 = i5 + 2;
  else
    i1 = i1 + 2;
  end
  s = -s;
end % for j
% end search all extrema and saddles

%% Use 6-point interpolant to get extremum-saddle position and hessian
for ii = 1:ls-1

  i = max(min(is(ii),nx-1),2);
  j = max(min(js(ii),ny-1),2);

  [xe,ye,fe,~,~,c,d,e,h] = asxy1(f,i,j);

  if debug
    fprintf('INT x,y=%7.4f,%7.4f xe,ye=%7.4f,%7.4f h=%7.4f\n',x(i),y(j),xe,ye,h);
  end

  fs   (ii) = fe;
  xs   (ii) = xe*dx+x(i);
  ys   (ii) = ye*dy+y(j);
  dx2fs(ii) = c*qidx;
  dy2fs(ii) = d*qidy;
  dxyfs(ii) = e*idxy;
end

%% annihilate close extremum-saddle pairs
for i=1:ls-1
  for j=1:ls-1
    if (ws(i) == 0 && ws(j) >= 2)
      di = is(i)-is(j);
      dj = js(i)-js(j);
      if (di*di + dj*dj <= dc)
        if debug
          fprintf('asxymexm.m cancelling  n,i,j=%d,%2d,%2d and %d,%2d,%2d\n', ...
            ws(i),is(i),js(i),ws(j),is(j),js(j));
        end
        ws(i) = 1; ws(j) = 1;
        break;
      end
    end
  end
end

%% remove axis duplicates due to equal neighbouring values
for i=1:ls-1
  for j=i+1:ls-1
    if (ws(i) == 0 && ws(j) == 0)
      di = is(i)-is(j);
      dj = js(i)-js(j);
      if (di*di + dj*dj == 1) && (f(is(i),js(i)) == f(is(j),js(j)))
        if debug
          fprintf('asxymexm.m removing duplicate from n,i,j=%d,%2d,%2d at %d,%2d,%2d\n', ...
            ws(i),is(i),js(i),ws(j),is(j),js(j));
        end
        ws(j) = 1;
      end
    end
  end
end
%
 
%% dispatch
n = ls-1; la = 1; ls = 1;
j = 1;
for i=1:n
  if debug
    fprintf('asxymexm.m dispatching n,i,j=%d,%2d,%2d\n',ws(j),is(j),js(j));
  end
  if (ws(j) == 0)
    st2 = la < dima;
    if (st2)
      fa   (la) = fs   (j);
      xa   (la) = xs   (j);
      ya   (la) = ys   (j);
      dx2fa(la) = dx2fs(j);
      dy2fa(la) = dy2fs(j);
      dxyfa(la) = dxyfs(j);
    end
    la = la + 1;
  elseif (ws(j) ~= 1)
    fs   (ls) = fs   (j);
    xs   (ls) = xs   (j);
    ys   (ls) = ys   (j);
    dx2fs(ls) = dx2fs(j);
    dy2fs(ls) = dy2fs(j);
    dxyfs(ls) = dxyfs(j);
    ls = ls + 1;
  end
  j = j+1;
end
na = la-1;
ns = ls-1;

if debug
  fprintf('asxymexm.m exit na,ns=%d,%d\n',na,ns);
end

stat = st1 && st2;

%% ASXYMEX.C
xa = xa(1:na);
ya = ya(1:na);
fa = fa(1:na);
dx2fa = dx2fa(1:na);
dy2fa = dy2fa(1:na);
dxyfa = dxyfa(1:na);
xs = xs(1:ns);
ys = ys(1:ns);
fs = fs(1:ns);
dx2fs = dx2fs(1:ns);
dy2fs = dy2fs(1:ns);
dxyfs = dxyfs(1:ns);

end

function [xe,ye,fe,a,b,c,d,e,h] = asxy1(f,i,j)

  nx = size(f,1);

  d0 = f((j-1)*nx+i  );      % 3 4 5
  d1 = f((j-1)*nx+i-1) - d0; % 2 0 6
  d2 = f((j-2)*nx+i  ) - d0; %   1
  d3 = f((j-2)*nx+i+1) - d0;
  d4 = f((j-1)*nx+i+1) - d0;
  d5 = f((j  )*nx+i+1) - d0;
  d6 = f((j  )*nx+i  ) - d0;

  %  6 point interpolant: f0 + a x + b y + c/2 x^2 + d/2 y^2 + e xy
  %  syms  d1  d2  d3  d4  d5  d6
  %  x = [  1;  0; -1; -1; -1;  0];
  %  y = [  0; -1; -1;  0;  1;  1];
  %  A = [x y .5*x.^2 .5*y.^2 x.*y];
  %  A([1 2 3 4 6],:)\[d1;d2;d3;d4;d6] % = [a;b;c;d;e] with d3
  %  A([1 2 4 5 6],:)\[d1;d2;d4;d5;d6] % = [a;b;c;d;e] with d5

  a = (d4 - d1) *     0.5;
  b = (d6 - d2) *     0.5;
  c =  d1 + d4           ;
  d =  d2 + d6           ;
  e = (d4 + d2 - d3)     ;
  h = c*d - e*e;
  if h ~= 0, h = 1./h;end
  xe = (e*b - a*d) * h; if abs(xe)>1, xe = sign(xe);end
  ye = (e*a - b*c) * h; if abs(ye)>1, ye = sign(ye);end
  
  fe = (0.5*c*xe + a)*xe + (0.5*d*ye + e*xe + b)*ye + d0;
end
