%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% evaluate frequency response of smoothing spline filter

f_cut				= 0.2;
lambda			= 1 / (2*pi*f_cut)^6;
n					= 500;
nf					= 2*n;
N					= 2*n+1;
font_size		= 28;

%% define function
% unit spacing, makes Nyquist frequency 0.5, simples
x					= -n : n;
xf					= linspace(-n, n, nf);
dxf				= xf(2) - xf(1);
% impulse response
y_imp				= zeros(1,N);
y_imp(n+1)		= 1;
% step function to get velocity impulse response
y_step			= (sign(x)+1)/2;
y1_step			= zeros(1,N);
y1_step(n+1)	= 1;
% ramp function to get acceleration impulse response
y_ramp			= x;
y_ramp(x<0)		= 0;
y1_ramp			= y_step;
y2_ramp			= y1_step;

%% fit spline
[f_imp , f1_imp , f2_imp , f3_imp ] = sspl(x,y_imp ,xf,lambda);
[f_step, f1_step, f2_step, f3_step]	= sspl(x,y_step,xf,lambda);
[f_ramp, f1_ramp, f2_ramp, f3_ramp] = sspl(x,y_ramp,xf,lambda);

%% get transfer function for velocity and accn from step and ramp response
freq				= [0:nf/2-1, -nf/2:-1] / (nf*dxf);
H_pos				= abs(fft(f_imp)) * dxf;   % FFT of impulse response
H_vel				= abs(fft(f1_step)) * dxf; % FFT of velocity impulse response
H_acc				= abs(fft(f2_ramp)) * dxf; % FFT of acceleration impulse response

freq				= fftshift(freq);
H_pos				= fftshift(H_pos);
H_vel				= fftshift(H_vel);
H_acc				= fftshift(H_acc);

H_butter			= sqrt(1 ./ (1 + (freq/f_cut).^12)) .*  sinc((freq)).^0;
H_spline			= sinc((freq)).^-2;

%% determine equivalent noise bandwidth
r					= freq >= 0;
%f_enbw			= trapz(freq(r), H_pos(r).^2);
f_enbw			= trapz(xf,f_imp.^2) / trapz(xf,f_imp)^2 / 2;

fprintf('f_cut = %0.3f\n', f_cut);
fprintf('f_enbw= %0.3f\n', f_enbw);
fprintf('%0.8f\t%0.4f\t%0.4f\n', lambda, f_cut, f_enbw);

%% plot impulse response
figure(1);
clf;
plot(x, y_imp, 'k.');
hold on;
plot(xf, f_imp, 'k-');
plot(xf, f1_step, 'b-');
plot(xf, f2_ramp, 'r-');

title('Impulse response');

%% plot step response
figure(2);
clf;
subplot(2,1,1);
plot(x, y_step, 'k.');
hold on;
plot(xf, f_step, 'k-');
title('Step response');

subplot(2,1,2);
plot(x, y1_step, 'k.');
hold on;
plot(xf, f1_step, 'k-');
title('First derivative of step response');

%% plot ramp response
figure(3);
clf;
subplot(2,2,1);
plot(x, y_ramp, 'k.');
hold on;
plot(xf, f_ramp, 'k-');
title('Ramp response');

subplot(2,2,2);
plot(x, y1_ramp, 'k.');
hold on;
plot(xf, f1_ramp, 'k-');
title('First derivative of ramp response');

subplot(2,2,3:4);
plot(x, y2_ramp, 'k.');
hold on;
plot(xf, f2_ramp, 'k-');
title('Second derivative of ramp response');

%% plot transfer function
figure(4);
clf;
loglog(	freq(freq>0)*2, H_pos(freq>0), 'k-', ...
			freq(freq>0)*2, H_vel(freq>0), 'k:', ...
			freq(freq>0)*2, H_acc(freq>0), 'k--');
hold on;
loglog(	freq(freq>0)*2, H_butter(freq>0), 'r-');

loglog(2*f_cut*[1 1], [1E-5 1E5], 'r--');
legstr= sprintf('$f_{c} = %0.2f$', f_cut);
L		= legend(legstr, 'Velocity', 'Acceleration');
set(L,'interpreter','latex','fontsize',font_size);


title('Frequency response of smoothing spline filter', 'interpreter', 'latex', 'fontsize', font_size);
xlabel('Frequency $\omega / \omega_{nyq}$', 'interpreter', 'latex', 'fontsize', font_size);
ylabel('Response $|H(j\omega)|$', 'interpreter', 'latex', 'fontsize', font_size);
set(gca, 'fontsize', font_size);
plotedit on;

xlim([1E-2 100]);
ylim([1E-10 1E2]);
grid on;

%set(gca,'yscale','linear');
%ylim([0 1]);
