%%[X,y,fp,info] =BcorNewtonE (G,b,I,J,kappa,tau )
%% This code is designed to solve the nearest correlation matrix problem with a simple upper bound %%%%%%%%%%%%%
%%        min 0.5*<X-G, X-G>
%%       s.t. X_ij =b_ij, i\in I,j \in J, i=1,2,...,m
%%          tau*I<=X<=kappa*I (symmetric and positive semi-definite, and  upper bounded) %%%%%%%%%%%%%%%
%%
%%     Based on the algorithm  in %%%%%
%%    ``Newton's method for computing the nearest correlation matrix with a simple upper bound%%%
%%    By Qingna Li,  Houduo Qi,  Donghui Li  %%%%%%%%%%%%


%% Input: 
%%    G: given matrix
%%    tau: lower bound , tau>=0, default value 0.
%%    kappa: upper bound,kappa >1;
%%    b:   the value of the constraints
%%    I,J:  indices for the constraints 

%% Output :  
%%     X: optimal solution
%%     y: dual optimal solution
%%     fp: primal function value 0.5||X-G||^2
%%    info: other informations 


%%     Send your comments and suggestions to 
%%    liqingna@yahoo.com.cn   or  hdqi@soton.ac.uk or dhli@hnu.cn
%%    Last update 15/04/2010

%% simply call  BcorNewtonE(G,b,I,J,kappa  )


function [X,y,fp,info] =BcorNewtonE(G,b,I,J,kappa,tau  )


t0=cputime;
[n,n1] =size(G);
m = length(b);

global  b0
G =(G+G')/2; % make G symmetric
b0 =b;

if ~exist('tau', 'var');    tau = 0; end
if ~exist('kappa', 'var');    kappa = []; end

pre = 1; 
y = zeros(m,1); %% starting point, or y = b-A(G)
for i = 1:m
    y(i) = b(i) -G(I(i),J(i));
end
if tau > 0
   G  =  G-tau*eye(n); % reset G
   Ind = find(I==J); b0(Ind) = b0(Ind) - tau; % reset b0   
   if ~isempty(kappa)  %%%%% if there is the simple upper bound, then reset the upperbound
       kappa = kappa -tau; % reset kappa   
   end
end


Iter_Whole=50;
Iter_inner =20; % Maximum number of Line Search in Newton method
maxit =200; %Maximum number of iterations in PCG
error_tol=1.0e-6; % termination tolerance
sigma_1=1.0e-4; %tolerance in the line search of the Newton method

k=0;
itf =0;
iterk =0;
itcg = 0;
itls = 0;
eig_t = 0;
fg_t = 0 ;
omega_t = 0;
prec_time = 0;
pcg_time = 0;

c = ones(m,1);
Fy=zeros(m,1);
d =zeros(m,1);

num = 12;
f_hist = zeros(num,1);
fprintf('\n**********************************************************************************')
fprintf('')
fprintf('\n      Nearest Correlation Matrix Problem with a Simple Upper Bound ')
fprintf('\n\nDim of matrix n = %2.0d, number of equality constraints: %d\n lower bound: tau = %f',n,m,tau)
if ~isempty(kappa); fprintf(',  upper bound: kappa = %2.0d',kappa); end

val_G = sum(sum(G.*G))/2;
X = zeros(n,n);
for i = 1:m
     X(I(i),J(i)) = y(i);
end
X = 0.5*(X + X');
X = G + X;
X = (X + X')/2;

t1 = cputime;
%  [P,D]=eig(C);  lambda=diag(D);
[P,lambda]= mexeig(X);
eig_t = eig_t+ cputime-t1;
P = real(P);
lambda = real(lambda);
if lambda(1) < lambda(n) % the eigenvalues are arranged in the decreasing order
    lambda = lambda(n:-1:1);
    P = P(:,n:-1:1);
end 

t1 = cputime;
[f0,Fy] = Grad(y,I,J,lambda,P,X,b0,kappa);  
fg_t = fg_t+cputime-t1;
f = f0;
itf = itf + 1;
b = b0-Fy;
norm_b = 1; %%%%% to make sure that the following loop is done at least once

 
t1=cputime;
Omega = Omega_matrix(lambda,kappa,n);
omega_t = omega_t+ cputime - t1;
x0=y; 
fprintf('\n It   Iter_linear      res        f         time \n')

%% start main loop
while (norm_b > error_tol & k< Iter_Whole)
    if pre == 1
        cg_time0 = cputime;
        c = precond_matrix(Omega,P,I,J); % comment this line for  no preing
        prec_time = prec_time + cputime -cg_time0;
    end
    pcg_time0 = cputime;
    [d,flag,relres,iterk] = pre_cg(b,I,J,min(0.5,norm_b),maxit,c,Omega,P,n,1.0e-10);     %% use cg method for linear equation.  
    %%%%%%[d,flag,iterk] = pminres(@(x)Jacobian_matrix(x,I,J,Omega,P,1.0e-10),@(x)(x./c),b,min(0.5,norm_b),maxit,n);    
    pcg_time = pcg_time + cputime -pcg_time0;
    itcg = itcg + iterk;
    if (flag~=0); % if CG is unsuccessful, use the negative gradient direction
%      d =b0-Fy;
        disp('..... Not a full Newton step......')
    end
    slope = (Fy-b0)'*d; %%% nabla f d
    y = x0 + d; %temporary x0+d 
    
    X = zeros(n,n);
    for i = 1:m
        X(I(i),J(i)) = y(i);
    end
    X = 0.5*(X + X');
    X = G + X;
    X = (X + X')/2;
    t1 = cputime;
    %[P,D] = eig(C);   lambda=diag(D);% Eig-decomposition: C =P*D*P^T
    [P,lambda] = mexeig(X); 
    eig_t = eig_t + cputime - t1;   
    P = real(P);
    lambda = real(lambda);
    if lambda(1) < lambda(n) % the eigenvalues are arranged in the decreasing order
        lambda = lambda(n:-1:1);
        P = P(:,n:-1:1);
    end 
    t1 = cputime;  
    [f,Fy] = Grad(y,I,J,lambda,P,X,b0,kappa);
    fg_t = fg_t +cputime -t1;
    k_inner=0;
%% start line search
    while(k_inner <=Iter_inner & f> f0 + sigma_1*0.5^k_inner*slope + 1.0e-6)
        k_inner = k_inner + 1;
        y = x0 + 0.5^k_inner*d; % backtracking   
        X = zeros(n,n);
        for i = 1:m
            X(I(i),J(i)) = y(i);
        end
        X = 0.5*(X + X');
        X = G + X;
        X = (X + X')/2;

        t1 = cputime;
         %[P,D] = eig(C);   lambda=diag(D);% Eig-decomposition: C =P*D*P^T
        [P,lambda] = mexeig(X); 
        eig_t = eig_t + cputime - t1;   
        P = real(P);
        lambda = real(lambda);
        if lambda(1) < lambda(n) % the eigenvalues are arranged in the decreasing order
            lambda = lambda(n:-1:1);
            P = P(:,n:-1:1);
        end 
        t1 = cputime;
        [f,Fy] = Grad(y,I,J,lambda,P,X,b0,kappa);       
        fg_t = fg_t + cputime - t1;
    end % end loop for line search
      
    itf = itf + k_inner + 1;
    itls = itls + k_inner;
    x0 = y;
    f0 = f;     
    k = k + 1;
    b = b0 - Fy;
    norm_b = norm(b);
    fprintf(' %2.0d   %6.0d       %d     %9.8e  %f \n',k,iterk,norm_b,f,cputime - t0)
    t1 = cputime;
    Omega = Omega_matrix(lambda,kappa,n);
    omega_t = omega_t + cputime - t1;
     
    if norm_b <= error_tol 
        break;
    end
         % slow convergence test
    if  (k < num)
        f_hist(k+1) = f;
    else
        for i = 1:num-1
            f_hist(i) = f_hist(i+1);
        end
        f_hist(num) = f;
    end  
    if ( k >= num-1 && f_hist(1)-f_hist(num) < 1.0e-10 )
        fprintf('\n Progress is too slow! :( ')
        break;
    end
end %end main loop ;

C = P';
lambda = max(0,lambda);
if ~isempty(kappa)  %%%% if there is a upper bound, then we have to calculate another projection
    lambda = min(kappa,lambda);
end
nonzero = find(lambda>0);
for i = 1:length(nonzero)
    C(nonzero(i),:)=lambda(nonzero(i))*C(nonzero(i),:);
end
X = P(:,nonzero)*C(nonzero,:);     
X = (X+X')/2;

fd = val_G-f;
fp = sum(sum((X-G).*(X-G)))/2;
if tau > 0
X = X + tau*eye(n); 
lambda = lambda + tau*ones(n,1);
end

info.fp = fp;
info.fd = fd;
info.gap = fp - fd;
info.lambda_max = max(lambda);
info.lambda_min = min(lambda);
info.time = cputime - t0;
info.res = norm_b;
info.it = k;
info.itf = itf;
info.itcg = itcg;
info.itls = itls;

fprintf('\n--------------------------------------------------------')
fprintf('------------------------')
fprintf('\nprimal f =  %9.8e, dual f =  %9.8e, gap = %9.8e ',fp,fd,fp-fd);
fprintf('\nlargest eigenvalue: %9.8e,  smallest eigenvalue %9.8e',max(lambda),min(lambda))
fprintf('\nNumber of iterations: Iter= %d, \nnumber of function evaluations: = %d',k,itf)
fprintf('\ntotal number of iterations in solving linear system:   %d',itcg); 
fprintf('\nRes=%5.2e, ', info.res); 
fprintf('\nCPU time      = %3.5f,',info.time);
[hh,mm,ss] = time(info.time);
fprintf('  %d:%d:%d',hh,mm,ss)
fprintf('\n--------------------------------------------------------')
fprintf('------------------------\n')
% 
%%% end of the main program











%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%-------    subfunctions  ----- %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%% time format %%%%%%%%%%%%%%%%%%%%%%%
 function [h,m,s] = time(t)
t = round(t); 
h = floor(t/3600);
m = floor(rem(t,3600)/60);
s = rem(rem(t,60),60);
%%% End of time.m%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 
%%%%%% To generate F(y) %%%%%%%
 
function [f,Fy]=Grad(y,I,J,lambda,P,X,b,kappa);
n = length(P);
m = length(b);
Fy = zeros(m,1);
const_sparse = 2;
if isempty(kappa) 
    %%  without upper bound constraint, it reduces to the standard nearest correlation matrix with equality constraints,
    %%   same as in Stress testing code 
    lambda1 = max(0,lambda);
    f =lambda1'*lambda1*0.5 -b'*y;
else
    lambda1 = min(kappa,max(lambda,0));
    lambda0 = lambda1 - lambda;
    f = -lambda0'*lambda0*0.5 + lambda'*lambda*0.5 - b'*y;
end

I1 = find(I == J); 
if length(I1)==m     %% only diagonal constraints   
    nozero = find (lambda1>0);
    Fy = P(:,nozero).*P(:,nozero)*lambda1(nozero) ;
else
    I1 = find(lambda1>=1.0e-18);
    r  = length(I1);
    lambda1 = lambda1(I1); 
    lambda1 = lambda1.^0.5;
    P1 = P(:,I1);
    if r >1
        P1 = P1*sparse(diag(lambda1));
    else
        P1 = lambda1*P1;
    end
    P1T = P1';
    if (m <= const_sparse*n) %% sparse form
        i=1;
        while (i<=m)
            Fy(i) = P1(I(i),:)*P1T(:,J(i));
            i=i+1;
        end
    else %% dense form
        P = P1*P1T;
        i=1;
        while (i<=m)
            Fy(i) = P(I(i),J(i));
            i=i+1;
        end
    end
end
%%%%%%%% end of Grad.m %%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%        %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the first -order difference of lambda
 
function omega = Omega_matrix(d,kappa,n)
if isempty(kappa)
    d1 = max(0,d);
    omega = ones(n,n);
    eta = find(d<0);
    delta = find(d == 0);
    diffzero = setdiff([1:n]',[delta ;eta]);
    n_eta = length (eta);
    n_delta = length (delta);
    omega([delta; eta], [delta; eta])= zeros(n_eta+n_delta, n_eta+n_delta);

    if n_eta > 0
      for i = 1:n_eta
          omega(eta(i),diffzero) = (-d1(diffzero))./(d(eta(i))-d(diffzero)); 
      end
      omega(eta,diffzero) = max(1.0e-15,omega(eta,diffzero));
       omega(diffzero,eta) = omega(eta,diffzero)';
    end
else
    d1 = min (kappa,max(0,d));
    omega = ones(n,n);
    alpha = find(d>kappa);
    beta = find (d==kappa);
    diffkappa = setdiff([1:n]',[alpha; beta]);

    n_alpha = length(alpha);
    n_beta = length (beta);
    omega([alpha; beta],[alpha; beta])=zeros(n_alpha+n_beta,n_alpha+n_beta);  %%%%%omega_(alpha,alpha)=0,omega(alpha,beta)=0,omega(beta,alpha)=0,omega(beta,beta)=0
    if n_alpha>0
        for i=1:n_alpha
            omega(alpha(i),diffkappa)=(kappa-d1(diffkappa))./(d(alpha(i))-d(diffkappa));
        end
        omega(alpha,diffkappa) = max(1.0e-15,omega(alpha,diffkappa));
        omega(diffkappa,alpha)=omega(alpha,diffkappa)';
    end
    eta=find(d<0);
    delta = find(d == 0);
    diffzero = setdiff([1:n]',[alpha; delta ;eta]);

    n_eta = length (eta);
    n_delta = length (delta);
    omega([delta; eta], [delta; eta])= zeros(n_eta+n_delta, n_eta+n_delta);
    if n_eta>0
        for i=1:n_eta
            omega(eta(i),diffzero)= (-d1(diffzero))./(d(eta(i))-d(diffzero)); 
        end
        omega(eta,diffzero) = max(1.0e-15,omega(eta,diffzero));
        omega(diffzero,eta)=omega(eta,diffzero)';
    end    
end 
%%%%%%%%%%%%%%%%%% end of  Omega_matrix.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 [p,flag,relres,iterk] = pre_cg(b,I,J,tol,maxit,c,Omega,P,n,epsilon_j)
    
% Initializations
m = length(b);
r = b;  %We take the initial guess x0=0 to save time in calculating A(x0) 
n2b =norm(b);    % norm of b
tolb = tol * n2b;  % relative tolerance 
%% tolb = max(tol, min(0.1,n2b)*n2b);
p = zeros(m,1);
flag=1;
iterk =0;
relres=1000; %%% To give a big value on relres
% pre 
z =r./c;  %%%%% z = M\r; here M =diag(c); if M is not the identity matrix 
rz1 = r'*z; 
rz2 = 1; 
d = z;
% CG iteration
for k = 1:maxit
   if k > 1
       beta = rz1/rz2;
       d = z + beta*d;
   end
%    w = Jacobian_matrix(d,Omega,P,n); % W =A(d)
   w = Jacobian_matrix(d,I,J,Omega,P,epsilon_j);
   denom = d'*w;
   iterk =k;
   relres = norm(r)/n2b;              %relative residue = norm(r) / norm(b)
   if denom <= 0 
       sssss=0
       p = d/norm(d); % d is not a descent direction
       break % exit
   else
       alpha = rz1/denom;
       p = p + alpha*d;
       r = r - alpha*w;
   end
   z = r./c; %  z = M\r; here M =diag(c); if M is not the identity matrix ;
   if norm(r) <= tolb % Exit if Hp=b solved within the relative tolerance
       iterk =k;
       relres = norm(r)/n2b;          %relative residue =norm(r) / norm(b)
       flag =0;
       break
   end
   rz2 = rz1;
   rz1 = r'*z;
end

return

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


%%%%%% To generate the Jacobain product with x: F'(y)(x) %%%%%%%
 
function Ax = Jacobian_matrix(x,I,J,Omega,P,epsilon_j)

n = length(P);
m = length(I);

Ax =zeros(m,1);
I1 =find(I==J);
if( length(I1) == m & m == n) %% there are only digonal constraints
    H =P;
    i=1;
    while (i<=n)
    H(i,:) = x(i)*H(i,:); % H=diag(x)*P
    i=i+1;
    end   
    H =P'*H; %% H =P^T* diag(x) *P
    H = Omega.*H; %% H =[Omega o P^T* diag(x) *P]

     H =H*P'; %%% Assign H*P' to H= [Omega o (P^T*diag(x)*P)]*P^T
     i=1;
     while (i<=n)
       Ax(i)=P(i,:)*H(:,i);
       Ax(i) = Ax(i) + epsilon_j*x(i); % add a small perturbation
       i=i+1; 
     end
else  %% there are more other equality constraints
    Z = zeros(n,n);
    for i = 1:m
       Z(I(i),J(i)) = x(i);
    end
    Z = 0.5*(Z + Z');
    %% %H1 = P*(Omega.*(P'*Z*P))*P';  
    H = P';
    H1 = H*Z;
    H1 = H1*P;
    H1 = Omega.*H1;
    H1 = P*H1;
    H1 = H1*H; 
    i = 1;
    while (i<=m)
          Ax(i) = H1(I(i),J(i));
          Ax(i) = Ax(i) + 1.0e-10*x(i);
          i = i+1;
    end
end
 

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



%%%%%% To generate the diagonal preconditioner%%%%%%%
function c = precond_matrix(Omega,P,I,J)

n = length(P);
m = length(I);
c = ones(m,1);

I1 =find(I==J);
if( length(I1) == m & m == n) %% there are only digonal constraints
    PS = P.^2;
    c = sum(PS.*(PS * Omega),2); %L_k = W_k*Q_k, v_ii = q_i^l_i
    c = max(c, 1.0e-8);
else
    H = P';
    H = H.*H;
    Omega0 = Omega.*H;
    H = H';
    i = 1;
    while (i<=m)
        c(i) = H(I(i),:)*Omega0(:,J(i));
        i = i + 1;
    end
    c = max(c, 1.0e-8);
end

%%%%%%%%%%%%%%%% end of precond_matrix.m %%%%%%%%%%%%%%%

%%%%%%%%%%%% preconditioned minres to solve linear system %%%%%%%%%%%
function [xMR, iflag, total_iters, resMR] = pminres(A_fun,M_fun,f,tol,n_max,N)

% pminres     preed MINRES algorithm 
% [xMR, iflag, total_iters, resMR] =
% pminres(aparams,mparams,f,n_max,tol,x0);
%   input
%          A_fun      A(x)
%          M_fun      M^(-1)*x
%          f            right-hand side vector
%          n_max        max number of iterations
%          tol          relative preed residual reduction factor 
%          x0           initial iterate
%   output
%          x            computed (approximate) solution
%          iflag        convergence flag
%                       0 for convergence to tol within n_max iterations
%                       1 if iterated n_max times but did not converge to
%                       tol
%          total_iters  total iteration count
%          resMR        vector of residual norms of iterates
%   PIFISS function: DJS; 31 January 2007
%Modification of pminres.m written by Bernd Fischer.
%Copyright 1996 B. Fischer.  
%Reproduced and distributed with permission of the copyright holder.
%Modifications to handle PIFISS data structures identified in line.

precond=1;
% Names of mvp and preing routines
%%Initialize

   n=1; %N=length(x0); 
      v_hat_old=zeros(N,1); %v_hat=f-feval(matvec,A,x0);
      v_hat=f;%-A_fun(x0);
      if f == zeros(N,1)
          v_hat = f-A_fun(ones(N,1));
      end
	  norm_r0=norm(v_hat);
      y=M_fun(v_hat);
      beta=sqrt(v_hat'*y);
      beta_old=1;
      c=1; c_old=1; s_old=0; s=0; 
      w=zeros(N,1); w_old=w; eta=beta;
      xMR=zeros(N,1);  norm_rMR=norm_r0; %norm_rGAL=norm_r0; 
      resMR=zeros(n_max,1); %resGAL=zeros(n_max,1); 
      resMR(n)=log10(norm_rMR/norm_r0);
      while (n < n_max+1) & (norm_rMR/norm_r0 > tol)  
          n=n+1;
          %%Lanczos
	     v=y/beta; 
         y=v_hat;  
         Av=A_fun(v);
         alpha=v'*Av;
		 beta;
         v_hat=Av-(alpha/beta)*v_hat-(beta/beta_old)*v_hat_old;
         v_hat_old=y; 
         y=M_fun(v_hat);   
         beta_old=beta; beta=sqrt(v_hat'*y);  
         %%QR factorization
         c_oold=c_old; c_old=c; s_oold=s_old; s_old=s;  
         r1_hat=c_old*alpha-c_oold*s_old*beta_old;
         r1    =sqrt(r1_hat^2+beta^2);
         r2    =s_old*alpha+c_oold*c_old*beta_old;
         r3    =s_oold*beta_old;
         %%Givens rotation
         c=r1_hat/r1;
         s=beta/r1; 
         %%Update
         w_oold=w_old; w_old=w; 
         w=(v-r3*w_oold-r2*w_old)/r1 ;
         xMR=xMR+c*eta*w; eta=-s*eta;
   %%CEP - Experiment with different stopping criteria if required
   %precres1=(f - feval(matvec, A,xMR));
     %precres2=feval(precondsolve,L,R,nedges,nel,precres1);
     %precres3=precres1'*precres2;
     %size(precres3);
     %norm_prec_res=sqrt(precres3);
     %norm_rMR=norm_prec_res;
     %norm_rMR=norm(newres2)
     norm_rMR=norm_rMR*abs(s) ; %Updated norm w.r.t. preed system
     resMR(n)= norm_rMR/norm_r0; %% log10(norm_rMR/norm_r0);
     if abs(c) > eps
         xGAL=xMR-s*eta*w/c; 
         %%Norm
	     norm_rGAL=norm_rMR/abs(c); %Updated norm w.r.t. preed system
	     %norm_rGAL=norm(f-feval( matvec,A,xGAL));
        % resGAL(n)=log10(norm_rGAL/norm_r0);
     else
        disp([int2str(n),' no Galerkin iterate exists']);
      end; %Galerkin

    end %while	
	resMR =resMR(1:n);
    %resGAL=resGAL(1:n);
    iflag=0; total_iters=n-1;
    %%% set flag if iteration count exceeded 
    if n==n_max+1, iflag = 1; end
return;
%%%%%%%%%%%%%%%% end of pminres %%%%%%%%%%%%%%%%%%%%%%%
