function[X,S,resvec] = NewtonMethodLineSearchInvPairs(poly, X, S, m, tol, maxit)
% Input:  poly  - cell array containing coefficients of matrix polynomial
%         (X,S) - approximation to invariant pair
%         m     - number of blocks in normalization matrix W; by default
%                 m = 1.
%         tol   - absolute tolerance to which residual should be reduced;
%                 by default tol = 10^(-14)*max(norm(poly{j}))
%         maxit - maximal number of iterations;
%                 by default maxit = 100
% Output: (X,S) - approximation to invariant pair
%         res   - residual norms in the course of the iteration

%define center of the circle, radius of circle and number of Nodes to
%approximate trapezoid rule
C = 1; R = 1.7; N = 20; 

l = length(poly)-1; [n,k] = size(X); eps0 = 10^(-1);

if nargin < 4
    m = 1;
end
m = min(m,l);
if nargin < 5
    normA = 0;
    for j = 0:l, normA = max( normA, norm( poly{j+1}, 'fro' ) );  end
    tol = 10^(-14)*normA;
end
tol = max( tol, 0 );
if nargin < 6,
    maxit = 100;
end

pS = zeros(k,k,l); resvec = [];
for it = 1:maxit  
    % Make sure invariant pair adheres to orthogonality condition
    [X,S,W] = orthpair(X,S,m);
    
    % Normalize S to be in Schur form
    [Q,S] = schur(S,'complex');
    X = X*Q; W = W*Q;
    
    % Precompute all required powers of S.
    pS(:,:,1) = eye(k);
    for j = 2:l+1, pS(:,:,j) = pS(:,:,j-1)*S; end
    
    % Compute residual
    res = zeros(n,k);
    for j = 0:l
        res = res + poly{j+1}*X*pS(:,:,j+1);
    end
    normres = norm(res,'fro')/norm(X,'fro');
    resvec = [resvec;normres];
    
    if normres / k < tol,
        break
    end
    
    % Solve system of linear matrix equations by forward substitution.
    % Initialize residuals and correction terms
    Ort = zeros(k); dX = zeros(n,k); dS = zeros(k);
    
    % Main loop for computing the ith columns of dX and dS
    for i = 1:k
        % Set up and solve linear sySem
        s = S(i,i);
        
        [T11,T12] = forwardSubtitutionBlocks(poly,X,S,i,C,R,N);
        T21 = zeros(k,n); T22 = zeros(k);
        DS  = eye(k);
        for j = 1:l
            if j < m
                T22 = T22 + W(j*n+1:(j+1)*n,:)'*X*DS;
            end
            DS = s*DS + pS(:,:,j+1);
        end
        
        for j = 0:m-1, T21 = T21 + s^j * W(j*n+1:(j+1)*n,:)'; end
        
        sol = [T11 T12; T21 T22] \ [res(:,i);Ort(:,i)];
        dX(:,i) = sol(1:n); dS(:,i) = sol(n+1:end);
        
        % Update right-hand side
        res(:,i+1:k) = res(:,i+1:k) - forwardSubtitutionUpdateResidual(poly,X,S,i,dS(:,i),dX(:,i),C,R,N);
        DS = zeros(k); DS(:,i) = dS(:,i);
        for j = 1:l
            if j < m
                Ort(:,i+1:k) = Ort(:,i+1:k) - W(j*n+1:(j+1)*n,:)' * ( dX(:,i)*pS(i,i+1:k,j+1) + X*DS(:,i+1:k) );
            end
            DS = DS*S + pS(:,:,j)*DS;
        end
    end
    
    RE = residual(poly,X-dX,S-dS,C,R,N);
    if(norm(RE,'fro')<=eps0)
        X = X - dX; S = S - dS;
    else
        AU = residual(poly,X,S,C,R,N);
        AU1 = linearSearchIntegral(poly,X,S,dX,dS,C,R,N);
        AU2 = linearSearchIntegral2(poly,X,S,dX,dS,C,R,N);
        
        alpha = norm(AU,'fro')^2;
        beta = trace(AU'*AU1 + AU1'*AU);
        gamma = trace(AU'*AU2 + AU2'*AU);
        theta = norm(AU1,'fro')^2;
        eta = trace(AU1'*AU2 + AU2'*AU1);
        psi = norm(AU2,'fro')^2;
        
        f = @(t)psi*t.^6 + eta*t.^5 + (theta-gamma)*t.^4 + (gamma-beta)*t.^3 + (alpha + beta)*t.^2 - 2*alpha*t + alpha;
        t = fminbnd(f, 0, 2);
        t = real(t);
        X = X - t*dX; S = S - t*dS;
    end
end

function [X,S,Q] = orthpair(X,S,m)
V = constructV(X,S,m);
[Q,R] = qr(V,0);
if m > 1,
    X = X / R;
else
    X = Q;
end
S = R*(S/R);

function V = constructV(X,S,m)
[n,k] = size(X);
V = zeros(n*m,k); pS = eye(k);
for i = 1:m,
    V((i-1)*n+1:i*n,:) = X*pS;
    pS = pS*S;
end

function[K11,K12] = forwardSubtitutionBlocks(poly,X,S,i,C,R,N)
% This function computes blocks K11 and K12 for the forward substitution
% approximating the contour integral by the trapezoid sum and using a
% circle as contour.

[n,k] = size(X); len = length(poly)-1;
K11 = zeros(n); K12 = zeros(n,k);

for j=0:N-1
    expon = exp(1i*j*2*pi/N); phi = (C+R*expon);
    % Computing T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end
    K11 = K11 + expon*T*(1/(phi-S(i,i)));
    K12 = K12 + expon*T*X/(phi*eye(k,k)-S)*(1/(phi-S(i,i)));
end
K11 = (R/N)*K11;
K12 = (R/N)*K12;

function[RES] = forwardSubtitutionUpdateResidual(poly,X,S,i,dS,dX,C,R,N)
% This function computes the matrix update of the residual for the
% forward substitution approximating the contour integral by the
% trapezoid sum and using a circle as contour.

[n,k] = size(X); len = length(poly)-1;
RES = zeros(n,k-i); S12 = S(i,i+1:k); S22 = S(i+1:end,i+1:end);

for j=0:N-1
    expon = exp(1i*j*2*pi/N); phi = (C+R*expon);
    % Computing T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end
    RES = RES + expon*T*dX*(1/(phi-S(i,i)))*S12/(phi*eye(k-i)-S22) + expon*T*X/(phi*eye(k)-S)*dS*(1/(phi-S(i,i)))*S12/(phi*eye(k-i)-S22);
end
RES = (R/N)*RES;

function[Res] = residual(poly,X,S,C,R,N)
% This function computes the residual for the matrix polynomial problem,
% using the contour integral (approximated using the trapezoid sum and a circle as parametrization)

[n,k] = size(X); len = length(poly)-1; 
Res = zeros(n,k); 
for j=0:N-1
    phi = (C+R*exp(1i*j*2*pi/N));
    % Compute T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end  
    
    Res = Res + T*X/(phi*eye(k,k)-S)*exp(1i*j*2*pi/N);
end
Res = (R/N)*Res;

function[integ] = linearSearchIntegral(poly,X,S,dX,dS,C,R,N)
%this function computes the first integral in the linear search method 
%(the one that is multiplied by t^2), using contour integral

[n,k] = size(X); len = length(poly)-1; 
integ = zeros(n,k); 
for j=0:N-1
    phi = (C+R*exp(1i*j*2*pi/N));
    % Compute T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end  
    
    integ = integ + T*( dX+X/(phi*eye(k,k)-S)*dS )/(phi*eye(k,k)-S)*dS/(phi*eye(k,k)-S)*exp(1i*j*2*pi/N);
end
integ = (R/N)*integ; 

function[integ2] = linearSearchIntegral2(poly,X,S,dX,dS,C,R,N)
%this function computes the second integral in the linear search method 
%(the one that is multiplied by t^3), using contour integral

[n,k] = size(X); len = length(poly)-1; 
integ2 = zeros(n,k); 
for j=0:N-1
    phi = (C+R*exp(1i*j*2*pi/N));
    % Compute T(phi)
    T = zeros(n,n);
    for m = 0:len
        T = T + phi^m*poly{m+1};
    end  
    
    integ2 = integ2 + T*dX/(phi*eye(k,k)-S)*dS/(phi*eye(k,k)-S)*dS/(phi*eye(k,k)-S)*exp(1i*j*2*pi/N);
end
integ2 = (R/N)*integ2;