function [c,s] = angleS(S1,S2,tol)
% Compute cosine and sine of the principal angles between the subspaces
% S1 and S2.
%
% Note: Use sine when comparing angles to zero and cosine when comparing
% to pi/2 to check orthogonality.
%
% Example:
%    u = randn(10,1);
%    v = u + 1e-10 * randn(10,1);
%    [c,s] = ltipack.angleS(u,v)
%    sqrt(1-c^2)
% Here S is o(1e-10) while C is exactly 1 because cos(1e-10)=1 to the
% machine precision.

%   Copyright 2018 The MathWorks, Inc.

% If U1 = span(S1), U2 = span(S2), and [U1,V1] is an orthonormal basis, 
% decompose
%    S2 = U1 * (U1'*S2) + V1 * (V1'*S2) 
% and use the C-S decomposition of the orthogonal matrix [U1'*U2 ; V1'*U2]
if nargin<3
   tol = 10*eps;
end
% n = row size of S1,S2
[U1,V1] = localGetBases(S1,tol);
[U2,V2] = localGetBases(S2,tol);
[n,p1] = size(U1);  % n = p1+size(V1,2)
p2 = size(U2,2);    % n = p2+size(V2,2)
if p1>=p2
   % Note: size(C,1) = p1 >= p2 but S0 can be tall or wide
   [~,~,~,c,s0] = gsvd(U1'*U2,V1'*U2);
   c = diag(c(1:p2,1:p2));
   s = zeros(p2,1);
   q = min(n-p1,p2); % min size of S0
   s(1:q,:) = diag(s0(1:q,1:q));
   s(q+1:p2,:) = sqrt(max(0,1-c(q+1:p2).^2));
else
   % Note: size(C,1) = p2 > p1 but S0 can be tall or wide
   [~,~,~,c,s0] = gsvd(U2'*U1,V2'*U1);
   c = diag(c(1:p1,1:p1));
   s = zeros(p1,1);
   q = min(n-p2,p1); % min size of S0
   s(1:q,:) = diag(s0(1:q,1:q));
   s(q+1:p1,:) = sqrt(max(0,1-c(q+1:p1).^2));
end

function [U,V] = localGetBases(S,tol)
% Computes orthonormal bases of range(S) and range(S)^perp.
% TOL is used to decide the rank of S.
m = size(S,2);
[Q,sv] = svd(S);
sv = diag(sv(1:m,:));
r = sum(sv>tol*sv(1));
U = Q(:,1:r);
V = Q(:,r+1:end);