function [aq] = rtcics(Fx,F,rA,zA,crq,czq,L)
% function [aq] = rtcics(Fx,F,rA,zA,crq,czq,L) 
% Real-time contouring with psi-toolbox method
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

npar = L.csnthreads;
rk = L.taur;
zk = L.tauz;

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

niter = L.P.itercs;
tol = L.P.tolcs;

% alloc
nA  = numel(rA);
npq = numel(F);
noq = numel(crq);

% fuq
sith = repmat(czq         ,1,npq);
coth = repmat(crq         ,1,npq);
s2th = repmat(czq.^2      ,1,npq);
c2th = repmat(crq.^2      ,1,npq);
scth = repmat((czq.*crq*2),1,npq);
psth = repmat(F(:).'      ,noq,1);
if nA>1 && npq>1
  rA = repmat(rA,1,npq);
  zA = repmat(zA,1,npq);
end

% initial guess, avoid 0
aq = repmat(tol,noq,npq);

% Gauss-Newton iteration
for kiter = 1:niter
  r = rA(:) + aq(:) .* coth(:);
  z = zA(:) + aq(:) .* sith(:);
  [f,gr,gz,hrr,hrz,hzz] = bsp2deval(rk,zk,c,r,z,[0,1,2],npar);
  p   = f;
  dp  = gz .* sith(:) + gr .* coth(:);
  d2p = hrr .* c2th(:) + hrz .* scth(:) + hzz .* s2th(:);
  daq = (sqrt(max(dp.^2+2*d2p.*(psth(:)-p),0)).*sign(dp) - dp) ./ d2p;
  aq(:) = aq(:) + daq;
  if max(abs(daq)) < tol, break, end
end

% % keep 0 on 0
% k0 = L.pQ == 0;
% aq(:,k0) = 0;

% dispatch in outputs
if kiter == niter
  warning('rtcics:noconv','no convergence')
end
% fprintf('rtcics done in %02d/%02d iterations with daq=%.3g\n',kiter,niter,max(abs(daq)));
end
