%IPM2MEX  Interior point method for AX>B and no equality constraints
%
% This is the MATLAB equivalent implementation of libmeq/ipm2.c.
% A more detailed help is available in IPM2MEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [x,s,z,stat] = ipm2mexm(uH,c,A,b,x0,s0,z0,tol,nit)
 ni = size(A ,1);
 x = x0;
 s = s0;
 z = z0;
 n = size(c,1);
 H = zeros(n);
 ku = triu(true(n,n));
 H(ku) = uH;
 H = H + triu(H,1)';
 for kt = 1:nit
  if any(s == 0), stat = false; return, end
  d = z./s;
  if any(~isfinite(d)), stat = false; return, end
  [U,p] = chol(H + A'*diag(d)*A);
  if p>0, stat = false; return, end
  rd = H*x + c - A'*z;
  rp = b - A*x;
  % search direction
  xp = d.*rp;
  dx = A'*xp - rd; % xd = A'*xp - rd;
  dx = U\(U'\dx);  % dx = U\(U'\xd);
  dz = A*dx - rp;  % dy = rp - A*dx;
  ds = s - dz;     % ds = -s - dy;
  k = (ds > 0); alphas = min([1 ; s(k)./ds(k)]);
  k = (dz > 0); alphaz = min([1 ; s(k)./dz(k)]);
  alpha = min(alphas,alphaz);
  dz = d.*dz; % dz = d.*dy;
  tau = (s - alpha*ds)'*(z - alpha*dz); % Since r0 and r1 can be both very small,
  tau = (tau/(s'*z)).^2*(tau/ni);       % this is more precise and less prone to NaNs
  % centering
  ec = (tau - ds.*dz)./z;
  xp = xp + d.*ec;
  dx = A'*xp - rd;     % xd = A'*xp - rd;
  dx = U\(U'\dx);      % dx = U\(U'\xd);
  dz = A*dx - rp - ec; % dy = rp + ec - A*dx;
  ds = s - dz - ec;    % ds = -s + ec - dy;
  k = (ds > 0); alphas = min([1 ; s(k)./ds(k)]);
  k = (dz > 0); alphaz = min([1 ; s(k)./dz(k)]);
  alpha = 0.99*min(alphas,alphaz);
  % update
  dz = d.*dz; % dz = d.*dy;
  x = x + alpha*dx;
  s = s - alpha*ds;
  z = z - alpha*dz;
  stat = norm([rd;s+rp],Inf) < tol;
  if stat, break, end
 end
 
end
