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

void FCT(FLT *aR, FLT *aQ, FLT * qQ, FLT *qR, FLT NF, int nQ, int nO, int nR, int naR) {
  /* Q-surface finder
   * Inputs:  aQ, size: [nO,nQ]
   *          qQ, size: [nQ]
   *          qR, size: [nR]
   * Outputs: aR, size: [nO,nR,naR] */
  
  FLT fac;
  int i, j, k, l, ic;
  
  /* default value for non-found points */
  for (i=0; i<nO*nR*naR; i++)
    aR[i] = NF;
    
  for (i=0; i < nR; i++, qR++) {
    ic = 0;
    for (j=nQ-1; j > 0; j--) {
      if (ic==naR) break; // found as many qR as required
      if (((*qR - qQ[j]) * (*qR - qQ[j-1])) < 0) {
        /* sign change - interpolate linearly */
        fac = (*qR - qQ[j])/(qQ[j] - qQ[j-1]);
        for (k=0; k < nO; k++) {
          l = k + (i + ic*nR)*nO; /* [k,i,ic] */
          aR[l] = aQ[k+j*nO] + (aQ[k+j*nO] - aQ[k+(j-1)*nO]) * fac;
        }
        # ifdef DEBUG
        printf("naR = %d, ic=%d. i=%d j=%d\n",naR,ic,i,j);
        printf("Found qQ=%2.2f intersection %d. fac=%2.2f\n",*qR,ic+1,fac);
        # endif
        ic++;
      }
      else if (*qR == qQ[j]){ // this point
        for (k=0; k < nO; k++) {
          l = k + (i + ic*nR)*nO; /* [k,i,ic] */
          aR[l] = aQ[k+j*nO];
        }
        # ifdef DEBUG
        printf("Found equal point qQ=%2.2f intersection %d. fac=%2.2f\n",*qR,ic+1,0.0);
        # endif
        ic++;
      }
      else if ((j==1) && (*qR == qQ[0])){ // last point
        for (k=0; k < nO; k++) {
          l = k + (i + ic*nR)*nO; /* [k,i,ic] */
          aR[l] = aQ[k + 0*nO];
        }
        # ifdef DEBUG
        printf("Found final point qQ=%2.2f intersection %d. fac=%2.2f\n",*qR,ic+1,1.0);
        # endif
        ic++;
      }
    }
  }
}
