%**************************************************************
%%%%%% This code is designed to solve   %%%%%%%%%%%%%%%%%%                        
%%%%%% min 0.5*||M-Ma||^2 + 0.5*||C-Ca||^2 + 0.5*||K-Ka||^2
%%%%%% s.t 1/sqrt(c1)*M*[R;zeros(n-k,k)]*Lambda^2 + 1/sqrt(c2)*C*[R;zeros(n-k,k)]*Lambda + K*[R;zeros(n-k,k)] = 0
%%%%%%      M>=0 (PSD), C=C^T, K>=0 (PSD) (PSD: Symmetric and positive semi-definite)
%%%%%%  
%%%%%% based on the algorithm in %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% "A Dual Optimization Approach to Inverse Quadratic Eigenvalue %%%%%
%%%%%% Problems with Partial Eigenstructure"  %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% By Zheng-Jian Bai, Delin Chu, and Defeng Sun    %%%%%%%%%%%%%%%%%%%
%%%%%% SIAM J. Sci. Comput. 29 (2007), to appear.
%%%%%%
%%%%%% Last Modified Date: March 18, 2008 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% The  input argument is the given symmetric Ma, Ca,and Ka,  %%%%%%%%
%%%%%% the eigendata (Lambda,R), where R is the R-factor of QR
%%%%%% factoriztion of the matrix X of k given eigenvectors, the
%%%%%% problem dimension n and the number k of given eigenpairs, and the
%%%%%% positive weighting parameters c1 and c2
%%%%%% The output are the optimal primal and dual solutions %%%%%%%%
%%%%%%%
%%%%%%% Send your comments and suggestions to    %%%%%%
%%%%%%% zjbai@xmu.edu.cn, matchudl@nus.edu.sg  or matsundf@nus.edu.sg %%%%
%%%%%                         
%%%%% Warning: Accuracy may not be guaranteed!!!!! %%%%%%%%
function [M,C,K,y,z]=IQEP_Newton(Ma,Ca,Ka,R,Lambda,n,k,c1,c2)
disp('%%%%%% Newton Methods Starts %%%%%%')
t0=cputime;

m=0;
f_eval =0;
Iter_Whole=200;
Iter_inner =20; % Maximum number of Line Search in Newton method
maxit0 =max(2000,n*k); % Maximum number of iterations in PCG at the 1st step
maxit =max(5000,n*k); % Maximum number of iterations in PCG
Inner =0;
tol =1.0e-6; % Tolerance for PCG
error_tol=1.0e-7; % termination tolerance (relative error)
sigma_1=1.0e-4; %tolerance in the line search of the Newton method

Lambda2=Lambda*Lambda;
S=R*Lambda/R; S2=S*S;

%%%%%% Preconditioners
RTR=R'*R; 
Q1=c1^(-1)*Lambda2'*RTR*Lambda2+c2^(-1)*Lambda'*RTR*Lambda+RTR;
Q1=Q1/2; % Preconditioner
Q3=RTR; % Preconditioner

Q10=RTR; % Preconditioner for the 0th setep. 
Ik=eye(k);
Q2=c1^(-1)*S2'*S2 + c2^(-1)*S'*S + Ik;
Q2 =Q2/2;
   
Res_b=zeros(300,1);

%%%%%% The denominator used in the stopping criterion %%%%%%%%%%%%
mck0 = max(sqrt((norm(Ma,'fro')/sqrt(c1))^2 + (norm(Ca,'fro')/sqrt(c2))^2+(norm(Ka,'fro'))^2),1); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

b01 =-zeros(k,k); 
b02=zeros(k,n-k); 
y=zeros(k,k);
z=zeros(k,n-k);
F1=zeros(k,k);
F2=zeros(k,n-k);

%%%%%% Initial guess (It should find optimal solutions with equations only)
y02=b01-c1^(-0.5)*Lambda2'*(R'*Ma(1:k,1:k)*R)-c2^(-0.5)*Lambda'*(R'*Ca(1:k,1:k)*R)-(R'*Ka(1:k,1:k)*R);
[y,flag,relres,iterk] = pre_cg0(y02,error_tol,maxit0,Q1,Q10,Lambda2,Lambda,RTR,k,c1,c2); % initial y
fprintf('---------Number of CG Iterations for the initial guess === %d \n', iterk)  
z02=b02-c1^(-0.5)*S2'*Ma(1:k,k+1:n)-c2^(-0.5)*S'*Ca(1:k,k+1:n)-Ka(1:k,k+1:n);
z=Q2\z02; % initial z

%%%%%% Re-assign strarting points when CG fails %% 
if flag~=0
    y=y-y;   %zeros 
    z=z-z;   %zeros 
end      
[A1,A2,A3]=Adjoint(y,z,Lambda2,Lambda,R,S2,S,c1,c2,n,k); % ajoint of A
M=Ma+A1;
M=(M+M')/2; % New M
C=Ca+A2;
C=(C+C')/2; % New C
K=Ka+A3;
K=(K+K')/2; % New K
[lam1,P1,lam3,P3]=ED(M,K);% Eigendecomposition of M,K

[f0,F1,F2]=gradient(M,C,K,lam1,P1,lam3,P3,n,k,S2,S,Lambda2,Lambda,R,c1,c2); 
const_matrix =0.5*(trace(Ma'*Ma)+trace(Ca'*Ca)+trace(Ka'*Ka));
f0=f0-trace(b01'*y)- trace(b02'*z) -const_matrix; %the objective function value
fprintf('Newton: function value computed here::::::::::::::::::::::::::: %12.8f \n',-f0)
f_eval =f_eval + 1;
b1=b01-F1;
b2=b02-F2;
norm_b=sqrt(norm(b1,'fro')^2 + norm(b2,'fro')^2);
res_initial=norm_b;
rel_b=norm_b/mck0;
fprintf('---------Norm of the Gradient:::::: %d \n',norm_b)
fprintf('---------Relative error of  the Gradient:::::: %d \n',rel_b)
Omega1 = omega_matrix(lam1,n);
Omega3 = omega_matrix(lam3,n);

x01=y;
x02=z;
d1=zeros(k,k);
d2=zeros(k,n-k); 
while (rel_b>error_tol & m<= Iter_Whole)
    [d1,d2,flag,relres,iterk]=pre_cg(b1,b2,tol,maxit,Q1,Q3,Q2,Omega1,P1,Omega3,P3,n,k,S2,S,norm_b,Lambda2,Lambda,R,c1,c2);  
    fprintf('---------Number of CG Iterations %d \n', iterk)
    if (flag~=0) % flag =0 means CG successful, otherwise not successful
        d1 = -(F1-b01);
        d2 = -(F2-b02);
    end % end loop for if
    slope =trace((F1-b01)'*d1)+trace((F2-b02)'*d2); %%% nabla f d
    y =x01+d1; %temporary x0+d 
    z =x02+d2;    
    [A1,A2,A3]=Adjoint(y,z,Lambda2,Lambda,R,S2,S,c1,c2,n,k); % Ajoint of A
    M=Ma+A1;
    M=(M+M')/2; % New M
    C=Ca+A2;
    C=(C+C')/2; % New C
    K=Ka+A3;
    K=(K+K')/2; % New K
    [lam1,P1,lam3,P3]=ED(M,K); % Eigendecomposition of M,K
    [f,F1,F2] =gradient(M,C,K,lam1,P1,lam3,P3,n,k,S2,S,Lambda2,Lambda,R,c1,c2);
    f=f-trace(b01'*y)- trace(b02'*z) -const_matrix; %objective function value
    k_inner=0;
    while(k_inner <=Iter_inner & f>f0+sigma_1*0.5^k_inner*slope)
        k_inner=k_inner+1;
        y = x01+0.5^k_inner*d1; % backtracking line search 
        z = x02+0.5^k_inner*d2;
        [A1,A2,A3]=Adjoint(y,z,Lambda2,Lambda,R,S2,S,c1,c2,n,k); % Ajoint of A
        M=Ma+A1;
        M=(M+M')/2; % New M
        C=Ca+A2;
        C=(C+C')/2; % New C
        K=Ka+A3;
        K=(K+K')/2; % New K
        [lam1,P1,lam3,P3]=ED(M,K); % Eigen-Decomposition of M,K
        [f,F1,F2] =gradient(M,C,K,lam1,P1,lam3,P3,n,k,S2,S,Lambda2,Lambda,R,c1,c2);
        f=f-trace(b01'*y)- trace(b02'*z) -const_matrix;  %objective function value
    end % end loop for while
   % fprintf('---------Number of Inner Iterations at the current step::::: %d \n', k_inner)
    f0=f;
    fprintf('Newton: function value computed here::::::::::::::::::::::::::: %12.8f \n',-f)
    f_eval =f_eval+k_inner+1;
    x01=y; 
    x02=z;
    m=m+1;
    b1=b01-F1;
    b2=b02-F2;
    norm_b=sqrt(norm(b1,'fro')^2+norm(b2,'fro')^2);
    rel_b=norm_b/mck0;
    fprintf('---------Norm of Gradient:::::: %d \n',norm_b)
    fprintf('---------Relative error of  the Gradient:::::: %d \n',rel_b)
    Res_b(m)=norm_b;
    Omega1 = omega_matrix(lam1,n);
    Omega3 = omega_matrix(lam3,n);
end %end loop for while

fprintf('Newton: Number of Iterations %d \n', m)
fprintf('Newton: Number of Function Evaluations %d \n', f_eval)
%fprintf('---------Norm of Initial Gradient:::::: %d \n',res_initial)

M =P1*diag(max(0,lam1))*P1';
C=C;
K=P3*diag(max(0,lam3))*P3';

time_used=cputime-t0;

fprintf('Newton: cputime used ============================================= %d \n', time_used)

%%% end of the main program

%%%%%%
%%%%%% To generate F(Y,Z) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%

function [f,F1,F2]= gradient(M,C,K,lam1,P1,lam3,P3,n,k,S2,S,Lambda2,Lambda,R,c1,c2)
f=0.0;
F1 =zeros(k,k);
F2=zeros(k,n-k);
% use  eigendecomposition of M
H1=P1';
i=1;
while (i<=n)
    H1(i,:)=max(lam1(i),0)*H1(i,:);
    i=i+1;
end % end loop for while
H1=P1(1:k,:)*H1;

H2=C(1:k,:);

% use eigendecomposition  of K   
H3=P3';
i=1;
while (i<=n)
    H3(i,:)=max(lam3(i),0)*H3(i,:);
    i=i+1;
end % end loop for while
H3=P3(1:k,:)*H3;

F1=c1^(-1/2)*Lambda2'*(R'*H1(:,1:k)*R)+c2^(-1/2)*Lambda'*(R'*H2(:,1:k)*R)+R'*H3(:,1:k)*R;% F1(Y,Z)
F2=c1^(-1/2)*S2'*H1(:,k+1:n)+c2^(-1/2)*S'*H2(:,k+1:n)+H3(:,k+1:n);% F2(Y,Z)

f=(norm(max(lam1,zeros(n,1))))^2;
f=f+(norm(C,'fro'))^2;
f=f+(norm(max(lam3,zeros(n,1))))^2;
f =0.5*f;  % f is the first part of the objective function

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of gradient.m %%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%   Form the adjoint of A(Y,Z): A^*(Y) %%%%%%%%%
%%%%%%

function [A1,A2,A3]=Adjoint(Y,Z,Lambda_square,Lambda,R,S2,S,c1,c2,n,k)

H1=R*(Lambda_square*Y)*R'; H1=H1+H1';
G1=S2*Z;
A1=0.5*c1^(-0.5)*[H1 G1;G1' zeros(n-k)]; % A1^*(Y,Z)

H2=R*(Lambda*Y)*R';H2=H2+H2';
G2=S*Z;
A2=0.5*c2^(-0.5)*[H2 G2;G2' zeros(n-k)]; % A2^*(Y,Z)

H3=R*Y*R';H3=H3+H3';
A3=0.5*[H3 Z;Z' zeros(n-k)]; % A3^*(Y,Z)

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of Adjoint.m %%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%   Eigenvalue Decomposition (ED) of M and K   %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [lam1,P1,lam3,P3]=ED(M,K)
[P1,D1]=eig(M); % ED of M
lam1=diag(D1);
[P3,D3]=eig(K); % ED of K
lam3=diag(D3);

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of ED.m



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the first -order difference of lambda
%%%%%%%

function omega = omega_matrix(lambda,n)
omega =ones(n,n);
i=1;
while (i<=n)
    j=1;
    while (j<=n)
        if abs(lambda(i)-lambda(j))>1.0e-10
            omega(i,j) = (max(0,lambda(i))-max(0,lambda(j)))/(lambda(i)-lambda(j));
            elseif max(lambda(i),lambda(j))<=1.0e-15
                omega(i,j)=0;
        end % end loop for if
        j=j+1;
    end % end loop for while
    i=i+1;
end % end loop for while

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of omega_matrix.m

%%%%%% PCG method  for the unconstrained case %%%%%%%
%%%%%% This is exactly the algorithm by  Hestenes and Stiefel (1952)
%%%%%% An iterative method to solve A(x) =b  
%%%%%% The symmetric positive definite matrix M is a preconditioner for A. 
%%%%%% See Pages 527 and 534 of Golub and van Loan (1996)

function [p1,flag,relres,iterk] = pre_cg0(b1,tol,maxit,M1,M2,Lambda2,Lambda,RTR,k,c1,c2);
% Initializations
r1 = b1;  % we take the initial guess x0=0 to save time in calculating A(x0) 
n2b1 =norm(b1,'fro');    % norm of b1
n2b=n2b1; % norm of b
tolb = tol;  % absolute tolerance 
p1 = zeros(k,k);
flag=1;
iterk =0;
relres=1000; %%% To give a big value on relres
% Precondition 
z1 = (M1\r1)/M2; %if M is not the identity matrix 
rz1 = trace(r1'*z1);
rz2 = 1; 
d1 = z1;
% CG iteration
m=1;
for m =1:maxit
   if m > 1
       beta = rz1/rz2;
       d1 = z1 + beta*d1;
   end
   w1=Jacobian_matrix0(d1,k,Lambda2,Lambda,RTR,c1,c2);%w = A(d); 
   denom =  trace(d1'*w1);
   iterk =m;
   norm_z1=norm(z1,'fro');
   norm_z=norm_z1;
   
   norm_inf=max(max(abs(r1)));
   relres =norm_inf;              %relative residue =norm(z) / norm(b)
   if denom <= 0 
       norm_d1=norm(d1,'fro');
       norm_d=norm_d1;
       p1 = d1/norm_d; % d is not a descent direction         
       break % exit
   else
       alpha = rz1/denom;
       p1 = p1 + alpha*d1;
       r1 = r1 - alpha*w1;
   end
   z1= (M1\r1)/M2; %  z = M*r; if M is not the identity matrix ;  
   norm_inf=max(max(abs(r1)));
   if norm_inf <= tolb % Exit if Hp=b solved within the relative tolerance
       iterk =m;
       relres =norm_inf;          %relative residue =norm(z) / norm(b)
       flag =0;
       break
   end
   rz2 = rz1;
   rz1 = trace(r1'*z1);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of pre_cg0.m %%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the Jacobain product with x: F'(y,z)(x) 
%%%%%% for the unconstrained case %%%%%%%
%%%%%%%

function Ax1= Jacobian_matrix0(x1,k,Lambda2,Lambda,RTR,c1,c2)
Ax1 =zeros(k,k);
Ax1=c1^(-1)*Lambda2'*RTR*((Lambda2*x1)+(Lambda2*x1)')*RTR;
Ax1=Ax1+ c2^(-1)*Lambda'*RTR*((Lambda*x1)+(Lambda*x1)')*RTR;
Ax1=Ax1+ RTR*((x1)+(x1)')*RTR;
Ax1=0.5*Ax1;

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of Jacobian_matrix0.m %%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% PCG method %%%%%%%
%%%%%% This is exactly the algorithm by  Hestenes and Stiefel (1952)
%%%%%% An iterative method to solve A(x) =b  
%%%%%% The symmetric positive definite matrix M is a preconditioner for A. 
%%%%%%  See Pages 527 and 534 of Golub and va Loan (1996)

function [p1,p2,flag,relres,iterk]=pre_cg(b1,b2,tol,maxit,M1,M3,M2,Omega1,P1,Omega3,P3,n,k,S2,S,norm_b,Lambda2,Lambda,R,c1,c2);
% Initializations
r1 = b1;  %We take the initial guess x0=0 to save time in calculating A(x0) 
r2=b2;
n2b1 =norm(b1,'fro');    % norm of b1
n2b2 =norm(b2,'fro');    % norm of b2
n2b=sqrt(n2b1^2+n2b2^2); % norm of b 
tolb = tol *n2b ;
p1 = zeros(k,k);
p2=zeros(k,n-k);
flag=1;
iterk =0;
relres=1000; %%% To give a big value on relres
% Precondition 
z1 =(M1\r1)/M3;  %%%%% z = M\r; if M is not the identity matrix 
z2=M2\r2;
rz1 = trace(r1'*z1)+trace(r2'*z2); 
rz2 = 1; 
d1 = z1;
d2=z2;
% CG iteration
m=1;
for m =1:maxit
   if m > 1
       beta = rz1/rz2;
       d1 = z1 + beta*d1;
       d2 = z2 + beta*d2;
   end
   [w1,w2]= Jacobian_matrix(d1,d2,Omega1,P1,Omega3,P3,n,k,S2,S,norm_b,Lambda2,Lambda,R,c1,c2); %w = A(d); 
   denom =  trace(d1'*w1)+trace(d2'*w2);
   iterk =m;
   norm_z1=norm(z1,'fro');
   norm_z2=norm(z2,'fro');
   norm_z=sqrt(norm_z1^2+norm_z2^2);
   relres =norm_z/n2b;              %relative residue =norm(z) / norm(b)
   if denom <= 0 
       norm_d1=norm(d1,'fro');
       norm_d2=norm(d2,'fro');
       norm_d=sqrt(norm_d1^2+norm_d2^2);
       p1 = d1/norm_d; % d is not a descent direction
       p2=d2/norm_d;       
       break % exit
   else
       alpha = rz1/denom;
       p1 = p1 + alpha*d1;
       p2 = p2 + alpha*d2;
       r1 = r1 - alpha*w1;
       r2 = r2 - alpha*w2;
   end
  z1 =(M1\r1)/M3; %  z = M\r; if M is not the identity matrix ;
  z2=M2\r2;
   if norm_z <= tolb  % Exit if Hp=b solved within the relative tolerance
       slope =trace(-b1'*p1)+trace(-b2'*p2); %%% nabla f d
       if slope <0
          iterk =m;
          relres =norm_z/n2b;          %relative residue =norm(z) / norm(b)
          flag =0;
          break
       end
   end
   rz2 = rz1;
   rz1 = trace(r1'*z1)+trace(r2'*z2);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of pre_cg.m %%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the Jacobain product with x: F'(y,z)(x) %%%%%%%
%%%%%%%

function [Ax1,Ax2]= Jacobian_matrix(x1,x2,Omega1,P1,Omega3,P3,n,k,S2,S,norm_b,Lambda2,Lambda,R,c1,c2)
Ax1 =zeros(k,k);
Ax2 =zeros(k,n-k);
%---------------------------
[A1,A2,A3]=Adjoint(x1,x2,Lambda2,Lambda,R,S2,S,c1,c2,n,k); % Ajoint of A: A^*(Y)
% use ED of M
A12=P1(1:k,:)'*(A1(1:k,k+1:n)*P1(k+1:n,:));
A12=A12+A12';
A1=P1(1:k,:)'*A1(1:k,1:k)*P1(1:k,:)+A12;
i=1;
while (i<=n)
    A1(i,:)=Omega1(i,:).*A1(i,:); 
    i=i+1;
end
A1 =(P1(1:k,:)*A1)*P1'; 

A2=A2(1:k,:);

% use ED of K
A32=P3(1:k,:)'*(A3(1:k,k+1:n)*P3(k+1:n,:));
A32=A32+A32';
A3=P3(1:k,:)'*A3(1:k,1:k)*P3(1:k,:)+A32;
i=1;
while (i<=n)
    A3(i,:)=Omega3(i,:).*A3(i,:); 
    i=i+1;
end
A3 =(P3(1:k,:)*A3)*P3';

Ax1=c1^(-0.5)*Lambda2'*R'*A1(:,1:k)*R+c2^(-0.5)*Lambda'*R'*A2(:,1:k)*R+R'*A3(:,1:k)*R; % F1'(Y,Z)
Ax2=c1^(-0.5)*S2'*A1(:,k+1:n) + c2^(-0.5)*S'*A2(:,k+1:n)+A3(:,k+1:n);% F2'(Y,Z)

epsilon_add= 1.0e-6*min(1.0,norm_b);
Ax1 =Ax1 + epsilon_add*x1;
Ax2 =Ax2 + epsilon_add*x2;

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% end of Jacobian_matrix.m %%%%%%