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

# ifdef SINGLE
# define FCT  sasxy
# define FCT1 sasxy1
# define BAAX sbaax
# define SQRT sqrtf
# define ABS  fabsf
# define SIGN copysignf
# else
# define FCT  dasxy
# define FCT1 dasxy1
# define BAAX dbaax
# define SQRT sqrt
# define ABS  fabs
# define SIGN copysign
# endif

# define CROSS(DN) if (     df   == FLTC(0.0))   df = DN; \
                   if (     dl   == FLTC(0.0))   dl = DN; \
                   else if (dl*DN < FLTC(0.0)) { dl = DN; n++; }
# define CLAMP(X) if (ABS(X) > FLTC(1.0)) X=SIGN(FLTC(1.0),X);
int FCT(FLT *xa, FLT *ya, FLT *fa, FLT *dx2fa, FLT *dy2fa, FLT *dxyfa, int dima, int *na,
        FLT *xs, FLT *ys, FLT *fs, FLT *dx2fs, FLT *dy2fs, FLT *dxyfs, int dims, int *ns,
        FLT *f, FLT fnan, FLT *x, FLT *y, int dc, int nx, int ny,
        FLT dx, FLT dy, FLT idx, FLT idy, bool *lim, int *ws)
{
 int i,j,n, la=0, ls=0, *is = ws+dims, *js = is+dims, st1=1, st2=1;
 
 if (dima) {
  *xa = *x;
  *ya = *y;
  *fa = *dx2fa = *dy2fa = *dxyfa = FLTC(0.0);
 }
 
 { /* search all extrema and saddles */
  
  /* Note: Saddle points along the boundary
   * 
   * We also need to look for saddle points in the boundary cells that
   * might have been left out due to a particular choice of orientation.
   * We replace missing values by their direct neighbor and we use the
   * neighboring cell to perform the interpolation.
   * See the redbook for a more detailed explanation.
  */
  int di,dj;
  FLT
   *f0 = f      , /* 3 4 5 initial and odd columns */
   *f1 = f0 -  1, /* 2 0 6 */
   *f2 = f0 - nx, /*   1   */
   *f3 = f2 +  1,
   *f4 = f0 +  1, /*   1   odd columns */
   *f5 = f4 + nx, /* 2 0 6 */
   *f6 = f5 -  1, /* 3 4 5 */
   d0,d1,d2,d3,d4,d5,d6,df,dl, s=FLTC(-1.0);
  /* Prepare for 1st column */
  f2 = f0;
  f3 = f4;
  dj = +1;
  for (j = 1; j <= ny; j++) {
   /* Prepare for last column */
   if (j == ny) {
    f6 = f0;
    f5 = f4;
    dj = -1;
   }
   /* Prepare for 1st row */
   if (s > FLTC(0.0)) {
    f3 = f2;
    f4 = f0;
    f5 = f6;
   } else {
    f1 = f0;
   }
   di = +1;
   for (i = 1; i <= nx; i++) {
    /* Prepare for last row */
    if (i == nx) {
     if (s > FLTC(0.0)) {
      f1 = f0;
     } else {
      f3 = f2;
      f4 = f0;
      f5 = f6;
     }
     di = -1;
    }

    /* half the number of sign changes from #1 to #1 */
    d0 = *f0++;
    d1 = *f1++ - d0;
    d2 = *f2++ - d0;
    d3 = *f3++ - d0;
    d4 = *f4++ - d0;
    d5 = *f5++ - d0;
    d6 = *f6++ - d0;
    df = dl = d1; n = 0;
    CROSS(d2)
    CROSS(d3)
    CROSS(d4)
    CROSS(d5)
    CROSS(d6)
    CROSS(df)
    n /= 2;

    if (*lim++ && (
        (!(di || dj) && (n != 1)) || /* inner grid n=0,2,3 saddle or extremum */
        ( (di || dj) && (n == 2))    /* boundary   n=2     saddle only */
       )) {
     # ifdef DEBUG
     printf("asxy.c ID 1-based     n,i,j=%d,%2d,%2d\n",n,i,j);
     if (s > FLTC(0.0)) {
      printf("ID         %7.4f\n",d1);
      printf(  "ID %7.4f %7.4f %7.4f\n",d2,d0,d6);
      printf(  "ID %7.4f %7.4f %7.4f\n",d3,d4,d5);
     } else {
      printf(  "ID %7.4f %7.4f %7.4f\n",d3,d4,d5);
      printf(  "ID %7.4f %7.4f %7.4f\n",d2,d0,d6);
      printf("ID         %7.4f\n",d1);
     }
     # endif

     st1 = ls < dims;
     if (st1) {
      ws   [ls] = n;
      is   [ls] = i;
      js   [ls] = j;
      ls++;
     }
    } /* n != 1 */
    /* Prepare for inner row */
    if (i ==  1) {
      if (s > FLTC(0.0)) {
        f3--;
        f4--;
        f5--;
      } else {
        f1--;
      }
      di = 0;
    }
   } /* for i */
   /* Prepare for inner column */
   if (j == 1) {
    f2 -= nx;
    f3 -= nx;
    dj  =  0;
   }
   /* Prepare for next column */
   if (s > FLTC(0.0)) {
    f3 += 2;
    f4 += 2;
    f5 += 2;
   } else {
    f1 += 2;
   }
   s = -s;
  } /* for j */
 } /* search all extrema and saddle points */

 { /* Use 6-point interpolant to get extremum-saddle position and hessian */
  for (int ii=0; ii<ls; ii++)
  {
   FLT a,b,c,d,e,h,xe,ye,fe;
   FLT qidx=idx*idx, qidy=idy*idy, idxy=idx*idy;
   /* is and js are 1-based indices */
   i  = is[ii]-1;
   i  = i ? (i < nx-1 ? i : nx-2) : 1;
   j  = js[ii]-1;
   j  = j ? (j < ny-1 ? j : ny-2) : 1;
   
   FCT1(f,i,j,nx,&xe,&ye,&fe,&a,&b,&c,&d,&e,&h);
   
   # ifdef DEBUG
   printf("INT x,y=%7.4f,%7.4f xe,ye=%7.4f,%7.4f h=%7.4f\n",x[i],y[j],xe,ye,h);
   # endif
   fs   [ii] = fe;
   xs   [ii] = xe*dx + x[i];
   ys   [ii] = ye*dy + y[j];
   dx2fs[ii] = c*qidx;
   dy2fs[ii] = d*qidy;
   dxyfs[ii] = e*idxy;
  }
 }
 
 { /* annihilate close extremum-saddle pairs */
  int di,dj;
  for (i=ls; i--;)
   for (j=ls; j--; )
    if (ws[i] == 0 && ws[j] >= 2) {
     di = is[i]-is[j];
     dj = js[i]-js[j];
     if (di*di + dj*dj <= dc) {
# ifdef DEBUG
      printf("asxy.c cancelling  n,i,j=%d,%2d,%2d and %d,%2d,%2d\n",
              ws[i],is[i],js[i],ws[j],is[j],js[j]);
# endif
      ws[i] = ws[j] = 1;
      break;
     }
    }
 }
 
 { /* remove axis duplicates due to equal neighbouring values */
  int di,dj;
  for (i=ls; i--;)
   for (j=i; j--; )
    if (ws[i] == 0 && ws[j] == 0) {
     di = is[i]-is[j];
     dj = js[i]-js[j];
     if ((di*di + dj*dj == 1) && f[(js[i]-1)*nx+(is[i]-1)] == f[(js[j]-1)*nx+(is[j]-1)]) {
# ifdef DEBUG
      printf("asxy.c removing duplicate from n,i,j=%d,%2d,%2d at %d,%2d,%2d\n",
              ws[i],is[i],js[i],ws[j],is[j],js[j]);
# endif
      ws[j] = 1;
     }
    }
 }
 n = ls; la = ls = 0;
 for (i=n, j=0; i--; j++) { /* dispatch */
  # ifdef DEBUG
  printf("asxy.c dispatching n,i,j=%d,%2d,%2d\n",ws[j],is[j],js[j]);
  # endif
  if (ws[j] == 0) {
   st2 = la < dima;
   if (st2) {
    fa   [la] = fs   [j];
    xa   [la] = xs   [j];
    ya   [la] = ys   [j];
    dx2fa[la] = dx2fs[j];
    dy2fa[la] = dy2fs[j];
    dxyfa[la] = dxyfs[j];
   }
   la++;
  }
  else if (ws[j] != 1) {
   fs   [ls] = fs   [j];
   xs   [ls] = xs   [j];
   ys   [ls] = ys   [j];
   dx2fs[ls] = dx2fs[j];
   dy2fs[ls] = dy2fs[j];
   dxyfs[ls] = dxyfs[j];
   ls++;
  }
 }
 *na = la;
 *ns = ls;
 
 # ifdef DEBUG
 printf("asxy.c exit na,ns=%d,%d\n",*na,*ns);
 # endif
 
 return (st1&&st2);

}

void FCT1(FLT *f, int i, int j, int nx,
          FLT *xe, FLT *ye, FLT *fe,
          FLT *a, FLT *b, FLT *c, FLT *d, FLT *e, FLT *h)
{
 int k;
 FLT d0, d1, d2, d3, d4, d5, d6;
 
 k  = j*nx+i;
 d0 = f[k];           /* 3 4 5 */
 d1 = f[k   -1] - d0; /* 2 0 6 */
 d2 = f[k-nx  ] - d0; /*   1   */
 d3 = f[k-nx+1] - d0;
 d4 = f[k   +1] - d0;
 d5 = f[k+nx+1] - d0;
 d6 = f[k+nx  ] - d0;

 /* 6 point interpolant: d0 + a x + b y + c/2 x^2 + d/2 y^2 + e xy
    syms  d1  d2  d3  d4  d5  d6
    x = [  1;  0; -1; -1; -1;  0];
    y = [  0; -1; -1;  0;  1;  1];
    A = [x y .5*x.^2 .5*y.^2 x.*y];
    A([1 2 3 4 6],:)\[d1;d2;d3;d4;d6] % = [a;b;c;d;e] with d3
    A([1 2 4 5 6],:)\[d1;d2;d4;d5;d6] % = [a;b;c;d;e] with d5
  */
 *a = (d4 - d1) * FLTC(0.5);
 *b = (d6 - d2) * FLTC(0.5);
 *c =  d1 + d4             ;
 *d =  d2 + d6             ;
 *e = (d4 + d2 - d3)       ;
 *h = (*c)*(*d) - (*e)*(*e);
 *h = *h == FLTC(0.0) ? FLTC(0.0) : FLTC(1.0) / *h;
 *xe = ((*e)*(*b) - (*a)*(*d)) * (*h); CLAMP(*xe)
 *ye = ((*e)*(*a) - (*b)*(*c)) * (*h); CLAMP(*ye)
 
 *fe = (FLTC(0.5)*(*c)*(*xe) +              (*a))*(*xe) + 
       (FLTC(0.5)*(*d)*(*ye) + (*e)*(*xe) + (*b))*(*ye) + d0;
}
