function [H_est,iter,time] = FFDIAG_mod(C0,V0,B_pre,err_bound,inum_max)
% Ken, 17aug. : I now use objective function change as the stopping criterion

%=====================================================================
% On the whole, I use the codes by the author of the paper and do some adjustments. 
% Programmer: Yi-Lin Chiour
% National Tsing Hua University, Hsinchu, Taiwan 
% E-mail: d9664531@oz.nthu.edu.tw
% Date: June. 08, 2010

% Reference: 
% A. Ziehe, P. Laskov, G. Nolte, and K.-R. Muller,ˇ§A fast algorithm for 
% joint diagonalization with non-orthogonal transformations and its application 
% to blind source separation,ˇ¨Journal of Machine Learning Research, vol. 5, pp. 777-800, 2004.

%======================================================================
% A practical implementation of the FFDIAF algorithm.
% [H_est,iter,time] = FFDIAG(C0,V0,err_bound,inum_max,B_pre)
%======================================================================
% Outputs: 
% H_est     : the estimated mixing matrix
% iter      : total number of iteration under the convergence of the FFDIAG algrithm. 
% time      : comutational time under the convergence of the FFDIAG algrithm.
%---------------------------------------------------
% Inputs:
% C0        : the covariance matrices of the observations.
% V0        : the diagonalization matrix.
% B_pre     : the prewhitenined matrix.
% err_bound : the tolerance between the currently estimated channel and the previously estimeated channel.
% previous channel.
% iter_max  : the maximal number of iteration. 

[m,n,k] = size(C0);
C       = C0;
Id      = eye(n);
V       = V0;
tic
for m=1:k
     C(:,:,m)=V*C(:,:,m)*V';
end
inum    = 1;
% sol_err = 1;
obj_diff= 10*err_bound;


% t = cputime;
%while (sol_err>err_bound & inum <=inum_max)
while (obj_diff > err_bound & inum <=inum_max)
      W     = getW(C);
      [f,e] = log2(norm(W,'inf'));
      s     = max(0,e-1);
      W     = W/(2^s );
      
     % Compute update
     V_new   = (Id+W)*V;
     sol_err = norm(V_new-V,'fro')^2/norm(V,'fro')^2;
     V       = V_new;

 
     % re-normalization
     V = diag(1./sqrt(diag(V*V')))*V;  % norm(V)=1 
     for i=1:k
         C(:,:,i) = V*C0(:,:,i)*V';
     end
     
     % evaluate the current objective value
     obj_cur= 0;
     for i=1:k
         obj_cur= obj_cur+ ( norm(C(:,:,i),'fro')^2 - sum(abs(diag(C(:,:,i))).^2) );
     end;
     if inum > 1, obj_diff= abs(obj_cur - obj_old)./abs(obj_old); end;
     %if inum > 1, obj_diff= abs(obj_cur - obj_old); end;
     obj_old= obj_cur;
  
  inum = inum+1;
end
time = toc;
% time = cputime - t;
iter = inum-1;
H_est = B_pre*pinv(V);




function W = getW(C)
% Reference: 
% A. Ziehe, P. Laskov, G. Nolte, and K.-R. Muller,ˇ§A fast algorithm for 
% joint diagonalization with non-orthogonal transformations and its application 
% to blind source separation,ˇ¨Journal of Machine Learning Research, vol. 5,
% pp. 777-800, 2004.

% function W = getW(C)
%======================================================================
% Outputs: 
% W         : update matrix with zeros on the main diagonal
%---------------------------------------------------
% Inputs:
% C       :  3-d array of matrices C_k.

[m,n,K] = size(C);
W = zeros(n);
z = zeros(n);
y = zeros(n);

for i=1:n
  for j=1:n
    
    for k=1:K
      z(i,j) = z(i,j)+C(i,i,k)*C(j,j,k);
      y(i,j) = y(i,j)+0.5*C(j,j,k)*(C(i,j,k)+conj(C(j,i,k)));
    end
  end
end

for i=1:n-1                                                              % It is expressed as the equ (17) of the paper.
  for j=i+1:n
    W(i,j) = (z(j,i)*y(j,i)-z(i,i)*y(i,j))/(z(j,j)*z(i,i)-z(i,j)^2);
    W(j,i) = ((z(i,j)*y(i,j)-z(j,j)*y(j,i))/(z(j,j)*z(i,i)-z(i,j)^2));
  end
end
    

