/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"

# ifdef SINGLE
# define FCT     sipmj
# define FCOPY   cblas_scopy
# define FGEMV   cblas_sgemv
# define IPM2    sipm2
# define ABS     fabsf
# else
# define FCT     dipmj
# define FCOPY   cblas_dcopy
# define FGEMV   cblas_dgemv
# define IPM2    dipm2
# define ABS     fabs
# endif

# define COPY(n,y,x)     FCOPY(n,x,1,y,1)                                    /* y = x */
# define GEMV(y,A,x,m,n) FGEMV(CblasColMajor,CblasNoTrans,m,n,FLTC(1.0),A,m,x,1,FLTC(1.0),y,1) /* y = A*x + y */

/* [Ie,aj,st] = ipmj(uAjj,Aje,Aej,Ie0,aj0,Ie_,aj_,Qcj,Xc,s,z,tol,iters) */
void FCT(FLT *Ie, FLT *aj, bool *st, FLT *uAjj, FLT *Aje, FLT *Aej, FLT *Ie0, FLT *aj0, FLT *Ie_,
         FLT *aj_, FLT *Qcj, FLT *Xc, FLT *s, FLT *z, FLT tol, int iters, FLT *w, int nj, int ne, int nc)
{
 FLT *aj1=w, *aj2=aj1+nj, *w1=aj2+nj; /* w size  nj*(nj+1)/2+4*nj+6*nc */
 FLT t,t0;
 int k,kit;

 /* Ie = Ie_; */
 COPY(ne,Ie,Ie_);
 /* aj1 = aj_; */
 COPY(nj,aj1,aj_);
 /* for kit=iters:-1:1 */
 for (kit=iters; kit--;) {
  /* aj2 = aj0 + Aje*Ie; */
  COPY(nj,aj2,aj0);
  GEMV(aj2,Aje,Ie,nj,ne);
  /* [aj,s,z,st] = ipm2mex(uAjj,aj2,Qcj,Xc,aj1,s,z,tol,iterj); */
  IPM2(aj,s,z,st,uAjj,aj2,Qcj,Xc,aj1,s,z,tol*1e6,1,w1,nj,nc);
  /* Ie = Ie0 + Aej*aj; */
  COPY(ne,Ie,Ie0);
  GEMV(Ie,Aej,aj,ne,nj);
  /* if norm(aj-aj1,Inf) < tol, break, end */
  t = FLTC(0.0);
  for (k=nj; k--;) {
   t0 = ABS(aj[k]-aj1[k]);
   if (t0 > t) t = t0;
  }
  *st = *st && (t < tol);
  if (*st) break;
  /* aj1 = aj; */
  COPY(nj,aj1,aj);
 }
}
