function [Q, R] = orthogonalize(M, x, A)
% Orthonormalizes a basis of tangent vectors in the Manopt framework.
%
% function [orthobasis, R] = orthogonalize(M, x, basis)
%
% M is a Manopt manifold structure obtained from a factory.
% x is a point on the manifold M.
% basis is a cell containing n linearly independent tangent vectors at x.
%
% orthobasis is a cell of same size as basis which contains an orthonormal
% basis for the same subspace as that spanned by basis. Orthonormality is
% assessed with respect to the metric on the tangent space to M at x.
% R is upper triangular of size n x n if basis has n vectors, such that:
%
%   basis{k} = sum_j=1^k orthobasis{j} * R(j, k).
%
% That is: we compute a QR factorization of basis.
%
% The algorithm is a modified Gram-Schmidt. If elements in the input basis
% are close to being linearly dependent (ill conditioned), then consider
% orthogonalizing twice, or calling orthogonalizetwice directly.
%
% See also: orthogonalizetwice grammatrix tangentorthobasis

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 28, 2016.
% Contributors: 
% Change log: 
%
%       Oct. 5, 2017 (NB):
%           Changed algorithm to a modified Gram-Schmidt and commented
%           about the twice-is-enough trick. Compared to the previous
%           version, this algorithm behaves much better if the input basis
%           is ill conditioned.

    assert(iscell(A), ...
         'The input basis must be a cell containing tangent vectors at x');

    n = numel(A);
    R = zeros(n);
    Q = cell(size(A));
    
    for j = 1 : n
        
        v = A{j};
        
        for i = 1 : (j-1)
           
            qi = Q{i};
            
            R(i, j) = M.inner(x, qi, v);
            
            v = M.lincomb(x, 1, v, -R(i, j), qi);
            
        end
        
        R(j, j) = M.norm(x, v);
        
        Q{j} = M.lincomb(x, 1/R(j, j), v);
        
    end

end
