% Example 4.5.2 Nonlinear Systems
% At the end of this file you will find functions that define a system of three equations and its Jacobian matrix. (These functions could be written as separate files, or embedded within another function as we have done here.) Our initial guess at a root is the origin.

clear
x = [0;0;0];   % column vector!

%
% We need the value (residual) of the nonlinear function, and its Jacobian, at this value for \mathbf{x}.

f = nlvalue(x);
J = nljac(x);

%
% We solve for the Newton step and compute the new estimate.

s = -(J\f);
x(:,2) = x(:,1) + s;

%
% Here is the new residual.

format short e
f(:,2) = nlvalue(x(:,2))

%
% We don't seem to be especially close to a root. Let's iterate a few more times.

for n = 2:7
    f(:,n) = nlvalue(x(:,n));
    s = -( nljac(x(:,n)) \ f(:,n) );
    x(:,n+1) = x(:,n) + s;
end

%
% We find the infinity norm of the residuals.

residual_norm = max(abs(f),[],1)'   % max in dimension 1

%
% We don't know an exact answer, so we will take the last computed value as its surrogate.

r = x(:,end);
x = x(:,1:end-1)

%
% The following will subtract r from every column of x.

z = size(x);
e = x - r*ones(1,z(2));

%
% Now we take infinity norms and check for quadratic convergence.

logerrs = log(abs( max(e,[],1)' ));
ratios = logerrs(2:end) ./ logerrs(1:end-1)

%
% For a brief time, we see ratios around 2, but then the limitation of double precision makes it impossible for the doubling to continue.
%
% Example 4.5.3 Usage
% We repeat a previous example. The system is again defined by its residual and Jacobian, but this time we implement them as a single function (found at the end of the file).
% Our initial guess is the origin. The output has one column per iteration.

tic
x1 = [0;0;0];   % column vector!
x = newtonsys(@nlsystem,x1);
[~,num_iter] = size(x)
x(:,end)
toc

%
% The last column contains the final Newton estimate. We'll compute the residual there in order to check the quality of the result.

r = x(:,end)
back_err = norm(nlsystem(r))

%
% Let's use the convergence to the first component of the root as a proxy for the convergence of the vectors.

log10( abs(x(1,1:end-1)-r(1)) )'

% The exponents approximately double, as is expected of quadratic convergence.
%
%% Finite Difference Approximation of Jacobian
% Use the function fdjac to compute the Jacobian.

tic
x = [0;0;0];
f = [0;0;0];

for k = 1:7
    f(:,k) = nlvalue(x(:,k));  
    s = -( fdjac(@nlvalue,x(:,k),f(:,k)) \ f(:,k) );
    x(:,k+1) = x(:,k) + s;
end
x(:,end)
toc

%
% Employing Broyden update of the Jacobian.
%

tic
x = [0;0;0];
f = [0;0;0];

f(:,1) = nlvalue(x(:,1)); 
Ak = fdjac(@nlvalue,x(:,1),f(:,1));
s = -( Ak \ f(:,1) );
x(:,2) = x(:,1) + s;

for k = 2:7
    f(:,k) = nlvalue(x(:,k)); 
    % Broyden update of the Jacobian.
    y = f(:,k)-f(:,k-1);
    Ak = Ak + (y-Ak*s)*(s'/(s'*s));
   
    s = -( Ak \ f(:,k) );
    x(:,k+1) = x(:,k) + s;
end
x(:,end)
toc

%
% Example 4.6.1 Levenberg
% We repeat a previous example. We need to code only the function defining the system, and not its Jacobian. The function is found at the end of this file.
% In all other respects usage is the same as for the newtonsys function.
%

x1 = [0;0;0];   
x = levenberg(@nlsystem2,x1);

%
% It's always a good idea to check the accuracy of the root, by measuring the residual (backward error).

r = x(:,end)
backward_err = norm(nlsystem2(r))

%
% Looking at the convergence of the first component, we find a subquadratic convergence rate, just as with the secant method.

log10( abs(x(1,1:end-1)-r(1)) )'

%
% This function defines the system of nonlinear equations. f_1=e^{x_2-x_1}-2, \quad f_2=x_1x_2+x_3, \quad f_3=x_2x_3+x_1^2-x_2. Then, \left\lbrack \begin{array}{ccc}
% -e^{\left\lbrace \left(x_2 -x_1 \right)\right\rbrace }  & e^{\left\lbrace \left(x_2 -x_1 \right)\right\rbrace }  & 0\\
% x_2  & x_1  & 1\\
% 2x_1  & x_3 -1 & x_2
% \end{array}\right\rbrack

function f = nlvalue(x)
    f = zeros(3,1);   % ensure a column vector output
    f(1) = exp(x(2)-x(1)) - 2;
    f(2) = x(1)*x(2) + x(3);
    f(3) = x(2)*x(3) + x(1)^2 - x(2);
end

% Here is a function that computes the Jacobian matrix.

function J = nljac(x)
    J(1,:) = [-exp(x(2)-x(1)),exp(x(2)-x(1)), 0];
    J(2,:) = [x(2), x(1), 1];
    J(3,:) = [2*x(1), x(3)-1, x(2)];
end

function J = fdjac(f,x0,y0)
% FDJAC   Finite-difference approximation of a Jacobian.
% Input:
%   f        function to be differentiated
%   x0       evaluation point (n-vector)
%   y0       value of f at x0 (m-vector)
% Output       
%   J        approximate Jacobian (m-by-n)

delta = sqrt(eps);   % FD step size
m = length(y0);  
n = length(x0);
J = zeros(m,n);
I = eye(n);
for j = 1:n
    J(:,j) = ( f(x0+delta*I(:,j)) - y0) / delta;
end

end

function [f,J] = nlsystem(x)
    f = zeros(3,1);   % ensure a column vector output
    f(1) = exp(x(2)-x(1)) - 2;
    f(2) = x(1)*x(2) + x(3);
    f(3) = x(2)*x(3) + x(1)^2 - x(2);
    J(1,:) = [-exp(x(2)-x(1)),exp(x(2)-x(1)), 0];
    J(2,:) = [x(2), x(1), 1];
    J(3,:) = [2*x(1), x(3)-1, x(2)];
end

%

function x = newtonsys(f,x1)
% NEWTONSYS   Newton's method for a system of equations.
% Input:
%   f        function that computes residual and Jacobian matrix
%   x1       initial root approximation (n-vector)
% Output       
%   x        array of approximations (one per column, last is best)

% Operating parameters.
funtol = 1000*eps;  xtol = 1000*eps;  maxiter = 40;

x = x1(:);  
[y,J] = f(x1);
dx = Inf;
k = 1;

while (norm(dx) > xtol) && (norm(y) > funtol) && (k < maxiter)
  dx = -(J\y);   % Newton step
  x(:,k+1) = x(:,k) + dx;

  k = k+1;
  [y,J] = f(x(:,k));
end

if k==maxiter, warning('Maximum number of iterations reached.')
end
end

function f = nlsystem2(x)
    f = zeros(3,1);   % ensure a column vector output
    f(1) = exp(x(2)-x(1)) - 2;
    f(2) = x(1)*x(2) + x(3);
    f(3) = x(2)*x(3) + x(1)^2 - x(2);
end

function x = levenberg(f,x1,tol)
% LEVENBERG   Quasi-Newton method for nonlinear systems.
% Input:
%   f         objective function 
%   x1        initial root approximation
%   tol       stopping tolerance (default is 1e-12)
% Output       
%   x         array of approximations (one per column)

% Operating parameters.
if nargin < 3, tol = 1e-12; end
ftol = tol;  xtol = tol;  maxiter = 40;

x = x1(:);     fk = f(x1);
k = 1;  s = Inf;        
Ak = fdjac(f,x(:,1),fk);   % start with FD Jacobian
jac_is_new = true;
I = eye(length(x));

lambda = 10; 
while (norm(s) > xtol) && (norm(fk) > ftol) && (k < maxiter)
    % Compute the proposed step.
    B = Ak'*Ak + lambda*I;
    z = Ak'*fk;
    s = -(B\z);

    xnew = x(:,k) + s;   fnew = f(xnew);
    
    % Do we accept the result?
    if norm(fnew) < norm(fk)    % accept
        y = fnew - fk;
        x(:,k+1) = xnew;  fk = fnew;  
        k = k+1;
        
        lambda = lambda/10;  % get closer to Newton
        % Broyden update of the Jacobian.
        Ak = Ak + (y-Ak*s)*(s'/(s'*s));
        jac_is_new = false;
    else                       % don't accept
        % Get closer to steepest descent.
        lambda = lambda*4;
        % Re-initialize the Jacobian if it's out of date.
        if ~jac_is_new
            Ak = fdjac(f,x(:,k),fk);
            jac_is_new = true;
        end
    end
end

if (norm(fk) > 1e-3)
    warning('Iteration did not find a root.')
end

end