function [Q,P,T] = SOAR(av, bv, N, n, q1)
% Q = SOAR(av, bv, N, n, q1)
%   computes an orthonormal basis Q of the second-order Krylov subspace:
%     span{ Q } = G_k(A,B,v_0).
%   using space-efficient SOAR procedure 
%
% 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)
%   n    :   dimension of second-order Krylov subspace.
%   v0   :   starting vector, of size n-by-1
%
% [Q,P,T] = SOAR(av, bv, N, n, q1)
%   also return the coefficient matrices P and T of the corresponding 
%   compact Krylov 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.
%
% Author:
%   Ding Lu, Fudan Univ. 2014/6/30.
%

Q = zeros(N,n+1);
P = zeros(N,n+1); 
T = zeros(n+1,n+1);

tol = 1.0*10^-12; % breakdown threshold

% initilize..
q1 = q1/norm(q1);
Q(:,1) = q1;
f = zeros(N,1);
P(:,1) = f;

for j = 1:n
    r = av(Q(:,j)) + bv(f); %Mat-Vec Multiplication
    alpha1 = norm(r);
    % MGS
    t = zeros(j,1);
    for jj = 1:j
        t(jj) = Q(:,jj)'*r;
        r = r - t(jj)*Q(:,jj); 
    end
    % re-orth
    if norm(r) < 0.717*alpha1 
        for jj = 1:j
            tt = Q(:,jj)'*r;
            r = r - tt*Q(:,jj); 
            t(jj) = t(jj) + tt;
        end
    end
    
    T(1:j,j) = t;
    beta = norm(r);
    T(j+1,j) = beta;
    
    % check for breakdown
    if beta>tol
        Q(:,j+1) = r / beta;
        ej=zeros(j,1);
        ej(j) = 1;
        f = Q(:,1:j)*(T(2:j+1,1:j)\ej);
    else
        T(j+1,j) = 1;
        Q(:,j+1) = zeros(N,1);
        ej=zeros(j,1);
        ej(j) = 1;
        f = Q(:,1:j)*(T(2:j+1,1:j)\ej);
        error('Breakdown in SOAR');
    end
    P(:,j+1) = f;
end
%END
