% Calling syntax: [z,status] = pnewt(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] = pnewt(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)));

J = sparse(1:n,1:n);
step_tol = 1e-2;
eval(['[f,J,domerr] = ' cpfj '(z,1);']);
if ~issparse(J) 
  error('Jacobian must be sparse');
end
residual = merit(z,f,l,u),
pert = 0.01*residual;

for iter=1:100
  basiscount = 0;
  needed = [];
  for i=1:length(z)
    if ~((z(i) <= l(i) & f(i) >= 0.) | (z(i) >= u(i) & f(i) <= 0.))
      basiscount = basiscount+1;
      needed(basiscount) = i;
    end
  end

  r = f(needed); B = J(needed,needed); 
  if condest(B) > 1e6 
     B = B + pert*speye(length(needed));
  end 
  d = B\r; 
  e = sparse(length(z),1); e(needed) = d;

  alpha = 1.0;
  while (alpha > step_tol) 
    znew = min(u,max(l,z - alpha*e));
    eval(['[fnew,J,domerr] = ' cpfj '(znew,0);']);
    resnew = merit(znew,fnew,l,u);
    if (resnew < residual) 
      break; end
    alpha = alpha * 0.5;
  end
  
  if (alpha <= step_tol)
    disp('linesearch failure'); return; end

  z = znew;
  residual = resnew,
  if (residual < 1e-7) 
    return; end
  pert = min(0.9*pert,0.01*residual);
  eval(['[f,J,domerr] = ' cpfj '(z,1);']);
end;

