function [U,S,V,STATS] = rd2svds(A,m,n,P,k,tol)
% 
%  RD2SVDS: Computes the k largest singular value and associated singular vectors
%           of a m x n matrix A such that A*V = U*S and A'*U = V*S, V'*V=I
%           and U'*U = I and S is a diagonal matrix.
%
%  CONVERGENCE: sqrt(|| A*v - u*s||^2 + || A^T*u - v*s ||^2))<= tol*||A||
%               where norm is the 2-norm and ||A|| is approximated by largest 
%               singular value of the projected matrix B over all iterations. 
%
%  INPUT:
%   A        -  m x n numeric matrix A or an M-file ('Afunc'). If the m x n matrix A 
%               is a filename then y = Afunc(x,m,n,'transpose'). If transpose = 'F', 
%               then y = A*x. If transpose = 'T', then y = A'*x.
%   m,n      - size of the m x n matrix A
%   P        -  n x 1 "right" starting vector & first column of Lanczos matrix P - no vector P = []
%   k        - number of desird singular triplets
%  tol       - user specified tolerance - if k > 1 then tolerance is changed to
%              min(sqrt(eps), 1d-2*tol_user) for 1,..,k-1 and to tol for k
%              singular triplet
%
% OUTPUT:
%  U   - m x k  left approximate singular vectors
%  V   - n x k  right approximate singular vectors
%  S   - k x k  diagonal matrix of singular values 
%  If k failed to converged, output k-1 converged of [] 
%  STATS =
%   numMatProds:  -> number of matrix-vector products with A and A^T
%   timeMatProds: -> total time computing products with A and A^T 
%   numIterRefRestart: -> number of restarts with Iterative Refined vectors
%   timeCompIterRef:   -> time computing iterative Refined vectors
%   timeTotal:  -> total time elasped
%   estimateSVmax: -> estimate of the maximum singular value over all iterations
%   convergedKVals: -> true if ALL k singular triplets converged, otherwise false
%   maxnormres -> maximum residual of values for exit
%
%  DATE MODIFIED: 7/11/21
%  VER:  1.0
%  
% AUTHORS: 
% James Baglama     email: jbaglama@uri.edu
% Vasilije Perovic  email: perovic@uri.edu
% Jennifer Picucci  email: jenniferpicucci@uri.edu
%
% REFERENCE:
% Baglama, J, Perovic, V, and Picucci, J, "Hybrid Iterative Refined Restarted 
% Lanczos Bidiagonalization Method",  2021 submitted Numerical
% Algorithms, preprint: http://www.math.uri.edu/~jbaglama/paper34.pdf

% Start timing count using the MATLAB tic command.
tStart = tic; 

% Initialize values - maxit_outer and maxit_inner can be changed by user.
maxit_outer= 2000; maxit_inner=100; sqrteps = eps^(1/2); f = zeros(n,1);  
tol_user=tol; V=zeros(n,k); U=zeros(m,k); S=zeros(k); B = zeros(2,2); Q = zeros(m,2);
if k > 1, tol = min(sqrteps,1d-2*tol_user); end; tol = max(tol,eps); numIterRef = 0;

% Initialization of values used for STATS output.
mprod = 0;               % Number of matrix vector products with A and A^T.
IterRefRestart = 0;      % Number of restarts with Iter Refined Vectors.
timeMatProds=0;          % Total time computing matrix vector products with A and A^T.
timeIterRef = 0;         % Total time computing Iter. Ref. values.
maxnormres = -1;         % maximum norm of all converged singular triplets
convergedKVals = 'TRUE'; % Intialize to TRUE.

% Begin iter_k for loop
for iter_k = 1:k 
    
   % Set values for each k value.
   iter = 1;  Smax=0; if iter_k == k, tol = max(tol_user,eps); end 

   % Set up starting vector P(:,1)
   if isempty(P), P = randn(n,1); end; if iter_k > 1, P = f - V*(f'*V)'; end 
   P = [P/norm(P) zeros(n,1)]; 

   % Matrix-vector product, orthogonalization, and deflation to get Q(:,1)
   Astart = tic; if ischar(A), Q(:,1) = feval(A,P(:,1),m,n,'F'); else, ... 
   Q(:,1) = A*P(:,1); end; mprod = mprod + 1; timeMatProds = timeMatProds + toc(Astart);
   if iter_k > 1, Q(:,1) = Q(:,1) - U*(Q(:,1)'*U)'; end; B(1,1) = norm(Q(:,1)); 
   Q(:,1) = Q(:,1)*(1/B(1,1)); 

   % Matrix-vector product, orthogonalization, and deflation to get P(:,2)
   Astart = tic; if ischar(A), P(:,2) = feval(A,Q(:,1),m,n,'T'); else,... 
   P(:,2) = (Q(:,1)'*A)'; end; mprod = mprod + 1; timeMatProds = timeMatProds + toc(Astart);
   if iter_k > 1, P(:,2) = P(:,2) - V*(P(:,2)'*V)'; end 
   P(:,2) = P(:,2) - P(:,1)*B(1,1); B(1,2) = norm(P(:,2)); P(:,2) = P(:,2)*(1/B(1,2));

   % Matrix-vector product, orthogonalization, and deflation to get Q(:,2)
   Astart = tic; if ischar(A), Q(:,2) = feval(A,P(:,2),m,n,'F'); else, ...
   Q(:,2) = A*P(:,2); end; mprod = mprod + 1; timeMatProds = timeMatProds + toc(Astart);
   if iter_k > 1, Q(:,2) = Q(:,2) - U*(Q(:,2)'*U)'; end
   Q(:,2) =  Q(:,2) - Q(:,1)*B(1,2); B(2,2) = norm(Q(:,2)); Q(:,2) = Q(:,2)*(1/B(2,2));

   % Matrix-vector product, orthogonalization, and deflation to get f
   Astart = tic; if ischar(A), f = feval(A,Q(:,2),m,n,'T'); else, ...
   f = (Q(:,2)'*A)'; end; mprod = mprod + 1; timeMatProds = timeMatProds + toc(Astart);
   if iter_k > 1, f = f - V*(f'*V)'; end; f = f - P(:,2)*B(2,2); 
   f = f - P*(f'*P)'; fnorm = norm(f);

   % Begin main iter while loop
   while (iter <= maxit_outer) 
    
       % Compute the largest singular value and associated vector of B. 
       [u_b,s_b,v_b] = svd(B); v_b = v_b(:,1); u_b = u_b(:,1); s_b = s_b(1,1); Smax = max(Smax,s_b);
       
       % Compute iterative refined Ritz value/vectors.
       IRstart = tic;
       iter_refined = 0; rho = Smax^2; rho_0 = rho; rconv = 0; v_rf_0 = zeros(2,1);
       T = B'*B; B_aug = [T; 0 B(2,2)*fnorm]; diff_rho_0 = -1;
       for i=1:maxit_inner
          B_aug(1,1) = T(1,1) - rho; B_aug(2,2) = T(2,2) - rho;
          [u_rf,s_rf,v_rf] = svd(B_aug,0);
          v_rf = v_rf(:,2); s_rf = s_rf(2,2); rho = norm(B*v_rf)^2;  
          diff_rho = abs((rho_0 - rho)/max(rho,rho_0)); 
          if (diff_rho < eps && abs(v_rf'*u_rf(1:2,2)) < sqrteps) ...
          || norm(abs(v_rf_0) - abs(v_rf)) < eps || s_rf < eps
             u_rf = B*v_rf; alpha = norm(u_rf); u_rf = u_rf/alpha; 
             rho = sqrt(rho); rconv = 1; break; % Converge iter. ref. exit.
          end 
          if abs(diff_rho_0 - diff_rho)/max(diff_rho,eps) < eps, break; end  % Stagnate exit
          if i >= 10 && mod(i,10) == 0, diff_rho_0 = diff_rho; end 
          rho_0 = rho; v_rf_0 = v_rf; 
       end
       if abs(v_b'*v_rf) > 0.9 && rconv == 1 && iter>1, iter_refined = 1; end
       timeIterRef = timeIterRef + toc(IRstart);
      
       % Convergence check.
       if ~iter_refined 
         B_aug = [-Smax*eye(2,2) B; B' -Smax*eye(2,2); zeros(1,4)]; B_aug(5,2) = fnorm; 
         [~,~,v] = svd(B_aug,0); v_c = v(3:4,4); v_c = v_c/norm(v_c);
         u_c = v(1:2,4); u_c = u_c/norm(u_c);  
       else
         v_c = v_rf; u_c = u_rf;
       end    
       rho_c = u_c'*B*v_c; 
       norm_res = sqrt(norm(B*v_c - rho_c*u_c)^2 + norm(B'*u_c - rho_c*v_c)^2 + (u_c(2)*fnorm)^2);
       if norm_res < tol*max(Smax,S(1,1)) &&  abs(v_b'*v_c) > 0.9
           maxnormres = max(maxnormres,norm_res); break; % Converge exit
       end 
      
       if iter_refined 
           
         % Count number of iter. refined restarts.
         numIterRef = numIterRef +1;
         
         % Restart with iterative refined Ritz value/vectors.
         f = P*(B'*u_rf - alpha*v_rf) + f*u_rf(2); B=[]; B(1,1) = alpha; 
         P(:,1) = P*v_rf; Q(:,1) = Q*u_rf; f = f - (P(:,1)'*f)*P(:,1);
         B(1,2) = norm(f); P(:,2) = f*(1/B(1,2));
   
      else
      
        % Restart with Ritz value/vectors 
        P = [P*v_b f*(1/fnorm)]; Q(:,1) = Q*u_b;
        B(1,1) = s_b; B(1,2) = fnorm*u_b(2);
    
      end
  
      % Matrix-vector product, orthogonalization, and deflation to get Q(:,2)
      Astart = tic; if ischar(A), Q(:,2) = feval(A,P(:,2),m,n,'F'); else,... 
      Q(:,2) = A*P(:,2); end; mprod = mprod + 1; timeMatProds = timeMatProds + toc(Astart);
      if iter_k > 1, Q(:,2) = Q(:,2) - U*(Q(:,2)'*U)'; end  
      Q(:,2) = Q(:,2) - Q(:,1)*(Q(:,2)'*Q(:,1));   
      B(2,2) = norm(Q(:,2)); Q(:,2) = Q(:,2)*(1/B(2,2));
  
     % Matrix-vector product, orthogonalization, and deflation to get f
     Astart = tic; if ischar(A), f = feval(A,Q(:,2),m,n,'T'); else, ...
     f = (Q(:,2)'*A)'; end; mprod = mprod + 1;timeMatProds = timeMatProds + toc(Astart);
     if iter_k > 1, f = f - V*(f'*V)'; end; f = f - P(:,2)*B(2,2); 
     fnorm = norm(f); 
  
     iter = iter+1;
 
   end % end: iter while loop
   
   % Deflation or output.
   U(:,iter_k)= Q*u_c; V(:,iter_k)= P*v_c; S(iter_k,iter_k) = rho_c; 
   if iter >= maxit_outer, convergedKVals='FALSE'; iter_k = iter_k-1; break; end       
end % end: iter_k for loop
if iter_k >= 1 
    U = U(:,1:iter_k); V = V(:,1:iter_k); S = S(1:iter_k,1:iter_k);
else
    U=[]; V=[]; S=[]; 
end
STATS.numMatProds = mprod;
STATS.timeMatProds = timeMatProds;
STATS.numIterRefRestart = numIterRef;
STATS.timeCompIterRef = timeIterRef;
STATS.timeTotal = toc(tStart);
STATS.estimateSVmax = Smax;
STATS.convergedKVals = convergedKVals;
STATS.maxnormres = maxnormres;
end % end: function