/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT1 sbf3i1
# define FCT2 sbf3i2
# define FCT3 sbf3i3
# define FCT4 sbf3i4
# define FCT5 sbf3i5
# define FCT6 sbf3i6
# else
# define FCT1 dbf3i1
# define FCT2 dbf3i2
# define FCT3 dbf3i3
# define FCT4 dbf3i4
# define FCT5 dbf3i5
# define FCT6 dbf3i6
# endif

void FCT1(FLT *Tyg, FLT *Tpg, FLT *ITpg, FLT *Fx, FLT FA, FLT FB, int8_t *Opy,
          FLT *ry, FLT *iry, int nzy, int nry,
          FLT *gNg, FLT *IgNg, FLT *fPg, FLT *fTg, int err, int nN) {
 int i, j, ny=nzy*nry, nN2=nN-2;
 unsigned int k,k1;
 FLT *t0=Tyg, *t1=t0+ny, *t2=t1+ny, pr0, pr1, pr2, f, f1, v, idF,
     *gN1=gNg+nN, *gN2=gN1+nN, *IgN1=IgNg+nN, *IgN2=IgN1+nN;
 Fx += nzy + 3; 
 Tpg[0] = Tpg[1] = Tpg[2] = ITpg[0] = ITpg[1] = ITpg[2] = FLTC(0.0);
 if (err) for (i=ny; i--;) *t0++ = *t1++ = *t2++ = FLTC(0.0);
 else {
  FB -= FA;
  idF = (nN - 1) / FB;
  for (i=nry; i--; Fx+=2)
  {
   pr0 = fPg[0]*(*ry  ) + fTg[0]*(*iry  );
   pr1 = fPg[1]*(*ry  ) + fTg[1]*(*iry  );
   pr2 = fPg[2]*(*ry++) + fTg[2]*(*iry++);
   for (j=nzy; j--; Fx++)
    if (*Opy++) {
     f = (*Fx - FA) * idF;
     k = f; if (k > nN2) k = nN2; k1 = k+1;
     f = f - k; f1 = FLTC(1.0) - f;
      Tpg[0] += (v = ( gNg[k]*f1 +  gNg[k1]*f) * pr0); *t0++ = v;
      Tpg[1] += (v = ( gN1[k]*f1 +  gN1[k1]*f) * pr1); *t1++ = v;
      Tpg[2] += (v = ( gN2[k]*f1 +  gN2[k1]*f) * pr2); *t2++ = v;
     ITpg[0] +=      (IgNg[k]*f1 + IgNg[k1]*f) * pr0 * FB;
     ITpg[1] +=      (IgN1[k]*f1 + IgN1[k1]*f) * pr1 * FB;
     ITpg[2] +=      (IgN2[k]*f1 + IgN2[k1]*f) * pr2 * FB;
    } else *t0++ = *t1++ = *t2++ = FLTC(0.0);
  }
 }
}

void FCT2(FLT *gQg, FLT *IgQg, FLT *FNQ, int nQ,
          FLT *gNg, FLT *IgNg, int nN) {
 int i, nN2=nN-2;
 unsigned int k,k1;
 FLT *t0=gQg, *t1=t0+nQ, *t2=t1+nQ, *T0=IgQg, *T1=T0+nQ, *T2=T1+nQ,  f, f1, idF=nN-1,
  *gN1=gNg+nN, *gN2=gN1+nN, *IgN1=IgNg+nN, *IgN2=IgN1+nN;
 for (i=nQ; i--;) {
  f = *FNQ++ * idF;
  k = f; if (k > nN2) k = nN2; k1 = k+1;
  f = f - k; f1 = FLTC(1.0) - f;
  *t0++ =  gNg[k]*f1 +  gNg[k1]*f;
  *t1++ =  gN1[k]*f1 +  gN1[k1]*f;
  *t2++ =  gN2[k]*f1 +  gN2[k1]*f;
  *T0++ = IgNg[k]*f1 + IgNg[k1]*f;
  *T1++ = IgN1[k]*f1 + IgN1[k1]*f;
  *T2++ = IgN2[k]*f1 + IgN2[k1]*f;
 }
}

void FCT3(FLT *aPpg, FLT *aTTpg, FLT *aPg, FLT *ahqTg, FLT *ag, FLT FA, FLT FB, FLT *fPg, FLT *fTg, FLT ids) {
 FLT cP=ids*FLTC(0.159154943091895 /* 1/2/pi */), cT=ids*FLTC(2e-7 /* mu0/2/pi */);
 FB -= FA;
   aPg[0] = (  aPpg[0] = ag[0] * fPg[0] * cP ) * FB;
   aPg[1] = (  aPpg[1] = ag[1] * fPg[1] * cP ) * FB;
   aPg[2] = (  aPpg[2] = ag[2] * fPg[2] * cP ) * FB;
 ahqTg[0] = ( aTTpg[0] = ag[0] * fTg[0] * cT ) * FB;
 ahqTg[1] = ( aTTpg[1] = ag[1] * fTg[1] * cT ) * FB;
 ahqTg[2] = ( aTTpg[2] = ag[2] * fTg[2] * cT ) * FB;
}

void FCT4(FLT *Tdg, FLT *Tgy, FLT *Fx, FLT FA, FLT FB, int8_t *Opy, int nzy, int nry,
          unsigned int *kd, FLT *fd, int nd, FLT *gNg, int nN, int ng) {
 int i, j, l, nN2=nN-2;
 unsigned int k, k1, *pkd;
 FLT *w1, *w2, f, f1, idF, *pg, *pf1, *pf2;
 Fx += nzy + 3; 
 for (i=nry*ng, w1=Tgy; i--;) *w1++ = FLTC(0.0);
 FB -= FA;
 idF = (nN - 1) / FB;
 for (i=nry, w1=Tgy; i--; Fx+=2, w1+=ng)
  for (j=nzy; j--; Fx++)
   if (*Opy++) {
    f = (*Fx - FA) * idF;
    k = f; if (k > nN2) k = nN2; k1 = k+1;
    f = f - k; f1 = FLTC(1.0) - f;
    for (l=ng, pg=gNg, w2=w1; l--; pg+=nN) *w2++ += pg[k]*f1 + pg[k1]*f;
   }
 for (i=ng, w1=Tgy, w2=Tgy+ng; i--; w1++, w2++)
  for (j=nd, pkd=kd, pf1=fd, pf2=fd+nd; j--;) {
   l = *pkd++ * ng;
   *Tdg++ = w1[l] * *pf1++ + w2[l] * *pf2++;
  }
}

void FCT5(FLT *gNg, FLT *IgNg, FLT FA, FLT FB, int ng, int nN,
          FLT *gA, FLT *IgA) { 
 for (int ii=ng; ii--;){
   gA[ii]  = gNg[ii*nN];
  IgA[ii] = IgNg[ii*nN]*(FB-FA);
 }
}

void FCT6(FLT *Qg, FLT *Xq) {
  Qg[0] = Qg[1] = Qg[2] = Xq[0] = FLTC(0.0);
}
