/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
/* function [Fi,Bri,Bzi,Brri,Brzi,Bzri,Bzzi] = qint(rx,zx,Fx,ri,zi,inM)
% [FI,BRI,BZI,BRRI,BRZI,BZZI] = qint(RX,ZX,FX,RI,ZI,INM)
% N-point quadratic interpolation (6<=N<=9);
%
% Interpolates flux FX on grids RX,ZX to vector of points RI,ZI.
% INM: Coefficients for the interpolation such that
% P = Y*INM. INM [N,6] is calculated by QINTC.m
% Y is the vector of N values of FX on neighbouring points.
%

% 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

*/


# include "meq.h"
# ifdef SINGLE
# define FCT   sqint
# define GEMV  cblas_sgemv
# else
# define FCT   dqint
# define GEMV  cblas_dgemv
# endif
# define INV(x) ( x ? FLTC(1.0) / x : FLTC(0.0) )




void FCT(FLT * Fi, FLT * Bri, FLT * Bzi, FLT * Brri, FLT * Brzi, FLT * Bzri, FLT * Bzzi, FLT *rx, FLT *zx, FLT * Fx, FLT *ri, FLT *zi, FLT *inM, int ni, int nr, int nz, int n) {

  int i, j, ir, iz, ir0, iz0;
  FLT idrx, idzx;
  FLT dr, dz, iri;
  FLT Y[9];
  FLT P[6];
  FLT i2pi = 0.159154943091895; /* 1/(2*pi) */

  /* TODO: this could be received as a parameter */
  idrx = FLTC(1.0)/(rx[1] - rx[0]);
  idzx = FLTC(1.0)/(zx[1] - zx[0]);

  for (i = 0; i < ni; i++) {
    /* Find closest grid point */
    ir = lround((ri[i] - rx[0])*idrx);
    ir = ir < 1 ? 1 : ir;
    ir = ir > nr -2 ? nr -2 : ir;
    dr = ri[i] - rx[ir];
    iz = lround((zi[i] - zx[0])*idzx);
    iz = iz < 1 ? 1 : iz;
    iz = iz > nz -2 ? nz -2 : iz;
    dz = zi[i] - zx[iz];
    /* Closest point is [iz,ir] */
    /* printf("[qint] i=%d ,ri=%f, zi=%f, ir=%d, iz=%d, rx=%f, zx=%f\n",i,ri[i],zi[i],ir,iz,rx[ir],zx[iz]); */
    Y[0] = Fx[iz  +nz*(ir  )]; /* center */
    Y[1] = Fx[iz  +nz*(ir+1)]; /* right */
    Y[2] = Fx[iz  +nz*(ir-1)]; /* left */
    Y[3] = Fx[iz+1+nz*(ir  )]; /* top */
    Y[4] = Fx[iz-1+nz*(ir  )]; /* bottom */
    Y[5] = Fx[iz+1+nz*(ir+1)]; /* top-right */
    Y[6] = Fx[iz-1+nz*(ir-1)]; /* bottom-left */
    Y[7] = Fx[iz+1+nz*(ir-1)]; /* top-left */
    Y[8] = Fx[iz-1+nz*(ir+1)]; /* bottom-right */

    /* Fit P = (inM.'*Y.').' */
    GEMV(CblasColMajor, CblasTrans, n, 6, FLTC(1.0), inM, n, Y, 1, FLTC(0.0), P, 1);
    /* printf("[qint] i=%d, P[0]=%f, P[1]=%f, P[2]=%f, P[3]=%f, P[4]=%f, P[5]=%f\n",i,P[0],P[1],P[2],P[3],P[4],P[5]); */
    
    /* Interpolate flux */
    Fi[i] = P[0] + P[1]*dr + P[2]*dz + P[3]*FLTC(2.0)*dr*dz + P[4]*dr*dr + P[5]*dz*dz;

    /* 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;
    */
    iri = INV(ri[i]);
    Bri[i] = - i2pi * iri * (P[2] + P[3]*FLTC(2.0)*dr + P[5]*FLTC(2.0)*dz);
    Bzi[i] =   i2pi * iri * (P[1] + P[3]*FLTC(2.0)*dz + P[4]*FLTC(2.0)*dr);

    /* Second derivatives */
    Brzi[i] = - i2pi * iri * (P[5] * FLTC(2.0));
    Bzzi[i] =   i2pi * iri * (P[3] * FLTC(2.0));
    Brri[i] = - i2pi * iri * (P[3] * FLTC(2.0)) - iri * Bri[i];
    Bzri[i] =   i2pi * iri * (P[4] * FLTC(2.0)) - iri * Bzi[i];
  } /* for (i = 0; i < ni; i++) */

}
