% **********************************************************
% This program computes an AGCD of two polynomials f and g, 
% defined by their coefficients; f is known to be exact, g 
% may be perturbed. 
% Warning: this is work in progress!
%
%
% Last update Feb 8, 2011.
% **********************************************************
%
% []=MMgcd(ff,g,r,pert_magnitude);
%
% ff = first cofactor 
% g = second cofactor  
% r = gcd  
% pert_magnitude = order of magnitude of the norm
%                  of the perturbation vector
%
% All polynomials are assumed to be monic.
%


function []=MMgcd(ff,g,r,pert_magnitude);

% *** Define cofactors and gcd ***
%ff=[1 2*rand(1,5)-1];
%g=[1 2*rand(1,4)-1];
%r=[1 2*rand(1,3)-1];

%ff=[1 1.8*rand(1,25)-0.9];
%g=[1 1.8*rand(1,25)-0.9];
%r=[1 1.8*rand(1,11)-0.9];

% *** Compute polynomials and add perturbation to g ***
f=conv(ff,r);
gg=conv(g,r);
pert=[0,2*rand(1,length(gg)-1)-1];
g=gg+pert_magnitude*pert;


%*****************************************
% setup
% (some notation: Mg is the multiplication matrix, 
% D is the rank-2 matrix Z1*Mg-Mg*Zm1)
%*****************************************
deg_threshold=1e-2;  % threshold used to estimate gcd degree;
% more refined methods will be employed later
f=f/f(1);
g=g/g(1);
n=length(f)-1;
m=length(g)-1;
F=compan(f);
F(:,n)=flipud(F(1,:).');
F(1,1:n-1)=zeros(1,n-1);
Fpow=F;

% *** this part is kept for computational purposes, it is not
% relevant for the algorithm ***
Fstore=zeros(n,n,m);  % here we store powers of F
Fstore(:,:,1)=F;
for j=1:m-1
    Fpow=F*Fpow;
    Fstore(:,:,j+1)=Fpow;
end
% let us compute Mg
Mg=g(m+1)*eye(n);
for j=1:m
    Mg=Mg+g(m-j+1)*Fstore(:,:,j);
end   
% *** end of superfluous part ***

% compute Bezoutians
B=bezoutian(f,g);
Bf1=bezoutian(f,1);

% we do not need the whole matrix Mg, but only 
% a few rows and columns, namely:
%
% first row of Mg
vec1=B(1,:)';
frMg=(Bf1\vec1)';
% last row of Mg
vec2=B(n,:)';
lrMg=(Bf1\vec2)';
% first column of Mg
vec3=[1;zeros(n-1,1)];
vec4=Bf1\vec3;
fcMg=B*vec4;
% second column of Mg
vec3=[0;1;zeros(n-2,1)];
vec4=Bf1\vec3;
scMg=B*vec4;
% last column of Mg
vec3=[zeros(n-1,1);1];
vec4=Bf1\vec3;
lcMg=B*vec4;

% compute the LU factorization of Mg; 
% later we will do this using structure;
% again, this is superfluous
[L,U,P]=lu(Mg);   % now P*Mg=L*U
%Udiag=abs(diag(U));

% compute Toeplitz-like generators for Mg
G=zeros(n,2);
H=zeros(2,n);
% G is made up of the first and last column 
% of Z1*Mg-Mg*Zm1
G=[[fcMg(n);fcMg(1:n-1)]-scMg,[lcMg(n);lcMg(1:n-1)]+fcMg];
auxrow=lrMg(2:n-1)-frMg(3:n);
H(1,:)=[1,auxrow./G(1,1),0];
H(2,n)=1;

% *** check that everything works properly ***
Z1=[zeros(1,n-1) 1; eye(n-1) zeros(n-1,1)];
Zm1=[zeros(1,n-1) -1; eye(n-1) zeros(n-1,1)];
D=Z1*Mg-Mg*Zm1;
if rank(D)==2 
    fprintf('displ. rank of mult. matrix: ok'); 
else
    fprintf('displ. rank of mult. matrix: not ok!');
end
norm(D-G*H);    
% *** end check ***

% compute diagonal of U using structure
[Udiag]=structured_diagonal(G,H);
j=n;
while Udiag(j)<deg_threshold
    j=j-1;
end
k=j;
d=n-j;

% now let us compute an initial guess for v;
% we do this by solving a system with the
% original matrix, because apparently it is
% not practical to exploit the computed LU 
% factorization (which is the pivoted LU of 
% the associated Cauchy-like matrix, not the 
% LU of the multiplication matrix!)
%
% we need to determine displacement generators
% for Mg(:,1:k), so we compute one more column:
%
% (k+1)-th column of Mg
vec3=[zeros(k,1);1;zeros(n-k-1,1)];
vec4=Bf1\vec3;
kkcMg=B*vec4;
% compute parameter for rectangular structure
theta=exp((k+1)*pi*i/lcm(n,k+1));
% compute Toeplitz-like generators for Mg(:,1:k+1)
G=zeros(n,2);
H=zeros(2,k+1);
% G is made up of the first and last column 
G=[[fcMg(n);fcMg(1:n-1)]-scMg,[kkcMg(n);kkcMg(1:n-1)]-theta*fcMg];
auxrow=lrMg(2:n-1)-frMg(3:n);
H(1,:)=[1,auxrow(1:k-1)./G(1,1),0];
H(2,k+1)=1;

% *** begin check ***
Z1=[zeros(1,n-1) 1; eye(n-1) zeros(n-1,1)];
Zm1=[zeros(1,k) theta; eye(k) zeros(k,1)];
D=Z1*Mg(:,1:k+1)-Mg(:,1:k+1)*Zm1;
if rank(D)==2 
    fprintf('displ. rank of reduced mult. matrix: ok'); 
else
    fprintf('displ. rank of reduced mult. matrix: not ok!');
end
norm(D-G*H)    
% *** end check ***

yy=compute_v(G,H,Mg(:,1:k+1));
yy=yy/yy(k+1);
y=real(yy(1:k));

%************************************************
% for the moment let us apply Newton's method;
% we will experiment with other minimization 
% strategies in the future 
conv_threshold=1e-9;
Fz=1; % residual
oldnFz=2;
nFz=1;
z0=[flipud(g.'); y];    % initial guess
z=z0;
v=[y;1;zeros(n-k-1,1)];
cb=1;  % counter for experiment in case one keeps g fixed for a while
fff=flipud(f(2:n+1)');

% *** begin Newton iteration ***
while (norm(Fz)>conv_threshold)
% compute displacement generators for the Jacobian
B=bezoutian(f,g);
% first row of Mg
vec1=B(1,:)';
frMg=(Bf1\vec1)';
% last row of Mg
vec2=B(n,:)';
lrMg=(Bf1\vec2)';
% first column of Mg
vec3=[1;zeros(n-1,1)];
vec4=Bf1\vec3;
fcMg=B*vec4;
% second column of Mg
vec3=[0;1;zeros(n-2,1)];
vec4=Bf1\vec3;
scMg=B*vec4;
% last column of Mg
vec3=[zeros(n-1,1);1];
vec4=Bf1\vec3;
lcMg=B*vec4;
% k-th column of Mg
vec3=[zeros(k-1,1);1;zeros(n-k,1)];
vec4=Bf1\vec3;
kcMg=B*vec4;

G=zeros(n,3);
H=zeros(3,m+k+1);
% we need to compute a few rows and columns of the Jacobian.
J=zeros(n,m+1);
J(:,1)=v;
w=v;
for j=2:m+1
    w=-fff*w(n)+[0;w(1:n-1)];
    J(:,j)=w;
end
N=n;
M=m+k+1;
MN=lcm(N,M);
theta=exp(M*pi*i/MN);
G(:,1)=[J(n,n-k);J(1:n-1,n-k)]-J(:,n-k+1);
G(:,2)=[J(n,m+1);J(1:n-1,m+1)]-fcMg;
G(:,3)=[kcMg(n);kcMg(1:n-1)]-theta*J(:,1);
firstrowJ=[J(1,:),frMg];
lastrowJ=[J(n,:),lrMg];
auxrow=lastrowJ(1:m+k)-firstrowJ(2:m+k+1);
H(1,:)=[zeros(1,n-k-1),1,auxrow(n-k+1:m)/G(1,1),0,auxrow(m+2:m+k)/G(1,1),0];
H(2,:)=[zeros(1,n-k-1),0,zeros(1,m-n+k),1,zeros(1,k-1),0];
H(3,:)=[zeros(1,n-k-1),0,zeros(1,m-n+k),0,zeros(1,k-1),1];
% *** verify that generators are correct ***
J=jacobian(g,v,Fstore,n,m,k);
Z1=[zeros(1,n-1) 1; eye(n-1) zeros(n-1,1)];
Zm1=[zeros(1,m+k) theta; eye(m+k) zeros(m+k,1)];
D=Z1*J-J*Zm1;
if rank(D)==3
    fprintf('displ. rank of Jacobian: ok'); 
else
    fprintf('displ. rank of Jacobian: not ok!');
end
norm(D-G*H)
% *** end check ***

%Mg=g(m+1)*eye(n);
oldnFz=nFz
%for j=1:m
%    Mg=Mg+g(m-j+1)*Fstore(:,:,j);
%end   % now Mg is the multiplication matrix (superfluous computation)
%Fz=Mg*v;        % functional applied to z
Fz=B*(Bf1\v);
%z=z-pinv(J)*Fz;
zzz=J\Fz;
zz=solve_jacobian(G,H,Fz,zzz);
z=z-zz;
v(1:k)=(z(m+2:m+k+1));
nFz=norm(Fz)
if (cb>0)
%if (cb>k)&&(mod(cb,2)==1)
%if nFz<0.1
fprintf('using g')
g=flipud(z(1:m+1));
end
cb=cb+1;
end
% *** end Newton iteration ***

norm(ff-fliplr(v(1:k+1)'))  % output difference between cofactors
norm(g-gg')   % output difference between exact and perturbed g

% *****************************************************************
% END of main function
% *****************************************************************

% *****************************************************************
% FUNCTION solve_jacobian
% *****************************************************************

function [z]=solve_jacobian(G1,G2,x,zzz)

% choose threshold used when checking whether
% the nullity of the Jacobian is too large

threshold=1e-9;
N=length(G1);
M=length(G2);
MN=lcm(N,M);
theta=exp(M*pi*i/MN);

% compute D0 (which is MxM), an auxiliary matrix
d0=ones(1,M);
a=0+(pi*i)/(MN);
alpha=exp(a);
for j=2:M
    d0(j)=d0(j-1)*alpha;
end
D0=diag(d0);

% compute D1 (which is NxN)
d1=ones(1,N);
beta=exp((2*pi*i)/N);
for j=2:N
    d1(j)=d1(j-1)*beta;
end
D1=d1;

% compute D2 (which is MxM and contains the
% shifted M-th roots of 1)
d2=ones(1,M);
beta=exp((2*pi*i)/M);
for j=2:M
    d2(j)=d2(j-1)*beta;
end
D2=exp(pi*i/MN)*d2;

% now D1 and D2 define a displacement operator for
% the Cauchy-like matrix C associated with the Jacobian

% compute displacement generators for this Cauchy-like matrix:
sN=sqrt(N);
sM=sqrt(M);
FD0=sM*ifft(D0);
G1_c=sN*ifft(G1);
G2_c=G2*FD0';

% now G1_c and G2_c are generators for C
% (i.e. D1*C - C*D2' = G1_c*G2_c)

% perform fast LU

L=zeros(N);
U=zeros(N,M);
P=eye(N);
P2=eye(M);


% for k=1:M
for k=1:N
    % the first generator is made orthogonal

    [Q,R]=qr(G1_c,0);
    G1_c=Q;
    G2_c=R*G2_c;

    % look for column of max magnitude in G2_c
    c=zeros(1,M-k+1);
    gg=abs(G2_c(:,k:M));
    gg=gg.*gg;
    c(k:M)=sum( gg,1);

    [val,col]=max(c);

    U(k,k:M)=(G1_c(k,:)*G2_c(:,k:M))./(D1(k)*ones(1,M-k+1)-D2(k:M));

    % swap s_k and s_col
    u=D2(k);
    D2(k)=D2(col);
    D2(col)=u;

    % swap psi_k and psi_col
    u=G2_c(:,k);
    G2_c(:,k)=G2_c(:,col);
    G2_c(:,col)=u;

    % swap columns in U
    u=U(:,k);
    U(:,k)=U(:,col);
    U(:,col)=u;

    % swap columns in P2
    u=P2(:,k);
    P2(:,k)=P2(:,col);
    P2(:,col)=u;
    L(k:N,k)=(G1_c(k:N,:)*G2_c(:,k))./((D1(k:N)-D2(k)*ones(1,N-k+1)).');
    [u,s]=max(abs(L(k:N,k)));
    s=s+k-1;
    U(k,k)=L(s,k);
    % swap t_k and t_q
    u=D1(k);
    D1(k)=D1(s);
    D1(s)=u;
    % swap phi_k and phi_q
    u=G1_c(k,:);
    G1_c(k,:)=G1_c(s,:);
    G1_c(s,:)=u;
    % swap rows in L
    u=L(k,:);
    L(k,:)=L(s,:);
    L(s,:)=u;
    % swap rows in P
    u=P(k,:);
    P(k,:)=P(s,:);
    P(s,:)=u;
    U(k,k+1:M)=(G1_c(k,:)*G2_c(:,k+1:M))./(D1(k)*ones(1,M-k)-D2(k+1:M));
    L(k,k)=1;
    L(k+1:N,k)=L(k+1:N,k)/U(k,k);
    G1_c(k+1:N,1)=G1_c(k+1:N,1)-G1_c(k,1)*L(k+1:N,k);
    G1_c(k+1:N,2)=G1_c(k+1:N,2)-G1_c(k,2)*L(k+1:N,k);
    G1_c(k+1:N,3)=G1_c(k+1:N,3)-G1_c(k,3)*L(k+1:N,k);
    G2_c(:,k+1:M)=G2_c(:,k+1:M)-G2_c(:,k)*(U(k,k+1:M)/U(k,k));

end
%L(M+1:N,M+1:N)=eye(N-M);
L(N+1:M,N+1:M)=eye(M-N);
%%y=L\(P*sN*ifft([x;abs(norm(p)^2+norm(q)^2+norm(g)^2-K)]));
%y=L\(P*sN*ifft([x;0]));
y=L(1:N,1:N)\(P*sN*ifft(x));

% detect nullity of Jacobian
%nullity=0;
%while abs(U(M-nullity,M-nullity))<threshold
%    nullity=nullity+1;
%end

%y=y(1:M-nullity);
%U=U(1:M-nullity,1:M-nullity);

y=U(1:N,1:N)\y;
y=[y;zeros(M-N,1)];

%y=[y;zeros(nullity,1)];
z=D0'*(1/sM)*fft(P2*y);

% compute a vector in Ker(S)
%nsp=[g; -p; -q];
%nsp=nsp/norm(nsp);
%z=z-dot(nsp,z)*nsp;   % projection of z on range(S)

% *****************************************************************
% FUNCTION structured_diagonal
% *****************************************************************

function [Ud,U,P]=structured_diagonal(G1,G2)

N=length(G1);

% compute the auxiliary matrix D0
d0=ones(1,N);
a=0+(pi*i)/N;
alpha=exp(a);
for j=2:N
    d0(j)=d0(j-1)*alpha;
end
D0=diag(d0);

% compute D1
d1=ones(1,N);
beta=alpha*alpha;
for j=2:N
    d1(j)=d1(j-1)*beta;
end
D1=d1;
% compute D2
D2=alpha*d1;
% now D1 and D2 are the diagonals of the
% matrices that define a displacement operator for
% the Cauchy-like matrix C associated with
% the multiplication matrix

% compute generators G1_c and G2_c  for C
sN=sqrt(N);
FD0=sN*ifft(D0);
G1_c=sN*ifft(G1);
G2_c=G2*FD0';

% perform fast LU
L=zeros(N);
U=zeros(N);
P=eye(N);
for k=1:N
    % the first generator is made orthogonal
    [Q,R]=qr(G1_c,0);
    G1_c=Q;
    G2_c=R*G2_c;
    % look for column of max magnitude in G2_c
    c=zeros(1,N-k+1);
    c(k:N)=conj(G2_c(1,k:N)).*G2_c(1,k:N)+conj(G2_c(2,k:N)).*G2_c(2,k:N);
    [val,col]=max(c);
    U(k,k:N)=(G1_c(k,:)*G2_c(:,k:N))./(D1(k)-D2(k:N));
    % swap s_k and s_col
    u=D2(k);
    D2(k)=D2(col);
    D2(col)=u;
    % swap psi_k and psi_col
    u=G2_c(:,k);
    G2_c(:,k)=G2_c(:,col);
    G2_c(:,col)=u;
    % swap columns in U
    u=U(:,k);
    U(:,k)=U(:,col);
    U(:,col)=u;
    % write permutation on P
    p=P(:,k);
    P(:,k)=P(:,col);
    P(:,col)=p;
    
    L(k:N,k)=(G1_c(k:N,:)*G2_c(:,k))./((D1(k:N)-D2(k)*ones(1,N-k+1)).');
    [u,s]=max(abs(L(k:N,k)));
    s=s+k-1;
    U(k,k)=L(s,k);
    % swap t_k and t_q
    u=D1(k);
    D1(k)=D1(s);
    D1(s)=u;
    % swap phi_k and phi_q
    u=G1_c(k,:);
    G1_c(k,:)=G1_c(s,:);
    G1_c(s,:)=u;
    % swap rows in L
    u=L(k,:);
    L(k,:)=L(s,:);
    L(s,:)=u;
    U(k,k+1:N)=(G1_c(k,:)*G2_c(:,k+1:N))./(D1(k)*ones(1,N-k)-D2(k+1:N));
    L(k,k)=1;
    L(k+1:N,k)=L(k+1:N,k)./U(k,k);
    G2_c(:,k+1:N)=G2_c(:,k+1:N)-G2_c(:,k)*(U(k,k+1:N)./U(k,k));
    G1_c(k+1:N,1)=G1_c(k+1:N,1)-G1_c(k,1)*L(k+1:N,k);
    G1_c(k+1:N,2)=G1_c(k+1:N,2)-G1_c(k,2)*L(k+1:N,k);
end
Ud=abs(diag(U));

% *****************************************************************
% FUNCTION bezoutian
% *****************************************************************
% Computes the Bezoutian of two polynomials f and g
% (deg f >= deg g and f(1) is the leading coefficient
% as per Matlab convention, same for g).
function B=bezoutian(f,g)

[nf,mf]=size(f);
if (nf<mf) 
    f=f.'; 
end
n=length(f)-1;   
f=flipud(f);
[ng,mg]=size(g);
if (ng<mg) 
    g=g.'; 
end
m=length(g)-1;
g=flipud(g);
% swap polynomials if deg f < deg g
flag=(m>n);
flag=1-2*flag; % flag=1 if degrees are ok, flag=-1 if swap is needed 
if flag==-1
    z=f;
    f=g;
    g=z;
    s=n;
    n=m;
    m=s;
end

g=[g; zeros(n-m,1)];

M1=toeplitz(g(1:n),[g(1),zeros(1,n-1)]);
M2=hankel(f(2:n+1),[f(n+1),zeros(1,n-1)]);
M3=toeplitz(f(1:n),[f(1),zeros(1,n-1)]);
M4=hankel(g(2:n+1),[g(n+1),zeros(1,n-1)]);
B=flag*(M1*M2-M3*M4);

% *****************************************************************
% FUNCTION compute_v
% *****************************************************************
%
% computes a vector of desired length in the
% approximate kernel of the multiplication matrix
function x=compute_v(G1,G2,A)

threshold=1e-9;  % tolerance for rank determination
N=length(G1);
M=length(G2);
MN=lcm(N,M);
theta=exp(M*pi*i/MN);

% compute D0 (which is MxM), an auxiliary matrix
d0=ones(1,M);
a=0+(pi*i)/(MN);
alpha=exp(a);
for j=2:M
    d0(j)=d0(j-1)*alpha;
end
D0=diag(d0);

% compute D1 (which is NxN)
d1=ones(1,N);
beta=exp((2*pi*i)/N);
for j=2:N
    d1(j)=d1(j-1)*beta;
end
D1=d1;

% compute D2 (which is MxM and contains the shifted M-th
% roots of 1)
d2=ones(1,M);
beta=exp((2*pi*i)/M);
for j=2:M
    d2(j)=d2(j-1)*beta;
end
D2=exp(pi*i/MN)*d2;

% now D1 and D2 define a displacement operator for the
% Cauchy-like matrix C associated with the cofator matrix

% compute displacement generators G1_c and G2_c for C
sN=sqrt(N);
FD0=sN*ifft(D0);
G1_c=sN*ifft(G1);
G2_c=G2*FD0';

% perform fast LU
L=zeros(N);
U=zeros(N,M);

P2=eye(M);

for k=1:M
    % the first generator is made orthogonal
    [Q,R]=qr(G1_c,0);
    G1_c=Q;
    G2_c=R*G2_c;

    % look for column of max magnitude in G2_c
    c=zeros(1,M-k+1);
    c(k:M)=conj(G2_c(1,k:M)).*G2_c(1,k:M)+conj(G2_c(2,k:M)).*G2_c(2,k:M);

    [val,col]=max(c);

    U(k,k:M)=(G1_c(k,:)*G2_c(:,k:M))./((D1(k)*ones(1,M-k+1)-D2(k:M)));


    % swap s_k and s_col
    u=D2(k);
    D2(k)=D2(col);
    D2(col)=u;

    % swap psi_k and psi_col
    u=G2_c(:,k);
    G2_c(:,k)=G2_c(:,col);
    G2_c(:,col)=u;

    % swap columns in U
    u=U(:,k);
    U(:,k)=U(:,col);
    U(:,col)=u;

    % swap columns in P2
    u=P2(:,k);
    P2(:,k)=P2(:,col);
    P2(:,col)=u;
    L(k:N,k)=(G1_c(k:N,:)*G2_c(:,k))./((D1(k:N)-D2(k)*ones(1,N-k+1)).');


    [u,s]=max(abs(L(k:N,k)));
    s=s+k-1;
    U(k,k)=L(s,k);
    % swap t_k and t_q
    u=D1(k);
    D1(k)=D1(s);
    D1(s)=u;
    % swap phi_k and phi_q
    u=G1_c(k,:);
    G1_c(k,:)=G1_c(s,:);
    G1_c(s,:)=u;
    % swap rows in L
    u=L(k,:);
    L(k,:)=L(s,:);
    L(s,:)=u;


    U(k,k+1:M)=(G1_c(k,:)*G2_c(:,k+1:M))./(D1(k)*ones(1,M-k)-D2(k+1:M));


    L(k,k)=1;
    L(k+1:N,k)=L(k+1:N,k)/U(k,k);
    G1_c(k+1:N,1)=G1_c(k+1:N,1)-G1_c(k,1)*L(k+1:N,k);
    G1_c(k+1:N,2)=G1_c(k+1:N,2)-G1_c(k,2)*L(k+1:N,k);

    G2_c(:,k+1:M)=G2_c(:,k+1:M)-G2_c(:,k)*(U(k,k+1:M)/U(k,k));

end

% compute a vector in the null space of the matrix
emme=M;
nullity=0;
while abs(U(emme-nullity,emme-nullity))<threshold
    nullity=nullity+1;
end
if nullity==0
    nullity=1;
end
nullity=nullity-1; % this is not the nullity now
r=-U(1:emme-1-nullity,emme-nullity);
U=U(1:emme-1-nullity,1:emme-1-nullity);
x=U\r;
x=[x;1;zeros(nullity,1)];
x=D0'*fft(P2*x); 


% *****************************************************************
% FUNCTION jacobian
% *****************************************************************
%*******************************************
% function that computes the Jacobian matrix
% (for use in quad_functional.m)
%*******************************************
function J=jacobian(g,v,Fstore,n,m,k)
J=zeros(n,m+k+1);
% derivatives wrt g
J(:,1)=v;
for j=1:m
    J(:,j+1)=Fstore(:,:,j)*v;
end
% derivatives wrt v
I=g(m+1)*eye(n);
for j=1:k    
auxv=I(:,j);
for i=1:m
    auxv=auxv+g(m-i+1)*Fstore(:,j,i);
end
J(:,m+j+1)=auxv;
end
Z1=[zeros(1,n-1) 1; eye(n-1) zeros(n-1,1)];
Zm1=[zeros(1,m+k) -1; eye(m+k) zeros(m+k,1)];
D=Z1*J-J*Zm1;
rank(D)