function [y,t] = bhbvm( fun, y0, k, s, h, n, nob )
%
% [y,[t]] = bhbvm( fun, y0, k, s, h, n, [nob] )
%
%                                          Fixed stepsize implementation of
% n integration steps with the  HBVM(k,s)  method, used with fixed stepsize 
% h, on the Hamiltonian problem described by the function fun, and starting 
% at y0. The argument fun has to be a string with the name of the function. 
% The discrete problem is solved by a blended iteration, when the last par-
% ameter nob is not specified, or functional iteration, otherwise. 
% N.B.: a numerical Jacobian is used.
% The problem must be a canonical (autonomous) Hamiltonian one, i.e.,
%
% q' = \nabla_p H(p,q),     p' = -\nabla_q H(p,q),
%
% where  H(p,q) is the Hamiltonian function. The function  fun has to be in 
% the form:
%
% y1 = fun(t,y)   % computes the rhs of y'=f(t,y), also in vector mode
%
% with   y(i,:) = [q(i,:) p(i,:)]   or   y(i,:) = [p(i,:) q(i,:)].
% Moreover,  fun(y) (i.e., with only 1 argument),  must return the value of 
% the  Hamiltonian  function at  y.  As an example,  the following function 
% implements the harmonic oscillator.
%
%  function y1 = pendulum( t, y )
%  %
%  %  Harmonic oscillator.
%  %
%  if nargin==2  % derivative
%     m = length(t);
%     if m==1, y = y(:).'; end
%     p = y(1:m,1);
%     q = y(1:m,2);
%     y1 = [-q p];
%  else % Hamiltonian function
%     y  = t;
%     [m,n] = size(y);
%     if min([m n])==1, y = y(:).'; m = 1; end
%     p = y(1:m,1);
%     q = y(1:m,2);
%     y1 = .5*( p.*p + q.*q );
%  end    
%  return
%
% Example of calling sequence:
%
%  y = bhbvm('pendulum',[1 0],2,2,pi/8,100);
%
%________________________________________________________
% B.I.T.                                 Rel: 2010-06-30.
% http://web.math.unifi.it/users/brugnano/HBVM/index.html
% arXiv:1002.2757
%________________________________________________________
warning off backtrace
nob    = nargin==7;  % noblended: functional iteration is used.
m      = length(y0);
y      = zeros(n+1,m);
y(1,:) = y0(:).';
r      = k-s;
t      = h*[0:n]'; 
[xi,beta,A,B,inds,indr] = ghbvmks2(k,s);  %% hbvm at Gaussian nodes
a1 = -A(s+1:k,1);    A1 = -A(s+1:k,inds+1); 
B1 = B(1:s,inds+1);  B2 = B(1:s,indr+1);  
if nob==0, C  = B1 +B2*A1;    gamma = min(abs(eig(C)));  condC = 1/cond(C);
           C = gamma*inv(C);  gamma = h*gamma;   
else, J0 = []; C = []; condC = 1;  
end
B1 = h*B1; B2 = h*B2; beta = h*beta(:).';  xi = h*xi; 
H0   = feval( fun, y(1,:) );  %% initial Hamiltonian
itot = 0;     t0 = 0;
for i = 1:n, %%disp(i)
   if nob==0
      [J0,f0] = numjacobian( fun, t(i), y(i,:) );
      J0 = gamma*J0.';  %% the transpose is needed
   else
      f0 = feval( fun, t(i), y(i,:) ); 
   end 
   [y(i+1,:),flag,it] = itera( xi, beta, a1, A1, B1, B2, C, inds, ...
                               indr, fun, t(i), y(i,:), f0, J0, condC ); 
    if flag==1, warning(sprintf('step %i',i)), 
       if sum(isnan(y(i+1,:))), warning('NaN: Exit'), y = y(1:i,:);
          if nargout==2, t = t(1:i); end, break, end
    end
    if r>0, y(i+1,:) = correggiH( fun, t(i+1), y(i+1,:), H0 ); end   
    itot = itot +it;
end  
if nob, disp(sprintf('number of functional iterations  = %i',itot)); 
else,      disp(sprintf('number of blended iterations  = %i',itot)); 
end
return


function [Jf,f0] = numjacobian( fun, t0, y0 )
%
% NUMERICAL JACOBIAN  (taken from the code BiM)
%
m  = length(y0);
Jf = zeros(m);
f0 = feval( fun, t0, y0 ); f0 = f0(:);
for i =1:m
  ysafe = y0(i);
  delt  = sqrt( eps*max(1e-5,abs(ysafe)) );
  y0(i) = ysafe +delt;
  f1    = feval( fun, t0, y0 );
  Jf(:,i) = ( f1(:) -f0 )/delt;
  y0(i) = ysafe;
end
return


function [y,flag,iter] = itera( xi, hbeta, a1, A1, hB1, hB2, C, ...
                                   inds, indr, fun, t0, y0, f0, J0, condC )
%
%  Blended or functional iteration for solving the discrete problem.
%
blen  = prod(size(J0))>0;
m     = length(y0);
r     = length(xi);
if blen, teta = inv( eye(m) -J0 );  end  
fatt  = 50;
err   = Inf;
sil   = length(indr)>0;
itmax = fatt*r*m;
iter  = 0;
s     = length(inds);
y1    = kron(ones(s,1),y0);
L     = max( 2*max(norm(f0),1)/max(norm(y0),1), 10 );
tol0  = max( sqrt(m)*condC, 1 )*eps;   if blen, tol0 = tol0/2; end
tol   = L*tol0;  
rhs1  = ones(s,1)*y0; 
t1    = t0+xi(inds);
if sil,  t2 = t0+xi(indr); rhs2 = a1*y0;  end   
ny0   = max(eps,norm(y0,inf));
flag  = 0;
while 1
    yold = y1;
    f1   = feval( fun, t1, y1 );
    BF   = rhs1 +hB1*f1; 
    if sil 
       y2 = rhs2 +A1*y1; 
       f2 = feval( fun, t2, y2 );  
       BF = BF +hB2*f2;
    end   
    if blen  % blended iteration
       F1   = y1 -BF;     F2 = C*F1;
       y1   = y1 -( F2 +( F1-F2 )*teta )*teta; 
    else     % functional iteration
       y1   = BF;        
    end
    err0 = err;   err  = norm( y1-yold, inf )/ny0; 
    if err<=tol0, break, end 
    if err>=err0 && err0<=tol, y1 = yold; break, end
    iter = iter+1; if iter>itmax, flag = 1; break, end
end
y(inds,:) = y1; t(inds,:) = t1;
if sil>0, y(indr,:) = rhs2 +A1*y1; t(indr,:) = t2; end
y  = y0 +hbeta*feval( fun, t, y );  %% new approximation
if flag
   warning(sprintf('Iteration convergence: error = %g',err)) 
end
return


function y = correggiH( fun, t, y, H0 )
%
%  Hamiltonian correction, if of the order of a multiple of eps.
%
d = feval( fun, t, y ); 
d = d(:).';  nd = d*d';
m = length(d)/2;
d = [-d(m+1:end) d(1:m)];  % the problem is in canonical form
H    = feval( fun, y );  % current Hamiltonian
alfa = (H0-H)/nd;
H1   = feval( fun, y+alfa*d );
H2   = feval( fun, y-alfa*d );
if min( [ abs(H0-H1), abs(H0-H2), abs(H-H0)/4 ] )>100*m*eps*max(1,abs(H0)) 
   return  % no update: difference too large.
else
   if     abs(H1-H0)>abs(H2-H0), y = y-alfa*d;
   elseif abs(H2-H0)>abs(H1-H0), y = y+alfa*d;
   end    
end
return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,beta,A,B,inds,indr] = ghbvmks2(k,s)
%
%   [x,beta,A,B,inds,indr] = ghbvmks2(k,s)  
%
%   Computes the Gauss-Legendre abscissae and weights x and beta on [0,1],
%   along with the pencil (A,B) of the hbvm(k,s) method, with basic points
%   at indexes inds and silent ones at indr.
%
%%%%%%%%%%%% number of silent points
r = k-s;   
%%%%%%%%%%%% Gauss-Legendre abscissae and weights on [0,1]
[x,beta] = lgwt01(k); [x,ind] = sort(x); beta = beta(ind);
%%%% indexes of the basic and silent points
if r>0
   [inds,indr] = trova( x, (1:s)/(s+1) );
else         %%%% no silent point
   inds = 1:s;       indr = [];
end                   
%%%% Vandermonde matrices and Integrals
W  = zeros(k,s+1);  
for i = 1:s+1
    y = legendre(i-1,2*x-1); W(:,i) = y(1,:).';
end   
W1 = zeros(k,s);
W1(:,1) = x; 
W1(:,2:s) = ( W(:,3:s+1) -W(:,1:s-1) )/2;   
%%%%%%%%%%%%%%% (A,B): basic points
A                 = zeros(k,k+1); B = A;
A(1:s,[1 inds+1]) = [-ones(s,1) eye(s)];   
B(1:s,2:end)      = W1(inds,1:s)*( W(:,1:s).' )*diag(beta);
%%%%%%%%%%%%%%% A: silent points
A(s+1:k,[1 inds+1]) = -W1(indr,1:s)*( W1(inds,1:s)\A(1:s,[1 inds+1]) );  
A(s+1:k,1)        = A(s+1:k,1) -1;
A(s+1:k,indr+1)   = eye(r);
%%%%%%%%%%%%%%% end
return


function [inds,indr] = trova( x, xs )
%
%  Computes the indexes inds of the basic points, 
%  and those of the silent ones, indr.
%
indr  = 1:length(x);
inds  = [];
xx    = x;
for i = 1:length(xs)
   [m,ind]   = min( abs( xx-xs(i) ) );
   inds      = [inds indr(ind)];
   indr(ind) = [];
   xx(ind)   = [];
end
return


function [x,w] = 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
