% SEMISMTH - nonlinear complementarity problems using SEMISMTH
%
% SEMISMTH solves an NCP of the form
%
%       0 <= z, F(z) >= 0 and z'F(z) = 0 
%
% Calling syntax: [z,status] = semismth(z,l,u,cpfunjac)
%
%        cpfunjac is the name of the m-file for 
%        evaluating the function F and its Jacobian J (without .m-extension).
%
%        The following m-file must be supplied (where default name is
%        'cpfunjac.m' unless stated otherwise in the variable cpfunjac).
%
%        'cpfunjac.m' contains function [f,J,domerr]=cpfunjac(z,jacflag) 
%                     that computes the function F 
%                     and if jacflag=1 the sparse Jacobian J at the point z.
%                     domerr returns the number of domain violations.
%
% If 'l','u' are omitted, z is assumed non-negative.
%
function [z,status] = semismth(z,l,u,cpfj)
%    ( M-file written Oct 1995 by Michael Ferris)
%

[n,m]=size(z);
if min(m,n) ~= 1
  error('Input point is not a vector');
end
if m > n 
  z = z'; n = m;
end
Big = 1e20;

if nargin < 4
  cpfj = 'cpfunjac';
  if nargin < 3
    u = Big*ones(size(z));
    if nargin < 2
      l = zeros(size(z));
    end
  end
end

[m1,n1]=size(l);
if(n1 ~= 1)
  l = l';
  if(n1 ~= n | m1 ~=1)
    error('Input arguments are of incompatible sizes');
  end 
elseif (m1 ~= n)
  error('Input arguments are of incompatible sizes');
end
l = max(l,-Big*ones(size(l)));
  
[m1,n1]=size(u);
if(n1 ~= 1)
  u = u';
  if(n1 ~= n | m1 ~=1)
    error('Input arguments are of incompatible sizes');
  end 
elseif (m1 ~= n)
  error('Input arguments are of incompatible sizes');
end 
u = min(u,Big*ones(size(u)));

if (any(l~=0) | any(u~=Big)) 
  error('only solves NCP');
end

J = sparse(1:n,1:n);
eval(['[f,J,domerr] = ' cpfj '(z,1);']);
if ~issparse(J)
  error([cpfj ' must return a sparse Jacobian']);
end

beta = 1e-4;
tol = 1.0e-12;

% initialize counters measuring algorithmic performance
iter=0;

% begin iterations
d = sqrt(z.^2 + f.^2);
phi=d-z-f;
tnorm=0.5*phi'*phi,

while tnorm > tol
  iter = iter + 1;

  degen = find(max(abs(z),abs(f)) < 1e-6);
  A = z; B = f;
  if (~isempty(degen)) 
    disp('degenerate indices');
    A(degen) = ones(size(degen));
    B(degen) = J(degen,degen)*A(degen);
  end;
  d = sqrt(A.^2 + B.^2);
  A = A./d - ones(n,1);
  B = B./d - ones(n,1);

  G= sparse(1:n,1:n,B)*J + sparse(1:n,1:n,A);
  if (condest(G) < 1.e8) 
    d=-G\sparse(phi);
  else d=-G'*phi;
  end;

  if (phi'*G*d > -1e-2*d'*d) 
     disp('insufficient decrease: try gradient step');
     d = -G'*phi;
  end

% inexact line search
  t=2;
  newtnorm=tnorm+1;
  while newtnorm > (1-beta*t/2)*tnorm
    t=t/2;
    newz=z+t*d;
    eval(['[newf,J,domerr] = ' cpfj '(newz,0);']);
    newphi = sqrt(newz.^2 + newf.^2) - newz - newf;
    newtnorm=0.5*newphi'*newphi;
  end

% update z and go for another iteration
  z=newz;
  phi=newphi;
  tnorm=newtnorm,
  eval(['[f,J,domerr] = ' cpfj '(z,1);']);
end





