%This function performs the first stage of the UIV for in vivo flow. It
%uses a rough correlation to find the pulse frequency and the relative
%phase.

%For full details see "Ultrasound Imaging Velocimetry with interleaved
%images for improved pulsatile arterial flow measurements: a new correction
%method, experimental and in vivo validation", J Royal Soc Interface, 2017

%K H Fraser, C Poelma 2016

direction = 'negative';
expected_cycles=7;
delta=31;
lines=128;
xpix=0.038/128;
ypix=0.001*0.0202;

no1=10;
for N=[10 6:9]
    sss       = 1; % scaling factor for display
    
    plotevery = 10; % plot every n frames (speeds up the calculation a bit)
    
    
    filename= ['../data/in_vivo/UIV/aorta_128lines_31delta_20000depth7000000freq']; % specify input file (including path)
    file_in=[filename int2str(N) '.bin'];

    
    first_frame = 1; % first of sequence to process
    last_frame = 160; % last frame to process
    
    filerange = first_frame:(last_frame-1); % we correlate frame i with i+1
     
    flow_region_x=[70:100];
    flow_region_y=[300:400];
    
    %%%%% This part reads the first frame of the sequence; this frame will
    %%%%% be used to determine image size and set up a range of parameters
    %%%%% This is also very specific for different file types (avi, tif, RF)
    
    [data, properties] = readTexoRF(file_in,first_frame,last_frame);
    data=shiftdim(data,1);
    for n=1:last_frame
        da=data(:,:,n);
        da=da';
        dataFlip(:,:,n)=da;
    end
    data=dataFlip;
    clear dataFlip;
    
    [x,y,t]=size(data);
    data=reshape(data,1,x*y*t);
    data=data(490:end-(x*y-490+1)); %readTexoRF doesn't seem to work correctly and reads the data from the wrong point
    data=reshape(data,x,y,t-1);
    
    data2=data;
    clear data; data=data2; clear data2;
    
    image1sequence=[2:2:lines*2];
    image2sequence=[delta+2:2:delta+1+lines*2];
    data1=data(:,image1sequence,:);
    data2=data(:,image2sequence,:);
    
    data1_s=data1(flow_region_y,flow_region_x,:);
    data2_s=data2(flow_region_y,flow_region_x,:);
    
    I1=data1_s(:,:,1)';
     I2=data2_s(:,:,1)';
     figure; imagesc(log(1+abs(hilbert(squeeze(I1))))');
     figure; imagesc(log(1+abs(hilbert(squeeze(I2))))');
    %%%%% end of first frame input part
    
      iters = 1; % specify number of iterations
    
    for www = 1:iters % loop over iterations
        
        clear Csum              % Sum of correlations, clear for new iteration
      
        % specify details for iteration 1
        ias1 = 16;          % interrogation area size x
        ias2 = 64;          % interrogation area size y
        iastep1 = ias1/2;   % distance between int. areas x (-> overlap)
        iastep2 = ias2/2;   % distance between int. areas y (-> overlap)
        
        displace_rangex = -(ias1-1):(ias1-1); % search area, copy from int. area
        displace_rangey = -(ias2-1):(ias2-1); % for averaging, quarter rule not relevant
        
        corf = zeros(2*ias1,2*ias2); % bias correction function size
        
        for a1 = 1:size(corf,1); % create bias correction function ('pyramid', see Westerweel PhD thesis, p.63)
            for a2 = 1:size(corf,2);
                delx = a1-ias1-1;
                dely = a2-ias2-1;
                corf(a1,a2) = ((1-abs(delx)/ias1)+(1-abs(dely)/ias2))./2;
            end
        end
        
        telC = 0;               % set a counter keeping track of number of frames used for an average
        loopcount = 0;          % set a counter for visualization purposes
        
        midx = ias1 + 1 + displace_rangex; % auxilary parameter to create search area matrix
        midy = ias2 + 1 + displace_rangey;
        
        validregion = corf.*0; % create matrix with displacement that are allowable
        validregion(midx,midy) = 1; % we multiply the correlation result with this "allowable" matrix later on
        
        corf(corf<0.2) = 0.2; % avoid dividing by small numbers (= noise blow-up), bias correction most important for sub-pixel result
        
        iar1 = (-ias1/2+1):(ias1/2); % set up area range variable
        iar2 = (-ias2/2+1):(ias2/2); % same, vertical
        
        xstart = iastep1;           % generate range of interrogation locations
        ystart = iastep2;           % we cut out interrogation areas at these positions
        xend   = size(I1,1)-iastep1;
        yend   = size(I1,2)-iastep2;
        xx = xstart:iastep1:xend;   % xx and yy are vectors containing locations
        yy = ystart:iastep2:yend;   %                   while
        [yg,xg] = meshgrid(yy,xx);  % xg and yg are matrices containing locations
        
        vx = xg.*0; % alocate memory to speed-up calculation
        vy = yg.*0;
        ph1 = vx.*0;
        
        % reset/allocate correlation plane average variable:
        Csum(1:(2*ias1),1:(2*ias2),1:length(xx),1:length(yy))=0; % this is the most memory intensive parameter
        
        for zz = filerange; % the main loop over the frames to process
            
            loopcount = loopcount + 1; % counter for display purposes
            
            clc % display some info:
            disp(['iteration ' num2str(www) ' / ' num2str(iters)])
            disp(['frame: ' num2str(zz)])
            
            
            %%%%%% this part read two (consecutive) frames - again, very
            %%%%%% specific for the data type under considereation.
            %%%%%% the end result should be: two matrices (I1 and I2).
            
            I1=squeeze(data1_s(:,:,zz)');
            I2=squeeze(data2_s(:,:,zz)');
            
            I1o = I1; % store original (for comparison)
            
            
            xsize = size(I1,1);
            ysize = size(I1,2);
            
            L1 = length(iar1)*2;
            L2 = length(iar2)*2;
            
            for i = 1:length(xx) % loop over interrogation regions
                for j = 1:length(yy) % same, in vertical directions
                    
                    % check if interrogation areas are within image:
                    valx1 = (xx(i)+iar1(1))>0  & (xx(i)+iar1(end))<size(I1,1);
                    valy1 = (yy(j)+iar2(1))>0  & (yy(j)+iar2(end))<size(I1,2);
                    valx2 = (xx(i)+iar1(1))>0  & (xx(i)+iar1(end))<size(I1,1);
                    valy2 = (yy(j)+iar2(1))>0  & (yy(j)+iar2(end))<size(I1,2);
                    
                    if  I1(xg(i,j),yg(i,j))~=0 && valx1 && valy1 && valx2 && valy2; % if the areas are valid, process them:
                        flag(i,j) = 1;
                        ia1 = I1(xx(i)+iar1,yy(j)+iar2); % cut out region 1
                        ia2 = I2(xx(i)+iar1,yy(j)+iar2); % cut out region 2
                        ia1 = ia1-mean(ia1(:)); % subtract mean
                        ia2 = ia2-mean(ia2(:));
                        C = fftshift(ifftn(fftn(ia2,[L1 L2]) .* conj(fftn(ia1,[L1 L2])),[L1 L2]));
                        mac = sum(sum(ia1.*ia1)); % maximum of autocorrelation
                        C = C./mac; % normalize
                        Csum(:,:,i,j) =  C; % add it to average
                        telC = telC + 1;
                    end % if valid region
                end %j loop
            end %i loop
            
            %         if zz == filerange(end) || mod(loopcount,plotevery) ==0 % occasionally analyze data (and at end of loop_
            
            for i = 1:length(xx)
                for j = 1:length(yy)
                    
                    dumC = squeeze(Csum(:,:,i,j)); % extract correlation from 4D matrix
                    dumC = dumC./corf.*validregion; % apply bias correction and valid region matrix
                    Cmax = max(dumC(:)); % determine maximum in correlation plane
                    ph1(i,j) = Cmax; % store max peak height
                    [dx,dy] = find(dumC==Cmax); % find location of maximum
                    warning off % this is to avoid annoying errors for log of zero or negative number
                    if length(dx)==1 && dx>1 && dy>1 && dx<(2*ias1) && dy<(2*ias2) % not at edge
                        mx = dx; my = dy;
                        ln0      = log(dumC(mx,my)); % this part performs a peak fit
                        lnimin   = log(dumC(mx-1,my)); % to obtain sub-pixel accuracy
                        lnjmin   = log(dumC(mx,my-1)); % we use a 2x3 point Gaussian fit
                        lniplus  = log(dumC(mx+1,my));
                        lnjplus   = log(dumC(mx,my+1));
                        eps_x = (lnimin-lniplus)/(2*lnimin-4*ln0+2*lniplus);
                        eps_y = (lnjmin-lnjplus)/(2*lnjmin-4*ln0+2*lnjplus);
                    else
                        eps_x = ias1; eps_y = ias2; % set to extreme value so we will notice bad peak fits %changed to zero - is this the cause of the noise?
                    end
                    warning on
                    dx = dx-ias1-1+eps_x; % correct peak fit location with our sub-pixel, fit-based correction
                    dy = dy-ias2-1+eps_y;
                    
                    if imag(dx)~=0 | imag(dy)~=0 | abs(eps_x)>1 | abs(eps_y) >1 % acceptable peak fit?
                        xg(i,j) = xx(i); yg(i,j) = yy(j); vx(i,j) = 0; vy(i,j) = 0; % bad fit = NaN
                    else
                        xg(i,j) = xx(i); yg(i,j) = yy(j); vx(i,j) = dx(1);  vy(i,j) = dy(1); % good fit = store data
                    end % good fit
                    
                end % j
            end% i
            if www==iters
                xyratio=xpix*ias1/(ypix*ias2);
                vmag=(vx(:,:).^2+(vy(:,:)*xyratio).^2).^0.5;

                pos=find(isfinite(vmag));
                vmag_mean(zz)=mean(vmag(pos));
            end
            
        end  % last of filerange ? (=data processed)
        
    end % iter loop
    
    %plot the extremely rough velocity magnitude estimate in time
    figure;plot(vmag_mean);
    ts=1:length(vmag_mean);
    span=1/(5*expected_cycles);
    vmag_mean2=vmag_mean;
    span=1/(expected_cycles);
    vmag_mean3 = smooth(ts,vmag_mean,span,'rlowess');
    vmag_mean2=vmag_mean2;
    hold on; plot(vmag_mean2,'b');
    
    imfile=[filename int2str(N) '-roughVel.jpg'];
    print('-djpeg',imfile)
    close all
    if N==no1
        data1_1=data1;
        data2_1=data2;
        phase2_1=0;
        vmag_mean2_1=vmag_mean2;
        savefile=[filename int2str(no1)];
        save(savefile,'data1_1','data2_1','vmag_mean2_1','phase2_1');
        allperiod(N)=0;
    else
        filename1= [filename int2str(no1)];
        load(filename1);
        signal_xcorr=xcorr(vmag_mean2_1,vmag_mean2);
        figure; plot(signal_xcorr);
        alpha=round(length(signal_xcorr)/3);
        beta=round(2*length(signal_xcorr)/3);
        max_xcorr=max(signal_xcorr(alpha:beta));
        phase2=find(signal_xcorr(alpha:beta)==max_xcorr);
        phase2=phase2+alpha;
        if length(phase2>1)
            phase2=phase2(1);
        end
        phase2=round(phase2-(length(signal_xcorr))/2);
        fft_xcorr=fft(signal_xcorr);
        max_fft_xcorr=max(real(fft_xcorr(5:30)).^2);
        frequency2=find((real(fft_xcorr(5:30)).^2)==max_fft_xcorr)+3;
        frequency2=frequency2/2;
        period2=length(filerange)/frequency2;
        
        allperiod(N)=period2;
        if phase2 >=0
            figure;plot([zeros(1,round(phase2)) vmag_mean2]);
            hold on; plot(vmag_mean2_1,'r');
            imfile=[ filename int2str(N) '-correlated.jpg'];
            print('-djpeg',imfile);
        else
            figure;plot([zeros(1,round(-1*phase2)) vmag_mean2_1],'r');
            hold on; plot(vmag_mean2);
            imfile=[ filename int2str(N) '-correlated.jpg'];
            print('-djpeg',imfile);
        end
        
        savefile=[filename int2str(N)];
        allperiod(N)=period2;
        save(savefile,'data1','data2','vmag_mean2','phase2','period2','allperiod');
    end
    
    
       clear data data1 data2 da
end