function  [X,out]  =  block_gcrodr(Aop,B,opts,varargin)
%   [X,out]  =  BLOCK_GCRODR(A,B,opts)
%   [X,out]  =  BLOCK_GCRODR(A,B,opts,Aargs)
%
% block_gcrodr attempts to solve the block system of linear equations with
% multiple right-hand sides A*X = B using subspace recycling techniques
% based on the GCRO-DR algorithm of Parks et al.
%
% This version uses no advanced matlab solving capabilities, such as '\'.
% Least squares problems are solved by generating rotations or reflections.
% The exception to this is, we do use the Matlab function qr().
%
% FUNCTION INFORMATIONS
% INPUTS:
%    A      An n x n matrix or a matvec procedure
%    B      The n x p block right-hand side
%    opts    Structure containing optional inputs or options
%    Aargs   In the case that A is a procedure, if it has extra arguments
%            that cannot be set ahead of time, they should be passed in the
%            proper calling order as arguments after opts.  
%    -opts.k      The size of the subspace being recycled.
%    -opts.m      The number of steps to be taken in the block Arnoldi method to
%                   construct a basis for the block Krylov-
%                   subspace K((eye(n)-C*C')*A,R0)
%    -opts.tol          The relative block residual tolerance, where the block residual
%                       is computed in the Frobenius norm.
%    -opts.nrestarts    Maximum number of times method will restart
%    -opts.M1           An optional left preconditioner or procedure.  If
%                       a matrix is passed in, then the code applies its inverse.
%                       If a procedure is inputted, it is assume the
%                       procedure applies the inverse.
%    -opts.M2           An optional right preconditioner or procedure.
%                       (same as for application of M1 holds for M2)
%    -opts.X0           The n x p block initial approximation.
%    -opts.U            Matrix containing columns of a recycle space generated
%                       during a previous linear solve.  If U = [] or is not
%                       defined, then no initial recycle space is assumed to
%                       exist.
%    -opts.Bextra       Vectors to add to right-hand sides to more quickly
%                       enlarge block Krylov subspace
% OUTPUTS:
%    X                  The approximation X0 + T produced by the algorithm
%    out                Structure containing all other outputs
%    -out.R             The block residual associated to X
%    -out.flag          a convergence flag:
%                           0 GCRODR converged to the desired tolerance tol 
%                               within maxcycles iterations.
%                           1 GCRODR iterated maxcycles times but did not converge. 
%    -out.relres        The preconditioned relative residual of the final
%                           approximation
%    -out.iter           both the outer and inner iteration numbers
%                           at which X was computed: 
%                           0 <= iter(1) <= maxcycles and 0 <= iter(2) <= m.
%    -out.resvecFro      A list of preconditioned residual Frobenius norms produced at every
%                        iteration
%    -out.resvec2norm    A list of preconditioned residual 2-norms norms produced at every
%                        iteration for each column of the right-hand side.
%    -out.U              Matrix containing columns of a recycle space generated
%                        during a previous linear solve.  If U = [] or is not
%                        defined, then no initial recycle space is assumed to
%                        exist.
%    -out.nmv            The number of matrix-block-vector multiplications
%                        required to solve the system
% SUBFUNCTIONS
%    block_gmres_cycle      Executes a cycle of block gmres 
%    block_gmres-gcrodr     Executes a cycle of recycled block GMRES 
%    getHarmRitzAugBlockKryl   Calculates harmonic Ritz vectors associated
%                              to the matrix A and the augmented block Krylov
%                              subspace.
%    getHarmRitzBlockKryl      Calculates harmonic Ritz vectors associated
%                              to the block Krylov subspace.
%    UTSolve_block             Solves a given upper triangular system.
%
% Kirk M. Soodhalter
% 01-Apr-2016 17:10:15
% kirk@math.soodhalter.com

    [n,p] = size(B);
    
    if ~isa(Aop,'function_handle')
        A = @(x) Aop*x;
    else
        if nargin >= 4
            A = @(x) Aop(x,varargin{:});
        else
            A = @(x) Aop(x);
        end
    end

    if ~exist('opts','var')
        opts = struct(); %empty structure
    end

    if isfield(opts,'tol')
        tol = opts.tol;
    else
        tol = 1e-8;
    end

    if isfield(opts,'nrestarts')
        nrestarts = opts.nrestarts;
    else
        nrestarts = 10;
    end

    if isfield(opts,'X0')
        X0 = opts.x0;
    else
        X0 = zeros(n,p);
    end

    if isfield(opts,'m')
        m = opts.m;
    else
        m = 50;
    end
    
    existM2 = isfield(opts,'M2'); 
    if ~(existM2)
       M2 = [];
    else
        if isa(opts.M2,'function_handle');
            M2 = @(x) opts.M2(x);
        else
            M2 = @(x) opts.M2\x;
        end
    end
    
    existM1 = isfield(opts,'M1'); 
    if ~(existM1)
       M1 = [];
    else
        if isa(opts.M1,'function_handle');
            M1 = @(x) opts.M1(x);
        else
            M1 = @(x) opts.M1\x;
        end
    end
    
    if isfield(opts,'k')
        k = opts.k;
    else
        k = 10;
    end
    
    if isfield(opts,'U')
        U = opts.U;
        isExistU = true;
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Set effective size of k %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%
        keff = size(U,2);
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Fill out remaining columns of U with zeros %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if keff < k+1
            U = [U zeros(n,k+1-keff)];
        end
    else
        U = zeros(n,k+1);
        isExistU = false;
        keff = 0;
    end
    
    if isfield(opts,'Bextra')
        Bextra = opts.Bextra;
        isBlockEnlarge = true;
    else
        Bextra = [];
        isBlockEnlarge = false;
    end
    
    if isfield(opts,'isComplex')
        isComplex = opts.isComplex;
    else
        isComplex = false;
    end    

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % % initialize solution update %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    T = zeros(size(X0));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    % initialize matvec count %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    nmv = 1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calculate initial preconditioned residual. %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    R = B - A(X0);
    if existM1
        R = M1(R);
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%
    % Append extra vectors %
    %%%%%%%%%%%%%%%%%%%%%%%%
    if isBlockEnlarge
       R = [R Bextra];
       L = size(R,2);
    else
       L = p;
    end
    rhsidx = 1:p;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calculate initial preconditioned residual norm. %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    resvecFro = zeros(m*nrestarts, 1);
    resvec2norm = zeros(m*nrestarts, p);
    resvecFro(1) = norm(R(:,rhsidx),'fro');
    for i = rhsidx
        resvec2norm(1,i) = norm(R(:,i));
    end
    resvecPointer = 2;
    fprintf('||R|| = %e\t\tnmv = %d\n',resvecFro(nmv),nmv-1);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Precondition rhs if available. %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if existM1
        Bnorm = norm(M1(B),'fro');
    else
        Bnorm = norm(B,'fro');
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %          Initialize the recycle space, i.e. initialize U and C          %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if isExistU
        cycle = 0;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % We have U from previous system solve.  %
        % Use U to compute new U and C           %
        % In practice, A = A_old + deltaA and    %
        % A * U = A_old*U + delta_A*U            %
        %       = C_old + delta_A*U              %
        % the deltaA matvec is cheap, presumably %
        % so we don't record the matvecs from    %
        % this update                            %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if existM2
            C = M2(U(:,1:keff));
        else 
            C = U(:,1:keff);
        end
        C = A(C); 
        if existM1
            C = M1(C);
        end

        nmv = nmv + 1;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Orthogonalize C, adjust U s.t. A*U=C %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [C,S] = qr(C,0);
        U(:,1:keff) = U(:,1:keff) / S;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Project Residual, update Approximation %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        T = T + U(:,1:keff)*C'*R(:,rhsidx);
        R = R - C*C'*R;
        
        resvecFro(resvecPointer) = norm(R,'fro');
        for i = rhsidx
            resvec2norm(resvecPointer,i) = norm(R(:,i));
        end
        fprintf('||R|| = %e (after one-step projection)\n',resvecFro(resvecPointer));
        resvecPointer = resvecPointer + 1;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Test for convergence:          %
        % Was solution in recycle space? %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if (resvecFro(1)/Bnorm < tol)
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Early Convergence, Compute Approximation and Exit %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            if(existM2)
             X = X0 + M2(T);
            else
             X = X0 + T;
            end     
            out.flag = 0;
            out.relres = resvecFro(1) / Bnorm;
            out.resvecFro = resvecFro;
            out.resvec2norm = resvec2norm;
            out.iter = [cycle 0];
            out.nmv = nmv - 1;    
            out.U = U(:,1:keff);
            return
        end   
    else
        cycle = 1;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % No recycle space, so run block GMRES to generate %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [T, R, W, H, j, resvecFro_cycle, resvec2norm_cycle] = block_gmres_cycle(A, T, R, m-1, M1, M2, tol*Bnorm,rhsidx);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Update residual vector and matvec counts %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        resvecFro(2:j+1) = resvecFro_cycle;
        resvec2norm(2:j+1,:) = resvec2norm_cycle;
        nmv = nmv + j;
        fprintf('||R|| = %e\t\tnmv = %d\n',resvecFro(nmv),nmv-1);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % form new U (the subspace to recycle) %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if k >= j*L 
            %%%%%%%%%%%%%%%%%%%
            % Keep everything %
            %%%%%%%%%%%%%%%%%%%
            keff = j*L;
            U(:,1:keff) = W(:,1:j*L);
        elseif k == 0
            U = [];
        else 
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Downselect to k vectors using harmonic Ritz vectors %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            [P,keff] = getHarmRitzBlockKryl(j, L, k, H, isComplex);
            U(:,1:keff) = W(:,1:j*L)*P;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Early Convergence, Compute Approximation and Exit %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if j < min(m,floor(k/p))
            if existM2
                X = X0 + M2(T);
            else
                X = X0 + T;
            end    
            out.flag = 0;
            out.iter = [cycle j*p];
            out.nmv = nmv - 1;
            out.relres = resvecFro(j + 1)/Bnorm;
            out.resvecFro = resvecFro;
            out.resvec2norm = resvec2norm;
            out.U= U(:,1:keff);
            return
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute C and adjust U for next cycle %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if (k >= j*L)
            %%%%%%%%%%%%%%%%%%%%%%
            % We kept everything %
            %%%%%%%%%%%%%%%%%%%%%%  
            [C,S] = qr(H,0);
            C = W * C;
            U(:,1:keff) = U(:,1:keff) / S;
        elseif k == 0
            U = [];
            C = [];
        else 
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % We kept a keff-dim subspace %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            [C,S] = qr(H*P,0);
            C = W * C;
            U(:,1:keff) = U(:,1:keff) / S;
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %                             Main Solver Body                            %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    while(resvecFro(nmv)/Bnorm > tol && cycle < nrestarts)
        if k == 0 %block gmres
            [T, R, W, H, j, resvecFro_cycle, resvec2norm_cycle] = block_gmres_cycle(A, T, R, m-1, M1, M2, tol*Bnorm,rhsidx);
        else %block gcrodr
            [T,R,W, H, Bk, j, resvecFro_cycle, resvec2norm_cycle] = block_gmres_gcrodr(A, T, R, m-1, M1, M2, C, U(:,1:keff), tol*Bnorm, rhsidx);
        end
        cycle = cycle + 1;
        resvecFro(nmv+1:nmv+j) = resvecFro_cycle;
        resvec2norm(nmv+1:nmv+j,:) = resvec2norm_cycle;
        resvecPointer = resvecPointer + j;
        nmv = nmv + j;
        fprintf('||R|| = %e\t\tnmv = %d\n',resvecFro(nmv),nmv-1);

        if (k-keff) >= j*L
            %%%%%%%%%%%%%%%%%%%
            % Keep everything %
            %%%%%%%%%%%%%%%%%%%
            keff_old = keff;
            keff = keff + j*L;
            U(:,1:keff) = [U W(:,1:j*L)];   
        elseif k == 0
            U = [];
        else
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % form updated U (the subspace to recycle) %
            % by constructing harmonic Ritz vectors    %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            clear D d;
            d = zeros(keff,1);
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Rescale U; Store inverses of the norms %
            % of columns of U in diagonal matrix D   %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            for i = 1:keff
                d(i) = norm(U(:,i));
                U(:,i) = U(:,i) / d(i);
            end
            D = diag(1./d);

            %%%%%%%%%%
            % Form G %
            %%%%%%%%%%
            G = zeros((j+1)*L + keff, j*L+keff);

            Drows = 1:keff;
            Dcols = 1:keff;
            G(Drows,Dcols) = D;

            Brows = 1:keff;
            Bcols = keff+1:keff+j*L;
            G(Brows, Bcols) = Bk;

            Hrows = keff+1:keff+(j+1)*L;
            Hcols = keff+1:keff+j*L;
            G(Hrows,Hcols) = H;
            keff_old = keff;


            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Get new harmonic Ritz vectors %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            [P,keff] = getHarmRitzAugBlockKryl(j, L, k, G, W, U(:,1:keff), C,keff, isComplex);

            %%%%%%%%%%%%%%
            % Form new U %
            %%%%%%%%%%%%%%
            U(:,1:keff) = [U(:,1:keff_old) W(:,1:j*L)]*P;

        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Early Convergence, Compute Approximation and Exit %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if j < m-1
            if existM2
                X = X0 + M2(T);
            else
                X = X0 + T;
            end
            out.relres = resvecFro(nmv)/Bnorm;
            out.resvecFro = resvecFro;
            out.resvec2norm = resvec2norm;
            out.nmv = nmv-1;
            out.flag = 0;
            out.iter = [cycle j*p];
            out.U = U(:,1:keff);
            return;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute C and adjust U for next cycle %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if (k-keff_old) >= j*L
            %%%%%%%%%%%%%%%%%%%%%%
            % We kept everything %
            %%%%%%%%%%%%%%%%%%%%%%

            %%%%%%%%%%
            % Form G %
            %%%%%%%%%%
            G = zeros((j+1)*L + keff_old, j*L+keff_old);

            Irows = 1:keff_old;
            Icols = 1:keff_old;
            G(Irows,Icols) = eye(keff_old);

            Brows = 1:keff_old;
            Bcols = keff_old+1:keff_old+j*L;
            G(Brows, Bcols) = Bk;

            Hrows = keff_old+1:keff_old+(j+1)*L;
            Hcols = keff_old+1:keff_old+j*L;
            G(Hrows,Hcols) = H;

            %%%%%%%%%%%%%%%%%%%%%%%
            % Compute C, Update U %
            %%%%%%%%%%%%%%%%%%%%%%%
            [Q,S] = qr(G,0);
            C = [C W] * Q;
            U(:,1:keff) = U(:,1:keff) / S;
        elseif k == 0
            C = [];
        else 
            %%%%%%%%%%%%%%%%%%%%%
            % G already created %
            %%%%%%%%%%%%%%%%%%%%%
            [Q,S] = qr(G*P,0);
            C = [C W] * Q;
            U(:,1:keff) = U(:,1:keff) / S;
        end

    end
    flag = 1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calculate final solution %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if existM2
        X = X0 + M2(T);
    else
        X = X0 + T;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calculate relative residual %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    out.relres = resvecFro(nmv)/Bnorm;

    %%%%%%%%%%%%%%%%%%%%%%%%
    % Correct matvec count %
    %%%%%%%%%%%%%%%%%%%%%%%%
    out.nmv = nmv - 1;
    
    out.iter = [cycle j*p];
    out.resvecFro = resvecFro;
    out.resvec2norm = resvec2norm;
    out.U = U(:,1:keff);
    out.flag = flag;
end 

function [X, R, W, H, iter, resvecFro, resvec2norm] = block_gmres_cycle(A, X, R, m, M1, M2, tol,rhsidx)
    [~,L] = size(R);
    p = length(rhsidx);
    resvecFro = zeros(m,1);
    resvec2norm = zeros(m,p);
    if L==1
        c= zeros(1,m);
        s= zeros(1,m);
    else
        House = cell(1,m);
    end

    if nargin < 7
        tol = 1e-6;
    end

    if nargin < 6 || isempty(M2)
        existM2 = 0;
    else
        existM2 = 1;
    end

    if nargin < 5 || isempty(M1)
        existM1 = 0;
    else
        existM1 = 1;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Get first block Arnoldi vector %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [W(:,1:L),S0] = qr(R,0);

    %%%%%%%%%%%%%%%%%%%%%%%%%
    % Initialize RHS for    %
    % least squares problem %
    %%%%%%%%%%%%%%%%%%%%%%%%%
    RHS = zeros((m+1)*L,L);
    RHS(1:L,1:L) = S0;

    iter = 0;
    %%%%%%%%%%%%%%%%%%%%
    % Iterate by block %
    %%%%%%%%%%%%%%%%%%%%
    for k = 1:L:m*L
        iter = iter + 1;
        
        %%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply Preconditioning %
        % If Available          %
        %%%%%%%%%%%%%%%%%%%%%%%%%
        if existM2
            V = M2(W(:,k:k+L-1));
        else
            V = W(:,k:k+L-1);
        end
        V = A(V);
        if existM1
            V = M1(V);
        end

        %%%%%%%%%%%%%%%%%
        % Block Arnoldi %
        % Process       %
        %%%%%%%%%%%%%%%%%
        for j=1:L:k
            H(j:j+L-1,k:k+L-1) = W(:,j:j+L-1)'*V;
            G(j:j+L-1,k:k+L-1) = H(j:j+L-1,k:k+L-1);
            V = V - W(:,j:j+L-1)*H(j:j+L-1,k:k+L-1);
        end
        [W(:,k+L:k+2*L-1),H(k+L:k+2*L-1,k:k+L-1)] = qr(V,0);
        G(k+L:k+2*L-1,k:k+L-1) = H(k+L:k+2*L-1,k:k+L-1);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute newest column of R from QR             %
        % factorization of H                             %
        % If only 1 RHS then we perform Givens rotations %
        % Otherwise we perform Householder reflections   %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if L == 1
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Perform previous rotations %
            % on new column              %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            for j = 1:iter-1
                h1 = G(j,iter);
                h2 = G(j+1,iter);
                G(j,iter)   = c(j) * h1 - s(j) * h2;
                G(j+1,iter) = s(j) * h1 + c(j) * h2;
            end

            %%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Calculate new rotations %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%
            y1 = G(iter,iter);
            y2 = G(iter+1,iter);
            rh1 = RHS(iter);
            if y2 == 0.0
                c(iter) = 1.0;
                s(iter) = 0.0;
            elseif abs(y2) > abs(y1)
                h1 = -(y1/y2);
                s(iter) = 1.0 / sqrt(1 + h1 * h1);
                c(iter) = s(iter) * h1;
            else
                h1 = -(y2/y1);
                c(iter) = 1.0 / sqrt(1 + h1 * h1);
                s(iter) = c(iter) * h1;
            end
            h1       = y1;
            h2       = y2;
            G(iter,iter)   = c(iter) * h1 - s(iter) * h2;
            G(iter+1,iter) = s(iter) * h1 + c(iter) * h2;
            RHS(iter)   = c(iter) * rh1;      
            RHS(iter+1) = s(iter) * rh1; 

        else
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Perform previous reflections %
            % on new column                %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            for j = 1:iter-1
                Growrange = (j-1)*L+1:(j+1)*L;
                Gcolrange = k:k+L-1;
                G(Growrange,Gcolrange) = House{j}*G(Growrange,Gcolrange);
            end

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Calculate new refletions %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%
            House{iter} = eye(2*L,2*L);
            for j=1:L
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Calculate current reflection %
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                curcol = k+j-1';
                Growrange = curcol:curcol+L;
                Gcolrange = curcol+1:curcol+(L-j);
                alpha = -sign(G(curcol,curcol))*norm(G(curcol:curcol+L,curcol));
                vrefl = G(curcol:curcol+L,curcol) - [alpha zeros(1,L)]';
                nvs = norm(vrefl)^2;

                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Apply new reflector to:                                           %
                % 1. The current column (implicitely)                               %
                % 2. To subsequent columns of H in current block                    %
                % 3. To House{iter} to store product of reflections for this column %
                % 4. RHS, the right-hand side of the least squares problem          % 
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                G(curcol:curcol+L,curcol) = [alpha zeros(1,L)]';%1
                G(Growrange,Gcolrange) =... 
                    G(Growrange,Gcolrange) -... 
                    2*vrefl*(vrefl'*G(Growrange,Gcolrange))/nvs;%2

                House{iter}(j:j+L,:) = House{iter}(j:j+L,:) - 2*vrefl*(vrefl'*House{iter}(j:j+L,:))/nvs;%3
                RHS(Growrange,:) = RHS(Growrange,:) - 2*vrefl*(vrefl'*RHS(Growrange,:))/nvs;%4

            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute residual norm  %
        % Stop loop if converged %
        %%%%%%%%%%%%%%%%%%%%%%%%%%
        resvecFro(iter) = norm(RHS(k+L:k+2*L-1,rhsidx),'fro');
        for i=rhsidx
            resvec2norm(iter,i) = norm(RHS(k+L:k+2*L-1,i));
        end
        if resvecFro(iter) <  tol
            break;
        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%
    % Solve Least Squares %
    % Calculate Solution  %
    %%%%%%%%%%%%%%%%%%%%%%%
    Y = UTSolve_block(G, RHS, iter*L, L);
    X = X + W(:,1:k+L-1)*Y(:,rhsidx);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Compute residual by applying plane rotations/reflections %
    % in reverse order to modified-lease-squares residual      %
    % This allows us to recover H*y without doing the matvec   %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    RES = zeros((iter+1)*L,L);
    RES(iter*L+1:(iter+1)*L,:) = -RHS(iter*L+1:(iter+1)*L,:);
    if L == 1
        %%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply plane rotations %
        %%%%%%%%%%%%%%%%%%%%%%%%%
        for i = j+1:-1:1
           RES(i)   = RES(i+1) * s(i);
           RES(i+1) = RES(i+1) * c(i);
        end
    else
        %%%%%%%%%%%%%%%%%%%%%
        % Apply reflections %
        %%%%%%%%%%%%%%%%%%%%%
        for j = iter:-1:1
            RESrowrange = (j-1)*L+1:(j+1)*L;
            RES(RESrowrange,:) = House{j}'*RES(RESrowrange,:);
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%
    % Now we have H*Y-E1*S0  %
    % Need to get rid of -S0 %
    % in first p x p block   %
    %%%%%%%%%%%%%%%%%%%%%%%%%%
    RES(1:L,:) = RES(1:L,:) + S0;
    R = R - W*RES;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Trim excess zero entries from resvecs %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    resvecFro = resvecFro(1:iter);
    resvec2norm = resvec2norm(1:iter,:);
end

function [X,R,W, H, B, iter, resvecFro, resvec2norm] = block_gmres_gcrodr(A, X, R, m, M1, M2, C, U, tol, rhsidx)
    [~,L] = size(R);
    kcurr = size(U,2);
    p = length(rhsidx);
    resvecFro = zeros(m,1);
    resvec2norm = zeros(m,p);
    B = zeros(kcurr,m);
    if L==1
        c= zeros(1,m);
        s= zeros(1,m);
    else
        House = cell(1,m);
    end
    
    if nargin < 5 || isempty(M1)
       existM1 = 0;
    else
       existM1 = 1;
    end
    if nargin < 6 || isempty(M2)
       existM2 = 0;
    else
       existM2 = 1;
    end

    %Get first block Arnoldi vector
    [W(:,1:L),S0] = qr(R,0);

    %%%%%%%%%%%%%%%%%%%%%%%%%
    % Initialize RHS for    %
    % least squares problem %
    %%%%%%%%%%%%%%%%%%%%%%%%%
    RHS = zeros((m+1)*L,L);
    RHS(1:L,1:L) = S0;
    iter = 0;
    %%%%%%%%%%%%%%%%%%%%
    % Iterate by block %
    %%%%%%%%%%%%%%%%%%%%
    for k = 1:L:m*L
        iter = iter+1;
        %%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply Preconditioning %
        % If Available          %
        %%%%%%%%%%%%%%%%%%%%%%%%%
        if existM2
            V = M2(W(:,k:k+L-1));
        else
            V = W(:,k:k+L-1);
        end

        V = A(V);
        if existM1
            V = M1(V);
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Orthogonalize with respect %
        % to Range(C)                %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        for i = 1:size(C,2)
            B(i, k:k+L-1) = C(:,i)'*V;
            V = V - C(:,i)*B(i, k:k+L-1);
        end

        %%%%%%%%%%%%%%%%%
        % Block Arnoldi %
        % Process       %
        %%%%%%%%%%%%%%%%%
        for j=1:L:k
            H(j:j+L-1,k:k+L-1) = W(:,j:j+L-1)'*V;
            G(j:j+L-1,k:k+L-1) = H(j:j+L-1,k:k+L-1);
            V = V - W(:,j:j+L-1)*H(j:j+L-1,k:k+L-1);
        end
        [W(:,k+L:k+2*L-1),H(k+L:k+2*L-1,k:k+L-1)] = qr(V,0);
        G(k+L:k+2*L-1,k:k+L-1) = H(k+L:k+2*L-1,k:k+L-1);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute newest column of R from QR             %
        % factorization of H                             %
        % If only 1 RHS then we perform Givens rotations %
        % Otherwise we perform Householder reflections   %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if L == 1
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Perform previous rotations %
            % on new column              %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            for j = 1:iter-1
                h1 = G(j,iter);
                h2 = G(j+1,iter);
                G(j,iter)   = c(j) * h1 - s(j) * h2;
                G(j+1,iter) = s(j) * h1 + c(j) * h2;
            end

            %%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Calculate new rotations %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%
            y1 = G(iter,iter);
            y2 = G(iter+1,iter);
            rh1 = RHS(iter);
            if y2 == 0.0
                c(iter) = 1.0;
                s(iter) = 0.0;
            elseif abs(y2) > abs(y1)
                h1 = -(y1/y2);
                s(iter) = 1.0 / sqrt(1 + h1 * h1);
                c(iter) = s(iter) * h1;
            else
                h1 = -(y2/y1);
                c(iter) = 1.0 / sqrt(1 + h1 * h1);
                s(iter) = c(iter) * h1;
            end
            h1       = y1;
            h2       = y2;
            G(iter,iter)   = c(iter) * h1 - s(iter) * h2;
            G(iter+1,iter) = 0;
            RHS(iter)   = c(iter) * rh1;      
            RHS(iter+1) = s(iter) * rh1; 

        else
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Perform previous reflections %
            % on new column                %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            for j = 1:iter-1
                Growrange = (j-1)*L+1:(j+1)*L;
                Gcolrange = k:k+L-1;
                G(Growrange,Gcolrange) = House{j}*G(Growrange,Gcolrange);
            end

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Calculate new refletions %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%
            House{iter} = eye(2*L,2*L);
            for j=1:L
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Calculate current reflection %
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                curcol = k+j-1';
                Growrange = curcol:curcol+L;
                Gcolrange = curcol+1:curcol+(L-j);
                alpha = -sign(G(curcol,curcol))*norm(G(curcol:curcol+L,curcol));
                vrefl = G(curcol:curcol+L,curcol) - [alpha zeros(1,L)]';
                nvs = norm(vrefl)^2;

                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Apply new reflector to:                                           %
                % 1. The current column (implicitely)                               %
                % 2. To subsequent columns of H in current block                    %
                % 3. To House{iter} to store product of reflections for this column %
                % 4. RHS, the right-hand side of the least squares problem          % 
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                G(curcol:curcol+L,curcol) = [alpha zeros(1,L)]';%1

                G(Growrange,Gcolrange) =... 
                    G(Growrange,Gcolrange) -... 
                    2*vrefl*(vrefl'*G(Growrange,Gcolrange))/nvs;%2

                House{iter}(j:j+L,:) = House{iter}(j:j+L,:) - 2*vrefl*(vrefl'*House{iter}(j:j+L,:))/nvs;%3
                RHS(Growrange,:) = RHS(Growrange,:) - 2*vrefl*(vrefl'*RHS(Growrange,:))/nvs;%4

            end

        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%
        % Compute residual norm  %
        % Stop loop if converged %
        %%%%%%%%%%%%%%%%%%%%%%%%%%
        resvecFro(iter) = norm(RHS(k+L:k+2*L-1,rhsidx),'fro');
        for i=rhsidx
            resvec2norm(iter,i) = norm(RHS(k+L:k+2*L-1,i));
        end
        if resvecFro(iter) <  tol
            break;
        end

    end

    %%%%%%%%%%%%%%%%%%%%p%%%
    % Solve Least Squares %
    % Calculate Solution  %
    %%%%%%%%%%%%%%%%%%%%%%%
    Y = UTSolve_block(G, RHS, iter*L, L);
    X = X + W(:,1:k+L-1)*Y(:,rhsidx);
    X = X - U*(B(:,1:k+L-1)*Y(:,rhsidx));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Compute residual by applying plane rotations/reflections %
    % in reverse order to modified-lease-squares residual      %
    % This allows us to recover H*y without doing the matvec   %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    RES = zeros((iter+1)*L,L);
    RES(iter*L+1:(iter+1)*L,:) = -RHS(iter*L+1:(iter+1)*L,:);

    if L == 1
        %%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply plane rotations %
        %%%%%%%%%%%%%%%%%%%%%%%%%
        for i = j+1:-1:1
           RES(i)   = RES(i+1) * s(i);
           RES(i+1) = RES(i+1) * c(i);
        end
    else
        %%%%%%%%%%%%%%%%%%%%%
        % Apply reflections %
        %%%%%%%%%%%%%%%%%%%%%
        for j = iter:-1:1
            RESrowrange = (j-1)*L+1:(j+1)*L;
            RES(RESrowrange,:) = House{j}'*RES(RESrowrange,:);
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%
    % Now we have H*Y-E1*S0  %
    % Need to get rid of -S0 %
    % in first p x p block   %
    %%%%%%%%%%%%%%%%%%%%%%%%%%
    RES(1:L,:) = RES(1:L,:) + S0;
    R = R - W*RES;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Trim excess zero entries from resvecs %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    resvecFro = resvecFro(1:iter);
    resvec2norm = resvec2norm(1:iter,:);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Drop unused extra preallocated values %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    B = B(:,1:k+L-1);
end

function [P,keff] = getHarmRitzAugBlockKryl(m, p, k, G, W, U, C, kcurr, isComplex)
    M = m*p+kcurr;
    P = zeros(m*p+kcurr,k+1);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Construct matrices A and B for %
    % generalized eigenvalue problem %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    B = G'*G;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    % A = G*| C'*U        0 | %
    %       | V_{m+1}'*U  I | %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    A = zeros(M+p,M);
    A(1:kcurr,1:kcurr) = C'*U;
    A(kcurr+1:kcurr+(m+1)*p,1:kcurr) = W'*U;
    A(kcurr+1:kcurr+m*p,kcurr+1:kcurr+m*p) = eye(m*p);
    A = G'*A;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Compute harmonic Ritz values %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [harmVecs, harmVals] = eig(A,B);
    harmVals=diag(harmVals);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Sort values by magnitude %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [~,iperm] = sort(abs(harmVals),'descend');

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Select k smallest eigenvectors                                 %
    % Optionally store k+1 vectors to capture complex conjugate pair %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    idx = 1;
    while(idx <= k)
      if (isreal(harmVals(iperm(idx))) || isComplex)
        P(:,idx) = harmVecs(:,iperm(idx));
        idx = idx + 1;    
      else
        P(:,idx) = real(harmVecs(:,iperm(idx)));
        P(:,idx+1) = imag(harmVecs(:,iperm(idx)));
        idx = idx + 2;      
      end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Return number of vectors selected %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    keff = idx-1;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Drop extra allocated vectors %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    P = P(:,1:keff);
end

function [P,keff] = getHarmRitzBlockKryl(m, p, k, H, isComplex)
    P = zeros(m*p,k+1);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Build Matrix for Eigenvalue problem %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    harmRitzMat = speye(m*p);
    harmRitzMat = harmRitzMat(:,((m-1)*p+1):m*p);
    harmRitzMat = H(1:m*p,:)'\harmRitzMat;
    harmRitzMat = harmRitzMat*(H(m*p+1:(m+1)*p,(m-1)*p+1:m*p)'*H(m*p+1:(m+1)*p,(m-1)*p+1:m*p));
    harmRitzMat = [H(1:m*p,1:(m-1)*p) (H(1:m*p,(m-1)*p+1:m*p)+harmRitzMat)];

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Compute harmonic Ritz values %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [harmVecs, harmVals] = eig(harmRitzMat);
    harmVals=diag(harmVals);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Sort values by magnitude %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [~,iperm] = sort(abs(harmVals));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Select k smallest eigenvectors                                 %
    % Optionally store k+1 vectors to capture complex conjugate pair %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    idx = 1;
    while(idx <= k)
      if (isreal(harmVals(iperm(idx))) || isComplex)
        P(:,idx) = harmVecs(:,iperm(idx));
        idx = idx + 1;    
      else
        P(:,idx) = real(harmVecs(:,iperm(idx)));
        P(:,idx+1) = imag(harmVecs(:,iperm(idx)));
        idx = idx + 2;      
      end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Return number of vectors selected %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    keff = idx-1;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Drop extra allocated vectors %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    P = P(:,1:keff);
end

function Y = UTSolve_block(R, RHS, n, p)
    Y = zeros(n,p);

    Y(n,:) = RHS(n,:) / R(n,n);
    for i = n-1:-1:1,
       Y(i,:) = RHS(i,:);
       for j = i+1:n,
          Y(i,:) = Y(i,:) - R(i,j) * Y(j,:);
       end
       Y(i,:) = Y(i,:) / R(i,i);
    end
end

