% multi_freq_ddm_int tests..
N = 2^10-1;
N_fft = 2^12-1;

fs = 44100;

dsc_idxs = ([-(N-1)/2:(N-1)/2]);

frqs_fft = [0:N_fft-1]*fs/N_fft;

the_frq = 416;
the_fm1 = 7000;
the_ph = 0.2;
the_amp = 0.1;
pk_offst_rt = 0.01;
%frqs = [438,442.5] * 2 *pi; % as many frequencies as desired...
fft_bin_hz = fs/N_fft;
t = [-(N-1)/2:(N-1)/2]/fs;
% monomials matlab style...
ply_len = 2;
mdl_poly = flipud(diag(ones(ply_len+1,1))); % vector of polynomials
mdl_poly_der = zeros(size(mdl_poly));% vector of time derivated polynomials

% the actual time domain values of time derivated polynomials
mdl_poly_der_vals = zeros(N,size(mdl_poly,1)); 

% window and time derivated window...
w  = 0.5+0.5*cos(2*pi*dsc_idxs/(N-1));
w_d = -pi*fs/(N-1)*sin(2*pi*dsc_idxs/(N-1));
w = w(:);
w_d = w_d(:);

% calculate time domain plys...
for k = 1:size(mdl_poly,1)
  pp = polyder(mdl_poly(k,:));
  mdl_poly_der(k,:) = [zeros(1,ply_len+1-size(pp,2)) pp ];
  mdl_poly_der_vals(:,k) = polyval( mdl_poly_der(k,:),t);
end

mdl_poly_der_vals = mdl_poly_der_vals(:,2:end);

sig_r = real(exp(the_amp+1j*(the_fm1*t.^2 + t*the_frq*2*pi + the_ph*pi))).';
%sig = hilbert(sig_r);
sig = sig_r;

[id_s id_e ] = zpzh_idxs(N);
sig_zp = sig .* w;
sig_zp = [sig_zp(id_s);zeros(N_fft-N,1);sig_zp(id_e)];

sig_fft = fft(sig_zp, N_fft);
[pk_vl,pk_idx] = max(abs(sig_fft));
pk_frq_hz = (pk_idx - 1) * fs / N_fft;
%pk_offst_rt = (fft_bin_hz/the_frq);
%frqs_test = [pk_frq_hz*(1 - pk_offst_rt),pk_frq_hz, pk_frq_hz*(1 + pk_offst_rt)] * 2 *pi; % as many frequencies as desired...
frqs_test = [pk_idx - 2, pk_idx - 1, pk_idx ] * 2 * pi * fs / N_fft; 
%frqs_test = [pk_idx - 0.5, pk_idx - 1.5, pk_idx ] * 2 * pi * fs / N_fft; 


[fks fks_drs] = fourier_kernel_win(frqs_test, N, fs, w, w_d);
[gdi, A, b,gdi_lns,r_lns] = ...
  gen_ddm_int(fks, fks_drs, mdl_poly_der_vals, sig, N, 0);

errs = A(:,1) - sig_fft(pk_idx-1:pk_idx+1);
assert( sum(errs < (1 + 1j) * 10^(-11)) == 3);

%gdi = gdi_lns;

err_rel_frq_log = log10(abs((imag(gdi(1)) / 2 / pi - the_frq)/(imag(gdi(1)) / 2 / pi) ));
err_rel_fm1_log = log10(abs((imag(gdi(2)) - the_fm1) / imag(gdi(2)) ));
assert(err_rel_frq_log < -3);
assert(err_rel_fm1_log < -2);
real(gdi(1)) 
real(gdi(2))
ply_est = fliplr(gdi.');
frq_est = imag(ply_est(end));
the_fm1
imag(ply_est(end-1))
frq_est/2/pi
%ply_est(end) = real(ply_est(end)) + 1j*(imag(ply_est(end)) - the_frq);
ply_est(end) = real(ply_est(end)); % hm....
wmp = win_mod_poly(N, t, [ply_est 0], w);

% this creates a bias but it's fast...

krnl_dmd = exp(1j * frq_est * t');
dtft_dmd = sum(w .* sig .* conj(krnl_dmd),1);
r0 = dtft_dmd / wmp;
err_rel_amp_log = log10(abs((abs(r0)*2 - exp(the_amp)) / exp(the_amp)) )
err_rel_phs_log = log10(abs((angle(r0) / pi - the_ph)/the_ph));

assert(err_rel_amp_log  < -2);
sprintf('%15.15f', (angle(r0) / pi-the_ph) )





%% plots
figure(3);clf;
subplot(211);
hold on;
plot(frqs_fft,abs(sig_fft),'.k-');
plot(frqs_test'/2/pi,abs(A(:,1)),'r*');
subplot(212);
hold on
plot(frqs_fft,angle(sig_fft),'.k-');
plot(frqs_test'/2/pi,angle(A(:,1)),'r*');

display('tests completed successfully!');
display('accuracy:');
display([ '     frq: ' num2str(err_rel_frq_log)]);
display([ '     amp: ' num2str(err_rel_amp_log)]);
display([ '     fm1: ' num2str(err_rel_fm1_log)]);
display([ '     phs: ' num2str(err_rel_phs_log)]);
display([ '     rcond: ' num2str(r_lns)]);

 
