function [V, RP, RQ, H, rk]  = TOAR(funav, funbv, n, k, v0)
% V = TOAR(FUNAV, FUNBV, N, K, V0)
%   computes an orthonormal basis V of the second-order Krylov subspace:
%     span{ V } = G_k(A,B,v_0).
%   using Two-Level Orthogonal ARnoldi process.  
%
% Input:
%   n    :   size of matrices A and B.
%   funav:   a fuction handle y = funav(x) for y = A*x,
%            where x and y are length-n vectors.
%   funbv:   function handle for y = B*x. (see funav)
%   k    :   dimension of second-order Krylov subspace.
%   v0   :   starting vector, of size n-by-1
%
% [V, RP, RQ, H, rk] = TOAR(FUNAV, FUNBV, N, K, V0)
%   also return the coefficient matrices R and H of the corresponding 
%   compact Arnoldi decomposition.
%
% References:
%   [1] Yangfeng Su, and Zhaojun Bai, SOAR: a second-order Arnoldi 
%       method for the solution of the quadratic eigenvalue problem,
%       SIAM J. Matrix Anal. Appl., 2005, 26(3): 640-659.
%   [2] Ding Lu, Yangfeng Su, and Zhaojun Bai, Stability Analysis 
%       of the two-level orthogonal Arnoldi procedure,
%       SIAM J. Matrix Anal. Appl., 2016. 37(1): 195--214.
%
% Author:
%   Ding Lu, Fudan Univ. 2014/6/30.
%


% Deflation tolerences
tol = 1.0*10-12;

% Prepare workspace
V  = zeros(n,k);
RP = zeros(k+1,k+1);
RQ = zeros(k+1,k+1);
H  = zeros(k+1,k);
rk = 0;

% Initilization 
v0 = v0/norm(v0);
V(:,1) = v0;
RP(1,1) = 1.0;
rk = 1;
 
p = v0;
q = zeros(n,1);
 
% Main loop
for i = 1:k-1
     % matrix vector product
     p = funav(p) + funbv(q);
     alpha1 = norm(p);
     
     % level-one orthogonalization: MGS
     x = zeros(rk,1);
     for jj = 1: rk
         x(jj) = V(:,jj)'*p;
         p = p -x(jj)*V(:,jj);
     end
      
     % re-orth
     if norm(p) < 0.717*alpha1
     	for jj = 1: rk
            tmp = V(:,jj)'*p;
	        p = p -tmp*V(:,jj);
            x(jj) = tmp + x(jj);
        end
     end
     
     alpha = norm(p);
     
     if alpha>tol
         nrk = rk +1;
         V(:,nrk) = p/alpha;
     else
         nrk = rk;
         disp('BREAK');
     end
        
     % level-two orthogonalization: MGS
     h =  zeros(i,1);
     sp = x;
     sq = RP(1:rk,i);
     alpha1 = norm([sp;sq]);
     for jj = 1:i
         h(jj) = RP(1:rk,jj)'*sp+RQ(1:rk,jj)'*sq;    
         sp = sp - RP(1:rk,jj)*h(jj);
         sq = sq - RQ(1:rk,jj)*h(jj);
     end
     % re-orth
     if norm([sp;sq]) < 0.717* alpha1
     	for jj = 1:i
            tmp = RP(1:rk,jj)'*sp+RQ(1:rk,jj)'*sq;    
	        sp = sp - RP(1:rk,jj)*tmp;
        	sq = sq - RQ(1:rk,jj)*tmp;
            h(jj) = tmp + h(jj);
        end
     end
     
     beta = norm([sp;sq;alpha]);
     if beta>tol
        sp = sp/beta;
        sq = sq/beta;
        alpha = alpha/beta;
     else
        disp('break');
     end 
     RP(1:rk,i+1) = sp;
     RQ(1:rk,i+1) = sq;
     RP(rk+1,i+1)   = alpha;
     H(1:i+1,i) = [h;beta];
     p = V(:,1:rk+1)*[sp;alpha];
     q = V(:,1:rk)*sq;
     rk = nrk;
 end

 %END
