function[Y,S,res] = invariantPair(coeffs,C,R,N)
% This function approximates an invariant pair (X,S) of P(\lambda). The
% approach used for this computation relies on the block version of the moment method.

% Input:  coeffs  -  cell array containing coefficients A_i of matrix polynomial
%              C  -  center of circle
%              R  -  radius of circle
%          nodes  -  number of nodes of the trapezoid rule to approximate the contour integral
% Output:  (X,S)  -  approximation to invariant pair
%            res  -  residual P(Y,S)

len = length(coeffs)-1; n = length(coeffs{1});

% compute number of eigenvalues in contour, P(phi)
numEig = numEigContour(coeffs,200,C,R,N);

if(numEig==0)
    disp('The matrix polynomial has no eigenvalues in this contour! Try choosing a new contour');
else
    for L=1:n
        mu=[]; H0=[]; H1=[];
        numMome = 2*round(numEig/L)-2;
        % choose random vectors/matrices u and v
        u = randi([-20,20],n,L); v = randi([-20,20],n,1);
        
        %computing s_k
        for k=0:numMome+1
            S = 0;
            for j=0:N-1
                expon = exp(1i*j*2*pi/N);
                phi = (C+R*expon);
                % Computing F(phi)
                F = zeros(n,n);
                for m = 0:len; F = F + phi^m*coeffs{m+1}; end
                % Computing inv(F(phi))v
                y = F\v;
                S = S + (phi^k)*y*expon;
            end
            if(L==1)
                s{k+1} = (R/N)*S;
            else
                s{k+1} = [(R/N)*S s{k+1}];
            end
            %compute mu_k = u^H*s_k
            mu{k+1} = u'*s{k+1};
        end
        
        %constructing H_0 and H_1
        for i=1:(numMome/2+1)
            for j=1:(numMome/2+1)
                H0((i-1)*L+1:i*L,(j-1)*L+1:j*L) = mu{i+j-1};
                H1((i-1)*L+1:i*L,(j-1)*L+1:j*L) = mu{i+j};
            end
        end
        %In case we have to break last block of H_0 and H_1
        H0 = H0(1:numEig,1:numEig); H1 = H1(1:numEig,1:numEig);
        if(rank(H0)==numEig)
            
            %Constructing Invariant Pair (Y,S)%
            Y = [s{1:numMome/2+1}]; Y = Y(:,1:numEig);
            S = H0\H1;
            %check if is invariant pair
            %powers of S%
            pS(:,:,1) = eye(numEig);
            for j = 2:len+1, pS(:,:,j) = pS(:,:,j-1)*S; end
            
            % Compute residual
            res = zeros(n,numEig);
            for j = 0:len, res = res + coeffs{j+1}*Y*pS(:,:,j+1); end
            return;
        end
    end
end
end

function[e] = numEigContour(poly,L,C,R,N)
% this function computes an approximation to the number of eigenvalues inside a circle
% Input:  poly  - cell array containing coefficients of matrix polynomial: P(x) = A_l x^l + ...+ A_2 x^2 + A_1 x + A_0
%            L  - number of sample vectors
%            C  - center of circle
%            R  - radius of circle
%            N  - number of nodes to approximate contour integral
% Output:    m - vector with approximations for the number of eigenvalues inside contour Gamma
len = length(poly)-1; n = length(poly{1}); v = zeros(n,L); expon = zeros(1,N); phi = zeros(1,N);

for i=1:L
    %random vector v_i
    index = randi(0:1,[n 1]);
    %(rand(n,1) <= 0.5)
    for k=1:n
        if(index(k)==1)
            v(k,i) = 1;
        else
            v(k,i) = -1;
        end
    end
end

S = 0;
for j=0:N-1
    expon(j+1) = exp(1i*j*2*pi/N);
    phi(j+1) = (C+R*expon(j+1));
    % Computing F(phi) and DF(phi)
    F = zeros(n,n); DF = zeros(n,n);
    for m = 0:len
        F = F + phi(j+1)^m*poly{m+1};
        if(m>0)
            DF = DF + m*phi(j+1)^(m-1)*poly{m+1};
        end
    end
    S1 = 0;
    for k=1:L
        S1 = S1 + v(:,k)'*(F\(DF*v(:,k)));
    end
    
    S = S + (R/N)*expon(j+1)*S1;
end
e = round(real((1/L)*S));
end