function [Z1,time_tot,time_res]=SKSM_cTri(A,B,m,tol)
%function [Z1,time_tot,time_res]=SKSM_cTri(A,B,m,tol)
%Solution of the Lyapunov equation 
%
% A*X+X*A+B*B^T=0,    A sym neg.def., B tall
%
% by the (standard) block Krylov subspace method (SKSM) with Galerkin condition,
% with enhanced computational strategy for the residual norm computation.
%
% Reference manuscript:
%
% Davide Palitta and Valeria Simoncini
% Computationally enhanced projection methods for symmetric Sylvester and Lyapunov matrix equations
% February 2016, ArXiv: 1602.05033
%
% email: davide.palitta3@unibo.it, valeria.simoncini@unibo.it
%
% REQUIREMENT: the mex file cTri.mexa64 makes use of LAPACK and C-BLAS subroutines. 
% 
%
% INPUT
% A: sym negative def. matrix  
% B: rhs matrix 
% m: max number of iterations
% tol: final residual accuracy (relative residual F-norm)
%
% OUTPUT
% Z1: tall matrix such that Z1*Z1^T is the approx solution (Z1 is truncated to sing.val >1e-12)
% time_tot: total cputime
% time_res: total time requested to compute the residual norm
%
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 

% Check the input
if norm(A-A','fro')>1e-12
    error('The coefficient matrix must be symmetric')
end

tic;
[n,s]=size(B); 
nrmb=norm(B,'fro')^2;

% Orthonormalize B
[U(1:n,1:s),beta]=qr(B,0);
gamma=beta';
gamma=full(tril(gamma)); 
beta2=beta*beta';

% Preallocation
T=zeros((m+1)*s,m*s);
TT=zeros(s+1,m*s);
time_res=zeros(m,1);
time_subtot=0;
        
t1=toc;
for j=1:m,
    tic
    jms=(j-1)*s+1;j1s=(j+1)*s;js=j*s;js1=js+1;
    
    %new basis block (double modified gram-schmidt)
    Up = A*U(1:n,end-s+1:end);
  
    for l=1:2
        K=min([j, 3]);
        for kk=1:K
              k1=(kk-1)*s+1; k2=kk*s;
              coeff= U(1:n,k1:k2)'*Up;
              T((j-K+kk-1)*s+1:(j-K+kk)*s,jms:js) = T((j-K+kk-1)*s+1:(j-K+kk)*s,jms:js)+ coeff; 
              Up = Up - U(1:n,k1:k2)*coeff; 
        end
    end
    
  
    if (j<2)
        [U(1:n,s+1:2*s),T(js1:j1s,jms:js)]=qr(Up,0);
        
        % prepare LL for dstrd
        for i=1:s
            TT(end-i+1,i:s)=diag(T(jms:js,jms:js),i-1);
        end
        LL=TT(:,1:j*s);
        
    else
        % move the blocks
        if j>2
            U(1:n,1:2*s)=U(1:n,s+1:3*s);
        end
        [U(1:n,2*s+1:3*s),T(js1:j1s,jms:js)]=qr(Up,0);
    
        % prepare LL for dstrd
        HH=zeros(s+1,s);
        for i=1:s+1
            HH(end-i+1,1:s)=diag(T(js-s+2-i:js-i+1,js-s+1:js));
        end
        TT(:,(j-1)*s+1:j*s)=HH;
        LL=TT(:,1:j*s);
    
    end
    t2=toc;
        
    tic
    % take the ((j+1)s,js) block of T
    tau = T(js1:j1s,jms:js);             
    
    % Set the matrix E. This matrix will be passed to dsbtrd in order to compute 
    % only the first and last s rows of the transformation matrix which tridiagonalizes T
    E=zeros(js,js);
    E(1:s,1:s)=speye(s);
    E(end-s+1:end,end-s+1:end)=speye(s);
    
    % Call cTri
    res=cTri(LL,E,tau',gamma);
    time_res(j)=toc;

    fprintf('It: %3d, Relative Res: %10.5e\n', j, res/nrmb)
    time_subtot=time_subtot+t2;
    
    if (res/nrmb<tol), 
         break
    end
    

end
tic
it=j;


% Compute the full eigendecomposition of T and the matrix Y
[Q,lambda]=eig(full(T(1:js,1:js)));
lambda=diag(lambda);
Q1=Q(1:s,:);
Y=-((Q1'*beta2)*Q1)./(lambda*ones(1,js)+ones(js,1)*lambda');

% Truncation strategy
[uY,sY]=eig(Y); [sY,id]=sort(abs(diag(sY)));
sY=flipud(sY);
uY=uY(:,id(end:-1:1));
is=sum(abs(sY)>1e-12);
Y0 = Q*(uY(:,1:is)*diag(sqrt(sY(1:is))));
t4=toc;

% two-pass
tic
U=[];
[U(1:n,1:s),~]=qr(B,0);
Z1=U(1:n,1:s)*Y0(1:s,:);

% separe j=1,2 from the other j's to avoid "if" within the "for" loop
j=1;
j1s=2*s;
js1=s+1;
Up = A*U(1:n,end-s+1:end);
U(1:n,s+1:2*s)=(Up-U(1:n,end-s+1:end)*T((j-1)*s+1:j*s,(j-1)*s+1:j*s))/T(j*s+1:(j+1)*s,(j-1)*s+1:j*s);
Z1=Z1+U(:,end-s+1:end)*Y0(js1:j1s,:);

j=2;
j1s=3*s;
js1=2*s+1;
Up = A*U(1:n,end-s+1:end);
U(1:n,2*s+1:3*s)=(Up-U(1:n,1:2*s)*T((j-2)*s+1:j*s,(j-1)*s+1:j*s))/T(j*s+1:(j+1)*s,(j-1)*s+1:j*s);
Z1=Z1+U(:,end-s+1:end)*Y0(js1:j1s,:);

for j=3:it-1
    j1s=(j+1)*s;
    js1=j*s+1;
    
    Up = A*U(1:n,end-s+1:end);
    U(1:n,1:2*s)=U(1:n,s+1:3*s);
    U(1:n,2*s+1:3*s)=(Up-U(1:n,1:2*s)*T((j-2)*s+1:j*s,(j-1)*s+1:j*s))/T(j*s+1:(j+1)*s,(j-1)*s+1:j*s);
    
    Z1=Z1+U(:,end-s+1:end)*Y0(js1:j1s,:);
end
time_twopass=toc;
fprintf('\nWhole V_m: %4d', it*s)
fprintf('\nStored basis vectors: %4d', 3*s)
fprintf('\ntime_twopass: %10.5e',time_twopass)


time_subtot=time_subtot+t4+t1+time_twopass;
time_res=sum(time_res);
time_tot=time_subtot+time_res;
      
