function [normpf, normdf, phi, X, y, iter, primalres] = n2(epsrhs,digits);
%        [normpf, normdf, phi, X, y, iter, primalres] = n2( epsrhs,digits);
% solves: max trace(LX) s.t. X psd, two constraints, n=2
%         
% input: digits ... duality gap tolerance
%        epsrhs ... right-hand-side epsilon, >=0
% output: phi ... optimal value of primal, phi =trace(LX)
%         X   ... optimal primal matrix
%         y   ... optimal dual vector
%%%%%%%%%%%%%%%%%%%
         %%Initialization
disp(' ');
disp('start of file n2.m - standard HKM on stable problem')
disp('epsrhs and digits are: ');
disp([sprintf('    %6.2e         %4g   ',epsrhs,digits)]);

if epsrhs<0
  disp('RHS negative - error on input');
  return
end
if nargin <2,
  digits = 11;                     % 6 significant digits of phi
end
n = 2;                          % problem size
%L=[-1 -1 ; -1 0];               % primal objective matrix (max. prob.)
L=[-1 1 ; 1 0];               % primal objective matrix (max. prob.)
A1=[0 1 ;1 1 ];
A2=[0 1 ;1 -1 ];
b = [epsrhs -epsrhs]';          % RHS
X = diag( [1;epsrhs] );         % initial primal matrix is pos. def.
y = [1;-1];                     % initial y is chosen so that
Z = y(1)*A1+y(2)*A2-L;          % initial dual slack Z is pos. def.
Rd=  (y(1)*A1+y(2)*A2-L)-Z;          % initial dual slack Z is pos. def.
phi = b'*y;                     % initial dual
psi = L(:)' * X( :);            % and primal costs
mu = Z( :)' * X( :)/( 2*n);     % initial complementarity
iter=0;                         % iteration count

disp(' ');
disp('at start  - using standard HKM direction')
disp(['iter  alphap          alphad        gap        lower        upper     pfeas         dfeas']);


primalres=[];   % save residual data
while (phi-psi) > max([1,abs(phi)]) * 10^(-digits)

      iter = iter + 1;          % start a new iteration
      Zi = inv( Z);             % inv(Z) is needed explicitly
      Zi = (Zi + Zi')/2;
      K=[ trace(A1*Zi*A1*X) trace(A1*Zi*A2*X)
          trace(A2*Zi*A1*X) trace(A2*Zi*A2*X)];
      RHS=[trace(A1*mu*Zi-Zi*Rd*X);  trace(A2*mu*Zi-Zi*Rd*X)] -b;
      dy =  pinv(K)*RHS;      % solve for dy
      dX =  X*(dy(1)*A1+dy(2)*A2+Rd)*Zi;
      dX=mu*Zi-X-((dX+dX')/2); % back substitute for dX
      dZ =  (dy(1)*A1+dy(2)*A2+Rd);

%dX
%'after direction find'

% line search on primal
      alphap = 2;               % initial steplength
      [dummy,posdef] = chol( X + alphap * dX );  % test if pos.def
      while posdef > 0,
              alphap = alphap * .8;
              [dummy,posdef] = chol( X + alphap * dX );
              end;
      %if alphap < 1, alphap = alphap * .95; end; % stay away from boundary
      alphap = alphap * .95;
% line search on dual;
      alphad = 2;
      [dummy,posdef] = chol( Z + alphad * dZ );
      while posdef > 0;
              alphad = alphad * .8;
              [dummy,posdef] = chol( Z + alphad * dZ );
              end;
      %if alphad < 1, alphad = alphad * .95; end;
       alphad = alphad * .95;
% update
      X = X + alphap * dX;
      y = y + alphad * dy;
      Z = Z + alphad * dZ;
      Rd=  (y(1)*A1+y(2)*A2-L)-Z;
      Rp= [trace(X*A1) trace(X*A2)]' - b; 
      mu = X( :)' * Z( :) / (2*n);
      if alphap + alphad > 1.8, mu = mu/2; end; % speed up for long steps
      phi = b' * y; psi = L( :)' * X( :);
% display current iteration
        disp(num2str([ iter alphap alphad (phi-psi) psi  phi norm(Rp) norm(Rd)]));

        end;            % end of main loop

%primalres=[mean(primalres) std(primalres)];

%normpf=norm(diag(X)-ones(n,1)/4);
%normdf=norm((diag(y)-L)-Z);
%disp('at end of mc - using standard HKM direction')
%disp(['norm(diag(X)-e/4) is: ',num2str(norm(diag(X)-ones(n,1)/4))]);
%disp(['norm((Diag(y)-L)-Z) is: ',num2str(norm((diag(y)-L)-Z))]);
disp('min eigs of X and Z at end are: ')
disp([sprintf('    %6.4e          %6.4e   ',min(eig(X)),min(eig(Z)) )]);

