%QINTMEX N-point quadratic interpolation (6<=N<=9)
%
% This is the MATLAB equivalent implementation of libmeq/qint.c.
% A more detailed help is available in QINTMEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [Fi,Bri,Bzi,Brri,Brzi,Bzri,Bzzi] = qintmexm(rx,zx,Fx,ri,zi,inM)
% Quadratic function
% y = c0 + c1*dr + c2*dz + c3*2*dr*dz + c4*dr^2 + c5*dz^2;
% fit 6 coefficients to n points (n=6 to 9)
%
% y7 y3 y5
% y2 y0 y1
% y6 y4 y8
%
% y(  0,  0) = y0;
% y( dr,  0) = y1;
% y(-dr,  0) = y2;
% y(0  , dz) = y3;
% y(0  ,-dz) = y4;
% y(dr , dz) = y5;
% y(-dr,-dz) = y6;
% y(-dr, dz) = y7;
% y( dr,-dz) = y8;
%
%
% This gives equations:
%
% c0                   = y0;
% c0 + c4*dr^2 + c1*dr = y1;
% c0 + c4*dr^2 - c1*dr = y2;
% c0 + c5*dz^2 + c2*dz = y3;
% c0 + c5*dz^2 - c2*dz = y4;
% c0 + c1*dr + c2*dz + c3*2*dr*dz + c4*dr^2 + c5*dz^2 = y5
% c0 - c1*dr - c2*dz + c3*2*dr*dz + c4*dr^2 + c5*dz^2 = y6
% c0 - c1*dr + c2*dz - c3*2*dr*dz + c4*dr^2 + c5*dz^2 = y7
% c0 + c1*dr - c2*dz - c3*2*dr*dz + c4*dr^2 + c5*dz^2 = y8

% derivatives and fields are then found analytically (COCOS=17)
% Br = -1/(2*pi*r) * dFdz;
% Bz =  1/(2*pi*r) * dFdr;
%
% Brr,Brz,Bzr,Bzz by further derivatives.
%
% The matrix for interpolating Y = [y0..yn] to P=[c0..c5] is pre-computed
% in qintc.m


ni = numel(ri);
nr = numel(rx);
nz = numel(zx);

rx = rx(:);
zx = zx(:);

Fi = zeros(size(ri));
if nargout>1
  [Bri,Bzi] = deal(zeros(size(ri)));
end
if nargout>3
  [Brri,Brzi,Bzri,Bzzi] = deal(zeros(size(ri)));
end

% Find index of central point for interpolation
idrx = 1/(rx(2) - rx(1)); % TODO: this could be received as a parameter
idzx = 1/(zx(2) - zx(1));
ir = round((ri(:) - rx(1))*idrx)+1;
iz = round((zi(:) - zx(1))*idzx)+1;

% handle cases at grid boundary
ir = min(max(ir,2),nr-1);
iz = min(max(iz,2),nz-1);

% local dr,dz
dr = ri(:)-rx(ir); dz= zi(:)-zx(iz);  % [ni,1]

% fit data
Y = Fx((iz+nz*(ir-1))+[0,nz,-nz,1,-1,nz+1,-nz-1,-nz+1,nz-1]);  % [ni,9]

% Fit: P = Y*inM;
n = size(inM,1); % number of Y points to use for fit
if n>9,  error('n too high'); end
P = Y(:,1:n)*inM; % [ni,6]
  
% interpolate flux:
Fi(:) = sum([ones(ni,1),dr,dz,2*dr.*dz,dr.^2,dz.^2].*P,2);

if nargout>1
  % Interpolate fields
  % derivatives:
  % dFdr = c1 + 2*c3*dz + 2*c4*dr;
  % dFdz = c2 + 2*c3*dr + 2*c5*dz;
  % Fields:
  % Br = -1/(2*pi*r) * dFdz;
  % Bz =  1/(2*pi*r) * dFdr;
  i2pi = 0.159154943091895; % 1/(2*pi)
  ir = 1./ri(:);
  Bri(:) = -i2pi*sum([zeros(ni,1),zeros(ni,1),         ir,2*dr.*ir,zeros(ni,1),   2*dz.*ir].*P,2);
  Bzi(:) =  i2pi*sum([zeros(ni,1),         ir,zeros(ni,1),2*dz.*ir,   2*dr.*ir,zeros(ni,1)].*P,2);
end

if nargout>3
  % second derivatives
  Brzi(:) = -i2pi*ir.*(P(:,6)*2);
  Bzzi(:) =  i2pi*ir.*(P(:,4)*2);
  Brri(:) = -i2pi*ir.*(P(:,4)*2) - ir.*Bri(:);
  Bzri(:) =  i2pi*ir.*(P(:,5)*2) - ir.*Bzi(:);
end
end % function
