function [x,nrmgseq,alphaseq] = newton(fghfun, x0, tol, maxit)
%% [x,nrmgseq,alphaseq] = newton(fghfun, x0, tol, maxit)
%% Solves a system of equations via Newton's method with Armijo
%% line search.  Input arguments:
%%   fghfun: A handle to a function that takes an argument x,
%%     the current iterate, and returns f, the function value,
%%     g, the gradient value, and h, the Hessian value at x.
%%   x0: The starting guess for x.
%%   tol: Iteration is terminated when ||g|| < tol
%%   maxit: Iteration is terminated when the number of iterations
%%       exceeds maxit.
%% Output arguments:
%%   x: The solution to the equations.
%%   nrmgseq: The norm of g, the gradient on each iteration.
%%   alphaseq: The sequence of alphas on each iteration.


%% The following two variables keep track of normg and alpha
%% on each iteration.  It is very inefficient in Matlab to iteratively
%% append to the tail (i.e., to execute nrmgseq = [nrmgseq;normg];
%% on each iteration, so instead this code uses the trick of doubling
%% the size when they fill up, and then truncating to the right length
%% at the end.

nrmgseq = [];
alphaseq = [];


x = x0;
[f, g, h] = fghfun(x);
n = length(g);
itcount = 0;

while itcount < maxit
  itcount = itcount + 1;
  normg = norm(g);
  if length(nrmgseq) < itcount
    nrmgseq = [nrmgseq; zeros(itcount+2,1)];
  end
  nrmgseq(itcount) = norm(g);
  if normg < tol
    break
  end
  
  %% Check if h is positive definite; if not, add a multiple of the
  %% identity to it.
  
  [R,p] = chol(h);
  if p > 0
    disp('h is not positive definite');
    h = h + speye(n) * norm(h,'inf');
  end
  step = -h \ g;
  if g'*step > 0
    error('not descent direction');
  end
  
  %% Armijo line search to determine alpha
  alpha = 1;
  newx = x;
  lsitcount = 0;
  while 1
    lsitcount = lsitcount + 1;
    if lsitcount > 40
      error('line search too many iterations');
    end
    newx = x + alpha * step;
    [newf, newg, newh] = fghfun(newx);
    if newf < f + g'*step*alpha/10
      break
    end
    alpha = alpha/2;
  end
  if length(alphaseq) < itcount
    alphaseq = [alphaseq; zeros(itcount+2,1)];
  end
  alphaseq(itcount) = alpha;
  x = newx;
  f = newf;
  g = newg;
  h = newh;
end
alphaseq = alphaseq(1:itcount - 1);
nrmgseq = nrmgseq(1:itcount);
