/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT   sminQ
# else
# define FCT   dminQ
# endif
# define INV(x) ( x ? FLTC(1.0) / (x) : FLTC(0.0) )

void FCT(FLT *aqmin, FLT *qmin, FLT * aQ, FLT *qQ, FLT sq, FLT NF, int nQ, int nmin) {
  /* minQ-surface finder */
  
  bool ismin = (sq > 0);
  int i, ic;
  FLT *aqmin_ = aqmin, *qmin_ = qmin;
  FLT p1, p2, iD, xmin, ymin, x12, x32, y12, y32;
  
  if (nmin) {
    /* default value for non-found points */
    for (i=nmin; i--;) {
      *aqmin_++ = NF;
      *qmin_++ = NF;
    }
    aqmin_=aqmin;
    qmin_=qmin;
    
    ic = 0;

    /* outer point */
    aQ+=nQ-1;
    qQ+=nQ-1;
    if (( ismin && (*qQ - *(qQ-1))<0) ||
        (!ismin && (*qQ - *(qQ-1))>0)) {
      *aqmin_++ = *aQ;
      *qmin_++ = *qQ;
      if (nmin==1)
        return;
      else
        ic++;
    }

    /* inner points */
    aQ--; qQ--;
    for (i=nQ-1;i-- > 1; aQ--, qQ-- ) {
      if (( ismin && ((*(qQ+1) - *qQ) > 0) && ((*qQ - *(qQ-1)) <= 0)) ||
          (!ismin && ((*(qQ+1) - *qQ) < 0) && ((*qQ - *(qQ-1)) >= 0))) { /* derivative sign change */
        /* quadratic fit */
        x32 = *( aQ+1) - *aQ;
        x12 = *( aQ-1) - *aQ;
        y32 = *(qQ+1) - *qQ;
        y12 = *(qQ-1) - *qQ;
      
        /* fast way, explicit inverse of 2x2 matrix */
        iD = INV(x32*x12*(x32-x12));
        p1 = ( x12*y32     - x32*y12     )*iD;
        p2 = ( x32*x32*y12 - x12*x12*y32 )*iD;
      
        /* (y-y0) = (p(1)(x-x0)^2 + p(2)(x-x0))*iD */
        /* with x0=aQ(kQ), y(0)=qQ(kQ); */
      
        xmin = FLTC(-0.5)*p2/p1;
        ymin = FLTC( 0.5)*xmin*p2;
      
        *aqmin_++ = *aQ + xmin;
        *qmin_++ = *qQ + ymin;
        ic++;
        if (ic==nmin) return;
      }
    }

    /* if still counting, then first point is a minimum */
    *aqmin_++ = *aQ;
    *qmin_++ = *qQ;
  }
}

