function [x, niter] = newtonsys(f, x0, tol, maxiter, varargin)
% NEWTONSYS Newton's method for a system of equations.
%    
% Input parameters:
%   f: Name of model function as a string. The function must be
%      defined in the following way: the first argument must take a
%      1xn column vector with the values x_1, x_2, ..., x_n for
%      the evaluation of f. The second argument of the function must
%      accept a FLAG. If FLAG is empty, the function must be
%      evaluated regularly, and the result must be returned as a
%      colunm vector. If the FLAG is set to 'jacobian', the
%      function must return the evaluation result of the jacobian
%      matrix instead (as nxn matrix). After the FLAG parameter,
%      the function may specify an arbitrary number of additional
%      parameters. See below for an example model function
%      definition.
%   x0: initial guess, a 1xn column vector.
%   tol: Error tolerance for the relative increment of x.
%   maxiter: maximum number of iterations to perform.
%   p1, p2, ..., pn: An arbitrary number of parameters that are
%                    passed on to the function f when evaluated.
%
% Output paramters:
%	  x: Approximation to the solution, 1xn column vector.
%   niter: Number of iterations performed.
%
%      Example for calling newton:
%        [x, niter] = newton('robotarm', [pi/2; pi/2], 1e-5, ...
%                            20, [4; 2], [3; 4])
%
%      Example for the definition of the function f:
%        function out = robotarm(x, flag, l, p)
%        if isempty(flag)
%          flag = '';
%        end
%        switch lower(flag)
%         case ''
%	         out = [l(1)*cos(x(1))-l(2)*cos(x(1)+x(2))-p(1); ...
%                 l(1)*sin(x(1))-l(2)*sin(x(1)+x(2))-p(2)];	
%         case 'jacobian'
%	         out = [-l(1)*sin(x(1))+l(2)*sin(x(1)+x(2)), ...
%                 l(2)*sin(x(1)+x(2)); ...
%                 l(1)*cos(x(1))+l(2)*cos(x(1)+x(2)), ...
%                 -l(2)*cos(x(1)+x(2))];	
%        end
%
% Note: The newton method will fail if the determinant of the
%       jacobian matrix becomes zero.
	
% Author : Andreas Klimke, Universität Stuttgart
% Version: 1.0
% Date   : June 13, 2003

% Initialization
x = x0;
x_old = x;
niter = 0;

for k = 1:maxiter

	% Iteration step
	x = x - feval(f, x, 'jacobian', varargin{:}) \ ...
			feval(f, x, [], varargin{:});
	niter = niter + 1;
		
	% Break condition
	if norm(x) < eps
		if norm(x-x_old) <= tol
			return
		end
	else
		if norm(x-x_old)/norm(x) <= tol
			return
		end
	end
	
	x_old = x;
end

warning('Maximum number of iterations reached.');