%RTCIMEX  Isoflux contouring
%
% This is the MATLAB equivalent implementation of libmeq/rtci.c.
% A more detailed help is available in RTCIMEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [a,dan,rq,zq,irq] = rtcimexm(a,er0,ez0,Fx,F,c,s,Opy,Fo,Opo,dap,r,coq,z,soq)
 [nz,nr] = size(Fx);
 no = length(c);
  
 er = er0 + a.*c; jr = min(uint32(floor(max(er,0))),nr-2);
 ez = ez0 + a.*s; jz = min(uint32(floor(max(ez,0))),nz-2);
 frq = er - double(jr);
 fzq = ez - double(jz);
 kq = nz*jr + jz + 1; % no +1 in C
 f0 = Fx(kq); f1 = Fx(kq+nz)-f0; f2 = Fx(kq+1)-f0; f3 = Fx(kq+nz+1)-f0-f1-f2;
 f4 = fzq.*f3 + f1;
 fq = repmat(F,no,1);
 df = (frq.*f3+f2).*s + f4.*c;
 fw = f0 + frq.*f4 + fzq.*f2;
 % Proximity to domain boundary
 kq = (nz-2)*(jr-1) + jz; % jz - 1 in C
 [Op0,Op1,Op2,Op3] = deal(zeros(no,1,'int8'));
 mask = (jr>0    & jz>0   ); Op0(mask) = Opy(kq(mask)     ); % lower left
 mask = (jr<nr-2 & jz>0   ); Op1(mask) = Opy(kq(mask)+nz-2); % lower right
 mask = (jr>0    & jz<nz-2); Op2(mask) = Opy(kq(mask)   +1); % upper left
 mask = (jr<nr-2 & jz<nz-2); Op3(mask) = Opy(kq(mask)+nz-1); % upper right
 zz = cat(3,Op0,Op1,Op2,Op3) - Opo;
 type = zeros(size(fq));
 type(Opo & any(zz,3)) = 1; % Adding Opo disables domain checks for gaps
 type(Opo & all(zz,3)) = 2;
 da = (fq-fw)./df;
 da(df==0) = 0;
 % Wrong domain (if in plasma domain)
 mask = (type == 2) & (da > 0);
 da(mask) = -da(mask);
 % Derivative reverses close to boundary (wrong branch of X-point)
 mask = df.*(Fo - fq) > 0 & type == 1 & da>0;
 da(mask) = -da(mask);
 % Escape regions of incorrect gradient
 mask = df.*(Fo - fq) > 0 & type == 0;
 da(mask) = sign((fw(mask)-fq(mask)).*df(mask))*dap;
 da = min(max(da,-dap),dap);
 %
 a = abs(a + da);
 dan = max(abs(da(:)));
 if nargout > 2
  rq = r + a.*coq;
  zq = z + a.*soq;
  irq = 1 ./ rq;
 end
end
