function [F,S]=sobi(X,m,p)
%
% Second  Order  Blind  Identification
%
%**************************************************
% blind identification by joint diagonalization   *
% of correlation  matrices.                       *
%                                                 *
% ------------------------------------------------*
% THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS *
% in estimating the cumulants                     *
% ------------------------------------------------*
%**************************************************
%
% [F,S]=SOBI(X,m,p) produces a matrix F of dimension [n,m] and a matrix S of 
% dimension [m,T] wich are respectively an estimate of the mixture matrix and 
% an estimate of the source signals from which the linear mixture observation 
% matrix X of dimension [n,T] is derived.
% Note: > n: sensor number.
%       > m: source number by default m=n.
%       > T: sample number.
%       > p: number of correlation matrices to be diagonalized by default p=4.
%

% Functions that will need to be implemented:
% eig   eigenvalues
%       [V,D] = EIG(X) produces a diagonal matrix D of eigenvalues and a full matrix V whose columns are the corresponding eigenvectors so that X*V = V*D.
% sort
% sqrtm matrix square root  X = sqrtm(A)  is equivalent to X*X = A
% diag
% inv   inverse of a square matrix
% ./    right array divide, element-by-element division
% norm  NORM(X,'fro') is the Frobenius norm, sqrt(sum(diag(X'*X))).
% '/    A Complex conjugate transpose followed by a right matrix divide
% eye(N)    is the N-by-N identity matrix
% conj  complex conjugate. For a complex X, CONJ(X) = REAL(X) - i*IMAG(X).

% References: 
% http://ieeexplore.ieee.org/iel5/10917/34366/01639299.pdf

[n,T]=size(X);
if nargin==1
    m=n; % source detection (hum...)
    p=4; % number of correlation matrices to be diagonalized
end;
if nargin==2
    p=4 ; % number of correlation matrices to be diagonalized
end;
pm=p*m; % for convenience
%%% whitening
Rx=(X*X')/T;
if m<n, %assumes white noise
  [U,D] = eig(Rx);
  [puiss,k] = sort(diag(D));
  ibl = sqrt( puiss(n-m+1:n) - mean(puiss(1:n-m)) );
  bl = ones(m,1) ./ ibl ;
  BL = diag(bl) * U(1:n,k(n-m+1:n))';
  IBL = U(1:n,k(n-m+1:n)) * diag(ibl);
else    %assumes no noise
   IBL=sqrtm(Rx);
   BL=inv(IBL);
end;
X=BL*X;

%%%computing correlation matrices
% Step S with increments of -0.1 for S = 1.0: -0.1: 0.0, do_some_task(S), end
% for var=<start>:<inc>:<end>
k=1;
for u=1:m:pm
    k = k+1;
    N = X(:,k:T) * X(:,1:T-k+1)'/(T-k+1);
    A(:,u:u+m-1) = norm(N,'fro') * N;
end;

%%%joint diagonalization
seuil = 1 / sqrt(T) / 100;
encore = 1;
V = eye(m);

while encore
    encore=0;
    for p=1:m-1,
        for q=p+1:m,
            %%% Givens rotations
            g=[   A(p,p:m:pm)-A(q,q:m:pm)  ;
                 A(p,q:m:pm)+A(q,p:m:pm)  ;
              i*(A(q,p:m:pm)-A(p,q:m:pm)) ];
            [vcp,D] = eig(real(g*g'));
            [la,K] = sort(diag(D));
            angles = vcp(:,K(3));
            angles = sign(angles(1)) * angles;
            c = sqrt(0.5+angles(1)/2);
            sr = 0.5*(angles(2)-j*angles(3))/c;
            sc = conj(sr);
            oui = abs(sr)>seuil ;
            encore = encore | oui ;
            if oui %%%update of the A and V matrices
                colp = A(:,p:m:pm);
                colq = A(:,q:m:pm);
                A(:,p:m:pm) = c*colp+sr*colq;
                A(:,q:m:pm) = c*colq-sc*colp;
                rowp = A(p,:);
                rowq = A(q,:);
                A(p,:) = c*rowp+sc*rowq;
                A(q,:) = c*rowq-sr*rowp;
                temp = V(:,p);
                V(:,p) = c*V(:,p)+sr*V(:,q);
                V(:,q) = c*V(:,q)-sc*temp;
            end%% if
        end%% q loop
    end%% p loop
end%% while
%%%estimation of the mixing matrix and signal separation
F = IBL * V;
S = V'*X;

