function Hess = evalHess(rho,Grho,ZGrho,VKVlist,VZKVlist,noiter,W) 
%function Hess = evalHess(rho,Grho,ZGrho,VKVlist,VZKVlist,noiter,W) 
%INPUT: rho: point of evaluation
%       Grho: the image of rho under G
%       ZGrho: the image of rho under Z(G(.))
%       VKVlist: data for linear map G
%       VZKVlist: data for linear map Z(G(.))
%       noiter: current number of iteration in GNQKD
%       W: matrix used for matrix representaion of hessian
%OUTPUT: Hessian of the objective function at rho, i.e., nabla^2 D(G(rho)||Z(G(rho)))



%%% Hess total Hessian; h1,h2 are separate terms

[U1,D1] = eig(Grho,'vector');
[U2,D2] = eig(ZGrho,'vector');
h1D1 = fdd(D1,noiter);
h1D2 = fdd(D2,noiter);

% construct hessian
k = length(rho);
Hess = zeros(k^2);
%%%%%%%%%%%%%%%%
for i = 1:k^2
E = reshape(W(:,i),k,k);
% the hessian for the first term
GE = sum_congruence(E,VKVlist,0);
temp1 = (U1*(((U1'*GE)*U1).*h1D1))*U1';
h1 = Hvec(sum_congruence(temp1,VKVlist,1));
% the hessian for the second term
ZGE = sum_congruence(E,VZKVlist,0);
temp2 = (U2*(((U2'*ZGE)*U2).*h1D2))*U2';
h2 = Hvec(sum_congruence(temp2,VZKVlist,1));
Hess(:,i) = h1-h2;
end

% % add them up
%Hess0 = Hess;  % save as check for neg eigs
Hess = (Hess+Hess')/2;

  %double-check psdness
  [V,D] = eig(Hess);
  if min(diag(D)) < 0
     %fprintf('in evalHess min/max eig Hess %g\n',...
     %  min(eig(Hess)),max(eig(Hess)))
     %fprintf(' min(real(eig(Hess0))) %g %g\n',min(eig(Hess0)))
     %keyboard
     % projection to the p.s.d. cone
     %M = (M+M')/2;
     IND = find(diag(D) > 0);
     Hess = V(:,IND)*D(IND,IND)*V(:,IND)';
     Hess = (Hess+Hess')/2;
  end
end

function F = fdd(v,noiter)
% compute the first divided difference of v for f(x) = log(x)
n = length(v);
F = zeros(n);
for i = 1:n
for j = i:n
%if abs(v(i)-v(j))>1e-12
if noiter < 20
	toler = 1e-12;
else
	toler = 1e-15;
end
if abs(v(i)-v(j))>toler
       F(i,j) = (log(v(i)) - log(v(j)))/(v(i)-v(j));
else
       F(i,j) = 1/v(i);
end
end
end

F = F + triu(F,1)';

end
