function [t,Q1,Q2,Q3,Q1_d,Q2_d,Q3_d,Q1_dd,Q2_dd,Q3_dd]=fftfilter_Hamed(Signal,filt_f0,filt_f1)

% clc
% clear
% close all

%% Constants

df0=0.03;                              % Frequency resolution in FFT 
MaxFr0=50;                              % Max Freq that we are interested in; this is used in down sampling, so you can neglect it for now

%% Load signals
% load('torque.mat')                      
      % Don't use the begininig of the signals; use the steady state part of it. I started from 20000 sample
q1=Signal(1,:);
q2=Signal(2,:);
q3=Signal(3,:);

Fs=250;    MaxFr=Fs/2;     N_fft=2^(nextpow2(MaxFr*2/df0)-1);        %% determine N_fft that will give you df0 resolution
dt=1/Fs;


%% Down sample (You can neglect this for now; In the future we can down sample the data, if we need to)
    tmp=fix(N_fft*MaxFr0/Fs);
    Ndec=1;%N_fft/2/tmp;
    
    q1f = fftfilt1(q1, N_fft, dt, 'decimate', Ndec);
    q2f = fftfilt1(q2, N_fft, dt, 'decimate', Ndec);
    q3f = fftfilt1(q3, N_fft, dt, 'decimate', Ndec);
    dt=dt*Ndec;

Fs=1/dt;    MaxFr=Fs/2;     N_fft=2^(nextpow2(MaxFr*2/df0)-1);        %%update sampling frequency and determine N_fft

%% Power Spectral Density (PSD) of Signals 

[psd_q1,f]=pwelch(q1f,N_fft,N_fft/2,N_fft,Fs);      %pwelch(signal,segment length,overlap length,N_fft,Fs) breaks the signal into segments with 50% overlab and applies hanning window 
[psd_q2,f]=pwelch(q2f,N_fft,N_fft/2,N_fft,Fs);
[psd_q3,f]=pwelch(q3f,N_fft,N_fft/2,N_fft,Fs);

figure(1)
subplot(3,1,1)                                      % plot PSD of signals
hold on
plot(f,10*log10(psd_q1))

subplot(3,1,2)
hold on
plot(f,10*log10(psd_q2))

subplot(3,1,3)
hold on
plot(f,10*log10(psd_q3))
 

%%  lowpass tapering from 0.5 to 0.6 Hz


q1f = fftfilt1(q1f, N_fft, dt, 'lowpass', [filt_f0, filt_f1]);
q2f = fftfilt1(q2f, N_fft, dt, 'lowpass', [filt_f0, filt_f1]);
q3f = fftfilt1(q3f, N_fft, dt, 'lowpass', [filt_f0, filt_f1]);

[psd_q1,f]=pwelch(q1f,N_fft,N_fft/2,N_fft,Fs);          % plot PSD of low passed signals
[psd_q2,f]=pwelch(q2f,N_fft,N_fft/2,N_fft,Fs);
[psd_q3,f]=pwelch(q3f,N_fft,N_fft/2,N_fft,Fs);

subplot(3,1,1)
hold on
plot(f,10*log10(psd_q1))
title('angle of joints 1')
legend('no filter', 'low pass')
xlabel('Frequency [Hz]'); ylabel('q_1[dB]')

subplot(3,1,2)
hold on
plot(f,10*log10(psd_q2))
title('angle of joints 2')
legend('no filter', 'low pass')
xlabel('Frequency [Hz]'); ylabel('q_2[dB]')

subplot(3,1,3)
hold on
plot(f,10*log10(psd_q3))
title('angle of joints 3')
legend('no filter', 'low pass')
xlabel('Frequency [Hz]'); ylabel('q_3[dB]')

Q1=q1f;
Q2=q2f;
Q3=q3f;
t=(0:dt:(length(q1f)-1)*dt)';

%% Differentiate signals using FFT filter


qd = fftint1([q1f;q2f;q3f], Fs, 0, Fs/2, 1, -1);     %fftint1(matrix of signals, Fs, low frequency of bandpass filter, high freq of band pass filter, coefficient of hanning window, -1 means differentiate once);
                                                     % Note that because we
                                                     % already applied low
                                                     % pass on the signal,
                                                     % we don't apply
                                                     % band-pass here
                                                     
[psd_q1d,f]=pwelch(qd(1,:),N_fft,N_fft/2,N_fft,Fs); % PSD of differentiated signals
[psd_q2d,f]=pwelch(qd(2,:),N_fft,N_fft/2,N_fft,Fs);
[psd_q3d,f]=pwelch(qd(3,:),N_fft,N_fft/2,N_fft,Fs);

figure (2)
subplot(3,1,1)
hold on
plot(f,10*log10(psd_q1d))
title('Velocity of joints 1')
xlabel('Frequency [Hz]'); ylabel('q_1^.[dB]')

subplot(3,1,2)
hold on
plot(f,10*log10(psd_q2d))
title('Velocity of joints 2')
xlabel('Frequency [Hz]'); ylabel('q_2^.[dB]')

subplot(3,1,3)
hold on
plot(f,10*log10(psd_q3d))
title('Velocity of joints 3')
xlabel('Frequency [Hz]'); ylabel('q_3^.[dB]')

Q1_d=qd(1,:);
Q2_d=qd(2,:);
Q3_d=qd(3,:);


qdd = fftint1(qd, Fs, 0, Fs/2, 1, -1);     %fftint1(matrix of signals, Fs, low frequency of bandpass filter, high freq of band pass filter, coefficient of hanning window, -1 means differentiate once);
                                                     % Note that because we
                                                     % already applied low
                                                     % pass on the signal,
                                                     % we don't apply
                                                     % band-pass here
                                                     
[psd_q1dd,f]=pwelch(qdd(1,:),N_fft,N_fft/2,N_fft,Fs); % PSD of differentiated signals
[psd_q2dd,f]=pwelch(qdd(2,:),N_fft,N_fft/2,N_fft,Fs);
[psd_q3dd,f]=pwelch(qdd(3,:),N_fft,N_fft/2,N_fft,Fs);

figure (3)
subplot(3,1,1)
hold on
plot(f,10*log10(psd_q1dd))
title('Acceleration of joints 1')
xlabel('Frequency [Hz]'); ylabel('q_1^.^.[dB]')

subplot(3,1,2)
hold on
plot(f,10*log10(psd_q2dd))
title('Acceleration of joints 2')
xlabel('Frequency [Hz]'); ylabel('q_2^.^.[dB]')

subplot(3,1,3)
hold on
plot(f,10*log10(psd_q3dd))
title('Acceleration of joints 3')
xlabel('Frequency [Hz]'); ylabel('q_3^.^.[dB]')

Q1_dd=qdd(1,:);
Q2_dd=qdd(2,:);
Q3_dd=qdd(3,:);
