clear all
clc


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%%%%%%%%%%% Experiment on Heart Tissue %%%%%%%%%%%%

%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 = [];
ToF_mean_array = [];
ind_array = [];

for dataset=1:15
    
    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.10, 'low');
            %     freqz(b,a)
            dataOut = filter(b,a,sample);
            %     figure
            %     plot(dataOut)
            
            [maximum,index] = max(abs(hilbert(dataOut)));
            ToF_temp = index*0.025;
            ToF = [ToF; ToF_temp];
        end
        ToF_mean = mean(ToF);
%         ToF_mean_storage = [ToF_mean_storage; ToF_mean];
        ToF_mean_array = [ToF_mean_array ToF_mean];
        
        ind = ToF_mean/0.025;
        ind_array = [ind_array ind];
        
        %%% Add constant time 6 µs %%%
        NewToF = ToF+6;
        
        figure
        plot(NewToF)
        axis([0 size(paSig,2) 5 35])
        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.10, 'low');
            %     freqz(b,a)
            dataOut = filter(b,a,sample);
            %     figure
            %     plot(dataOut)
            
            [maximum,index] = max(abs(hilbert(dataOut)));
            ToF_temp = index*0.025;
            ToF = [ToF; ToF_temp];
        end
        ToF_mean = mean(ToF);
%         ToF_mean_storage = [ToF_mean_storage; ToF_mean];
        ToF_mean_array = [ToF_mean_array ToF_mean];
        
        ind = ToF_mean/0.025;
        ind_array = [ind_array ind];
        
        %%% Add constant time 6 µs %%%
        NewToF = ToF+6;
        
        figure
        plot(NewToF)
        axis([0 size(paSig,2) 5 35])
        title('Time of Flight from Signal')
        xlabel('Frame')
        ylabel('Time of Flight (µs)')
    end
end