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

# ifdef SINGLE
# define FCT sipmh
# define FCOPY   cblas_scopy
# define FSPMV   cblas_sspmv
# define FGEMV   cblas_sgemv
# define IPM1    sipm1
# define ABS fabsf
# else
# define FCT dipmh
# define FCOPY   cblas_dcopy
# define FSPMV   cblas_dspmv
# define FGEMV   cblas_dgemv
# define IPM1    dipm1
# define ABS fabs
# endif

# define COPY(n,y,x)     FCOPY(n,x,1,y,1)                                    /* y = x */
# define SET(n,y,a)      FCOPY(n,&a,0,y,1)                                   /* y = a */
# define SPMV(n,u,x,y)   FSPMV(CblasColMajor,CblasUpper,n,1.0,u,x,1,1.0,y,1) /* y = U*X+y */ 
# 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,Jh,st] = ipmh(Ahd,Aed,Ahe,Aeh,Ahh,uAhh,Yd,Ie0,Jh0,sIp,nit,tol) */
void FCT(FLT *Ie, FLT *Jh, bool *st, FLT *Ahd, FLT *Aed, FLT *Ahe, FLT *Aeh, FLT *Ahh, FLT *uAhh,
         FLT *Yd, FLT *Ie0, FLT *Jh0, FLT sIp, int nit, FLT tol, FLT *w, int nh, int ne, int nd)
{
 FLT *Gh1=w, *Ie1=Gh1+nh, *Gh=Ie1+ne, *Jh1=Gh+nh, *z=Jh1+nh, *w1=z+nh; /* w size nh*(nh+1)/2+8*nh+ne */
 FLT t,t0;
 int k,kit;

 /* Gh1 = zeros(nh,1); Gh1 = Gh1 + Ahd*Yd */
 memset((void *)Gh1,0,nh*sizeof(FLT));
 GEMV(Gh1,Ahd,Yd,nh,nd);
 /* Ie1 = zeros(ne,1); Ie1 = Aed*Yd */
 memset((void *)Ie1,0,ne*sizeof(FLT));
 GEMV(Ie1,Aed,Yd,ne,nd);
 /* Gh = Gh1; Gh  = Gh + Ahe*Ie0; */
 COPY(nh,Gh,Gh1);
 GEMV(Gh,Ahe,Ie0,nh,ne);
 /* Gh1 = Gh1 + Ahe*Ie1; */
 GEMV(Gh1,Ahe,Ie1,nh,ne);
 /* Jh1 = Jh0 */
 COPY(nh,Jh1,Jh0);
 /* z = repmat(sIp,nh,1) */
 SET(nh,z,sIp);
 
 /* for kit=nit:-1:1 */
 for (kit=nit; kit--;) {
  /* Do a single IPM iteration */
  /* [Jh,z,st] = ipm1mex(uAhh,Gh,sIp,Jh0,z,tol,nit); */
  IPM1(Jh,z,st,uAhh,Gh,sIp,Jh1,z,tol*1e6,1,w1,nh);
  /* if norm(Jh1-Jh,Inf) < tol, break, end */
  t = FLTC(0.0);
  for (k=nh; k--;) {
   t0 = ABS(Jh1[k]-Jh[k]);
   if (t0 > t) t = t0;
  }
  *st = *st & (t < tol);
  if (*st) {break;}
  /* Gh = Gh1 + Ahh*Jh; */
  COPY(nh,Gh,Gh1);
  GEMV(Gh,Ahh,Jh,nh,nh);
  /* Jh1 = Jh; */
  COPY(nh,Jh1,Jh);
 }
 /* Ie = Ie1 + Aeh*Jh0; */
 COPY(ne,Ie,Ie1);
 GEMV(Ie,Aeh,Jh,ne,nh);
}
