%function notch

data = dataStruct.data;
t = dataStruct.timeVec;
t = t/1000;
Fs = 1/t(2);            % sampling frequency

fn = 5000;   % cutoff frequency
Q = 1/sqrt(2);
wn = 2*pi*fn;
tau  = 1/wn;
tau1 = tau/Q;
tau2 = tau*Q;
x = tf('s');
LPF = 1 / (tau^2*x^2 + tau1*x + 1);

for channel = 1:64
    s = data(:,channel);
    s = s - mean(s);
    S = fft(s);

    Smag = abs(S);
    Slen = length(S);
    Smag = Smag(2:floor(Slen/2)+1);
    f = (1:length(Smag))*Fs/length(S);

    Snew = S;
    filterBW = 20;
    fbinwidth = Fs/length(S);
    peaks = find( Smag > 11 );
    disp( f(peaks) );
    for fNull = f(peaks)
        [closest_f,closest_i] = min( abs(f-fNull) );
        delta_i = ceil( filterBW / fbinwidth );
    
        for middle_i = [ closest_i+1 length(Snew)-closest_i+1 ]
            atten = abs(Snew(middle_i));
            for i = middle_i - delta_i : middle_i + delta_i
                if ( i < 1 || i > length(Snew) )
                    continue;
                end
    %            Snew(i) = Snew(i) / (1 + atten*exp(-(i-middle_i)^2/delta_i));
    %            Snew(i) = Snew(i) / (1 + atten/(1+abs(i-middle_i)));
                Snew(i) = Snew(i) / (1 + atten/(0.1+abs(i-middle_i)^3));
            end
        end
    end

    Smag = abs(Snew);
    Slen = length(Snew);
    Smag = Smag(2:floor(Slen/2)+1);
    is = real(ifft(Snew));
    z_data(:,channel) = lsim( LPF, is, t );
    disp( sprintf( 'Channel %d done.', channel ) );
end

save( 'z_data', 'z_data', 't', 'Fs' );
return;