/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT1 sbfef1
# define FCT2 sbfef2
# define FCT3 sbfef3
# define FCT5 sbfef5
# define FCT6 sbfef6
# define FCT8 sbfef8
# else
# define FCT1 dbfef1
# define FCT2 dbfef2
# define FCT3 dbfef3
# define FCT5 dbfef5
# define FCT6 dbfef6
# define FCT8 dbfef8
# 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,
          int nP, int nT, int err) {
   int i,j,ny = nzy*nry;
   FLT   FAB = FA - FB;
   FLT  *TyP0 =  Tyg        ,  *TyP1 =  Tyg +        ny,  *TyP2 =  Tyg +  2    *ny,  *TyP3 =  Tyg +  3    *ny,
        *TyT0 =  Tyg + nP*ny,  *TyT1 =  Tyg + (1+nP)*ny,  *TyT2 =  Tyg + (2+nP)*ny,  *TyT3 =  Tyg + (3+nP)*ny,
        *TpP0 =  Tpg        ,  *TpP1 =  Tpg +  1       ,  *TpP2 =  Tpg +  2       ,  *TpP3 =  Tpg +  3       ,
        *TpT0 =  Tpg + nP   ,  *TpT1 =  Tpg +  1+nP    ,  *TpT2 =  Tpg +  2+nP    ,  *TpT3 =  Tpg +  3+nP    ,
       *ITpP0 = ITpg        , *ITpP1 = ITpg +  1       , *ITpP2 = ITpg +  2       , *ITpP3 = ITpg +  3       ,
       *ITpT0 = ITpg + nP   , *ITpT1 = ITpg +  1+nP    , *ITpT2 = ITpg +  2+nP    , *ITpT3 = ITpg +  3+nP    ,
       g0,g1,g2,g3,Ig0,Ig1,Ig2,Ig3,g;
   /* Initialise arrays */
   for (i=nP+nT;i--;)
     *Tpg++ = (*ITpg++ = 0);
   Tpg -= nP+nT; ITpg -= nP+nT;
   /* */
   Fx += nzy + 3;
   if (err) {
     for (i=ny; i--;)
       switch (nP) {
        case 4:  *TyP3++ = 0;
        case 3:  *TyP2++ = 0;
        case 2:  *TyP1++ = 0;
        case 1:  *TyP0++ = 0;
       }
       switch (nT) {
        case 4:  *TyT3++ = 0;
        case 3:  *TyT2++ = 0;
        case 2:  *TyT1++ = 0;
        case 1:  *TyT0++ = 0;
       }
   }
   else {
    for (i=nry; i--; ry++, iry++, Fx+=2)
     for (j=nzy; j--; Fx++, TyP0++, TyP1++, TyP2++, TyP3++, TyT0++, TyT1++, TyT2++, TyT3++)
      if (*Opy++) {
        g0  = FLTC(1.0);
        g1  = *Fx - FB ;
        g3  = g1 - FAB; /* Fx-FA */
        g2  = g1 * g3;
        g3  = g2 * (g1 + g3);
       Ig0  = g1;
       Ig3  = g1 * g1;
       Ig1  = 0.5 * Ig3;
       Ig2  = Ig1 * (0.66666666666666666666 * g1 -FAB);
       Ig3 *= 0.5 * g2 * g2;
       switch (nP) {
        case 4:  *TyP3  = g =  *ry *  g3;
                *ITpP3 +=      *ry * Ig3;
                 *TpP3 += g             ;
        case 3:  *TyP2  = g =  *ry *  g2;
                *ITpP2 +=      *ry * Ig2;
                 *TpP2 += g             ;
        case 2:  *TyP1  = g =  *ry *  g1;
                *ITpP1 +=      *ry * Ig1;
                 *TpP1 += g             ;
        case 1:  *TyP0  = g =  *ry *  g0;
                *ITpP0 +=      *ry * Ig0;
                 *TpP0 += g             ;
       }
       switch (nT) {
        case 4:  *TyT3  = g = *iry *  g3;
                *ITpT3 +=     *iry * Ig3;
                 *TpT3 += g             ;
        case 3:  *TyT2  = g = *iry *  g2;
                *ITpT2 +=     *iry * Ig2;
                 *TpT2 += g             ;
        case 2:  *TyT1  = g = *iry *  g1;
                *ITpT1 +=     *iry * Ig1;
                 *TpT1 += g             ;
        case 1:  *TyT0  = g = *iry *  g0;
                *ITpT0 +=     *iry * Ig0;
                 *TpT0 += g             ;
       }
      } else {
       switch (nP) {
        case 4:  *TyP3  = 0;
        case 3:  *TyP2  = 0;
        case 2:  *TyP1  = 0;
        case 1:  *TyP0  = 0;
       }
       switch (nT) {
        case 4:  *TyT3  = 0;
        case 3:  *TyT2  = 0;
        case 2:  *TyT1  = 0;
        case 1:  *TyT0  = 0;
       }
      }
   }
}

void FCT2(FLT *gQg, FLT *IgQg, FLT *FNQ, int nQ, int nP, int nT) {
   FLT  *gQP0 =  gQg        ,  *gQP1 =  gQg +        nQ,  *gQP2 =  gQg +  2    *nQ,  *gQP3 =  gQg +  3    *nQ,
        *gQT0 =  gQg + nP*nQ,  *gQT1 =  gQg + (1+nP)*nQ,  *gQT2 =  gQg + (2+nP)*nQ,  *gQT3 =  gQg + (3+nP)*nQ,
       *IgQP0 = IgQg        , *IgQP1 = IgQg +        nQ, *IgQP2 = IgQg +  2    *nQ, *IgQP3 = IgQg +  3    *nQ,
       *IgQT0 = IgQg + nP*nQ, *IgQT1 = IgQg + (1+nP)*nQ, *IgQT2 = IgQg + (2+nP)*nQ, *IgQT3 = IgQg + (3+nP)*nQ,
       g0,g1,g2,g3,Ig0,Ig1,Ig2,Ig3,g;
   int i;
   for (i=nQ; i--;) {
     g0  = FLTC(1.0);
     g2  = *FNQ++;
     g1  = g2 - 1.0;
     g3  = g2 * 2.0;
    Ig0  = g1;
    Ig1  = 0.5 * g1 * g1;
    Ig2  = Ig1 * 0.33333333333333333333 * (g3 + 1.0);
    Ig3  = Ig1 *g2 * g2;
     g2 *= g1;
     g3  = g2 * (g3 - 1.0);
     switch (nP) {
      case 4:  *gQP3++ =  g3;
              *IgQP3++ = Ig3;
      case 3:  *gQP2++ =  g2;
              *IgQP2++ = Ig2;
      case 2:  *gQP1++ =  g1;
              *IgQP1++ = Ig1;
      case 1:  *gQP0++ =  g0;
              *IgQP0++ = Ig0;
     }
     switch (nT) {
      case 4:  *gQT3++ =  g3;
              *IgQT3++ = Ig3;
      case 3:  *gQT2++ =  g2;
              *IgQT2++ = Ig2;
      case 2:  *gQT1++ =  g1;
              *IgQT1++ = Ig1;
      case 1:  *gQT0++ =  g0;
              *IgQT0++ = Ig0;
     }
   }
}

void FCT3(FLT *aPpg, FLT *aTTpg, FLT *aPg, FLT *ahqTg, FLT *ag, 
	  FLT FA, FLT FB, FLT ids, int nP, int nT) {
   FLT FBA1 = FB - FA,
       FBA2 = FBA1*FBA1, FBA3 = FBA2*FBA1;
   FLT cP   = ids*0.159154943091895 /* 1/2/pi */,
       cT   = ids*2e-7 /* mu0/2/pi */;
   switch (nP) {
    case 4:   aPg[3   ] = (  aPpg[3   ] = ag[3   ] * cP * FBA3 ) * FBA1;
    case 3:   aPg[2   ] = (  aPpg[2   ] = ag[2   ] * cP * FBA2 ) * FBA1;
    case 2:   aPg[1   ] = (  aPpg[1   ] = ag[1   ] * cP * FBA1 ) * FBA1;
    case 1:   aPg[0   ] = (  aPpg[0   ] = ag[0   ] * cP        ) * FBA1;
   }
   switch (nT) {
    case 4: ahqTg[3+nP] = ( aTTpg[3+nP] = ag[3+nP] * cT * FBA3 ) * FBA1;
    case 3: ahqTg[2+nP] = ( aTTpg[2+nP] = ag[2+nP] * cT * FBA2 ) * FBA1;
    case 2: ahqTg[1+nP] = ( aTTpg[1+nP] = ag[1+nP] * cT * FBA1 ) * FBA1;
    case 1: ahqTg[  nP] = ( aTTpg[  nP] = ag[  nP] * cT        ) * FBA1;
   }
}

void FCT5(FLT *gA, FLT *IgA, FLT FA, FLT FB, int nP, int nT) {
   FLT FAB1 = FA - FB,
       FAB2 = FAB1*FAB1, FAB3 = FAB2*FAB1;
   switch (nP) {
    case 4: gA[   3] = FLTC(0.0); IgA[   3] =               FLTC(0.0);
    case 3: gA[   2] = FLTC(0.0); IgA[   2] = -0.166666666666666*FAB3;
    case 2: gA[   1] =      FAB1; IgA[   1] =  0.500000000000000*FAB2;
    case 1: gA[   0] = FLTC(1.0); IgA[   0] =                    FAB1;
   }
   switch (nT) {
    case 4: gA[3+nP] = FLTC(0.0); IgA[3+nP] =               FLTC(0.0);
    case 3: gA[2+nP] = FLTC(0.0); IgA[2+nP] = -0.166666666666666*FAB3;
    case 2: gA[1+nP] =      FAB1; IgA[1+nP] =  0.500000000000000*FAB2;
    case 1: gA[  nP] = FLTC(1.0); IgA[  nP] =                    FAB1;
   }
}

void FCT6(FLT *Qqg, FLT *Xq, FLT FA, FLT FB, FLT rA, FLT irA, FLT ids, int nP, int nT, int nq) {
   int nPq = nP>2 ? nP-2 : 0, nTq = nT>2 ? nT-2 : 0;
   int i;
   /* int nq = nPq + nTq + 2*((nP+nT)>0); */
   FLT FBA = FB-FA, FBA2 = FBA*FBA, FBA3 = FBA2*FBA;
   /* Arrays init */
   for (i = nq*(nP+nT);i--;)
     *Qqg++ = 0;
   for (i = nq;i--;)
     *Xq++ = 0;
   Xq -= nq; Qqg -= nq*(nP+nT);
   /* */
   switch (nP) { case 4: Qqg[     3*nq+    1] = 3.464101615137754*FBA3*rA*ids; /* sqrt(12) */
                 case 3: Qqg[     2*nq      ] = 2.000000000000000*FBA2*rA*ids;
                 case 2:
                 case 1: Qqg[           nq-2] = rA*ids;
                         Qqg[           nq-1] = rA*ids;
   }
   switch (nT) { case 4: Qqg[(nP+3)*nq+nPq+1] = 3.464101615137754*FBA3*irA*ids;
                 case 3: Qqg[(nP+2)*nq+nPq  ] = 2.000000000000000*FBA2*irA*ids;
                 case 2:
                 case 1: Qqg[(nP  )*nq+ nq-2] =  irA*ids;
                         Qqg[(nP  )*nq+ nq-1] = -irA*ids;
   }
   /* See bfef.m for help on last 2 reg. constraints */
}

void FCT8(FLT *Bty, FLT *Fx, FLT FA, FLT FB, int8_t *Opy,
          FLT *ag, FLT rBt, int nP, int nT, FLT ids, FLT *iry, int nzy, int nry) {
}

