%% User settings
clear all;
close all;
data = load('buffera12.txt');
Fsamp = 20e3; %Sampling frequency in Hz
filterByZeroMult = true; 

%% Load Data
yRaw = data(:,2).';
NRaw = length(yRaw);
dt = 1/Fsamp;
tRaw = dt*(0:NRaw-1);
yRaw(NRaw/2:end) = yRaw(NRaw/2:end) + 1e-6;

NRaw = length(yRaw);
figure(1);
plot(tRaw,yRaw,'k');
set(gcf, 'Position', [1091 536 371 311]) 
title('yRaw');
xlabel('Time (s)'); 
ylabel('Voltage (V)');

%% Remove linear bias for better linear filtering
coeffs = polyfit(tRaw,yRaw,1);
linBias = (coeffs(1)*tRaw + coeffs(2));
yPreFilt = yRaw - linBias;

%% Compute FFT
Y = fft(yPreFilt);
dF = Fsamp/NRaw;
f = dF*(0:NRaw-1);

%% Plot data frequency content
fNyq = Fsamp/2;

figure(2);

loglog(f,2*abs(31000*Y).^2/NRaw/Fsamp);
xlim([0,fNyq]);
xlabel('Freqency (Hz)');
ylabel('FFT Power Spec.');
set(gcf, 'Position', [1091 536 371 311]) 


%% Remove 2kHz Dither
Y_posF = Y(1:NRaw/2);
Y_negF = fliplr(Y(NRaw/2+1:end));
f_subNyq = dF*(0:NRaw/2-1);

%Filter using low-pass:
     cutoff = 1e3; %Hz
    sharpness = 100;
    if filterByZeroMult
        filterFunc = ones(1,NRaw/2);
        filterFunc(f_subNyq > cutoff) = 0;
    else
        filterFunc = (1 + exp(sharpness*(f_subNyq/cutoff - 1))).^(-1);
    end

    Y_posF = Y_posF.*filterFunc;
    Y_negF = Y_negF.*filterFunc;

Y_dithFilt = [Y_posF fliplr(Y_negF)]; 

figure(2);
subplot(311); hold on;
semilogy(f,abs(Y_dithFilt).^2/NRaw,'g'); hold off;
ylim([1e-15,1]);

subplot(312); hold on;
plot(f,unwrap(angle(Y_dithFilt)),'g'); hold off;

subplot(313); hold on;
semilogy(abs(Y_dithFilt).^2/NRaw,'g'); hold off;

y_dithFilt = real(ifft(Y_dithFilt));

figure(1);
hold on;
plot(tRaw,y_dithFilt+linBias,'g'); hold off;
set(gcf, 'Position', [1091 536 371   311]) 

%% Remove line noise
Y_posF = Y_dithFilt(1:NRaw/2);
Y_negF = fliplr(Y_dithFilt(NRaw/2+1:end));
f_subNyq = dF*(0:NRaw/2-1);

lineCenters = 60:60:fNyq; %Hz
lineBW = 8; %Radius to remove line noise at each center (Hz); (used 6 for exosomes)

numLineCenters = length(lineCenters);
for q=1:numLineCenters
    if filterByZeroMult
        ampPosF = 0;
        phPosF = 1;
        ampNegF = 0;
        phNegF = 1;
    else
        ampPosF = mean(abs(Y_posF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
        phPosF = mean(angle(Y_posF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
    
        ampNegF = median(abs(Y_negF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
        phNegF = mean(angle(Y_negF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
    end
    
    Y_posF( abs(f_subNyq-lineCenters(q)) < lineBW ) = ...
        ampPosF * exp(1i*phPosF);
    Y_negF( abs(f_subNyq-lineCenters(q)) < lineBW ) = ...
        ampNegF * exp(1i*phNegF);          
end

Y_lineFilt = [Y_posF fliplr(Y_negF)]; 

figure(2);
subplot(311); hold on;
semilogy(f,abs(Y_lineFilt).^2/NRaw,'r'); hold off;

subplot(312); hold on;
plot(f,unwrap(angle(Y_lineFilt)),'r'); hold off;

subplot(313); hold on;
semilogy(abs(Y_lineFilt).^2/NRaw,'r'); hold off;

y_lineFilt = real(ifft(Y_lineFilt));

figure(1);
hold on;
plot(tRaw,y_lineFilt+linBias,'r'); hold off;

%% Remove 100 Hz multiples noise
Y_posF = Y_lineFilt(1:NRaw/2);
Y_negF = fliplr(Y_lineFilt(NRaw/2+1:end));
f_subNyq = dF*(0:NRaw/2-1);

lineCenters = 100:100:fNyq; %Hz
lineBW = 6; %Radius to remove line noise at each center (Hz);

numLineCenters = length(lineCenters);
for q=1:numLineCenters
    if filterByZeroMult
        ampPosF = 0;
        phPosF = 1;
        ampNegF = 0;
        phNegF = 1;
    else
        ampPosF = mean(abs(Y_posF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
        phPosF = mean(angle(Y_posF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
    
        ampNegF = median(abs(Y_negF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
        phNegF = mean(angle(Y_negF( (abs(f_subNyq-lineCenters(q)) < 2*lineBW) & ...
            (abs(f_subNyq-lineCenters(q)) > lineBW) )));
    end
    Y_posF( abs(f_subNyq-lineCenters(q)) < lineBW ) = ...
        ampPosF * exp(1i*phPosF); 
    Y_negF( abs(f_subNyq-lineCenters(q)) < lineBW ) = ...
        ampNegF * exp(1i*phNegF);          
end

Y_100Filt = [Y_posF fliplr(Y_negF)]; 

figure(2);
subplot(311); hold on;
semilogy(f,abs(Y_100Filt).^2/NRaw,'c'); hold off;

subplot(312); hold on;
plot(f,unwrap(angle(Y_100Filt)),'c'); hold off;

subplot(313); hold on;
semilogy(abs(Y_100Filt).^2/NRaw,'c'); hold off;

y_100Filt = real(ifft(Y_100Filt));

figure(1);
hold on;
plot(tRaw,y_100Filt+linBias,'c'); hold off;
Y_allFilt = [Y_posF fliplr(Y_negF)]; 

%% Plot final filtered data:
figure(2);
loglog(f,2*abs(31000*Y_allFilt).^2/NRaw/Fsamp,'m'); hold off;
xlabel('Frequency (Hz)')
ylabel('fm^2\times s')

y_allFilt = real(ifft(Y_allFilt));

figure(1);
hold on;
plot(tRaw,y_allFilt+linBias,'m');
hold off;

legend('Raw data','2kHz Removed','60Hz Removed','100Hz Removed','Final filtered')

meanCenter = mean(y_allFilt+linBias);
yUnshift_fm  = 31000*(y_allFilt+linBias-meanCenter);
figure(3);
plot(tRaw,yUnshift_fm,'k');
xlabel('Time (s)');
ylabel('Shift (fm)');
yUnshift2=yUnshift_fm';

%% Median Filter:
    %mfSizes = [5,11,101,1001];
    mfSizes = 1001;
    mfColors = ['b','r','c','m','y','b'];
    numMFs = length(mfSizes);
    y_mf = zeros(numMFs,NRaw);
    for m = 1:numMFs
        if mfSizes(m) <= 1001
            
            %y_mf(m,:) = medfilt1(y_allFilt,mfSizes(m)); %not letting the linear bias be added back in 
            y_mf(m,:) = medfilt1(y_allFilt+linBias,mfSizes(m));
        else
            %y_mf(m,:) = medfilt1(y_allFilt,mfSizes(m),NRaw/100); %not letting the linear bias be added back in
            y_mf(m,:) = medfilt1(y_allFilt+linBias,mfSizes(m),NRaw/100);
        end
    end

figure(3); hold on;
for m=1:numMFs
    if m == numMFs
        linwid = 1;
    else
        linwid = 1;
    end
    plot(tRaw,31000*(y_mf(m,:)-meanCenter),mfColors(m),'LineWidth',linwid);

end
hold off;
fs=12;
set(gca, 'Fontsize', fs')
xlabel('Time (s)');
ylabel('Resonance shift (fm)');
box on;
set(gcf, 'Position', [1091 536 371   311]) 

%% Write output:

timeInterval = [4.75, 5.25]; %s
keeps = 1; %keep every nth point

fileNames = cell(1,numMFs+1);
for m=1:numMFs
    if m==0 %Save the version with just linear filtering (no Median)
        yOut = 31000*(y_allFilt+linBias-meanCenter);
        fileName = 'FilteredNoMF.txt';
    else
      yOut = 31000*(y_mf(m,:)-meanCenter);
        fileName = ['FilteredMF' num2str(mfSizes(m)) '.txt'];
    end
    
    yOut = yOut(1:keeps:end);
    tOut = tRaw(1:keeps:end);
    
    M = [];
    M(:,1)= tOut((tOut >= timeInterval(1)) & (tOut <= timeInterval(2)))';
    M(:,2)=yOut((tOut >= timeInterval(1)) & (tOut <= timeInterval(2)))';
    dlmwrite(fileName, M, 'delimiter', '\t', 'precision', 6);
    
    fileNames{m+1} = fileName;
end

%% Do steps finder (This requires code from the authors of Nature 442, 709-712 (10 August 2006))
data = cell(1,numMFs);
indexes = cell(1,numMFs);
lijst = cell(1,numMFs);
properties = cell(1,numMFs);
initval = cell(1,numMFs);
for m=1:numMFs
    figure(4);
    [data{m+1}, indexes{m+1},lijst{m+1},properties{m+1},initval{m+1}, judystepnumber]=...
        Steps_Find(fileNames{m+1});
    fprintf('Done step pre-scanning for filter %d of %d.\n',m,numMFs);
end

%% Do second part of steps finder (steps evaluate) (This requires code from the authors of Nature 442, 709-712 (10 August 2006))
%stepNumbers=1000;
stepNumbers = judystepnumber;
numStepNums = length(stepNumbers);
stepInfo = cell(numMFs+1,numStepNums);
for m = 1:numMFs
    for s = 1:numStepNums
        figure(); %5
        stepInfo{m+1,s}=Steps_Evaluate(data{m+1},indexes{m+1},lijst{m+1},...
            properties{m+1},initval{m+1},stepNumbers(s));
        fprintf('Done with step set %d of %d.\n',s,numStepNums);
    end
    fprintf('Done step finding for filter %d of %d.\n',m,numMFs);
end

%% Plot results
binCenters = -10 : .1 : 10; %units of am
numMFs=1;
numStepNums
for m = 1:numMFs
    figNum = 50+m;
    figure(figNum);
    for s = 1:numStepNums
        stepAmplitudes = stepInfo{m+1,s}(5,:);
        [muhat,sigmahat] = normfit(stepAmplitudes);
        xtest = linspace(binCenters(1),binCenters(end),1000);
        ytest = length(stepAmplitudes)*(binCenters(2)-binCenters(1))/...
        sigmahat/sqrt(2*pi) * exp(-(xtest-muhat).^2./(2*sigmahat^2));
        hist(1e3*stepAmplitudes,binCenters);
        h = findobj(gca,'Type','patch');
        set(h,'FaceColor',[0 0 .4],'EdgeColor',[black])
        set(gca, 'TickDir', 'out')
        set(gcf, 'Position', [1091 536 371   311]) 
        xlabel('Step amplitude (am)');
        ylabel('Counts')
        xlim([0.9*binCenters(1),1.1*binCenters(end)]);
             end
end