function[cond] = condNumInvPair(coeffs,X,S,C,R,nodes)
% This function approximates the condition number for an invariant pair (X,S)
% Input:  coeffs  -  cell array containing coefficients A_i of the matrix polynomial
%          (X,S)  -  Invariant pair (X,S)
%              C  -  center of circle
%              R  -  radius of circle
%          nodes  -  number of nodes of the trapezoid rule to approximate the contour integral
% Output:   cond  -  condition number of the invariant pair (X,S)

len = length(coeffs)-1; BB = [];

% C is the center of the circle and R the radius. Last parameter is the number of nodes 
[N,M,B] = condNumIntegrals(coeffs,X,S,C,R,nodes);
P = [N M];

for i=len+1:-1:1
    BB = [BB norm(coeffs{i},'fro')*B(:,:,i)];
end

[U,SIG,V] = svd(P); s = diag(SIG);
P = zeros(size(P'));
for j=1:length(s)
    if(s(j)<10^(-12))
        P(j,j) = 0;
    else
        P(j,j) = 1/s(j);
    end    
end

cond = norm( V*P*U'*BB )/norm( [norm(X,'fro') norm(S,'fro')] );
end

function[N,M,B] = condNumIntegrals(poly,X,S,C,R,nodes)
% This function computes the INTEGRALS FOR THE CONDITION NUMBER,
% approximating the contour integral with the trapezoid sum and using a
% circle as contour.

[n,k] = size(X); len = length(poly)-1; %AU = zeros(n*k,n*n);
N = zeros(n*k,n*k); M = zeros(n*k,k*k);
for jj = 1:len+1, B(:,:,jj) = zeros(n*k,n*n); end 

for j=0:nodes-1
    expon = exp(1i*j*2*pi/nodes); phi = (C+R*expon);
    % Compute T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end
    N = N + expon*kron( inv(phi*eye(k,k)-S).', T );
    M = M + expon*kron( inv(phi*eye(k,k)-S).', T*X/(phi*eye(k,k)-S) );    
    
    for ii = 1:len+1
        B(:,:,ii) = B(:,:,ii) + expon*kron((phi^(ii-1)*X/(phi*eye(k,k)-S)).', eye(n,n));
    end       
end
N = (R/nodes)*N; M = (R/nodes)*M; B = (R/nodes)*B;
end