%% MCHI, cv5, file 1 (cv5_1) - Numerical solution to SNAE (Newtons method)
%
% Author:       Martin Isoz
% Organisation: ICT Prague
% Date:         10. 10. 2014
%
% License: This code is published under MIT License, please do not abuse
% it.
%

% Note: F(x) = 0 ... system of n nonlinear algebraic equations

%% Caller 'script'
function caller
close all

% solution of problem #1
X0      = 2;                                                                %set the initial guess
eps     = 1e-5;                                                             %set the desired preciosion
maxiter = 10;                                                               %set maximal # of iterations
OPT     = [eps,maxiter];                                                    %create vector with options

[X,FVAL] = newtonM(@problem1,X0,OPT);                                       %call the solver

fprintf(1,'\nFound solution of X = [');                                     %write out the solution
for j = 1:numel(X)
    fprintf(1,'%12.3e ',X(j));
end
fprintf(1,'] with F(X) = [');
for j = 1:numel(X)
    fprintf(1,'%12.3e ',FVAL(j));
end
fprintf(1,']\n\n');


% solution of problem #2
X0      = [1 1];                                                            %initial guess has to be 2-DIM
eps     = 1e-5;                                                             %nothing else is changed
maxiter = 20;
OPT     = [eps,maxiter];

[X,FVAL] = newtonM(@problem2,X0,OPT);

fprintf(1,'\nFound solution of X = [');
for j = 1:numel(X)
    fprintf(1,'%12.3e ',X(j));
end
fprintf(1,'] with F(X) = [');
for j = 1:numel(X)
    fprintf(1,'%12.3e ',FVAL(j));
end
fprintf(1,']\n\n');

% solution of problem #3
X0      = [2 0.5];                                                          %initial guess has to be 2-DIM
eps     = 1e-5;                                                             %nothing else is changed
maxiter = 20;
OPT     = [eps,maxiter];

% create vector of parameters for the problem3
alpha = 1;beta = 1;gamma = 1;delta = 1;
pars  = [alpha beta gamma delta];                                           %same structure as in problem3 function

[X,FVAL] = newtonM(@(x) problem3(x,pars),X0,OPT);                           %need to call the solver via anonymous function

fprintf(1,'\nFound solution of X = [');
for j = 1:numel(X)
    fprintf(1,'%12.3e ',X(j));
end
fprintf(1,'] with F(X) = [');
for j = 1:numel(X)
    fprintf(1,'%12.3e ',FVAL(j));
end
fprintf(1,']\n\n');
end

%% Newtons method
function [X,FVAL] = newtonM(FUN,X0,OPT)                                     %n-dimensional newton method
% n-dimensional newtons method for solving of SNAE
%
% FUN ... function handle to a mapping defining the set of eqs F(x) = 0
%     ... FUN has to return a column vector
% X0  ... initial guess
% OPT ... optional variable containing options for the solution
%     ... OPT has to have a structure of [eps,maxiter]
% eps ... optional argument, set precision, default: eps = 1e-4;
% maxiter optional argument, maximal number of iterations,
%         default: maxiter = 20
%
%

% basic input check
if nargin < 2
    error('My:Err1','Not enough input arguments')
elseif nargin == 2
    eps     = 1e-4;
    maxiter = 20;
elseif nargin == 3
    eps     = OPT(1);
    maxiter = OPT(2);
else
    error('My:Err2','Too many input arguments')
end

if size(feval(FUN,X0),1) ~= numel(X0) || size(feval(FUN,X0),2) ~= 1
    error('My:Err3','FUN has to return a column vector of size N')
end

% auxiliary input processing
X0   = reshape(X0,numel(X0),1);                                             %transform X0 to a column vector

% define auxiliary variables
k       = 0;                                                                %iteration counter
delta   = eps+1;                                                            %precision check
XOLD    = X0;

% write out 0-th iteration
fprintf(1,'k = %03d | Xk = [',k);
for j = 1:numel(XOLD)
    fprintf(1,'%12.3e ',XOLD(j));
end
fprintf(1,'] | delta = %1.3e\n',delta);

% calculation cycle itself
while k < maxiter && delta >= eps                                          %goal precision or maxiter not reached
    % calculate the next iteration
    JAC = jacobiM(FUN,XOLD);                                                %get Jacobi matrix at given point
    DX  = JAC\-feval(FUN,XOLD);                                              %get DX
    XNEW= XOLD + DX;                                                        %currently undumped newton
    % precision check
    delta = norm(XNEW-XOLD);                                                %is this OK?
%     delta = norm(feval(FUN,XNEW)-feval(FUN,XOLD));                          %alternative
    % update the solution
    XOLD= XNEW;
    k   = k+1;
    fprintf(1,'k = %03d | Xk = [',k);
    for j = 1:numel(XOLD)
        fprintf(1,'%12.3e ',XOLD(j));
    end
    fprintf(1,'] | delta = %1.3e\n',delta);
end

% check the result
if delta < eps
    fprintf(1,'\nCONVERGED\n');
else
    warning('My:Wrng1','Newton did not converge, solution might be inacurate')
end

% assign output variables
X   = XOLD;
FVAL= feval(FUN,XOLD);

% nested functions - yes, it is a possibility in MATLAB
    function JAC = jacobiM(FUN,X)
    % function that returns a jacobian of a mapping FUN at X
    % FUN is a function handle (passed from newtonM, X is a point
        h   = 1e-3;                                                         %step for numerical derivation
        JAC = zeros(numel(X));                                              %allocate the output matrix
        for i = 1:numel(X)
            e_i      = zeros(numel(X),1);e_i(i) = 1;                        %create a vector of zeros with 1 at i-th place
            JAC(:,i) = (feval(FUN,X+e_i*h) - feval(FUN,X))./h;              %forward calculation of a first derivative
        end
    end
end

%% Equations to be solved
function REZ = problem1(x)
REZ     = x.^2 - 1;
end

function REZ = problem2(x)
REZ = zeros(numel(x),1);

REZ(1) = pi - x(1) + sin(x(2));
REZ(2) = 1 + cos(x(1));
end

function REZ = problem3(x,pars)
% Note: RHS corresponds to a general predator-prey model (Lotka-Voltera
% equations. Thus this problem is finding of a steady state of a SODE

REZ = zeros(numel(x),1);

% unwrap parameters
alpha = pars(1);
beta  = pars(2);
gamma = pars(3);
delta = pars(4);

% calculate the residues
REZ(1) = x(1)*(alpha - beta*x(2));
REZ(2) = -x(2).*(gamma - delta*x(1));
end