% freqinstosc.m
%
% P. Flandrin

clf

Ns = 256; % nombre d'Žchantillons du signal
f0 = 0.04;
a = 2*Ns^(-4);

sig = fmconst(Ns,f0) + a*(1:Ns)'.^4;

amp = 1.2*max(max(real(sig)),abs(min(real(sig))));

t = 1:Ns;
dt = 1;
clear zr;
clear zi;
zr = real(sig(1));
zi = imag(sig(1));

theta = linspace(0,1,100);

xc = [];
yc = [];

a = real(sig); x = a(3:length(a));
b = imag(sig); y = b(3:length(b));

a = diff(real(sig)); dx = a(2:length(a));
b = diff(imag(sig)); dy = b(2:length(a));

ddx = diff(diff(real(sig)));
ddy = diff(diff(imag(sig)));

xc = x - (dy.*(dx.^2 + dy.^2))./(dx.*ddy - dy.*ddx);
yc = y + (dx.*(dx.^2 + dy.^2))./(dx.*ddy - dy.*ddx);

r = sqrt((x - xc).^2 + (y - yc).^2);

sigo = (x - xc) + i*(y - yc);

fi = [f0 -diff(unwrap(angle(sig')))/(2*pi)];
fio = [f0 f0 f0 -diff(unwrap(angle(sigo')))/(2*pi)];

vect = xc + i*yc;

asup = xc + r;
ainf = xc - r;

subplot(2,1,1)
plot(real(sig))
amp = 1.2*max(max(real(sig)),abs(min(real(sig))));
axis([1 Ns-2 -2*amp amp])
hold on
plot([1 Ns],[0 0])
plot([1 Ns],[-amp -amp],'r--')
plot([1 Ns],[-1.5*amp -1.5*amp],'k--')
set(gca,'YTick',[]);set(gca,'XTick',[])
title(['signal reel'])
hold off

pause

for k = 1:dt:Ns-2

	subplot(2,1,1)
	plot(real(sig))
	amp = 1.2*max(max(real(sig)),abs(min(real(sig))));
	axis([1 Ns-2 -2*amp amp])
	hold on
	plot([k k],[real(sig(k)) real(sig(k))],'ob')
	plot([k k],[0 real(sig(k))],'b')
	plot([1 Ns],[0 0])
	plot(abs(sig(1:k)),'r--')
	plot(-abs(sig(1:k)),'r--')
	plot(asup(1:k),'k--')
	plot(ainf(1:k),'k--')
	plot([1 Ns],[-amp -amp],'r--')
	plot([1 Ns],[-1.5*amp -1.5*amp],'k--')
	plot(20*(fi(1:k)-fi(1))-amp,'r')
	plot(20*(fio(1:k)-fio(1))-1.5*amp,'k')
	title(['signal reel'])
	set(gca,'YTick',[]);set(gca,'XTick',[])
	hold off

	subplot(2,1,2)
	zr = [zr real(sig(k+2))];
	zi = [zi imag(sig(k+2))];
	z = zr + i*zi;
	plot(z,'--')
	axis('square')
	axis([-amp amp -amp amp])
	hold on
	plot(z(max(size(z))),'or')
	plot(z(max(size(z))-1),'+r')
	plot([0 zr(max(size(z)))],[0 zi(max(size(z)))],'r')
	plot([0 zr(max(size(z))-1)],[0 zi(max(size(z))-1)],'r')
	plot([zr(max(size(z))-1) zr(max(size(z)))],[zi(max(size(z))-1) zi(max(size(z)))],'r')
	plot([-amp amp],[0 0]);
	plot([0 0],[-amp amp]);
	title('signal analytique')
	set(gca,'YTick',[]);set(gca,'XTick',[])

	zo = (xc(k) + i*yc(k)) + r(k)*exp(i*2*pi*theta);

	plot([xc(k) x(k)],[yc(k) y(k)],'+-k')
	plot(zo,'k')

	hold off

	pause(1/100)

end

