function [ root ] = newton_raphson(func,x,der,tol,niter)
% INPUT:
% func = handle of a function.
% x    = starting solution vector [x1,x2,...,xn].
% der  = finite difference: 'for1' (for 1st forward), 'for2' (for 2nd 
%        forward), or 'cen' (for centered) 
% tol  = error tolerance (default is 1.0e4*eps).
% OUTPUT:
% root = solution vector.

if nargin == 2
    der = 'for';
    tol = 1.0e4*eps; 
    niter =30;    
elseif nargin == 3
    tol = 1.0e4*eps; 
    niter =30;
elseif nargin == 4
    niter =30;
end

% x must be a column vector 
if size(x,1) == 1 
    x = x'; 
end  

for iter = 1:niter
    
    % choice of forward or centered difference
    switch der
        case 'for1'
        [jac,f0] = jacobian_for1(func,x);
        case 'for2'
        [jac,f0] = jacobian_for2(func,x);
        case 'cen'
        [jac,f0] = jacobian_cen(func,x);
    end
    
    % Stop if f(x) is closer to 0 than the tolerance
    if sqrt(f0'*f0/length(x)) < tol
        root = x; return
    end

    dx = jac\(-f0);
    x = x + dx;

    % Stop if dx is smaller than some multiple of the tolerance
    if sqrt(dx'*dx/length(x)) < tol*max(abs(x),1.0)
        root = x; return
    end
end

error('Too many iterations')

% Jacobian: Returns the Jacobian matrix and f(x).

function [jac,f0] = jacobian_for1(func,x) 

h = 1.0e-4;
n = length(x);
jac = zeros(n,n);
f0 = feval(func,x);

for i = 1:n
    temp = x(i);
    x(i) = temp + h;
    f1 = feval(func,x);
    x(i) = temp;
    jac(:,i) = (f1 - f0)/h;
end

end


function [jac,f0] = jacobian_for2(func,x) 

h = 1.0e-4;
n = length(x);
jac = zeros(n,n);
f0 = feval(func,x);

for i = 1:n
    temp = x(i);
    x(i) = temp + h;
    f1_plus = feval(func,x);
    x(i) = temp + 2*h;
    f2_plus = feval(func,x);    
    x(i) = temp;    
    jac(:,i) = (-f2_plus + 4*f1_plus - 3*f0)/(2*h);
end

end


function [jac,f0] = jacobian_cen(func,x) 

h = 1.0e-4;
n = length(x);
jac = zeros(n,n);
f0 = feval(func,x);

for i = 1:n
    temp = x(i);
    x(i) = temp + h;
    f1_plus = feval(func,x);
    x(i) = temp - h;
    f1_minus = feval(func,x);    
    x(i) = temp;    
    jac(:,i) = (f1_plus - f1_minus)/(2*h);
end

end


end

