function [ x,y,s,out ] = mehrotramsocp( A,b,c,K,options )
%MEHROTRAMSOCP test version
% Mehrotra predictor corrector method for Mixed Second Order Cone Programs.
% Using dense arithmetic, i.e. sparsity is not exploited.
% 
% min c^T x | A * x = b, x \in K
% 
% where K is a structure such that 
%
%       K.l \ge 0 is the number of nonnegative variables and
%       K.q is a vector with the dimensions (\ge 2 each) of the SO-cones,
%       if there are no nonnegative variables, K.l is 0  or undefined,
%       if there are no SO-cones,              K.q is [] or undefined.
%
% and x \in K iff 
%     x(1:K.l) \ge 0,
%     || x(K.l+2:K.l+K.q(1)) || <= x(K.l+1),
%     followed by the SO-cones of dimensions K.q(2) ... K.q(end).
%
% Mandtory input:
%   A, b, c of dimensions conforming with above problem formulation.
% Optional input:
%   K,       (default is all nonnegative variables)
%   options, (a structure with 
%      options.tol,   some relative stopping tolerance (default 1.0e-10)
%      options.maxit, stopping the algorithm with an error message after
%                     maxit iterations)
%      options.p_l,   where p_l (printlevel) > 0 for debugging purposes
%
% NOTE: The input format of sedumi (Jos Sturm) is similar but allows 
%       much more general data!
%
% Output:
%   x,   approximate primal solution 
%   y,s, approximate dual solution (ideally A'*y+s=c, s \in K, x'*s=0)
%   out, structure with fields
%        mu:   final value of mu
%        done: stopping information
%             1 normal termination
%             2 ill-conditioning of A*W*A'
%             3 short steps in interior-point algorithm
%             4 back solve failed (highly inaccurate)
%             5 iteration limit reached
%             6 trivial solution x = 0, optimal if there is an opt. sol.
%        residual: final primal-dual residual (for the normalized problem)
%
% This is an experimental code without any guarantees - 
% - free for use at your own risk.
%
% Florian Jarre, March 17, 2016
%



% Complete the input:
[m,n] = size(A);
if (nargin < 4)
   K.l = n;
end
if ~isfield(K,'l')
   K.l = 0;
end
if ~isfield(K,'q')
   K.q = [];
end
if (nargin < 5)
   options.tol   = 1.0e-10;
   options.maxit = 300; 
else
   if ~isfield(options,'tol')
      options.tol   = 1.0e-10;
   end
   if ~isfield(options,'maxit')
      options.maxit = 300;
   end
end

dummy = K.l + sum(K.q);
if dummy ~= n; error(' dimensions of K and of A do not match '); end;
pS = length(K.q); % number of second order cones.
pK = pS + K.l;    % overall number of cones, scalar product of e^Te.

eee        = zeros(n,1); % the central element ``e''
eee(1:K.l) = 1;
i          = K.l;
for j = 1:pS
   eee(i+1)=1; i=i+K.q(j);
end


% Preprocessing and initialization:
done = 0; % not done yet
recover_y = 0; % y does not need to be recovered separately
AAt   = A*A.';
[R,p] = chol(AAt);  % needs minimum degree ordering for sparse problems
if p > 0; 
   disp(' AAt not positive definite in mehrotramsocp'); 
   if norm(A,'fro') == 0
      if norm(b) > 0
         disp('inconsistent linear equations in mehrotramsocp')
         done = 7;
         residual = 1;
      else
         done = 6; % A = 0 and b = 0
         residual = 0;
      end
   end
% Orthogonalize the rows of A and eliminate linearly dependent rows
   tolA = eps^.75; % tolerance for estimating singularity of A
   [~,R,E] = qr(A.',0);
   rank_est = sum(abs(diag(R))>tolA*max(abs(diag(R))));
   if rank_est < m
      if norm(A*(A\b)-b) > tolA*norm(b)
         disp('linear equations in mehrotramsocp seem to be inconsistent')
         residual = 1; 
         done = 8;
      end
   end
   Aold = A; mold = m;
   recover_y = 1; % recompute y of correct dimension at the end
   A = A(E(1:rank_est),:);
   b = b(E(1:rank_est)); 
   m = rank_est;
   A = (R(1:rank_est,1:rank_est).')\A; % orthonormalize
   b = (R(1:rank_est,1:rank_est).')\b;
   R = eye(m);
end;
clear AAt;

A = R.'\A; %for stability make (nearly) orthonormal rows (dense case only!)
b         = R.'\b;
bnorm_old = norm(b);
dyold     = A*c;           % remember A'*dyold is subtracted from c below
c         = c - A.'*dyold; % change c to lie in the null space of A
R_old     = R;             % remember R (to rescale y in the end)
cnorm_old = norm(c);

if bnorm_old == 0
   done = 6;
   bnorm_old = 1;
   residual = 1; % since the dual solution might not be feasible
end
if cnorm_old == 0
   c = eee;
   cnorm_old = norm(c);
   if options.p_l > 0
      disp('zero objective function in mehrotramsocp changed to e^T*x');
   end
end

b = b / bnorm_old;
c = c / cnorm_old;
nb1 = norm(b);
nc1 = norm(c);

x0        = eee;           x  = x0;
y0        = zeros(m,1);    y  = y0;
s0        = x0;            s  = s0;
mu0       = x.'*s/pK;      mu = mu0;
res0      = norm(A*x-b);
res1      = norm(A.'*y+s-c);
quot0     = 10.e-2*mu/(res0+1);
quot1     = 1.0e-2*mu/(res1+1);
iter      = 0;
%initialit = ceil(log(n)/log(10)); % first some plain centering
initialit = 40;  



% Main loop:
while done == 0
   iter = iter+1;
       
   %str_g = sprintf('mu = %0.5g',mu); disp(str_g)
   
% Compute the scaling point of x and s
   w   = zeros(n,1);
   eta = zeros(n,1);
   if K.l > 0 
      if min(min(x(1:K.l)),min(s(1:K.l))) <= 0 
         disp(' iterate in mehrotramsocp is not positive ');
         done = 3;
         correction = eps-2*min(min(x(1:K.l)),min(s(1:K.l)));
         x(1:K.l) = x(1:K.l)+correction;
         s(1:K.l) = s(1:K.l)+correction;
      end
      eta(1:K.l) = (x(1:K.l)./s(1:K.l)).^(0.5);
   end
   i = K.l;
   for j = 1:pS
      njj  = K.q(j);
      xbar = x(i+1:i+njj); 
      sbar = s(i+1:i+njj);
      detx = xbar(1)^2-xbar(2:njj).'*xbar(2:njj);
      dets = sbar(1)^2-sbar(2:njj).'*sbar(2:njj);
      if min([x(i+1),s(i+1)]) <= 0 || detx <= 10*eps*x(i+1)^2 ...
              || dets <= 10*eps*s(i+1)^2
          disp(' iterate in mehrotramsocp is not in interior of SOC ');
          done = 3;
          xbar(1) = max(xbar(1),norm(xbar(2:end))*(1+1.0e-10)+1.0e-10);
          sbar(1) = max(sbar(1),norm(sbar(2:end))*(1+1.0e-10)+1.0e-10);
          detx = xbar(1)^2-xbar(2:njj).'*xbar(2:njj);
          dets = sbar(1)^2-sbar(2:njj).'*sbar(2:njj);
          x(i+1) = xbar(1);
          s(i+1) = sbar(1);
      end
      xbar  = xbar/sqrt(detx);
      sbar  = sbar/sqrt(dets);
      sbari = -sbar; sbari(1) = sbar(1);
      tmpg  = 1+xbar.'*sbar;
      gamma = 1/sqrt(2*tmpg);
      w(i+1:i+njj)   = gamma*(xbar+sbari);
      eta(i+1:i+njj) = (detx/dets)^.25;
      eta(i+1)       = -eta(i+1); % to allow a simple rank-1-update for W
      i=i+K.q(j);
   end

% Compute AW
   AW = A .* (ones(m,1)*eta.'); % avoid sparse computation in octave
   i = K.l;
   for j = 1:pS
      njj             = K.q(j);
      wj              = w(i+1:i+njj); 
      wj(1)           = wj(1)+1;
      wj              = sqrt(-eta(i+1)/wj(1)) * wj; % eta(i+1) is negative
      AW(:,i+1:i+njj) = AW(:,i+1:i+njj) + (A(:,i+1:i+njj)*wj)*wj.';
      i               = i+njj;
   end

   AWAt  = AW*AW.';
   [R,p] = chol(AWAt);  
   %R     = qr(AW.',0); R = triu(R); R = R(1:m,1:m); %less rounding errors?
   %p = 0; if min(abs(diag(R))) < 1.0e-14 * max(abs(diag(R))); p=1; end
   if p > 0; R = 1.0e16*eye(size(AWAt)); done = 2; end;

% predictor step
   p = b-A*x;
   q = c-(y.'*A).'-s;
   if iter <= initialit
      if norm(p) < 0.01 && norm(q) < 0.01 % XXX rather strict phase-1-test 
         initialit = iter-1;              % no need for continuing phase 1
      end
   end
   v = wmult(w,eta,s,K);
   residualfirst = 0; % the priority is not to first reduce the residual
   if mu < max(res0*quot0,res1*quot1)
      residualfirst = 1; % now, priority is to reduce the residual first
      r = max(res0*quot0,res1*quot1)*jinv(v,K)-v; % some centering
   else
      r = -v; % affine scaling 
   end
   if iter <= initialit
      r = jinv(v,K)-v; % keep mu at mu = 1
   end
   rhs  = p + AW*(wmult(w,eta,q,K)-r);
   
   [dxp,~,dsp,failback] = gmres_solve(A,AW,AWAt,R,rhs,p,q,r,w,eta,K);   
   if failback == 1
      done = 4;
   end
   
% corrector step
   alxmax = alphamax(x,dxp,K);
   alsmax = alphamax(s,dsp,K);
   if residualfirst == 1 % make sure residual reduces not ... 
      munew = max(res0*quot0,res1*quot1); % ... much slower than mu
      target = munew;
   else
      munew  = (x+alxmax*dxp).'*(s+alsmax*dsp)/pK;
      target = munew; % this target could be reached by the predictor step
      munew  = min(0.5*mu,mu*(munew/mu)^3);
   end
   tmp    = jmult(winvmult(w,eta,dxp,K),wmult(w,eta,dsp,K),K);
   theta  = 1; % possible damping of the Mehrotra corrector
   r      = munew*jinv(v,K)-v-theta*linvmult(v,tmp,K);
   if iter <= initialit
      theta = 1; % possible damping of the Mehrotra corrector
      r = jinv(v,K)-v-theta*linvmult(v,tmp,K);
   end

   rhs    = p + AW*(wmult(w,eta,q,K)-r);
   
   [dx,dy,ds,failback] = gmres_solve(A,AW,AWAt,R,rhs,p,q,r,w,eta,K); 
   if failback == 1
      done = 4;
   end
   
   if norm(dx) == 0 
      alxmax = 1;
   else
      alxmax = alphamax(x,dx,K);
   end
   if norm(ds) == 0 
      alsmax = 1;
   else
      alsmax = alphamax(s,ds,K);
   end
   eta_x  = max(0.5,min(0.95,alxmax)^3); %XXXX damping of step length
   eta_s  = max(0.5,min(0.95,alsmax)^3); 
%   eta_x  = min(0.95,0.35+0.65*alxmax)^2; % damping of step length
%   eta_s  = min(0.95,0.35+0.65*alsmax)^2; 
   eta_x  = min(eta_x,eta_s); eta_s=eta_x; %same damping in primal and dual
   if iter <= initialit
      eta_x = min(eta_x,0.5); eta_s = min(eta_s,0.5);
   end
   alx    = min(1,eta_x * alxmax);
   als    = min(1,eta_s * alsmax);
      
   xold=x;yold=y;sold=s;muold=mu; % remember these for possible safety step
   x  = x + alx*dx;
   s  = s + als*ds;
   y  = y + als*dy;
   if alx < 0.001; done = 3;  end
   if als < 0.001; done = 3;  end
   mu = x.'*s/pK;
   if iter > initialit && mu >= 0.75*(target+muold) && ...
                          residualfirst == 0
% slow reduction of mu, but not because munew had been increased
      if options.p_l >= 2
         str_g = sprintf('safety step in mehrotramsocp, mu = %0.5g',mu);
         disp(str_g)
      end
      r   = muold*jinv(v,K)-v;
      rhs = p + AW*(wmult(w,eta,q,K)-r);
      [dxp,dyp,dsp,failback] = gmres_solve(A,AW,AWAt,R,rhs,p,q,r,w,eta,K); 
      if failback == 1
         done = 4;
      end
      alxmax = alphamax(xold,dxp,K);
      alsmax = alphamax(sold,dsp,K);
      x    = xold + 0.5*alxmax*dxp;
      y    = yold + 0.5*alsmax*dyp;
      s    = sold + 0.5*alsmax*dsp;
      %[iter,muold,target,mu,x'*s/pK] % For debugging purposes
      mu   = x.'*s/pK;
   end
   
   res0      = norm(A*x-b);
   res1      = norm(A.'*y+s-c);
   angle     = x.'*s/((norm(x)+1)*norm(s)+1);
   residual = max([res0/nb1,res1/nc1,angle]); 
   if iter >= options.maxit
      done = 5;
   end
   if residual <= options.tol 
      done = 1; %normal termination
   end
   
end % of Main loop


if residual <= max(1.0e-8,10*options.tol), 
   done = 1; %still consider this a normal termination
end

if options.p_l > 0
if done == 2
   disp(' mehrotramsocp: early termination due to ill-conditioning ')
   tmp = sprintf('Final residual: %5.0g',residual);
   disp(tmp)
end
if done == 3
   disp(' mehrotramsocp: early termination due to short step length ')
   tmp = sprintf('Final residual: %5.0g',residual);
   disp(tmp)
end
if done == 4
   disp('mehrotramsocp: termination due to rounding errors in backsolve')
   tmp = sprintf('Final residual: %5.0g',residual);
   disp(tmp)
end
if done == 5
   disp(' mehrotramsocp: termination since iteration limit is reached ')
   tmp = sprintf('Final residual: %0.5g',residual);
   disp(tmp)
end
if done == 6
   disp(' in mehrotramsocp:')
   disp('x = zero is optimal, if there is a finite optimal solution')       
   x = zeros(n,1);
   y = zeros(m,1); % this dual solution might not be feasible
   s = zeros(n,1); % this dual solution might not be feasible
end
if done == 7
   x = pinv(A)*b;
   s = zeros(n,1); % this dual solution might not be feasible
   if recover_y == 0
      y = zeros(m,1); % this dual solution might not be feasible
   else
      y = zeros(mold,1);
   end
end
end % of options.p_l > 0

% Postprocessing, rescaling the result.
x = x * bnorm_old;
y = y * cnorm_old;
s = s * cnorm_old;

y = y + dyold; % because above, A'*y + s = cnew = cold - A'*dyold
y = R_old\y;   % because above, A was rescaled with inv(R_old')
if recover_y == 1
   y = pinv(Aold.')*(c-s); 
end

if options.p_l > 1
   tmp = sprintf('Number of iterations in mehrotramsocp: %5.0f',iter);
   disp(tmp)
end

out.mu       = mu;
out.done     = done;
out.residual = residual;


end








function xs = jmult(x,s,K)
% compute the Jordan-product of x and s

pS = length(K.q);
xs = s;
if K.l > 0; xs(1:K.l) = x(1:K.l).*s(1:K.l); end
i = K.l;
for j = 1:pS
   njj           = K.q(j);
   xs(i+1)       = x(i+1:i+njj).'*s(i+1:i+njj);
   xs(i+2:i+njj) = x(i+1)*s(i+2:i+njj)+s(i+1)*x(i+2:i+njj);
   i             = i+njj;
end
end








function v = linvmult(x,s,K)
% multiply s with inv(L(x)) (not with L(inv(x)):

pS = length(K.q);
v = zeros(size(s));
i = K.l;
if K.l > 0
   if min(abs(x(1:K.l))) == 0; error(' linvmult divide by zero'); end
   v(1:K.l) = s(1:K.l)./x(1:K.l); 
end
for j = 1:pS
   njj           = K.q(j);
   xj            = x(i+1:i+njj); 
   xj1           = xj(1); % to avoid confusion with sign change later
   xj(1)         = -xj(1);
   detxj         = xj1^2-xj(2:njj).'*xj(2:njj);
   if xj1   == 0; error(' linvmult divide by zero (SOC1)'); end
   if detxj == 0; error(' linvmult divide by zero (SOC2)'); end
   v(i+2:i+njj)  = s(i+2:i+njj)/xj1;  
   v(i+1:i+njj)  = v(i+1:i+njj) + xj*((xj.'*s(i+1:i+njj))/(detxj*xj1)); 
   i             = i+njj;
end
end








function ws = wmult(w,eta,s,K)
% multiply s with W given by w and eta:

pS = length(K.q);
ws = eta.*s;
i = K.l;
for j = 1:pS
   njj           = K.q(j);
   wj            = w(i+1:i+njj); 
   wj(1)         = wj(1)+1;
   wj            = sqrt(-eta(i+1)/wj(1)) * wj; % eta(i+1) is negative
   ws(i+1:i+njj) = ws(i+1:i+njj) + (s(i+1:i+njj).'*wj)*wj;
   i             = i+njj;
end
end








function ws = winvmult(w,eta,s,K)
% multiply s with W^{-1} given by w and eta:

pS = length(K.q);
ws = s./eta;
i = K.l;
for j = 1:pS
   njj           = K.q(j);
   wj            = w(i+1:i+njj); 
   wj(1)         = -(wj(1)+1);
   wj            = sqrt(1/(eta(i+1)*wj(1))) * wj; % eta(i+1),wj(1) negative
   ws(i+1:i+njj) = ws(i+1:i+njj) + (s(i+1:i+njj).'*wj)*wj;
   i             = i+njj;
end
end








function si = jinv(s,K)
% compute the Jordan-inverse of s

pS = length(K.q);
si = s;
if K.l > 0; si(1:K.l) = 1./s(1:K.l); end
i = K.l;
for j = 1:pS
   njj           = K.q(j);
   sj            = s(i+1:i+njj); 
   detsj         = sj(1)^2-sj(2:njj).'*sj(2:njj);
   sj(1)         = -sj(1);
   si(i+1:i+njj) = (-1/detsj)*sj;
   i             = i+njj;
end
end








function [dx,dy,ds,fail] = gmres_solve(A,AW,AWAt,R,rhs,p0,q0,r0,ww,eta,K)
% do a back solve one step of iterative refinement
% (this used to be gmres but due to the use of cg-correction in the 
%  backsolve, one step of iterative refinement is sufficient and does
%  not deteriorate due to rounding errors when the backsolve is poor)

p_l   = 0; % XXX pass p_l on from main program

tol             = 1.0e-10;  % relative tolerance aimed for
tol2            = 1.0e-1;  % minimum required relative tolerance
[b1,b2,b3,~] = backsolve(A,AW,AWAt,R,rhs,p0,q0,r0,ww,eta,K);

p1 = p0-A*b1;
q1 = q0-A.'*b2-b3;
r1 = r0-(winvmult(ww,eta,b1,K)+wmult(ww,eta,b3,K));

res0 = norm([p1;q1;r1]);
         
if res0 < tol*norm([p0;q0;r0]) 
   fail = 0;
   dx = b1;
   dy = b2;
   ds = b3;
else % refine once
   % disp('refine')
   rhss = p1 + AW*(wmult(ww,eta,q1,K)-r1);
   [dx,dy,ds,~] = backsolve(A,AW,AWAt,R,rhss,p1,q1,r1,ww,eta,K);
   dx = b1+dx; 
   dy = b2+dy;
   ds = b3+ds;

   fail = 0;
   pp = p0-A*dx; 
   qq = q0-A.'*dy-ds;
   rr = r0-(winvmult(ww,eta,dx,K)+wmult(ww,eta,ds,K));
   res1 = norm([pp;qq;rr]);

   if res1 > res0
      dx = b1; dy = b2; ds = b3; 
      pp = p1; qq = q1; rr = r1;
      if p_l >1
         disp('residual was increased during gmres')
      end
   end

   if norm(pp) > tol2*norm(p0) || norm(qq) > tol2*norm(q0) || ...
         norm(rr) > tol2*norm(q0)
      dx = zeros(size(dx)); dy = zeros(size(dy)); ds = zeros(size(ds)); 
      if p_l >1
         disp('refinement failed')
      end
      fail = 1;
   end
end % of refine once

end








function [dx,dy,ds,fail] = backsolve(A,AW,AWAt,R,rhs,p,q,r,w,eta,K)
% do a back solve with several steps of conjugate gradient refinement
p_l = 0; % XXX pass p_l on from main program

fail = 0; % assume the backsolve runs well
tmp  = R.' \ rhs;
dy   = R   \ tmp;
ds   = q - A.'*dy;
dx   = wmult(w,eta,r-wmult(w,eta,ds,K),K); 

pp = p - A*dx;
qq = q - (A.'*dy+ds);
rr = r - (winvmult(w,eta,dx,K)+wmult(w,eta,ds,K)); 

res0 = norm([p;q;r]);
res1 = norm([pp;qq;rr]);

dx0=dx;dy0=dy;ds0=ds;

if res1 > 1.0e-3*res0 && p_l > 1
   disp('ill conditioning in backsolve'); 
end
if res1 > 1.0e-10*res0
   %disp('refine') 
   rhs = pp + AW*(wmult(w,eta,qq,K)-rr);
   
   ddy = ownpcg(AWAt,R,rhs);
   dds = qq - A.'*ddy;
   ddx = wmult(w,eta,rr-wmult(w,eta,dds,K),K); 

   dx = dx+ddx;
   dy = dy+ddy;
   ds = ds+dds;
      
   pp   = p - A*dx;
   qq   = q - (A.'*dy+ds);
   rr   = r - (winvmult(w,eta,dx,K)+wmult(w,eta,ds,K)); 
   res2 = norm([pp;qq;rr]);

   if res2 > res1
      dx = dx0;
      dy = dy0;
      ds = ds0;
      res2 = res1;
      if p_l > 1
         disp('cg refinement failed'); 
      end
   end
   if res2 > 1.0e-3*res0;
      if p_l > 1
         disp('backsolve failed'); 
      end
      dx = zeros(size(dx));
      dy = zeros(size(dy));
      ds = zeros(size(ds));
      fail = 1; 
   end

end

end








function x = ownpcg(A, R, b)
% Solve A*x = b  --  or minimize   0.5*x'*A*x-b'*x
% A preconditioning Matrix R with A \approx R'*R must be provided.
% (One may simply use R = eye(n) but that is inefficient)

% Define auxiliary quantities
[n,~]  = size(A);           % sizes are not checked for compatibility
x      = zeros(n,1);        % start at x = 0
maxit  = min(10,n);         % Maximum number of iterations
tol    = 1.0e-10;           % stopping criterion
Rt     = R.';               % to avoid transposition at each iteration

% Initialize
Axmb   = -b;                % in general: A*x-b
g      = R\(Rt\Axmb);       % preconditioned gradient
s      = -g;                % search direction
gAxmb  = g.'*Axmb;  
tol    = tol^2*abs(gAxmb);  % error relative to right hand side
k      = 0;

while abs(gAxmb) > tol && k < maxit
   
   As     = A*s;
   lambda = -s.'*Axmb/(s.'*As);
   x      = x+lambda*s;
   Axmb   = Axmb+lambda*As;
   g      = R\(Rt\Axmb);   
   gAxmbn = g.'*Axmb;
   gamma  = gAxmbn/gAxmb;
   s      = -g+gamma*s;
   gAxmb  = gAxmbn;
   k      = k+1;
   
end

%tmp = sprintf('Number of iterations in cg: %5.0f',k);
%disp(tmp)

end








function almax = alphamax(x,dx,K)
% compute the longest feasible step <= 2 in K

pS = length(K.q);
almax = 2;
if K.l > 0;
   crit = find(dx(1:K.l) < 0);
   if ~isempty(crit)
      almax = min(almax,min(-x(crit)./dx(crit)));
      if almax < 0; error('error0 in alphamax'); end
   end
end
i = K.l;
for j = 1:pS
   njj           = K.q(j);
   xj            = x(i+1:i+njj); 
   dxj           = dx(i+1:i+njj); 
   detxj         =  xj(1)^2- xj(2:njj).'* xj(2:njj);
   detdxj        = dxj(1)^2-dxj(2:njj).'*dxj(2:njj);
   tmp           = xj(1)*dxj(1)-xj(2:njj).'*dxj(2:njj);
   
   if dxj(1) < norm(dxj(2:njj)) % (step length in SOC must be checked) ***
       
   % first the numerically instable cases
   instab = 0; % no instable case detected
   if tmp == 0
      if detdxj < 0
         almax = min(almax, sqrt(abs(detxj/detdxj)));
      end
      instab = 1;
   end
   if abs(detxj*detdxj) <= 1.0e-6*tmp^2 && tmp < 0
      almax = min(almax, abs(detxj/(2*tmp)));
      instab = 1;
   end
   if detdxj >= 0 && tmp >= 0 % then almax = inf
      error('error1 in alphamax'); % this should not happen here
      %instab = 1;
   end
   
   % now the standard case
   if ~instab
      tmproot = tmp^2-detdxj*detxj;
      if tmproot < -1.0e-10*tmp^2; almax = 0; end % rounding errors
      tmproot = sqrt(abs(tmproot));
      al1 = (-1/detdxj) * (tmp + tmproot); 
      al2 = (-1/detdxj) * (tmp - tmproot);
      if min(al1,al2) <= 0
         al1 = max(al1,al2);
      else
         al1 = min(al1,al2);
      end
      if al1 < 0; error('error3 in alphamax'); end
      almax = min(almax, al1);
   end
   
   end % (end of: step length in SOC must be checked) ********************
   i = i+njj;
end
end
