nbsinu(s , n , prob);

function [L , R] = nbsinu(s , n , prob);


% Fast Algorithm returning an estimate of the number of sinusoids inside a corrupted signal in with a
% white Gaussian noise. This algorithm use a sub-space method based on chi-square statistics of
% eigen values of the Autocorrelation Matrix.
%
% Usage :   [L , R] = nbsinu(s , n , prob);
%
% Inputs 
% ------
%
% 	s       Observation (1 x T) or (T x 1)
%
% 	n       size of the autocorrelation matrix (< T) (default n = T/4)
%
% 	prob    Confidence probability (default prob = 0.95) 
%
% Ouputs
% ------
%
% 	L       Number of sinusoids estimated
%
% 	R       Autocorrelation matrix (Autocentroid) (n x n)
%
%
%
% Example 
% -------
%
% clear, close all hidden
% N       = 512;
% L_vrai  = 4;
% fe      = 2000;
% F       = (0:(fe/2)/(N/2-1):(fe/2));
% snr     = [-10 -10 -10 -10];
% f       = [25 230 500 780];
% n       = 32;
% prob    = 0.95;
% y       = zeros(1,N);
% t       = (0:(N-1))/fe;
%
% for l = 1:L_vrai
%   y      = y + sqrt(2)*10^(snr(l)/20)*sin(2.*pi.*f(l).*t + 2.*pi.*rand);
% end
%
% s       = y + randn(1 , N);
%
%
% [L , R] = nbsinu(s , n , prob);
%
%
% figure
% subplot(211)
% plot((1:N) , y , (1:N) , s , 'r');
% axis tight
% title(['Clean & Measured processes : L = ' int2str(L_vrai) ', {\bf L estimated = '  int2str(L) '}' ] , 'fontsize' , 11 , 'fontname' , 'times')
% legend(['Clean'] , ['Noisy'] , 0)
% subplot(212)
% P = (1/N)*abs(fft(s)).^2;
% plot(F , P(1:N/2))
% xlabel('Hz' , 'fontsize' , 12 , 'fontname' , 'times')
% title(['Periodogram : {\bf L estimated = ' int2str(L) '}' ], 'fontsize' , 11 , 'fontname' , 'times')

% Ref : Jean-Jacques Fuchs, "Estimating the number of sinusoids in additive white noise", 
% IEEE Trans on Acoustics, speech and signal processing, vol. 36, no 12, december 1988.
%
% Author  : Sbastien PARIS

% Version 1.1 : add some small speed improvements


warning off

T            = length(s);

if ( nargin < 2)
    
    n    = 2^nextpow2(T/8);
    
end


if ( (n < 1) | (n >= T))
    
    error('n must be > 1 and < T');
    
end


if (nargin < 3)
    
    prob = 0.95;
    
end


sige              = zeros(1 , n/2 - 2);

cte               = (T - n + 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

R                 = autocentro(s , n);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[vect , val]      = eig(R);

[lambda , d2]     = sort(diag(val));

sige              = cumsum(lambda)./(1 : n)';

sige2             = cte./(sige.*sige);

d3                = d2(end : -1 : 1);

lambda            = lambda(d3);

vectorder         = vect(: , d3);


%%%%%%%%%%%%%%%%%%%%%%%%%%%  Shift Matrix S  %%%%%%%%%%%%%%%%%%%%%

S                 = matshift(n);   %(n x n(n + 1)) where (S(: , 1 : n) = eye(n) and S(: ,  n*n + 1: n*n + n) = zeros(n);)

%%%%%%%%%%%%%%%%%% Initialization k = n %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

k                 =  n;

vectv             =  ( lambda(n - k + 1 : n) - sige(k) );

MA                =  sige2(k)*vectv'*inv(estimeq(S , vectorder(1 : n , n - k + 1 : n) , k))*vectv;


%%%%%%%%%%%%% iterations versus k %%%%%%%%%%%%%%%%%%%%%%%%%%



while( (MA > chi2_inv(prob , k) ) & ( k > 0 )) 
    
    k          =  k - 2;
    
    vectv      =  ( lambda( n - k + 1 : n) - sige(k) );
    
    MA         =  sige2(k)*vectv'*inv(estimeq(S , vectorder( : , n - k + 1 : n) , k))*vectv;
    
end

L            = (n - k)/2;


warning on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function u  = autocentro(s , n)

%
% u = autocentro(s , n);
%


J                      = zeros(n);

J(n : (n-1) : n*n - 1) = 1;

v                      = buffer(s , n , n - 1);

R                      = (v*v')/(length(s) - n + 1);

u                      = 0.5*(R + J*R*J);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function u = estimeq(S , U2 , k);

%
% u = estimeq(S , U2 , k);
%


[n vv uu] = size(S);

n2        = n*n;

U2t       = U2';

U3        = U2.*U2;

V         = (U2t*S(: , 1 : n))*U3 + (U2t*S(: , n2 + 1 : n2 + n))*U3 ;

for y = 2 : n 
    
    t       = (y - 1)*n + 1;
    
    T       = (U2t*S(: , t : t + n - 1) )*U2;
    
    V       = V + 2*T.*T;
    
end

u         = 2*V;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function u = matshift(n);

% u = matshift(n);


n2             = n*n;

n1             = n - 1;

u              = sparse([] , [] , [] , n , n*(n + 1) , (n + 1)*n*0.5 );

On             = ones(1 , n);

vect1          = (1 : n + 1 : n2)';

vect2          = (0 : n2 : n1*n2);

vect3          = (0 : n1);

R              = tril(vect1(: , On) + vect2(On , :) - vect3(On , :));

u(R(R~=0))     = 1;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function f = chi2_inv(p , a);

b     = a*0.5;

x     = max(b - 1 , 0.1);

dx    = 1;

tiny  = 256*eps;

while ( any( any( abs(dx) > tiny*max(x , 1)) ) )
    
    cdf     = gammainc(x , b);
        
    I0      = (x < 0);
        
    cdf(I0) = 0;
    
    pdf     = x.^(b - 1).* exp(-x)./gamma(b);
        
    pdf(I0) = 0;
        
    dx      = (cdf - p)./pdf;
    
    x       = x - dx;
        
    x       = x + 0.5*(dx - x).*(x < 0);
    
    
end

f = x*2;