/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"

# ifdef SINGLE
# define ABS  fabsf
# define FCT  srtci
# define FCT1 srtci1
# else
# define ABS  fabs
# define FCT  drtci
# define FCT1 drtci1
# endif
/*  [a[no,np],dan] = 
     rtci(ai[no,np],ero[1|no],ezo[1|no],F[nz,nr],Fp[np],cdr[no],cdz[no],Opy[ny],Fo[no],Opo,dap) */
void FCT(FLT *a, FLT *dan, FLT *ai, FLT *ero, FLT *ezo, FLT *F, FLT *Fp,
         FLT *cdr, FLT *cdz, int8_t *Opy, FLT *Fo, int8_t Opo, FLT dap,
         int nr, int nz, int np, int no, int io) {

 int i, j, k, jr, jz, nr2=nr-2, nz2=nz-2;
 FLT dam=-dap, *pcr, *pcz, *per, *pez, er, ez, F0, F1, F2, F3, F4, dF, da, vai, vcr, vcz;
 FLT Fw;
 int8_t Op0, Op1, Op2, Op3;
 bool any, all;
 
 io = io != 1;
 *dan = FLTC(0.0);
 for ( j=np; j--; Fp++ ) {
  for ( i=no, pcr=cdr, pcz=cdz, per=ero, pez=ezo; i--; per+=io, pez+=io ) {
   vai = *ai++;
   vcr = *pcr++;
   vcz = *pcz++;
   jr = er = *per + vai * vcr;
   jz = ez = *pez + vai * vcz;
   if (jr < 0) jr = 0; else if (jr > nr2) jr = nr2;
   if (jz < 0) jz = 0; else if (jz > nz2) jz = nz2;
   er -= jr;
   ez -= jz;
   k = jr * nz + jz;
   F0 = F[k     ]               ;
   F1 = F[k+nz  ] - F0          ;
   F2 = F[k+ 1  ] - F0          ;
   F3 = F[k+nz+1] - F0 - F1 - F2;
   F4 = ez * F3 + F1;
   dF = (er * F3 + F2) * vcz + F4 * vcr;
   Fw = F0 + er*F4 + ez * F2;
   /* Determine if any or all cell corners are in a different domain than the origin */
   k = (jr-1) * nz2 + jz-1;
   Op0 = ((jr>0   && jz>0  ) ? Opy[k      ] : 0 ) - Opo;
   Op1 = ((jr<nr2 && jz>0  ) ? Opy[k+nz2  ] : 0 ) - Opo;
   Op2 = ((jr>0   && jz<nz2) ? Opy[k+    1] : 0 ) - Opo;
   Op3 = ((jr<nr2 && jz<nz2) ? Opy[k+nz2+1] : 0 ) - Opo;
   any = Opo && (Op0 || Op1 || Op2 || Op3); /* Adding Opo disables domain checks for gaps */
   all = Opo && (Op0 && Op1 && Op2 && Op3);
   if (dF == FLTC(0.0)) da = FLTC(0.0);
   else                 da = (*Fp - Fw) / dF;        /* Standard Newton step */
   if      (all)                                     /* Wrong domain, back off */
   {
    if (da>0) da = -da;
   }
   else if (dF*(Fo[io*(no-1-i)] - *Fp)  > FLTC(0.0)) /* gradient direction is incorrect */
   {
    if (any & (da>0)) da = -da;                      /* if any, we are on the wrong branch of an X-point */
    else if (!any) da = dF*(Fw - *Fp)<0 ? dam : dap; /* Escape regions of incorrect gradient */
   }
   if      (da+dam > FLTC(0.0)) da = dap;
   else if (da+dap < FLTC(0.0)) da = dam;
   
   *a++ = ABS(vai + da);
   da = ABS(da);
   if (da > *dan) *dan = da;
  }
 }
}

/*  y[no,np] = rtci1(a[no,np],c[no],x[1|no]) % y=a*c+x */
void FCT1(FLT *y, FLT *a, FLT *c, FLT *x, int np, int no, int io) {
 int i, j;
 FLT *pc, *px;
 if (io == 1)
  for ( j=np; j--; ) {
   for ( i=no, pc=c; i--; ) {
    *y++ = (*a++) * (*pc++) + (*x);
   }
  }
 else
  for ( j=np; j--; ) {
   for ( i=no, pc=c, px=x; i--; ) {
    *y++ = (*a++) * (*pc++) + (*px++);
   }
  }
}
