/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT sfsgi
# else
# define FCT dfsgi
# endif
# define HIPI  FLTC( 0.159154943091895)

/*  [Q0Q,Q1Q,Q2Q,Q3Q,Q4Q,iqQ] = fsgi(M1q,M2q,rq,irq,rA,FA,FB,BA,lX,rB,iTQ,idoq) */
void FCT(FLT *Q0Q, FLT *Q1Q, FLT *Q2Q, FLT *Q3Q, FLT *Q4Q, FLT *iqQ,
         FLT *M1q, FLT *M2q, FLT *rq, FLT*irq,
         FLT rA, FLT FA, FLT FB, FLT BA, bool lX, FLT rB, FLT *iTQ,
         FLT idoq, int npq, int noq) {
 int i, j, nqX = npq - lX;
 FLT *pQ0Q = Q0Q, *pQ1Q = Q1Q, *pQ2Q = Q2Q, *pQ3Q = Q3Q, *pQ4Q = Q4Q, *piqQ = iqQ,
     *pM1q = M1q, *pM2q = M2q, *prq = rq, *pirq = irq, *piTQ = iTQ, vM1q, vM2q, vrq, virq;
 FLT s0, s1, s2, s3, s4;
 FLT irA = rA ? FLTC(1.0) / rA : FLTC(0.0),
     irB,
     FAB = FLTC(4.0) * (FA - FB),
     CA = HIPI * BA,
     Cq = FAB * idoq,
     C1 = HIPI * Cq,
     C3 = FAB * FAB;
 /* axis */
 *pQ0Q++ = irA;
 *pQ1Q++ = HIPI * CA * irA;
 *pQ2Q++ = irA * irA;
 *pQ3Q++ = FLTC(0.0);
 *pQ4Q++ = FLTC(0.0);
 *piqQ++ = CA * rA * *piTQ++;
 
 for (j = nqX; j--; ) {
  
  /* theta integrals */
  s0 = s1 = s2 = s3 = s4 = 0.0;
  for (i = noq; i--; ) {
   vM1q = *pM1q++;
   vM2q = *pM2q++;
   vrq  = *prq++ ;
   virq = *pirq++;
   s0 += vM1q       ; /* sum(M1q     ) */       
   s1 += vM1q * vrq ; /* sum(M1q.* rq) */
   s2 += vM1q * virq; /* sum(M1q.*irq) */
   s3 += vM2q * virq; /* sum(M2q.*irq) */
   s4 += vM2q * vrq ; /* sum(M2q.* rq) */
  }
  s1 = 1.0 / s1;
  
  *pQ0Q++ =      s1 * s0     ;
  *pQ1Q++ = C1 * s1          ;
  *pQ2Q++ =      s1 * s2     ;
  *pQ3Q++ = C3 * s1 * s3     ;
  *pQ4Q++ = C3 * s1 * s4     ;
  *piqQ++ = Cq / s2 * *piTQ++;
  
  /* X point */
  if (lX) {
   irB = rB ? FLTC(1.0) / rB : FLTC(0.0);
   *pQ0Q = irB;
   *pQ1Q = FLTC(0.0);
   *pQ2Q = irB * irB;
   *pQ3Q = FLTC(0.0);
   *pQ4Q = FLTC(0.0);
   *piqQ = FLTC(0.0);
  }
 }
}
