function [y,t,itot] = hbvmks( fun, y0, k, s, h, N )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%    [y,t,itot] = hbvmks( fun, y0, k, s, h, N )
%
%    y1 = fun(t,y) calcola il secondo membro dell'equazione
%     y' = f(t,y)  anche in forma vettoriale.
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
warning off backtrace
m  = length(y0);
y = zeros(N+1,m);
t = h*[0:N]';
y(1,:) = y0(:).'; 
[xi,beta,II,PPO,PP,XX] = makePIO(k,s);  %% HBVM(k,s) 
H0 = feval(fun,y(1,:)); 
itot = 0; 
for i = 1:N
   [y(i+1,:),it] = itera1( h, h*xi, h*II, PPO, fun, t(i), y(i,:) );
   if flag, warning(sprintf('main: step %i, at time %g',i,t(i))); end
   itot = itot +it;    
end 
disp(sprintf('number of fixed-point iterations = %i',itot));
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% AUXILIARY FUNCTIONS %%%%%%%%%%%%%%%%%%%

function L = computeL( fun, t0, y0 )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  L = computeL( fun, t0, y0 )    Estimates the local Lipschitz constant.
%
%  Rel. June 25, 2011.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m  = length(y0);
f0 = feval( fun, t0, y0 );
s  = rand(size(y0));
s  = (s>.5)-(s<.5);
dy = s.*sqrt( eps*max(1e-5,abs(y0)) );
f0 = feval( fun, t0, y0+dy )-f0;
L  = norm(f0,inf)/norm(dy,inf);
return


function [y,iter] = itera1( h, hxi, hII, PPO, fun, t0, y0 )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m     = length(y0);
[r,s] = size(hII);
tol0  = sqrt(m)*eps;   
t     = t0+hxi;        
ey0   = ones(r,1)*y0;   
LL    = max( computeL( fun, t0, y0 ), 10 );
tol   = LL*tol0;  
ny0   = diag(1./(1+abs(y0)));
flag  = 1;
fatt  = 50;
err   = Inf;
itmax = fatt*r*m;
iter  = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y     = ey0;
for iter = 1:itmax
    BG   = PPO*feval(fun,t,y);
    err0 = err;             
    yold = y;  
    y    = ey0 +hII*BG;
    err  = norm( (y-yold)*ny0, inf );    
    if err<=tol0 || ( err>=err0 && err0<=tol ), flag = 0; break, end 
end
y = y0 + h*BG(1,:); %% new approximation
if flag
   warning(sprintf('itera: convergence error = %g',err)) 
end
return

%%%%%%%%%%%%%%%%%%%%%%%%% "STABLE" FUNCTIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,beta,II,PPO,PP,XX] = makePIO(k,s)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   [x,beta,II,PPO,PP] = makePIO(k,s)  
%
%   Computes the Gauss-Legendre abscissae and weights (x, beta) on [0,1],
%   along with the matrices required by the hbvm(k,s) method:
%
%   PP = (P_j(x_i)), II = (int_0^x_i P_j)  kxs, and PPO = PP'*diag(beta).
%
%   Rel. February 16, 2011.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[x,beta,y] = lgwt01(k); [x,ind] = sort(x); beta = beta(ind); y = y(ind);
II = zeros(k,s+1);
for i = 1:s+1
    tmp = legendre(i-1,y); II(:,i) = sqrt(2*i-1)*tmp(1,:).';
end   
ii  = 2*sqrt(4*[1:s].^2-1);
XX  = diag(1./ii, -1 );
XX  = XX-XX.';
XX(1,1) = 1/2;
PP  = II(:,1:s);
PPO = PP'*diag(beta);   
II  = II*XX(:,1:s);  
XX  = XX(1:s,1:s);
return

function [x,w,y] = lgwt01(N)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lgwt01.m
%
% This script computes the Legendre-Gauss nodes and weights on the
% interval [0,1] with truncation order N.
%
% Adapted from the file written by Greg von Winckel - 02/25/2004.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a = 0; b = 1;  %%%%%%%%% normalization on the interval [0,1]
N = N-1;
N1 = N+1; N2 = N+2;
xu = linspace(-1,1,N1)';
% Initial guess
y = cos((2*(0:N)'+1)*pi/(2*N+2))+(0.27/N1)*sin(pi*xu*N/N2);
% Legendre-Gauss Vandermonde Matrix
L = zeros(N1,N2);
% Derivative of LGVM
Lp = zeros(N1,N2);
% Compute the zeros of the N+1 Legendre Polynomial
% using the recursion relation and the Newton-Raphson method
y0 = 2;
% Iterate until new points are uniformly within epsilon of old points
while max(abs(y-y0))>eps
    L(:,1) = 1; Lp(:,1) = 0;
    L(:,2) = y; Lp(:,2)=1;
    for k=2:N1
        L(:,k+1) = ( (2*k-1)*y.*L(:,k)-(k-1)*L(:,k-1) )/k;
    end
    Lp = (N2)*( L(:,N1)-y.*L(:,N2) )./(1-y.^2);   
    y0 = y;  y = y0-L(:,N2)./Lp;
end
% Linear map from[-1,1] to [a,b]
x = (a*(1-y)+b*(1+y))/2;      
% Compute the weights
w = (b-a)./((1-y.^2).*Lp.^2)*(N2/N1)^2; 
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%% END OF AUXILIARY FUNCTIONS %%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%