function[Y,S,res] = invariantPairCircles(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 the matrix polynomial
%              C  - list with centers of the circles. The first entry must contain the 
%                   center of the main circle (which contains the smaller circles).
%              R  - list with radius of the circles. The first entry must contain the 
%                   radius of the main circle (which contains the smaller circles).
%          nodes  - number of nodes of the trapezoid rule to approximate the contour integral.
% Output:  (Y,S)  - approximation to invariant pair
%            res  - residual P(Y,S)

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


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

if(intersectionCircles(C(2:end),R(2:end))==1)
    disp('Some circles intersect! Try choosing new circles.');
    return;
end

for i=2:length(C)
    numEig1 = numEigContour(coeffs,200,C(i),R(i),N);
    numEig = numEig - numEig1;
    if(numEig1==0)
        ind = [ind i];
    end
end

%throwing away circles where we dont have eigenvalues
C(ind)=[]; R(ind)=[];

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
            for i=1:length(C)
                S{i} = zeros(n,1);
                for j=0:N-1
                    expon = exp(1i*j*2*pi/N);
                    phi(i) = (C(i)+R(i)*expon);
                    % Computing F(phi)
                    F{i} = zeros(n,n);
                    for m = 0:len
                        F{i} = F{i} + phi(i)^m*coeffs{m+1};
                    end
                    
                    % Computing inv(F(phi))v
                    y{i} = F{i}\v;
                    S{i} = S{i} + (phi(i)^k)*y{i}*expon;
                end
            end
            if(L==1)
                s{k+1} = (R(1)/N)*S{1};
                for i=2:length(S)
                    s{k+1} = s{k+1} - (R(i)/N)*S{i};
                end
            else
                sum = (R(1)/N)*S{1};
                for i=2:length(S)
                    sum = sum - (R(i)/N)*S{i};
                end
                
                s{k+1} = [sum 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 && insideCircle(C,R,eig(H1,H0))== 1)
        %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

function[int] = intersectionCircles(C,R)
% this function tells if some circles intersect
% Input:       C  - list with centers of the circles
%              R  - list with radius of the circles
% Output:    int  - value 1 if there is intersection, 0 if dont

n = length(C); a = []; Sn = []; Rn = []; int = 0;
%plotCircles(C,R);
for i=1:n
    for j=1:n
        dist = abs(C(i)-C(j));
        if( (i~=j) && (double(dist)<=double( ( R(i)+R(j)) ) ) )
            int = 1;
            return;
        end
    end
end
end

function[in] = insideCircle(C,R,vals)
%this function decides if the values of a given list are inside of a circle of center C and radius R.
% Input:       C  - list with centers of the circles
%              R  - list with radius of the circles
%           vals  - list of values
% Output:    int  - value 1 if there is intersection, 0 if dont
n = length(vals); n1 = length(C); in = 1;

for j=1:n1
    for i=1:n
        if(j==1 && abs(vals(i)-C(j))>=R(j))
            in = 0;
        elseif(j>1 && abs(vals(i)-C(j))<R(j))
            in=0;
        end
    end
end
end