/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT1 sbf3p1
# define FCT2 sbf3p2
# define FCT3 sbf3p3
# define FCT5 sbf3p5
# define FCT6 sbf3p6
# define FCT8 sbf3p8
# else
# define FCT1 dbf3p1
# define FCT2 dbf3p2
# define FCT3 dbf3p3
# define FCT5 dbf3p5
# define FCT6 dbf3p6
# define FCT8 dbf3p8
# 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,
          bool circ, int err) {
 int i, j, ny=nzy*nry;
 FLT *t0=Tyg, *t1=t0+ny, *t2=t1+ny, vr, vir, vF, v0, v1, v2;
 Fx += nzy + 3;
 Tpg[0] = Tpg[1] = Tpg[2] = ITpg[0] = ITpg[1] = ITpg[2] = FLTC(0.0);
 FA -= FB;
 if (err) for (i=ny; i--;) *t0++ = *t1++ = *t2++ = FLTC(0.0);
 else if (circ) {
  for (i=nry; i--; iry++, Fx+=2)
   for (j=nzy; j--; Fx++) {
    *t0++ = *t2++ = FLTC(0.0);
    if (*Opy++) { Tpg[1] += (v1 = (vF =*Fx - FB) * *iry); *t1++ = v1; ITpg[1] += FLTC(0.5) * vF * v1; }
    else         *t1++ = FLTC(0.0);
   }
 } else
  for (i=nry; i--; ry++, iry++, Fx+=2)
   for (j=nzy; j--; Fx++)
    if (*Opy++) {
     Tpg[0] += (v0 = (vF = *Fx - FB) * *ry); *t0++ = v0;
     Tpg[1] += (v1 = vF * *iry)            ; *t1++ = v1; 
     Tpg[2] += (v2 = (vF - FA) * v1)       ; *t2++ = v2;
     ITpg[0] += ((v1 = FLTC(0.5) * vF * vF) * *ry);
     ITpg[1] += (v1 *= *iry);
     ITpg[2] += (FLTC(0.66666666666666666666) * vF - FA) * v1;
    } else *t0++ = *t1++ = *t2++ = FLTC(0.0);
}

void FCT2(FLT *gQg, FLT *IgQg, FLT *FNQ, int nQ) {
 int i;
 FLT *t0=gQg, *t1=t0+nQ, *t2=t1+nQ, *T0=IgQg, *T1=T0+nQ, *T2=T1+nQ, vF, v0;
 for (i=nQ; i--;) {
  *t0++ = *t1++ = v0 = (vF = *FNQ++) - FLTC(1.0);
  *t2++ = v0 * vF;
  *T0++ = *T1++ = (v0 = FLTC(0.5) * v0 * v0);
  *T2++ = ( vF + FLTC(0.5) ) * FLTC(0.66666666666666666666) * v0;
 }
}

void FCT3(FLT *aPpg, FLT *aTTpg, FLT *aPg, FLT *ahqTg, FLT *ag, FLT FA, FLT FB, 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] * FB *      cP) * FB;
 ahqTg[1] = ( aTTpg[1] = ag[1] * FB *      cT) * FB;
 ahqTg[2] = ( aTTpg[2] = ag[2] * FB * FB * cT) * FB;
  aPpg[1] = aPpg[2] = aTTpg[0] = aPg[1] = aPg[2] = ahqTg[0] = FLTC(0.0);  
}

void FCT5(FLT *gA, FLT *IgA, FLT FA, FLT FB, bool circ) {
 FLT FAB = FA-FB, FAB2=FLTC(0.5)*FAB*FAB;
 gA[1] = FAB; IgA[1] = FAB2; gA[2] = FLTC(0.0);
 if (circ) {
  gA[0] =      IgA[0] = IgA[2] = FLTC(0.0);
 } else {
  gA[0] = FAB; IgA[0] = FAB2;
               IgA[2] = FLTC(-0.333333333333333333)*FAB2*FAB;
 }
}

void FCT6(FLT *Qg, FLT *Xq, FLT FA, FLT FB, FLT irA, FLT ids) {
 FB -= FA;
 Qg[0] = Qg[1] = Xq[0] = FLTC(0.0);
 Qg[2] = FLTC(2.0)*FB*FB*irA*ids;
}

void FCT8(FLT *Bty, FLT *Fx, FLT FA, FLT FB, int8_t *Opy,
          FLT *ag, FLT rBt, int err, FLT ids, FLT *iry, int nzy, int nry) {
 int i, j;
 FLT v1, a1, a2;
 ids *= FLTC(1e-7 /* mu0/4/pi */)/rBt;
 FA -= FB;
 a2  = ag[2]*ids;
 a1  = ag[1]*ids - FA*a2;
 a2 *= FLTC(0.66666666666666666666);
 Fx += nzy + 3;
 if (err) for (i=nzy*nry; i--;) *Bty++ = FLTC(0.0);
 else
  for (i=nry; i--; iry++, Fx+=2)
   for (j=nzy; j--; Fx++)
    if (*Opy++) {
     v1 = *Fx - FB;
     *Bty++ = ((a2*v1 + a1)*v1*v1 + rBt) * *iry;
    } else *Bty++ = rBt * *iry;
}

