% AR_IDENTIF_TONES2.M
%
% P. Flandrin, 30 sept. 2003 -- 25 nov. 2003
%
% exemple de modˇlisation AR de 2 frˇquences dans un bruit blanc
% vs. corrrˇlogramme
 
% ----- signal

N = 50; % nombre de points du signal
Nxc = 10;
t = 1:N;
SNR = 10;
Nfft = 256; % nombre de points pour le calcul de la FFT
pmax = 15; % ordre max du mod¸le estimˇ
cmax = 50; % nombre max de coeffs cepstraux

f1 = .22;
df = .04;
f2 = f1+df;

indf1 = 1+fix(f1*Nfft);
indf2 = 1+fix(f2*Nfft);

phi = pi/5;
tone = real(fmconst(N,f1) + 1*fmconst(N,f2,fix(N/4)));%sin(2*pi*f1*t) + .7*sin(2*pi*f2*t+phi);
%tone = tone';
noise = randn(size(tone));
x = sigmerge(tone,noise,SNR);
x = x';

figure(1)
clf

subplot(221)
plot([1 N],[0 0],'k')
hold on
plot(x,'o-r');%stem(x,'r')
title(['signal --- ',int2str(N),' points, SNR = ',int2str(SNR),' dB'])
axis([1 N 1.1*min(x) 1.1*max(x)])
xlabel('temps')
set(gca,'YTick',[])
set(gca,'XTick',[])
hold off

pause

xc = fftshift(xcorr(x));%.*(window(N,'triang'))';
xceff = xc(1:Nxc);
xf = imag(fft(xceff,Nfft));

subplot(222)
% plot(log((abs(xf(1:Nfft/2))).^2),'r')
plot(xf(1:Nfft/2),'r')
hold on
plot([1 Nfft/2],[0 0])
V = axis;
axis([1 Nfft/2 V(3) V(4)])
set(gca,'XTick',[])
set(gca,'YTick',[])
xlabel('frequence')
ylabel('DSP')
title(['correlogramme sur ',int2str(Nxc),' retards'])
hold on
plot([indf1 indf1],[V(3) V(4)],'k')
plot([indf2 indf2],[V(3) V(4)],'k')
hold off

pause

% ----- identification

Pth = 1;

[AA,KK,CC,P,FPE,popt] = AR_levinson2(x,pmax,cmax);

subplot(223)
plot(log(P),'b+-')
hold on
plot(log(FPE),'r+-')
plot(Nxc,log(FPE(Nxc)),'r','LineWidth',6)
xlabel('ordre du modele')
ylabel('log(erreur)')
title('bleu = puissance; rouge = FPE')
hold off

% pause

subplot(224)
Nf = Nfft/2; % nombre de points du spectre
f = linspace(0,.5,Nf);
z = exp(i*2*pi*f);

M = [];
M(1,:) = ones(1,Nf);
for mm = 2:pmax+1
	
	M(mm,:) = 1./z.^(mm-1);
	
end

Aest = [1 AA(Nxc,1:Nxc)];
Sest = 1./(abs(Aest*M(1:Nxc+1,:))).^2;
% plot(f,log(Sest),'r')
plot(f,Sest,'r')
set(gca,'XTick',[])
set(gca,'YTick',[])
W = axis;
axis([0 0.5 W(3) W(4)])
xlabel('frequence')
ylabel('DSP')
title(['estimee par AR(',int2str(Nxc),')']) 
hold on
plot([f1 f1],[W(3) W(4)],'k')
plot([f2 f2],[W(3) W(4)],'k')
hold off

% subplot(223)
% hold on
% [X,Y] = ginput(1);
% pp = round(X(1));
% plot(pp,log(FPE(pp)),'b','LineWidth',6)
% hold off
% 
% subplot(224)
% hold on
% App = [1 AA(pp,1:pp)];
% Spp = 1./(abs(App*M(1:pp+1,:))).^2;
% % plot(f,log(Spp),'b','LineWidth',2)
% plot(f,Spp,'b','LineWidth',2)
% title(['estimˇe par AR(',int2str(pp),')']) 
% plot([f1 f1],[W(3) W(4)],'k')
% plot([f2 f2],[W(3) W(4)],'k')
% hold off