function [EVals, EVecs, resHist, cummatvec] = bleigifp(varargin)
%
%  BLEIGIFP :  computes a few (algebraiclly) smallest or largest
%  eigenvalues/eigenvectors of the symmetric matrix eigenvalue problem:
%
%             A x = lambda x   or    A x = lambda B x
%
%  where A and B are symmetric and B is positive definite. It is best suited  
%  for problems where the eigenvalues desired are severely clustered 
%  or have multiplicity greater than one. 
%
%   [d, x] = bleigifp(A)       => smallest eigenvalue/eigenvector of A;
%   [d, X] = bleigifp(A, k)    => k smallest eigenvalues/eigenvectors of A;
%
%   [d, x] = bleigifp(A, B)    => smallest eigenvalue/eigenvector of (A, B);
%   [d, X] = bleigifp(A, B, k) => k smallest eigenvalues/eigenvectors of (A, B);
%
%  Computation of the k largest eigenpairs of (A, B) may be demanded through
%  use of the options structure described below.
%
%   [d, X, res] = bleigifp(...)
%
%  returns the two norm residual history for each eigenvalue.
%
%  A and B (if provided) may be sparse matrices or functions that define
%  matrix/vector multiplication.  In the latter case, the dimension of the
%  the problem must be provided through the options structure (see below).
%  It is also desirable to have estimates of the norms of the operators,
%  also provided through the options structure.
%
%  The options structure (opt in the calls below) may be provided to use any
%  a priori information, to control computational cost, to optimize performnce,
%  or to specify which end of the spectrum to compute.  The calls
%
%    bleigifp(A, opt), bleigifp(A, k opt), bleigifp(A, B, opt),
%    bleigifp(A, B, k, opt)
%
%  take the following optional inputs defined by opt:
%
%    opt.ADAPTTOL:        tolerance used to determine when Ritz values are
%                         clustered.  Used in helping to decide when to adapt
%                         the block size and can be used to control the growth 
%                         of blocksize.  This value must lie in the interval
%                         (0,1).  
%                         default:  0.1
%
%    opt.ARGSA:           optional arguments to pass to the function
%                         defining multiplication by A.  Valid only if
%                         A is a function handle or a string.  This
%                         should be either a matrix or a cell array.
%
%    opt.ARGSB:           optional arguments to pass to the function
%                         defining multiplication by B.  Valid only if
%                         B is a function handle or a string.  This
%                         should be either a matrix or a cell array.
%
%    opt.ARGSL:           optional arguments to pass to the function
%                         defining multiplication by L.  Valid only if
%                         L is a function handle or a string.  This
%                         should be either a matrix or a cell array.
%
%    opt.BLOCKSIZE:       the initial block. It should be set to the 
%    (or opt.BS)          multiplicity or the cluster size of the eigenvalues 
%                         desired. Use with the option opt.UPDATEP = 'no' 
%                         to fix the block size, otherwise the block size 
%                         will be adaptively determined.  
%                         default:  2
%
%    opt.DISP:            file (name or MATLAB file ID) to which the
%                         output generated is written.  If a filename
%                         is provided, an fid is generated by a call
%                         to fopen.  If a file ID is provided, that
%                         file ID is checked against valid fids.
%                         Whenever an invalid value is provided, the
%                         default is assumed.  Set opt.DISP to zero
%                         to disable output altogether.
%                         default:  1 (corresponds to stdout).
%
%    opt.ILUTHRESH:       threshold used in incomplete LU for computing
%    (or opt.ILU)         preconditioner.  (i.e. used in luinc.m);
%                         Special cases:  0 = exact LU; 1 = 0-level ILU
%                         default:  1e-04
%
%    opt.INITIALVEC:      the first opt.BLOCKSIZE of this matrix will 
%                         be used as the initial vectors, the rest will be
%                         disregarded.
%
%    opt.INNERITERATION:  set the number of inner iterations to control the
%    (or opt.INNERIT)     memory requirement;  Use with the option
%                         opt.UPDATEM = 'no' to fix the number of inner
%                         iterations.
%                         default:  between 1 and 128 as adaptively determined.
%
%    opt.MAXBLOCKSIZE:    set the maximum block size.
%    (or opt.MAXBS)       default:  5*opt.BLOCKSIZE.
%
%    opt.MAXITERATION:    set the maximum number of (outer) iterations.
%    (or opt.MAXIT)       default:  500
%
%    opt.NORMA:           an estimate of the 1-norm of A.
%
%    opt.NORMB:           an estimate of the 1-norm of B.
%
%    opt.PRECONDITIONER:  user supplied preconditioner. It may be a
%    (or opt.PRECON)      matrix or a function.
%
%    opt.SIGMA:           determines whether to seek the largest or smallest
%                         algebraic eigenvalue(s).  At this time, the only
%                         permitted values of this option are 'SA' and 'LA'.
%                         default:  'SA'
%
%    opt.SIZE:            the size of A.
%
%    opt.TOLERANCE:       termination tolerance for the residual;
%    (or opt.TOL)         default:  10*eps*sqrt(n)*(||A||+|lambda|*||B||)
%
%    opt.UPDATEM:         controls whether or not to allow for adaptive update
%                         of the number of inner iterations.  The only value
%                         which will cause m to not be updated is 'no'.
%                         Any other value is silently ignored.
%
%    opt.UPDATEP:         controls whether or not to allow for adaptive update
%                         of the block size when using the block method. The
%                         only value which will cause the block size to not be
%                         updated is 'no'. Any other value is silently ignored.
%                         The block size is also not adapted when using the
%                         single vector algorithm.
%
%    opt.USEPRECON:       set to an approximate eigenvalue to start 
%                         preconditioned iterations with a preconditioner
%                         computed using opt.USEPRECON as the shift.
%
%
%  BLEIGIFP implements a block generalization of the inverse free Krylov subspace 
%  method to deal with multiplicity and clustering. The block algorithm is 
%  presented in 
%
%         P. Quillen, Generalizations of an Inverse Free Krylov Subspace Method 
%         for the Symmetric Generalized Eigenvalue Problem, Ph.D. Thesis, 
%         Department of Mathematics, University of Kentucky, Lexington, KY, 2005.   
%
%
%  BLEIGIFP is developed by Pat Quillen (pquillen@mathworks.com) and Qiang Ye
%  (qye@ms.uky.edu). This work was supported by NSF under Grant CCR-0098133 
%  and DMS-0411502.
%
%  This program is provided for research and educational use and is
%  distributed through http://www.ms.uky.edu/~qye/software.
%  Neither redistribution nor commercial use is permitted without
%  consent of the author.
%
%  This version dated 24 September 2012

%  Process Inputs.
if (nargin < 1)
  error('BLEIGIFP:tooFewInput', 'Input Error:  Too few input parameters.');
elseif (nargin > 4)
  error('BLEIGIFP:tooManyInput', 'Input Error:  Too many input parameters.');
end

%  Default everything.
n = []; m = 0; k = 1; p = 2; maxp = 5*p;
B = []; L = []; X = [];
argsA = {}; argsB = {}; argsL = {};
options = [];
usePrecon = 1;  usePreShift = []; eta = 1e-4;
iterMax = 500; tolerance = [];
fid = 1; closeFile = 0;
fixInnerIt = false;
adaptive = true;
adapt_tol = 1e-04;
scalA = 1;
normA = []; normB = [];

%  First, determine A, and possibly its size.
A = varargin{1};
%  Default A to be a function.
AisMatrix = 0;
if (isnumeric(A))
  %  Proclaim A to be a matrix.
  AisMatrix = 1;
  n = size(A, 1);
  if (any(size(A) ~= n))
    error('BLEIGIFP:notSquare', 'Input Error:  Matrix A is not square.');
  end
elseif (ischar(A))
  %  Convert A to a function handle.
  A = str2func(A);
  %  Automatic preconditioning not allowed for matrices defined by functions.
  usePrecon = 0;
elseif (isa(A, 'function_handle'))
  %  Automatic preconditioning not allowed for matrices defined by functions.
  usePrecon = 0;
else
  error('BLEIGIFP:notOperator', ...
    'Input Error:  A must be either a matrix or a function.');
end

%  Next, look for the options struct.  It will always be the last argument.
%  It is necessary to do this now to determine the size of A, so that we
%  may have a chance at determining the nature of the remaining arguments.
%  That is, we must know the size of A in order to be able to distinguish the
%  remaining arguments.  Right now, we'll only look for the size argument
%  and check it.
options_specified = 0;
if (isa(varargin{nargin}, 'struct'))
  options = varargin{nargin};
  options_specified = 1;
  names = fieldnames(options);
  I = strcmpi('SIZE', names);
  if (any(I))
    A_size = options.(names{I});
    if (AisMatrix)
      if (A_size ~= n)
        error('BLEIGIFP:wrongSize', ...
          'Input Error:  options.size not equal to the size of A.');
      end
    else
      n = A_size;
    end
  elseif (~AisMatrix)
    error('BLEIGIFP:sizeNeeded', ...
      'Input Error:  need to input options.size = (dimension of A).');
  end
elseif (~AisMatrix)
  error('BLEIGIFP:sizeNeeded', ...
    'Input Error:  need to input options.size = (dimension of A).');
elseif (nargin == 4)
  error('BLEIGIFP:badArgFour', ...
    'Input Error:  input parameter four must be of type struct.');
end

%  Check the second argument for B or k.
%  We only must perform this check when there are either only two input
%  parameters given, or when there are three given and the third one is
%  the options structure.
if ((~options_specified && (nargin == 2)) || (options_specified && (nargin == 3)))
  if (isnumeric(varargin{2}))
    size_va2 = size(varargin{2});
    if (size_va2(1) ~= size_va2(2))
      error('BLEIGIFP:badArgTwoSquare', ...
        'Input Error:  input parameter two must be square.');
    end
    if (size_va2(1) == n)
      B = varargin{2};
    elseif (size_va2(1) == 1)
      k = varargin{2};
    else
      error('BLEIGIFP:badArgTwoSize', ...
        'Input Error:  Matrix B must be the size of A.');
    end
  else
    B = varargin{2};
  end
elseif (nargin > 2)
  B = varargin{2};
  k = varargin{3};
end

%  Now check B for legitimacy.
if (~isempty(B))
  if (ischar(B))
    %  Convert B to a function handle.
    B = str2func(B);
    %  Automatic preconditioning not allowed for matrices defined by functions.
    usePrecon = 0;
  elseif (isa(B, 'function_handle'))
    %  Automatic preconditioning not allowed for matrices defined by functions.
    usePrecon = 0;
  elseif (~isnumeric(B))
    error('BLEIGIFP:badArgTwoType', ...
      'Input Error:  B must be either a matrix or a function.');
  end
end

%  Check k for legitimacy.
if (k > n)
  error('BLEIGIFP:badK', ...
    'Number of eigenvalues sought is greater than the matrix size.');
end

%  Validate the remainder of the options.
if (options_specified)
  I = strcmpi('DISP',names);
  if (any(I))
    fidStr = options.(names{I});
    if (ischar(fidStr))
      %  If a filename is provided, open and validate.
      fid = fopen(fidStr, 'W+');
      if (fid == -1)
        warning('BLEIGIFP:openFileFailed', ...
          'Unable to open file %s.  Displaying to screen.', fidStr);
        fid = 1;
      else
        closeFile = 1;
      end
    elseif (isnumeric(fidStr))
      %  Validate the given file ID by checking if the fid
      %  corresponds to an open file.
      fid = fidStr;
      %  0, 1, 2 are always valid fids corresponding to /dev/null,
      %  stdout, and stderr, respectively.
      if (fid ~= 0 && fid ~= 1 && fid ~= 2)
        open_fids = fopen('all');
        if isempty(find(open_fids == fid,1))
          warning('BLEIGIFP:badFileID', ...
            'Invalid File ID in field DISP.  Displaying to screen.');
          fid = 1;
        end
      end
    else
      warning('BLEIGIFP:badFileID', ...
        'Invalid value for option structure field DISP.  Displaying to screen.');
    end
  end

  I = strcmpi('NORMA', names);
  if (any(I))
    normA = options.(names{I});
  end

  I = strcmpi('NORMB', names);
  if (any(I))
    normB = options.(names{I});
  end
  
  I = strcmpi('BLOCKSIZE', names);
  if (~any(I))
    I = strcmpi('BS', names);
  end
  if (any(I))
    p = options.(names{I});
    if p > n
      error('BLEIGIFP:blockSizeTooBig', '%s%s', ...
        'Input Error:  Initial block size is larger than the ', ...
        'size of the problem.');
    elseif p <= 1,
      warning('BLEIGIFP:wrongBlockSize', '%s\n%s', ...
        'Block size shoule be at least 2.', ...
        '         Setting to 2.');
      p = 2;  
    end
  end

  %  Set the max block size.
  maxp = 5*p;
  I = strcmpi('MAXBLOCKSIZE', names);
  if (~any(I))
    I = strcmpi('MAXBS', names);
  end
  if (any(I))
    maxp = options.(names{I});
    if (maxp > n)
      warning('BLEIGIFP:maxpTooBig', '%s\n%s', ...
        'Max block size is larger than the size of the problem.', ...
        '         Setting to n.');
      maxp = n;
    end
  end

  I = strcmpi('INITIALVEC', names);
  if (~any(I))
    I = strcmpi('V0', names);
  end
  if (any(I))
    X = options.(names{I});
    if (size(X, 1) ~= n)
      error('BLEIGIFP:badInitVecs', ...
        'Input Error:  incorrect size of initial vectors.');
    end
    if (size(X,2) < p)
      warning('BLEIGIFP:wrongNumInitBlockVec', ...
        'Number of initial vectors less than block size.');
    end
  end

  I = strcmpi('TOLERANCE', names);
  if (~any(I))
    I = strcmpi('TOL', names);
  end
  if (any(I))
    tolerance = options.(names{I});
    if (tolerance <= 0)
      error('BLEIGIFP:badTol', ...
        'Input Error:  Invalid tolerance; must be positive.');
    end
  end

  I = strcmpi('INNERITERATION', names);
  if (~any(I))
    I = strcmpi('INNERIT', names);
  end
  if (any(I))
    m = options.(names{I});
    if (m < 0 || m >= n)
      error('BLEIGIFP:badInnerIt', ...
        'Input Error:  Invalid inner iteration number.');
    end
  end

  I = strcmpi('UPDATEM', names);
  if (any(I))
    updateStr = options.(names{I});
    if (ischar(updateStr))
      if (strcmpi(updateStr, 'NO'))
        fixInnerIt = true;
      end
    end
  end

  I = strcmpi('UPDATEP', names);
  if (any(I))
    updateStr = options.(names{I});
    if (ischar(updateStr))
      if (strcmpi(updateStr, 'NO'))
        adaptive = false;
      end
    end
  end
 
  I = strcmpi('ADAPTTOL', names);
  if (any(I))
    adapt_tol = options.(names{I});
    if (~(adapt_tol > 0 && adapt_tol < 1))
      error('BLEIGIFP:badAdaptTol', ...
        'Input Error:  Invalid tolerance for adaptive block sizing.');
    end
  end
  
  I = strcmpi('ILUTHRESH', names);
  if (~any(I))
    I = strcmpi('ILU', names);
  end
  if (any(I))
    eta = options.(names{I});
    if (eta < 0)
      error('BLEIGIFP:badILUThresh', ...
        'Input Error:  Invalid incomplete factorization threshold.');
    end
  end

  I = strcmpi('PRECONDITIONER', names);
  if (~any(I))
    I = strcmpi('PRECON', names);
  end
  if (any(I))
    L = options.(names{I});
    if (isnumeric(L))
      if (any(size(L) ~= n))
        error('BLEIGIFP:badPreconSize', '%s%s', ...
          'Input Error:  Invalid preconditioner; ', ...
          'preconditioner must be square with same size as A.');
      end
    elseif (ischar(L))
      L = str2func(L);
    elseif (~isa(L, 'function_handle'))
      error('BLEIGIFP:badPreconType', ...
        'Input Error:  Preconditioner must be a matrix or a function.');
    end
  end

  %  usePrecon, if zero, indicates that preconditioning is not permitted.
  if (~usePrecon)
    %  Only warn if opt.PRECON has not been provided.
    if (isempty(L))
      warning('BLEIGIFP:noPreconAvail', '%s%s', ...
        'A and/or B are functions.  Cannot compute preconditioner using ', ...
        'opt.USEPRECON.  opt.USEPRECON, if present, will be ignored.');
    end
  else
    I = strcmpi('USEPRECON', names);
    if (any(I))
      usePreShift = options.(names{I});
      if (~isempty(usePreShift) && ~isempty(L))
        error('BLEIGIFP:badUsePrecon', ...
          '%s%s', 'Input Error:  invalid opt.useprecon as a ', ...
          'preconditioner has been provided.');
      end
      if (ischar(usePreShift))
        if (strcmpi(usePreShift, 'NO'))
          usePrecon = 0;
        end
      elseif (~isempty(usePreShift))
        if (any(size(usePreShift) > 1))
          error('BLEIGIFP:badPreShift', ...
            'Input Error:  Invalid shift; must be a real number.');
        end
      end
    end
  end

  I = strcmpi('MAXITERATION', names);
  if (~any(I))
    I = strcmpi('MAXIT', names);
  end
  if (any(I))
    iterMax = options.(names{I});
    if (iterMax <= 0)
      error('BLEIGIFP:badMaxIt', 'Input Error:  Invalid maximum iteration.');
    end
  end

  I = strcmpi('ARGSA', names);
  if (any(I))
    argsA = options.(names{I});
    %  If the value of argsA is a numeric type, wrap it as a cell, since
    %  we require argsA to be cell array.
    if (isnumeric(argsA))
      argsA = {argsA};
    elseif (~isa(argsA, 'cell'))
      error('BLEIGIFP:badArgsA', ...
        'Input Error:  argsA should be numeric or a Cell array.');
    end
  end

  I = strcmpi('ARGSB', names);
  if (any(I))
    argsB = options.(names{I});
    %  If the value of argsB is a numeric type, wrap it as a cell, since
    %  we require argsB to be cell array.
    if (isnumeric(argsB))
      argsB = {argsB};
    elseif (~isa(argsB, 'cell'))
      error('BLEIGIFP:badArgsB', ...
        'Input Error:  argsB should be numeric or a Cell array.');
    end
  end

  I = strcmpi('ARGSL', names);
  if (any(I))
    argsL = options.(names{I});
    %  If the value of argsL is a numeric type, wrap it as a cell, since
    %  we require argsL to be cell array.
    if (isnumeric(argsL))
      argsL = {argsL};
    elseif (~isa(argsL, 'cell'))
      error('BLEIGIFP:badArgsL', ...
        'Input Error:  argsL should be numeric or a Cell array.');
    end
  end

  I = strcmpi('SIGMA', names);
  if (any(I))
    sigma = options.(names{I});
    if (ischar(sigma))
      sigma = upper(sigma);
      if (isequal(sigma, 'LA'))
        scalA = -1;
      elseif (~isequal(sigma, 'SA'))
        error('BLEIGIFP:badSigmaValue', ...
          'Input Error:  Invalid string for sigma!!');
      end
    else
      error('BLEIGIFP:badSigmaType', ...
        'Input Error:  Invalid type for opt.sigma!!');
    end
  end
end

%  Set default norm estimates
if (isempty(normA))
  if (isnumeric(A))
    normA = norm(A, 1);
  else
    warning('BLEIGIFP:noAnorm', ...
      'options.Anorm (estimate of ||A||_1) is not provided.');
    normA = norm(feval(A, ones(n, 1), argsA{:}), 1)/n;
  end
end
if (isempty(normB))
  if (isempty(B))
    normB = 1;
  elseif (isnumeric(B))
     normB = norm(B, 1);
  else
    warning('BLEIGIFP:noBnorm', ...
      'options.Bnorm (estimate of ||B||_1) is not provided.');
    normB = norm(feval(B, ones(n, 1), argsB{:}), 1)/n;
  end
end

%  Set default tolerance.
toleranceA = 10*eps*normA*sqrt(n);
if(~isempty(B))
  toleranceB = 10*eps*normB*sqrt(n);
else
  toleranceB = 0;
end
if (~isempty(tolerance))
  toleranceVec = [tolerance, 0];
  if (tolerance < toleranceA)
    warning('BLEIGIFP:lowTol', ...
      'Tolerance may be set too low. Suggested value: %E.', toleranceA);
  end
else
  toleranceVec = [toleranceA, toleranceB];
end

%  Setup preconditioner, if necessary.
%  This involves converting A or B to sparse if a preconditioner of
%  any kind is called for.
if (usePrecon)
  %  Check sparsity for A and B, provided they aren't simply scalars.
  if (~issparse(A) && (n > 1))
    warning('BLEIGIFP:AnotSparse', ...
      'A is not in the sparse format...converting A to sparse.');
    A = sparse(A);
  end
  if (~isempty(B) && (n > 1))
    if (~issparse(B))
      warning('BLEIGIFP:BnotSparse', ...
        'B is not in the sparse format...converting B to sparse.');
      B = sparse(B);
    end
  end
  if (~isempty(usePreShift))
    if fid ~= 0
      fprintf(fid, '%s%s\n', 'Computing threshold ILU factorization ', ...
              'using the shift provided.');
    end
    if (~isempty(B))
      L = ildlte(A-usePreShift*B, eta);
    else
      L = ildlte(A-usePreShift*speye(n), eta);
    end
  end
end

%  Build the appropriate operators.
if (isnumeric(A))
  Afunc = @(x) A*x;
else
  Afunc = @(x) A(x, argsA{:});
end
if (~isempty(B))
  if (isnumeric(B))
    opB = @(x) B*x;
  else
    opB = @(x) B(x, argsB{:});
  end
else
  opB = [];
end
if (~isempty(L))
  if (isnumeric(L))
    opL = @(x) L'\(L\x);
  else
    opL = @(x) L(x, argsL{:});
  end
else
  opL = [];
end

if (scalA == -1)
  opA = @(x) -Afunc(x);
else
  %  opA has already been determined to be the function that affects
  %  multiplication by A.
  opA = Afunc;
end

if (m < 1)
  m = min(n-1, 2);
  if (~isempty(opL))
    m = 1;
  end
end
%  Balance block size and number of inner iterations to not be larger than
%  the dimension of the problem.
if (maxp*(m+1) > n)
  [newM, newMaxP] = balanceSizes(m, p, n);
  maxp = newMaxP;
  if newMaxP < p || newM < m
    warning('BLEIGIFP:tooManyIts', ...
      'Block size and inner iterations too large.  Using adjusted sizes.');
    m = newM;
    p = maxp;
  end
end

%  Call the main function depending on the requested block size.
start_t = cputime;
[EVals, EVecs, resHist, cummatvec] = blifree(opA, opB, n, m, toleranceVec, iterMax, k, ...
  X, p, maxp, adaptive, adapt_tol, opL, fid, fixInnerIt);
                                                  
%  Transform the EVals according to scalA.
EVals = scalA*EVals;

time_req = cputime - start_t;
if fid ~= 0
  fprintf(fid, '\n-----\n  CPU Time:  %f.\n', time_req);
end

%  Close the file if necessary.
if (closeFile)
  fclose(fid);
end
%%%  END bleigifp  %%%

function [Evals, Evecs, resHist, cummatvec] = blifree(opA, opB, n, m, ...
  toleranceVec, maxits, w, X, p, max_p, adaptive, adapt_tol, opL, ...
  fout, fixInnerIt)
%
%  Main function for BLEIGIFP:
%    Implementing Block Inverse Free Preconditioned Krylov Subspace method for
%    finding eigenvalues of a symmetric definite pencil.
%

%  Set up local variables from opts struct.
px = size(X,2) + 1;
if (p >= px) 
  X(:, px:p) = rand(n, p-px+1) - 0.5;
else
  X = X(:,1:p); 
end
Wm = [];

innerits = m;
use_diff = true;

%  Define statistics struct.
stats.amult = 0;
stats.bmult = 0;
stats.precon = 0;
stats.rkdef = 0;

%  Data needed to determine when to bump the number of inner iterations.
conv = ones(maxits, 1);
rate = ones(10, 1);
if (fixInnerIt)
  changeCounter = -maxits;
else
  changeCounter = -10;
end
priorM = innerits;
priorRate = -eps;
%  maxPossibleM does not allow for the number of inner iterations to grow
%  beyond the largest possible dimension.
maxm = min(128, fix((n/max_p) - 1));
  
%  Orthonormalize X with respect to B.
if ~isempty(opB)
  BX = opB(X);
  stats.bmult = stats.bmult + p;
  [X, BX, rk] = two_sided_mgs(X, BX);
  if rk < p
    [X, BX] = add_and_orth(X, BX, p - rk, [], [], 0, opB);
    stats.rkdef = stats.rkdef + p - rk;
  end
else
  X = orth(X);
  BX = X;
end

AX = opA(X);
stats.amult = stats.amult + p;
[Vm, Theta] = eig(X'*AX);
Theta = diag(Theta);
%  Normalize the columns of Vm to maintain the B-orthnormality of X.
for i = 1:p
  Vm(:,i) = Vm(:,i)/norm(Vm(:,i));
end
%  Update based on the eigen computation above.
X = X*Vm;
AX = AX*Vm;
BX = BX*Vm;
R = AX - BX*diag(Theta); 
res = computeResidualNorms(R);
%  Initialize the diff vectors.
diff = [];
Bdiff = [];

if fout ~= 0
  fprintf(fout, '\nComputing Eigenvalues 1 to %d:\n', p);
  fprintf(fout, '  Iter      Eigenvalue        Residual   InnerIt BlockSz\n');
  fprintf(fout, '  ====================================================== \n');
end

%  Preallocate the return values.
Evals = zeros(w,1);
Evecs = zeros(n,w);
BEvecs = zeros(n,w);
numconverged = 0;
lam_max = 0;
lmax_out = max(Theta);
%  Make lmax_out a little larger in case it happens to be an eigenvalue.
lmax_out = lmax_out + max(1, eps(lmax_out));
%  Begin outer iteration.
for k = 1:maxits
  if fout ~= 0
    for rpi = 1:p
      fprintf(fout, '%6d%16.6e%16.6e%10d%8d\n', k, Theta(rpi), ...
        res(rpi), innerits, p);
    end
    fprintf(fout, '\n');
  end
  %  Put res in the right place in resHist
  resHist(k,numconverged+(1:p)) = res(1:p);
  cummatvec(k,1) = stats.amult;
  %  Check for convergence.
  tol = (toleranceVec(1) + abs(Theta').*toleranceVec(2))*sqrt(p);
  toextract = (res < tol);
  %  Only pull off converged Ritz pairs if the lowest contiguous block
  %  has all converged.
  numnew = sum(toextract);
  newlyConverged = numnew > 0 && (find(toextract,1,'last') == numnew);
  if newlyConverged
    convinds = numconverged + (1:numnew);
    Evals(convinds,1) = Theta(toextract);
    Evecs(:,convinds) = X(:,toextract);
    tmp = sqrt(diag(X(:,toextract)'*BX(:,toextract)));
    BEvecs(:,convinds) = BX(:,toextract)/diag(tmp);
    if fout ~= 0
      fprintf(fout, '  ------------\n');
      fprintf(fout, '  Eigenvalue %d converged.\n', convinds);
    end
    numconverged = numconverged + numnew;
  end
  if (numconverged >= w)
    break;
  end
  %  Replace the converged eigenvalues with the next p candidates.
  if newlyConverged
    %  Assign lam_max for deflation.
    lam_max = lmax_out;
    if isempty(Wm)
      %  If there are no extra Ritz vectors, add random vectors.
      X(:,toextract) = rand(n,numnew) - 0.5;
      if ~isempty(opB)
        BX = opB(X);
        stats.bmult = stats.bmult + p;
        [X, BX] = two_sided_mgs(X, BX);
      else
        X = orth(X);
        BX = X;
      end
      AX = opA(X);
      stats.amult = stats.amult + p;
      for kdef = 1:numconverged
        AX = AX + (lam_max - Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*X);
      end   
      [Vm, Theta_m] = eig(X'*AX);
      [Theta, isort] = sort(diag(Theta_m));
      Vm = Vm(:,isort);
      %  Make sure that Vm is absolutely orthonormal.
      Vm = orth(Vm);
      X = X*Vm;
      AX = AX*Vm;
      BX = BX*Vm;
    else
      %  Otherwise, there are some extra Ritz vectors to use, so use them.
      Theta(toextract) = Sigma(1:numnew);
      X(:,toextract) = Z*Wm(:,1:numnew);
      %  Sort the Ritz values in ascending order.
      [Theta, isort] = sort(Theta);
      X = X(:,isort);
      %  Reorthogonalize X and compute BX.
      if ~isempty(opB)
        BX = opB(X);
        stats.bmult = stats.bmult + p;
        [X, BX] = two_sided_mgs(X, BX);
      else
        X = orth(X);
        BX = X;
      end
      %  Recompute AX and R.
      AX = opA(X);
      stats.amult = stats.amult + p;
      for kdef = 1:numconverged
        AX = AX + (lam_max - Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*X);
      end
    end
    R = AX - BX*diag(Theta);
    %  Reset the diff vectors.
    if use_diff
      diff = [];
      Adiff = []; 
      Bdiff = [];
    end
    %  Reset innerits, etc.
    innerits = m;
    if fixInnerIt
      changeCounter = -maxits;
    else
      changeCounter = -10;
    end
    priorM = innerits;
    priorRate = -eps;
  end

  %  Build the basis:
  %  Run an Arnoldi-like process using the operator AX - BX\Theta, and do so
  %  in the inner product defined by B.

  %  Pre-allocate for Am and Bm, the projected matrices.
  Am = zeros(p*m + p);

  %  Pre-allocate space for Z, AZ, BZ.
  Z = zeros(n, p*m + p);
  AZ = zeros(n, p*m + p);
  BZ = zeros(n, p*m + p);

  %  Initialize the first blocks.
  %  Note:  this assumes that X comes in as B-orthonormal.
  Z(:,1:p) = X;
  AZ(:,1:p) = AX;
  BZ(:,1:p) = BX;
  Am(1:p,1:p) = X'*AX;

  %  vecindex is a pointer to where the current block begins.
  vecindex = p;
  blocksize = p;
  for i = 1:innerits
    %  Apply the preconditioner.
    if ~isempty(opL)
      R = opL(R);
      stats.precon = stats.precon + p;
    end
    %  Orthogonalize against the previous blocks.
    if ~isempty(opB)
      tmpBZ = opB(R);
      stats.bmult = stats.bmult + p;
      for j = 1:i
        offset_j = (j-1)*p;
        mylocZ = Z(:,offset_j+1:offset_j+p);
        mylocBZ = BZ(:,offset_j+1:offset_j+p);
        thisalpha = mylocBZ'*R;
        R = R - mylocZ*thisalpha;
        thisalpha = mylocZ'*tmpBZ;
        tmpBZ = tmpBZ - mylocBZ*thisalpha;
      end
      %  Reorthogonalize.
      for j = 1:i
        offset_j = (j-1)*p;
        mylocZ = Z(:,offset_j+1:offset_j+p);
        mylocBZ = BZ(:,offset_j+1:offset_j+p);
        thisalpha = mylocBZ'*R;
        R = R - mylocZ*thisalpha;
        thisalpha = mylocZ'*tmpBZ;
        tmpBZ = tmpBZ - mylocBZ*thisalpha;
      end

      [R, tmpBZ, rk] = two_sided_mgs(R, tmpBZ);
      %  Complete the block with random vectors, if some vectors were
      %  deflated out.
      if rk < p
        [R, tmpBZ] = add_and_orth(R, tmpBZ, p - rk, Z, BZ, vecindex, opB);
        stats.rkdef = stats.rkdef + p - rk;
      end
    else
      R = orth_against(R, [], Z, BZ, 1, vecindex);
      [R, tmpBZ, rk] = two_sided_mgs(R, R);
      %  Complete the block with random vectors, if some vectors were
      %  deflated out.
      if rk < p
          [R, tmpBZ] = add_and_orth(R, tmpBZ, p - rk, Z, BZ, vecindex, opB);
          stats.rkdef = stats.rkdef + p - rk;
      end
    end
    Z(:,vecindex+1:vecindex+blocksize) = R;
    %  Compute next R.
    tmpAZ = opA(R);
    stats.amult = stats.amult + p;
    %  Handle deflation.
    for kdef = 1:numconverged
      tmpAZ = tmpAZ + (lam_max-Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*R);
    end
    R = tmpAZ - tmpBZ*diag(Theta(1:end));
    AZ(:,vecindex+1:vecindex+blocksize) = tmpAZ;
    BZ(:,vecindex+1:vecindex+blocksize) = tmpBZ;
    Am(1:vecindex+blocksize,vecindex+1:vecindex+blocksize) = ...
      Z(:,1:vecindex+blocksize)'*tmpAZ;
    vecindex = vecindex + blocksize;
  end

  %  Add diff vecs if necessary.
  if ~isempty(diff)
    [tmpZ, tmpAZ,tmpBZ,index] = orth_diff(diff, Adiff, Bdiff, Z, AZ, BZ, 1, vecindex);
    tmpAZ(:,index) = opA(tmpZ(:,index));
    if ~isempty(opB)
      tmpBZ(:,index) = opB(tmpZ(:,index));
    end
    stats.amult = stats.amult +sum(index);
    % Handle deflation.
    for kdef = 1:numconverged
      tmpAZ(:,index) = tmpAZ(:,index) + ...
          (lam_max-Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*tmpZ(:,index));
    end
    [tmpZ, tmpAZ,tmpBZ, rk] = two_sided_mgs_diff(tmpZ, tmpAZ, tmpBZ, ...
        opA, opB, lam_max, Evals, BEvecs, numconverged);
    Z(:,vecindex+1:vecindex+rk) = tmpZ;
    AZ(:,vecindex+1:vecindex+rk)= tmpAZ;
    if ~isempty(opB)
      BZ(:,vecindex+1:vecindex+rk) = tmpBZ;
    else
      BZ(:,vecindex+1:vecindex+rk) = tmpZ;
    end
    Am(1:vecindex+rk,vecindex+1:vecindex+rk) = ...
      Z(:,1:vecindex+rk)'*tmpAZ;
  end
  projSize = size(Z,2);
  Am = triu(Am(1:projSize,1:projSize)) + triu(Am(1:projSize,1:projSize),1)';

  %  Compute p smallest eigenvalues and eigenvectors of (Am, Bm).
  [Vm, Theta_m] = eig(Am);
  [Th_sort, isort] = sort(diag(Theta_m));
  %  Keep the p+1:2p Ritz pairs in case of convergence and block-size
  %  augmentation.
  Wm = Vm(:,isort(p+1:end));
  Sigma = Th_sort(p+1:end);
  Vm = Vm(:,isort(1:p));
  %  Cache lmax_out to be used as lam_max in the event that we have some
  %  eigenvalues converge.
  lmax_out = Th_sort(end);
  %  Normalize columns of Vm.
  for i = 1:p
    Vm(:,i) = Vm(:,i)/norm(Vm(:,i));
    Wm(:,i) = Wm(:,i)/norm(Wm(:,i));
  end
  %  Update the matrices Theta and X.
  X = Z*Vm;
  Theta = Th_sort(1:p);
  AX = AZ*Vm;
  if ~isempty(opB)
    BX = BZ*Vm;
  else
    BX = X;
  end
  %  Update the residuals.
  R = AX - BX*diag(Theta);
  res = computeResidualNorms(R);

  %  Update the diff vectors if necessary.
  if (use_diff)
    vsize = size(Vm, 1);
    for i = 1:p
      tmpvec = Vm([1:i-1,i+1:vsize],i);
      Vm(i,i) = -(tmpvec'*tmpvec)/Vm(i,i);
    end
    diff = Z*Vm;
    Adiff = AZ*Vm; 
    Bdiff = BZ*Vm;
  end

  %  Determine whether or not to increase the block size.
  if (adaptive && (p < max_p))
    ratio = (Theta(p) - Theta(1))/(Sigma(end) - Theta(1));
    if ratio < adapt_tol
      p = p+1;
      %  Note that using the p+1 Ritz pair when B == I results in a
      %  singular R.  Use a randomly chosen next vector.
      nextVec = rand(n,1) - 0.5;
      nextVec = nextVec - X*(BX'*nextVec);
      if ~isempty(opB)
        BX(:,p) = opB(nextVec);
        stats.bmult = stats.bmult + 1;
      else
        BX(:,p) = nextVec;
      end
      tmpval = sqrt(nextVec'*BX(:,p));
      nextVec = nextVec/tmpval;
      X(:,p) = nextVec;
      BX(:,p) = BX(:,p)/tmpval;
      tmpAvec = opA(nextVec);
      stats.amult = stats.amult + 1;
      for kdef = 1:numconverged
        tmpAvec = tmpAvec + (lam_max-Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*nextVec);
      end
      Theta(p) = nextVec'*tmpAvec;
      AX(:,p) = tmpAvec;
      R(:,p) = AX(:,p) - BX(:,p)*Theta(p);
      res(p) = norm(R(:,p));
      %  Reset the diff vectors.
      if (use_diff)
        diff = [];
        Bdiff = [];
      end
    end
  end
  %%%  END inlined BLARNO  %%%
 
  % Take the first residual norm to test convergence rate.
  conv(k) = res(1);
  
  %  Check on convergence rate and update mValue.
  changeCounter = changeCounter+1;
  if (changeCounter >= 19)
    rate = [rate(2:10); aveRate(conv, k-changeCounter+4, k)];
    [innerits, priorM, priorRate, fixM] = updateM(rate, innerits, priorM, ...
                                                  priorRate, maxm);
    changeCounter = (changeCounter + fixM*maxits)*(1-fixM);
  elseif (changeCounter >= 10)
    rate = [rate(2:10); aveRate(conv, k-changeCounter+4, k)];
  end
end   %  END outer iteration.

%  Warn if fewer than w eigenpairs were found.
if (numconverged < w)
  warning('BLEIGIFP:notAllFound', 'Not all requested eigenvalues were found.');
  Evals = Evals(1:numconverged);
  Evecs = Evecs(:,1:numconverged);
end
if fout ~= 0
  fprintf(fout, '  Total # of multiplications by A:            %10d.\n', ...
          stats.amult);
  if ~isempty(opB)
    fprintf(fout, '  Total # of multiplications by B:            %10d.\n', ...
            stats.bmult);
  end
  if ~isempty(opL)
    fprintf(fout, '  Total # of applications of preconditioner:  %10d.\n', ...
            stats.precon);
  end
end
%%%  END blifree  %%%

function [X, BX, rk] = two_sided_mgs(X, BX)
%  Orthonormalize X in the B-inner product by bi-orthogonalizing X and BX.
n = size(X,2);
deftol = eps(sqrt(n));
rk = 0;
keep = false(n, 1);
for i = 1:n
  alpha = sqrt(X(:,i)'*BX(:,i));
  X(:,i) = X(:,i)/alpha;
  BX(:,i) = BX(:,i)/alpha;
  if alpha > deftol
    keep(i) = true;
    rk = rk + 1;
    for j = i+1:n
      alpha = X(:,j)'*BX(:,i);
      X(:,j) = X(:,j) - alpha*X(:,i);
      alpha = X(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BX(:,i);
    end
  end
end
X = X(:,keep);
BX = BX(:,keep);
%  Reorthogonalize.
for i = 1:rk
  alpha = sqrt(X(:,i)'*BX(:,i));
  X(:,i) = X(:,i)/alpha;
  BX(:,i) = BX(:,i)/ alpha;
  for j = i+1:rk
    alpha = X(:,j)'*BX(:,i);
    X(:,j) = X(:,j) - alpha*X(:,i);
    alpha = X(:,i)'*BX(:,j);
    BX(:,j) = BX(:,j) - alpha*BX(:,i);
  end
end
%%%  END two_sided_mgs  %%%

function [X, AX,BX, rk] = two_sided_mgs_diff(X, AX,BX,opA,opB,lam_max,Evals,BEvecs,numconverged)
%  Orthonormalize X in the B-inner product by bi-orthogonalizing X and BX.
deftol = eps;
scaleddiffnorm=1e-02*sum(abs(X),1);
n = size(X,2);
rk = 0;
keep = false(n, 1);
for i = 1:n
  alpha = sqrt(X(:,i)'*BX(:,i));
  if norm(X(:,i),1) < scaleddiffnorm(i)
    X(:,i) = X(:,i)/alpha;
    AX(:,i) = opA(X(:,i));
    for kdef = 1:numconverged
      AX(:,i) = AX(:,i) + (lam_max-Evals(kdef))*BEvecs(:,kdef)*(BEvecs(:,kdef)'*X(:,i));
    end
    if ~isempty(opB)
      BX(:,i) = opB(X(:,i));
    else
      BX(:,i) = X(:,i);
    end
  else
    X(:,i) = X(:,i)/alpha;
    AX(:,i) = AX(:,i)/alpha;
    BX(:,i) = BX(:,i)/alpha;
  end
  if alpha > deftol
    keep(i) = true;
    rk = rk + 1;
    for j = i+1:n
      alpha = X(:,j)'*BX(:,i);
      X(:,j) = X(:,j) - alpha*X(:,i);
      AX(:,j) = AX(:,j) - alpha*AX(:,i);
      alpha = X(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BX(:,i);
    end
  end
end
X = X(:,keep);
AX = AX(:,keep);
BX = BX(:,keep);
%  Reorthogonalize.
for i = 1:rk
  alpha = sqrt(X(:,i)'*BX(:,i));
  X(:,i) = X(:,i)/alpha;
  AX(:,i) = AX(:,i)/alpha;
  BX(:,i) = BX(:,i)/ alpha;
  for j = i+1:rk
    alpha = X(:,j)'*BX(:,i);
    X(:,j) = X(:,j) - alpha*X(:,i);
    AX(:,j) = AX(:,j) - alpha*AX(:,i);
    alpha = X(:,i)'*BX(:,j);
    BX(:,j) = BX(:,j) - alpha*BX(:,i);
  end
end
%%%  END two_sided_mgs_diff  %%%

function [X, BX] = orth_against(V, BV, Z, BZ, firstZ, lastZ)
X = V;
BX = BV;
nx = size(X,2);
%  Orthogonalize against the Z vectors.
for i = firstZ:lastZ
  for j = 1:nx
    alpha = X(:,j)'*BZ(:,i);
    X(:,j) = X(:,j) - alpha*Z(:,i);
    if ~isempty(BX)
      alpha = Z(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BZ(:,i);
    end
  end
end
for i = firstZ:lastZ
  for j = 1:nx
    alpha = X(:,j)'*BZ(:,i);
    X(:,j) = X(:,j) - alpha*Z(:,i);
    if ~isempty(BX)
      alpha = Z(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BZ(:,i);
    end
  end
end
%%%  END orth_against  %%%

function [X, AX, BX, index] = orth_diff(V, AV, BV, Z, AZ, BZ, firstZ, lastZ)
X = V;
diffnorm=sum(abs(X),1);
AX = AV;
BX = BV;
nx = size(X,2);
%  Orthogonalize against the Z vectors.
for i = firstZ:lastZ
  for j = 1:nx
    alpha = X(:,j)'*BZ(:,i);
    X(:,j) = X(:,j) - alpha*Z(:,i);
    AX(:,j) = AX(:,j) - alpha*AZ(:,i);
    if ~isempty(BX)
      alpha = Z(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BZ(:,i);
    end
  end
end
for i = firstZ:lastZ
  for j = 1:nx
    alpha = X(:,j)'*BZ(:,i);
    X(:,j) = X(:,j) - alpha*Z(:,i);
    AX(:,j) = AX(:,j) - alpha*AZ(:,i);
    if ~isempty(BX)
      alpha = Z(:,i)'*BX(:,j);
      BX(:,j) = BX(:,j) - alpha*BZ(:,i);
    end
  end
end
index = sum(abs(X),1) < 1e-02*diffnorm; 
%%%  END orth_diff  %%%

function [X, BX] = add_and_orth(V, BV, numtoadd, Z, BZ, vecindex, opB)
%  Add numtoadd vectors to the basis and orthogonalize.
%  This assumes that BV = opB(V);
%  Additionally, orthgogonalize against the B-orthogonal vectors Z.
m = size(V, 1);
X = rand(m, numtoadd) - 0.5;
%  Orthogonalize against all the vectors
if ~isempty(opB)
  BX = opB(X);
  [X, BX] = orth_against(X, BX, V, BV, 1, size(V,2));
  [X, BX] = orth_against(X, BX, Z, BZ, 1, vecindex);
else
  X = orth_against(X, [], V, BV, 1, size(V,2));
  X = orth_against(X, [], Z, BZ, 1, vecindex);
  BX = X;
end
[X, BX, rk] = two_sided_mgs(X, BX);
if rk < numtoadd
  error('BLEIGIFP:add_and_orth:depend', '%s%d%s%d\n', ...
        'Couldn''t add enough linearly independent vectors!  rk = ', rk, ...
        '  numtoadd = ', numtoadd);
end
X = [V, X];
BX= [BV, BX];
%%%  END add_and_orth  %%%

function res = computeResidualNorms(R)
n = size(R,2);
res = zeros(1,n);
for k = 1:n
    res(k) = norm(R(:,k));
end
%%%  END computeResidualNorms  %%%

function rate = aveRate(conv, k, iter)
%  Compute average linear convergence rate of conv over steps k+1 to iter.
rate = 0;
if (iter-k < 2 || k < 1)
   return;
end
y = log10(conv((k+1):iter));
xAve = (iter - k+1)/2;
xyAve = ((1:iter-k)*y)/(iter-k) - xAve*sum(y)/(iter-k);
xAve = ((iter-k)^2-1)/12;
rate = xyAve/xAve;
%%%  END aveRate  %%%

function [mValue, priorM, priorRate, fixM] = updateM(rate, mValue, ...
                                               priorM, priorRate, maxm)
%  Adaptive update of mValue:  inner iteration
fixM = 0;
%  Update m when rate stagnates.
if ((max(rate)-min(rate)) < 0.1*(-rate(10)) || min(rate) > 0)
  %  Update m by k times.
  k = 2;
  %  Use larger k if slower convergence.
  if ((rate(10) > -0.001) && 8*mValue <= maxm)
    k = 8;
  elseif ((rate(10) > -0.01) && 4*mValue <= maxm)
    k = 4;
  end
  %  Increase m by testing acceleration rate.
  incFlag = (rate(10)/priorRate)*(priorM/mValue);
  if (incFlag > 1.05)
    if(2*mValue > maxm)
      fixM = -1;
    else
      priorM = mValue;
      priorRate = rate(10);
      mValue = k*mValue;
      fixM = 1;
    end
  elseif ((rate(10) > -0.001) && 2*mValue <= maxm)
    mValue = k*mValue;
    fixM = 1;
  elseif ((rate(10) > -0.01) && 2*mValue <= maxm)
    mValue = k*mValue;
    fixM = 1;
  elseif (incFlag < 0.9)
    mValue = priorM;
    fixM = -1;
  end
end
%%%  END updateM  %%%

function L = ildlte(A, eta)
%  ILDLT   Threshold incomplete LDL^T  using ilu
n = size(A, 1);
%  Compute L U factors.
options.type = 'ilutp';
options.thresh = 0;
options.udiag = 1;
if (eta == 2)
  options.milu = 'row';
  options.droptol = 1e-4;
  [L, U] = ilu(A, options);
elseif (eta == 1)
  %  Repair diagonal if necessary to get around fact the ilu ignores udiag
  %  when requesting ilu(0).
  d = diag(A);
  d(d==0) = 1e-8;
  [L, U] = ilu(A+spdiags(d,0,size(A,1),size(A,2)), struct('type','nofill'));
else
  options.droptol = eta;
  [L, U] = ilu(A, options);
end
%  Scale diagonals to get L.
d = diag (U);
d = sqrt(abs(d));
for k = 1:n
  if (d(k) < 1e-8)
    d(k) = 1e-8;
  end
end
L = L * spdiags(d, 0, n, n);
%%%  END ildlte  %%%

function [m,p] = balanceSizes(m, p, n)
%  The requested sizes are too large.
%  Attempt to find new sizes that fit.
%  Favor p being larger.
%  The absolute minimum allowed value for m is 1.
while (m > 1 && (m+1)*p > n)
  m = m-1;
end
if ((m+1)*p > n)
  while (p > 1 && (m+1)*p > n)
    p = p-1;
  end
end
if ((m+1)*p > n)
  error('BLEIGIFP:problemTooSmall', ...
    'Cannot find parameters small enough for this problem');
end
%%%  END balanceSizes  %%%

%%%  END bleigifp.m  %%%
