function [Y, C, infos] = CENewton(D, pars)
%
% call: [Y, infos] = CENewton(D, pars) to provide some parameters
% Previous version is NCGMDS.m in LLED folder
%
% To solve problem (P):
%   min 0.5*<Y-D, Y-D>
%   s.t. Y_ii - Y_nn =0, i = 1,...,n-1
%        Y \in \K^n_- (almost negative semidefinite cone in \S^n)
% Note: If diagonals are all zero, one should use the code: ENewton2016.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input: D  -- predistance matrix (diag(D) =0, D >= 0)
%       
% pars: -- parameters needed as input
%
% pars.y0 -- starting point, a vector of length(D)
%              default: use map_initial to get one;
% pars.eig -- eigensolver
%             1 (for eig.m, default)
%             0 (for mexeig.m provided by defeng)
% pars.scaling -- = 1 no scaling 
%                 = sum(sum(D))/nnz(D)  (default)
% pars.tol -- error tolerance (default: 1.0e-6)
% pars.nls -- maxumum number of linear search (default: 3)
% pars.printyes -- printyes = 1 print information
%                             0 no output information (default)
% Output:
%      Y -- Best almost nonpositive matrix from D (optimal solution of (P)
%      C -- Constant that defines the diagonals
%           Y-C or Y - 0.5*(a*ones(1,n)+ones(n,1)*a') is the EDM
%           where a = diag(Y)
%    infos:
%      infos.X  = Embedding coordinates generated by Y
%                 -0.5JYJ = X^TX, and X =[x1, x2, ..., xn]
%      infos.y  = the optimal Lagrange multiplier corresponding 
%                 to the diagonal constraints
%      infos.Z  = Z the complementarity matrix
%                 Y - (D + \A^*(y)) - Z = 0
%                 and Y is the output (trace(YZ) = 0 in theory)
%      infos.lambda: eiegnvalues of (-0.5JYJ) (decreasing order)
%      infos.P: eigenvectors of (-0.5JYJ) (corresponding to lambda)
%      infos.rank:  Embedding_Dimension;
%      infos.Iter = k;
%      infos.feval = f_eval; number of function evaluations;
%      infos.t = time_used; total cpu used
%      infos.res = norm_b; norm of the gradient at the final iterate
%                  This is the residual one finally got.
%      infos.f = val_obj; final objective function value 
%      infos.EigenD -- Total number of eigen decompositions

%%
%  This code is designed to solve %%%%%%%%%%%%%
%   min 0.5*<Y-D, Y-D>
%   s.t. Y_ii - Y_11 =0, i= 2,...,n
%        Y \in \K^n_- (almost negative semidefinite cone in \S^n)
%
% This problem appears in the EDM formulation od SSNE model.
% Its diagonally weighted version will be in another file
%
% In this version: D is first scaled to D/max(max(D)) to bring
%                  all distances between 0 and 1.
% In the outout: Information is scaled back. The intermidate output 
%                information is for the scaled problem.
%% Based on the paper
% A Convex Matrix Optimization for the Additive Constant Problem in Multidimensional Scaling
% with Application to LLE by Houduo Qi
%%          
% Send your comments and suggestions to    %%%%%%
%        hdqi@soton.ac.uk      %%%%%%
%
%
%%%%% Warning: Accuracy may not be guaranteed!!!!! %%%%%%%%
%%
%
t0 = tic;
n = length(D);
% scale D so that D_{ij} \in [0,1]
 Sfactor = sum(sum(D))/nnz(D); % Sfactor = 1 to return to unscaled version
%Sfactor = max(max(D));
%%%%%
% set up parameters
%
if ~isfield(pars, 'scaling') % default
    if Sfactor > 1
       pars.scaling = Sfactor;
    else
        pars.scaling = 1; % no scaling
    end
else
    pars.scaling = 1;
end
    
if ~isfield(pars, 'y')
    pars.y = zeros(n-1,1);
end
y = pars.y;

if ~isfield(pars, 'tol')
    pars.tol = 1.0e-6;
end

if ~isfield(pars, 'printyes')
    pars.printyes = 0;
end
%
Sfactor = pars.scaling;
D    = D/Sfactor;
prnt = pars.printyes;
y    = y/Sfactor;
error_tol = pars.tol/Sfactor;
% if error_tol < 1.0e-9
%    error_tol = 1.0e-7;
% end
% positive_eigenlevel = pars.eigenlevel;
% positive_eigenyes = pars.eigenyes;
%
if prnt
   fprintf('\n ******************************************************** \n')
   fprintf( '          The Semismooth Newton-CG Method (CENewton.m)        ')
   fprintf('\n ******************************************************** \n')
   fprintf('\n The information of this problem is as follows: \n')
   fprintf(' Dim. of    sdp      constr  = %d \n',n)
end

D = (D+D')/2; % make D symmetric
%
% calculate JDJ
%
De  = sum(D, 2); % row sums
eDe = sum(De); % total sum of D
JDJ = -De*(ones(1,n)/n);
JDJ = JDJ + JDJ';
JDJ = JDJ + D + eDe/n^2;
%JDJ = (JDJ + JDJ')/2; % make JDJ symmetric
%
k           = 0;
f_eval      = 0;
EigenD      = 0; % eigen decomposition number
Iter_Whole  = 200;
if ~isfield(pars, 'nls') % nls: number of linear search
    pars.nls = 3; % maximum number of linear search is three
end
Iter_inner  = pars.nls; % Maximum number of Line Search in Newton method
maxit       = 200; %Maximum number of iterations in PCG
tol_cg      = 1.0e-2; %relative accuracy for CGs
sigma_1     = 1.0e-4; %tolerance in the line search of the Newton method
prec_time   = 0;
pcg_time    = 0;
eig_time    = 0;

%
% calculate Y = J(D+\A^*(y))J
%
Y = JyJ(y);
Y = JDJ + Y;
Y = (Y+Y')/2;
%
%%%%% eigendecomposition
%
eig_time0 = tic;
[P,lambda] = qieig(Y); 
eig_time = eig_time + toc(eig_time0); 
EigenD   = EigenD + 1;
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[f0, Fy] = gradient(y, D, lambda, P);
f = f0;
f_eval = f_eval + 1; % number of function evaluations increased by 1
b = - Fy;
norm_b = norm(b);

Initial_f = - f0;
if prnt
    fprintf('Initial Dual Objective Function value==== %d \n', Initial_f)
    fprintf('Newton: Norm of Gradient %d \n',norm_b)
end

 Omega12 = omega_mat(lambda,n);
 x0 = y;

 tt = toc(t0);
[hh,mm,ss] = time(tt);

if prnt
    fprintf('\n   Iter.   Num. of CGs     Step length      Norm of gradient     time_used ')
    fprintf('\n    %d         %2.0d            %3.2e                      %3.2e         %d:%d:%d ',0,str2double('-'),str2double('-'),norm_b,hh,mm,ss)
end

 while (norm_b>error_tol && k < Iter_Whole)

    prec_time0 = tic;
    c = precond_matrix(Omega12,P); % comment this line for  no preconditioning
    prec_time = prec_time + toc(prec_time0);
%     %%% Test c 
%     TI = eye(n-1);
%     Tc = ones(n-1,1);
%     for i=1:n-1
%         Ta = TI(:, i);
%         Tx = Jacobian_matrix(Ta, Omega12, P, n);
%         Tc(i) = Tx(i);
%     end
%     norm(c-Tc)
%     pause
    %%%% end of Test c
    pcg_time0 = tic;
    %tol_cg = min(5.0e-3, 0.1*norm(b));
    [d,flag,~,iterk]  = pre_cg(b,tol_cg,maxit,c,Omega12,P); % pcg is best
    %[d, flag, ~, iterk] = psqmr_qi(b, tol, maxit, c, Omega12, P); 
    pcg_time = pcg_time + toc(pcg_time0);
 %d =b0-Fy; gradient direction
 %fprintf('Newton: Number of CG Iterations %d \n', iterk)
  
    if (flag~=0); % if CG is unsuccessful, use the negative gradient direction
      d = -Fy;
         disp('..... Not a full Newton step......')
    end
    slope = (Fy)'*d; %%% nabla f d 
    k_inner = 0;
    
    while ( 1~= 2)
       rho = 0.5^k_inner;
       y   = x0 + rho*d;
       Y   = JyJ(y);
       Y   = (JDJ + Y);
       Y   = (Y+Y')/2;
       ta  = tic;
       [P, lambda] = qieig(Y);
       eig_time = eig_time + toc(ta);
    
       [f, Fy] = gradient(y, D, lambda, P);
    
       if (k_inner >= Iter_inner) || ( f <= f0 + sigma_1*rho*slope + 1.e-6)
          break
       end
       k_inner = k_inner + 1;
    end

    if (k_inner > 1)
        fprintf('\n number of line serach: %d', k_inner)
    end
    f_eval = f_eval + k_inner + 1; % number of function evaluations
                         % is increased over the line search
                         % and the one before it
    x0 = y;
    f0 = f;

    k = k+1;
    b = -Fy;
    norm_b = norm(b);
    tt = toc(t0);
    [hh,mm,ss] = time(tt); 
    %   fprintf('Newton: Norm of Gradient %d \n',norm_b)
    if prnt      
        fprintf('\n   %2.0d         %2.0d             %3.2e          %3.2e         %d:%d:%d', ...
        k,iterk,0.5^k_inner,norm_b,hh,mm,ss)
    end   
    Omega12 = omega_mat(lambda,n);
 end
 %end loop for while i=1;

Ip = find(lambda>0); % could set to 1.0e-7
In = find(lambda <0);
r    = length(Ip);
rank = length(In); 
infos.rank = rank;

% \A^*(y) = diag(y; -sum(y));
y0 = -sum(y);
z  = [y; y0];
if (r==0)
    Y = D + diag(z);
elseif (r==n)
    Y = D + diag(z) - Y;
elseif (r<=n/2)
    lambda1 = abs(lambda(Ip));
    lambda1 = lambda1.^0.5;
    P1 = P(:, 1:r);
    P1 = P1*sparse(diag(lambda1));
    Y = P1*P1';
    Y = D + diag(z) -  Y;% Optimal solution X* 
else     
    lambda2 = abs(lambda(r+1:n));
    lambda2 = lambda2.^0.5;
    P2 = P(:, r+1:n);
    P2 = P2*sparse(diag(lambda2));
    Y = Y + P2*P2'; 
    Y = D + diag(z) - Y;% Optimal solution X* 
end

 Y = (Y+Y')/2;
 
 Final_f = - Sfactor^2*f;
 %
 % the diagonals of Y to should be constant
 %
  C = Y(1,1);
 %
 % dual matrix Z
 Z =  Y - (D + diag(z)); % Z belong to the dual cone of \K^n_-;
% comp_gap = sum(sum(Y.*Z)) % complementarity gap trace(YZ) = 0 in theory

 val_obj   = sum(sum((Y-D).*(Y-D)))/2;
 val_obj   = val_obj*Sfactor^2;
 norm_b    = norm_b*Sfactor;
 y         = Sfactor*y;
 lambda    = Sfactor*lambda;
 time_used = toc(t0); 
% fprintf('\n')
% set output information
infos.Pold      = P;
infos.lambdaold = lambda;
infos.rank   = rank;
infos.Iter   = k;
infos.feval  = f_eval;
infos.t      = time_used;
infos.res    = norm_b;
infos.f      = val_obj;
infos.Z  = Sfactor*Z; % optimal dual solution in \K_-^*
infos.y  = y;
infos.EigenD = EigenD;
%
% Put the sign (-1) back
%
 Y = Sfactor*Y;
 C = Sfactor*C;
% 
% To recover the Euclidean distance matrix, denoted by Y0 by Y0 = Y-C;
% However, due to computational error, the diagonals of Y may not be equal.
% Hence, C is just the theoretical value we should get
% In order to get a true EDM Y_0, we compute
% check the complementarity condition:
% Y- Dold + diag(y) - Z = 0 % here Dold = -D*Sfactor
%  Z = Y + Sfactor*D + diag(y); % = infos.Z
%  comp_gap = sum(sum(Y.*Z));

% Embedding coordinates generated by the calculated nearest EDM: Y
% - 0.5JYJ = 0.5 * Sfactor * \Pi_{\S^n_+} (J(-D + \A^*(y))J) = X^TX
% Note: It is those negative eigenvalues in lambda that gives X^TX
%
lambda = abs(lambda);       % lambda has been mulitplied by Sfactor
lambda = lambda(end:-1:1);
P = P(:, end:-1:1);
infos.lambda = lambda;
infos.P = P;
lambda = 0.5*lambda(1:rank); % positive eigenvalues in -0.5JYJ
P = P(:, 1:rank);

P = P*diag(lambda.^(0.5));
P = P';
infos.X = P;                 % embedding points in columns

infos.dualVal   = Final_f;
infos.primVal   = val_obj;
infos.res = norm_b;
infos.r = rank;

if prnt
    fprintf('\n\n')
    fprintf('Norm of Gradient == %d \n', full(norm_b))
    fprintf('Number of Iterations == %d \n', k)
    fprintf('Number of Function Evaluations == %d \n', f_eval)
    fprintf('Final Dual Objective Function value ========== %d \n', full(Final_f))
    fprintf('Final Original Objective Function value ====== %d \n', full(val_obj))
    fprintf('Embedding dimension ================= %d \n', full(rank))

    fprintf('Computing time for computing preconditioners == %d \n', prec_time)
    fprintf('Computing time for linear systems solving (cgs time) ====%d \n', pcg_time)
    fprintf('Computing time for  eigenvalue decompostions (calling mexeig time)==%d \n', eig_time) 
    fprintf('Total computing time (in s) ==== =====================%d \n',time_used)
end
% 


%%% end of the main program

%%% To change the format of time 
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);
return
%%% End of time.m

function [P,lambda] = qieig(X)
X = full(X);
[P, Lambda] =  eig(X);   %% X= P*diag(D)*P'
P           =  real(P);
lambda      =  real(diag(Lambda));
    
% make sure eigenvalues are in decreasing order
if issorted(lambda)
    lambda = lambda(end:-1:1);
    P      = P(:,end:-1:1);
elseif issorted(lambda(end:-1:1))
    return;
else
    [lambda, Inx] = sort(lambda,'descend');
    P = P(:,Inx);
end
return

%%%%%% To generate J*\A^*(y)J
%
function Y = JyJ(y)
% y is column vector, to compute J\A^*(y)J
n  = length(y) + 1;
y0 = - sum(y);
y  = [y; y0];   % sum(y) = 0
Y  = bsxfun(@times, -(y/n), ones(1,n));
Y  = (Y+Y');
Y(1:(n+1):end) = Y(1:(n+1):end) + y';   % Y = Y + diag(z);

return
%% end of JyJ

%%%%%% To generate F(y) %%%%%%%
%%%%%%%
% this function has been tested and is right
function [f,Fy] = gradient(y, D, lambda, P)

n  = length(y)+1;
f  = 0.0;
Fy = zeros(n-1,1);

y0 = -sum(y);
z  = [y; y0]; % \A^*(y) = diag(z)
 
%lambdap=max(0,lambda);
%H =diag(lambdap); %% H =P^T* H^0.5*H^0.5 *P

r = sum(lambda>0);
lambda1 = lambda(1:r).^0.5;
P = P(:,1:r)';
for i = 1:r;
    P(i,:) = lambda1(i)*P(i,:);
end

% last diagonal element
 a = D(n,n) + z(n) - P(:,n)'*P(:,n);
 
 i=1;
 while (i<=n-1)
       Fy(i) = D(i,i) + z(i) - P(:,i)'*P(:,i) - a;
       i=i+1;     
 end
 
% Compute the objective function 
%
r = sum(lambda<0); % negative eigenvalues
if (r >= 1)
   lambdan = lambda((n-r+1):end);
   f = lambdan'*lambdan;
end
%
z0 = sum(D,2);
z  = z0 + z;
a0 = sum(z0)/n;
 
f = f + 2*sum(z.^2)/n - a0^2;
f = 0.5*( f - sum(sum(D.^2)) );
return

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

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

%%%%%%%%%%%%%%        %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the essential part of the first -order difference of d
%%%%%%%
function Omega12 = omega_mat(lambda,n)
%We compute omega only for 1<=|idx|<=n-1
idx.idp = find(lambda > 1.0e-9);
idx.idm = setdiff([1:n],idx.idp);
n =length(lambda);
r = length(idx.idp);
 
if ~isempty(idx.idp)
    if (r == n)
        Omega12 = ones(n,n);
    else
        s = n-r;
        dp = lambda(1:r);
        dn = lambda(r+1:n);
        Omega12 = (dp*ones(1,s))./(abs(dp)*ones(1,s) + ones(r,1)*abs(dn'));
        %  Omega12 = max(1e-15,Omega12);

    end
else
    Omega12 =[];
end

    %%***** perturbation *****
    return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% end of omega_mat.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,tol,maxit,c,Omega12,P)
% Initializations
n = length(c) + 1;
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 
p = zeros(n-1,1);
flag=1;
iterk =0;
relres=1000; %%% To give a big value on relres
% Precondition 
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,Omega12,P); % W =A(d)
%    wT = T_Jacobian_matrix(d,Omega12,P,n);
%    norm(w-wT)
%    pause
   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 Jacobian product with x: F'(y)(x)
function Ax = Jacobian_matrix(x, Omega12, P)

n = length(x) + 1;
[r, ~]  = size(Omega12); 
tau = 1.0e-10;
% tau = 0;
x0 = sum(x);
z  = [x; -x0]; % \A^*(x) = diag(z)

if (r==0) % Omega = 0
    Ax = (1 + tau)*x + x0;
    return
elseif (r==n) % Omega = E this case should not happen
    Ax = 2/n*(x+x0) + tau*x;
    return
end

P1 = P(:,1:r);
P2 = P(:,r+1:n);
q  = P'*z;
q0 = P(:, 1:r)*q(1:r);
q0 = q0(1:n-1) - q0(n);

if (r<n/2)
   H1 = P1'*sparse(diag(z));
   Omega12 = Omega12.*(H1*P2);
   H = [(H1*P1)*P1' + Omega12*P2'; Omega12'*P1'];
%%%%%%%%%%%%%%%%%% 1st way
   H = P.*H';
   d = sum(H, 2);
   d = d(1:n-1) - d(n);
%%%%%%%%%%%%%%%%%% 2nd way
%    i=1;
%     while (i<=n)
%         Ax(i) = P(i,:)*H(:,i); % part from the digonal part
%         i = i+1;
%     end
%%%%%%%%%%%%%%%%%%%% end of 2nd way
    Ax = (x+x0) - d + (2/n)*q0 + tau*x; 
%
else % if r>=n/2, use a complementary formula.
     %H = ((E-Omega).*(P'*Z*P))*P';               
     H2 = P2'*sparse(diag(z));
     Omega12 = 1 - Omega12;
     Omega12 = Omega12.*((H2*P1)');
     H = [Omega12*P2'; Omega12'*P1' + (H2*P2)*P2'];
%%%%%%%%%%%%%%%%%% 1st way
     H = P.*H';
     d = sum(H, 2);
     d = d(1:n-1) - d(n);
%%%%%%%%%%%%%%%%%% 2nd way
%    i=1;
%     while (i<=n)
%         Ax(i) = P(i,:)*H(:,i); % part from the digonal part
%         i = i+1;
%     end
%%%%%%%%%%%%%%%%%%%% end of 2nd way
     Ax = d + (2/n)*q0 + tau*x;
end
return
%%% End of Jacobian_matrix.m  
   

%%%%%% To generate the diagonal preconditioner%%%%%%%
%%%%%%%

function c = precond_matrix(Omega12, P)

n = size(P, 1);
r = size(Omega12, 1);
c = 2*ones(n-1,1);
cn = ones(n,1);
dn = ones(n,1);

Omega12_old = Omega12;

if (r==0) % Omega = 0
    return % c = 2*ones(n-1,1);
elseif (r==n) % Omega = E (this case is impossible)
    t = 4/n;
    c = t*ones(n-1,1);
    return
end
%
H = P';
H = H.*H;
%
% compute c0
%
P1T = P(:, 1:r)';
a = P1T(:, n);
P1T = P1T(:, 1:n-1) - a(:, ones(1, n-1));
P1T = P1T.^2;
c0  = (2/n)*sum(P1T);
c0  = c0';

% compute the diagonal part
if (r < n/2)
   H12 = H(1:r,:)'*Omega12;
   for i=1:n
       dn(i) = (sum(H(1:r,i))).^2;
       dn(i) = dn(i) +2.0*(H12(i,:)*H(r+1:n,i));
   end
else % r>=n/2, use a complementary formula
    Omega12 = 1 - Omega12;
    H12 = Omega12*H(r+1:n,:);
    for i=1:n
        dn(i) = (sum(H(r+1:n,i)))^2;
        dn(i) = dn(i) + 2.0*(H(1:r,i)'*H12(:,i));
        dn(i) = 1 - dn(i);
    end
end

% compute the cross term
H = P';
a = H(:, n);
H = H.*a(:, ones(1,n));

if (r < n/2)
   H12 = H(1:r,:)'*Omega12_old;
   for i=1:n
       cn(i) = (sum(H(1:r,i))).^2;
       cn(i) = cn(i) +2.0*(H12(i,:)*H(r+1:n,i));
   end
else % r>=n/2, use a complementary formula
    Omega12 = 1 - Omega12_old;
    H12 = Omega12*H(r+1:n,:);
    for i=1:n
        cn(i) = (sum(H(r+1:n,i)))^2;
        cn(i) = cn(i) + 2.0*(H(1:r,i)'*H12(:,i));
        cn(i) = - cn(i);
    end
end

% the preconditioner is 2 - ( dn(2:n) + dn(1) - 2*cn(2:end))
c = 2 - ( dn(1:n-1) + dn(n) - 2*cn(1:n-1) );
c = c+c0;
c = max(1.0e-8, c);


return

