%% Caller 'script'
function caller
close all

% solving problem #2 - defined as BVP

% define the BVP, we chose boundary conditions in such way that out known
% exact solution would be usable as reference
TSPAN   = [0.001 1];                                                            %first I define a TSPAN
YL      = exactSol2(TSPAN(1));                                              %value of exact solution at left side of TSPAN
YR      = exactSol2(TSPAN(2));                                              %value of exact solution at right side of TSPAN
% BC      = [YL(1) YR(1);NaN NaN];                                            %boundary conditions (NxN matrix)
% BC      = [NaN NaN;YL(2) YR(2)];                                            %boundary conditions (NxN matrix)
% BC      = [YL(1) NaN;NaN YR(2)];                                            %boundary conditions (NxN matrix)
BC      = [NaN YR;NaN YL];                                            %boundary conditions (NxN matrix)
nSteps  = 20;                                                               %define number of steps to be used in solver

% create initial guess for the BVP solver
% Y0Guess = [YL(1) 0.9];                                                      %Y0 = [2/3 1]; - corresponding initial condition
Y0Guess = [0.01 YR(2)];

[TOUT,YOUTBVP4]  =   bvp4M(@problem2,TSPAN,BC,Y0Guess,nSteps);

figure
plot(TOUT,exactSol2(TOUT));hold on;
plot(TOUT,YOUTBVP4,'s');
legend('xExact','yExact','xBVP4','yBVP4','Location','Best')
axis tight
title('Solution of problem 2','FontSize',18,'FontWeight','Bold')
end

%% Methods

% boundary value problem solver (4th order)
function [TOUT,YOUT] = bvp4M(ODEFUN,TSPAN,BC,Y0G,nSteps)
% ODEFUN ... handle to a function defining the solved SODE
% TPSAN  ... time span for the independent variable
% BC     ... boundary condition (I need to have N BC for system of N ODE)
% Y0G    ... initial guess for the initial condition
% nSteps ... number of steps to use in embedded ODE solver, implicitely
%            defines the step length and is much easier to implement
%
% note: step length, h = (TSPAN(1)-TSPAN(2))/nSteps

% transpose BC - writing it in rows is more natural, but for the solution
% structure, it is better to have it in columns
BC = BC';

% allocate variables
TOUT = linspace(min(TSPAN),max(TSPAN),nSteps);                              %I can create this vector right away

% resave missing initial conditions in a special variable
Y0Miss = Y0G(isnan(BC(1,:)));

% find suitable initial condition (the missing ones)
Y0Miss = newtonM(@getReziduals,Y0Miss);                                     %at the time, call with built in pars is enough

% get together the found conditions with what is known
Y0G(isnan(BC(1,:))) = Y0Miss;

% get the actual solution
[t,YOUT] = rk4M(ODEFUN,TSPAN,Y0G,nSteps);

% nested functions
    function rez = getReziduals(X)
        IC       = Y0G;IC(isnan(BC(1,:))) = X;                              %construct initial condition
        [t,YVOL] = rk4M(ODEFUN,TSPAN,IC,nSteps);
%         rez = YVOL(end,~isnan(BC(2,:))) - BC(2,~isnan(BC(2,:)));            %subtract only defined boundary conditions
        rez = (YVOL(end,~isnan(BC(2,:))) - BC(2,~isnan(BC(2,:))))./...      %why does this work better?
            abs(BC(2,~isnan(BC(2,:))));
        rez = rez';                                                         %I need to return a column vector for newton
    end

end

% Runge-Kutta method (4th order)
function [TOUT,YOUT] = rk4M(ODEFUN,TSPAN,Y0,nSteps)                         %with tspan we input time step (not adaptive)
% calculate needed the step length
H = (max(TSPAN)-min(TSPAN))/nSteps;                                         %I will end exactly in TEND BUT H is not pretty

% allocate variables
TOUT = linspace(min(TSPAN),max(TSPAN),nSteps);                              %I can create this vector right away
YOUT = zeros(nSteps,numel(Y0));                                             %but I can only allocate this

% save initial condition
TOUT(1) = min(TSPAN);YOUT(1,:) = reshape(Y0,1,numel(Y0));

for i = 2:nSteps
   k1 = H*reshape(feval(ODEFUN,TOUT(i-1),YOUT(i-1,:)),1,numel(Y0));
   k2 = H*reshape(feval(ODEFUN,TOUT(i-1)+0.5*H,YOUT(i-1,:)+0.5*k1),1,numel(Y0));
   k3 = H*reshape(feval(ODEFUN,TOUT(i-1)+0.5*H,YOUT(i-1,:)+0.5*k2),1,numel(Y0));
   k4 = H*reshape(feval(ODEFUN,TOUT(i-1)+1*H,YOUT(i-1,:)+1*k3),1,numel(Y0));
   YOUT(i,:) = YOUT(i-1,:) + 1/6*(k1+k4) + 1/3*(k2+k3);
end
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

%% Problem definitions

% second equation to be solved (x in R2)
function dxdt = problem2(t,x)                                               %%I do not need to input t -> ~

dxdt = zeros(numel(x),1);                                                   %allocate space for output (general)
fi=1;
dxdt(1) = (-2/t)*x(2)+fi^2*x(1);
dxdt(2) = x(2);
%problem specific
end

% exact solution to the problem 2
function xExact = exactSol2(t)

xExact = zeros(numel(t),2);
fi=1;
% xExact(:,1) = exp(t) - exp(-2*t)./3;
% xExact(:,2) = exp(-t);
xExact(:,1)=1./t .*(exp(t*fi)-exp(-t*fi))/(exp(fi)-exp(-fi));
xExact(:,2)=1./t .*(exp(t*fi)-exp(-t*fi))/(exp(fi)-exp(-fi));
end