clear all
clc


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%%%%%%%%%%% Signal with Adjustable Filter %%%%%%%%%%%%

%filtering

%%% Note: Assume Sampling Freq (Fs)= 40 MHz --> Period (Ts)= 1/Fs = 0.025 µs
%%% Total Time = N x Ts = 2000 x 0.025 = 50 µs
%%% N=2000--->ToF=50µs
%%% According to Younsu config, Cutoff Freq (Fc)= 318 KHz


ToF_mean_storage = [];

for dataset=1:30
    
    if (dataset <= 9)
        load(strcat('0',num2str(dataset)))
        
        ToF = [];
        
        for i=1:size(paSig,2)
            sample = paSig(:,i);
            %     figure
            %     plot(sample)
            
            [b,a] = butter(8,0.15, 'low');
            %     freqz(b,a)
            dataOut = filter(b,a,sample);
            %     figure
            %     plot(dataOut)
            
            [maximum,index] = max(abs(hilbert(dataOut)));
            ToF_temp = index*52/2080;
            ToF = [ToF; ToF_temp];
        end
        ToF_mean = mean(ToF);
        ToF_mean_storage = [ToF_mean_storage; ToF_mean];
        
        figure
        plot(ToF)
        axis([0 size(paSig,2) 0 30])
        title('Time of Flight from Signal')
        xlabel('Frame')
        ylabel('Time of Flight (µs)')
        
    else
        load(num2str(dataset))
        
        ToF = [];
        
        for i=1:size(paSig,2)
            sample = paSig(:,i);
            %     figure
            %     plot(sample)
            
            [b,a] = butter(8,0.15, 'low');
            %     freqz(b,a)
            dataOut = filter(b,a,sample);
            %     figure
            %     plot(dataOut)
            
            [maximum,index] = max(abs(hilbert(dataOut)));
            ToF_temp = index*52/2080;
            ToF = [ToF; ToF_temp];
        end
        ToF_mean = mean(ToF);
        ToF_mean_storage = [ToF_mean_storage; ToF_mean];
        
        figure
        plot(ToF)
        axis([0 size(paSig,2) 0 30])
        title('Time of Flight from Signal')
        xlabel('Frame')
        ylabel('Time of Flight (µs)')
    end
end