%This function performs the second stage of the UIV process for pulsatile
%flow. An iterative, correlation averaging method is used. The code is for
%images which are NOT 'interlaced'.

%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


function pulsepiv_phantom_f(depth, frames,filename)
pos=[];
allT=[];
arb=0; %an arbitrary phase for making the cycle look nice
xpix=0.038/128;
ypix=0.001*0.0202;
sss       = 1; % scaling factor for display

plotevery = 1; % plot every n frames (speeds up the calculation a bit)

m=1;
for n=[1:10]
    infiles{m}=[filename int2str(n)]
    m=m+1;
end

file_in = infiles{10}; % specify input file (including path)
load(file_in);

lines=128;
timestep=(lines)*(2*depth/1540+23.488e-6); %in s

D=mean(allperiod(2:10));
E=std(allperiod(2:10));
A=allperiod(2:10)>(D-E);
B=allperiod(2:10)<(D+E);
C=A+B;
pos=find(C==2);
period1=mean(allperiod(pos+1));

split=50;
file_in = infiles{1}; % specify input file (including path)
load(file_in);
first_frame = 1; % first of sequence to process
last_frame = frames; % last frame to process

if file_in==infiles{1};
    data=data_1;
    phase2=phase2_1;
end

xrange=1:128;
yrange=70:1040;
data=data(yrange,xrange,:);
I1=data(:,:,1)';
filerange = first_frame:(last_frame-2); % we correlate frame i with i+1
 
phase=phase2;

iters = 4; % specify number of iterations
[a,b]=size(I1);
Iavg=zeros(a,b,split);
tel1=zeros(split,1);

for www = 1:iters % loop over iterations
    
    clear Csum              % Sum of correlations, clear for new iteration
    
    if www==1               % specify details for iteration 1
        ias1 = 32;          % 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)
    elseif www==2           %
        ias1 = 16;          % same for step 2, remember to change 'iters'!
        ias2 = 32;
        iastep1 = ias1/2;
        iastep2 = ias2/2;
    elseif www==3
        ias1 = 8;
        ias2 = 16;
        iastep1 = ias1/2;
        iastep2 = ias2/2;
    elseif www==4           % extend if needed for more iterations
        ias1 = 4;          % only works if iters is set correctly
        ias2 = 8;
        iastep1 = ias1/2;
        iastep2 = ias2/2;
    end
    
    %    if www==1    % use this section to limit the search area
    %        displace_rangex = -15:15;  % initial search area
    %        displace_rangey = -15:15;  % must be SMALLER than interrogation area
    %    else
    %        displace_rangex = -5:5;  % note: second iterations use a pre-shift
    %        displace_rangey = -5:5;  % so these ranges are with respect to previous result
    %    end
    
    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
    xg1=xg; yg1=yg;
    for n=1:split; xg(:,:,n)=xg1; yg(:,:,n)=yg1; end
    clear xg1 yg1;
    
    vx = xg.*0; % alocate memory to speed-up calculation
    vy = yg.*0;
    vx2 = vx.*0;
    vy2 = vy.*0;
    ph1 = vx.*0;
    
    if www ==1 % for first iteration: set pre-shift to zero
        prex = vx.*0; % prex: horizontal preshift at each interrogation area
        prey = vy.*0;
    else % interpolate previous result at new locations
        clear prex prey
        for t=1:split
            xgo = xgo+rand(size(xgo))./1E3; % we need to add noise to avoid interpolation crashes
            ygo = ygo+rand(size(ygo))./1E3; % this noise is negligible compared to the locations
            xgop=xgo(:,:,t);
            ygop=ygo(:,:,t);
            vxp=Vx(:,:,t);
            vyp=Vy(:,:,t);
            xgp=xg(:,:,t);
            ygp=yg(:,:,t);
            prex1 = griddata(xgop,ygop,vxp,xgp,ygp,'linear'); % linear interpolation
            prey1 = griddata(xgop,ygop,vyp,xgp,ygp,'linear');
            nv = isnan(prex1); % detect problems with previous interpolation
            xgt = xgp+rand(size(xgp))./1000; % repeat, but with more robust 'nearest' interpolation
            ygt = ygp+rand(size(ygp))./1000;
            prex1(nv) = griddata(xgt(nv==0),ygt(nv==0),prex1(nv==0),xgt(nv),ygt(nv),'nearest');
            prey1(nv) = griddata(xgt(nv==0),ygt(nv==0),prey1(nv==0),xgt(nv),ygt(nv),'nearest');
            nv = isnan(prex1); % still bad interpolation results? set them to zero pre-shift
            prex1(nv) = 0;
            prey1(nv) = 0;
            prex(:,:,t)=prex1;
            prey(:,:,t)=prey1;
        end
    end
    clear Vxv Vyv
    % reset/allocate correlation plane average variable:
    Csum(1:(2*ias1),1:(2*ias2),1:length(xx),1:length(yy),1:split)=0; % this is the most memory intensive parameter
    
    % create shift at each pixel location to deform the image
    
    imx = 1:size(I1,1);
    imy = 1:size(I1,2);
    [imx,imy] = meshgrid(imx,imy);
    for t = 1:split;
        shiftx(:,:,t) = interp2((xg(:,:,t))',(yg(:,:,t))',(prex(:,:,t))',imx,imy,'linear')'; % interpolate local shift from previous iteration
        shifty(:,:,t) = interp2((xg(:,:,t))',(yg(:,:,t))',(prey(:,:,t))',imx,imy,'linear')';
    end
    shiftx(isnan(shiftx))=0; % no proper interpolation possible (e.g. at edges)? Don't deform in that case
    shifty(isnan(shifty))=0;
    
    flag = prex.*0; % this is a debugging/analysis parameter, we can assign a particular value (e.g. to label bad vectors)
    for n=1:length(infiles)
        file_in=infiles{n};
        load(file_in);
        if n==1
            phase=0
            period=period1;
            data=data_1;
        else
            phase=phase2;
            period=period1;
        end
        data=data(yrange,xrange,:);
        for zz = 1:length(filerange)-1; % the main loop over the frames to process
            
            loopcount = loopcount + 1; % counter for display purposes
            
            %clc % display some info:
            disp(['file ' num2str(n)])
            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).
            repeatRate=period;

            t=ceil((zz+phase+arb)/period*split); %,(filerange-phase)/period
            t2=ceil((zz+phase+arb)/period-1)*split;
            t=t-t2;
            pos=find(allT==t);
            if length(pos)==0
                allT=[allT t];
            end

            I1=squeeze(data(:,:,zz)');
            I2=squeeze(data(:,:,zz+1)');
            
            %%%%%% end of file/frame i/o part
            
            if www==1 % determine average images
                Iavg(:,:,t)=Iavg(:,:,t)+(I1+I2)/2;
                tel1(t)=tel1(t)+1;
                
                if tel1(t)>1
                    I1 = I1-Iavg(:,:,t)./tel1(t);        % subtract average from frames
                    I2 = I2-Iavg(:,:,t)./tel1(t);       % note that we use a "running average" for the first iteration
                end
            end
            
            if www>1
                if tel1(t)>1
                    I1 = I1-Iavg(:,:,t)./tel1(t);        % subtract average from frames
                    I2 = I2-Iavg(:,:,t)./tel1(t);       % note that we use a "running average" for the first iteration
                end
            end
            
            
            if www>1
                I2o = I2;   % deform image; this effectively removes the need for pre-shifting
                I2i = interp2(imx,imy,I2',imx+shiftx(:,:,t)',imy+shifty(:,:,t)','linear',0); %
                I2 = I2i;
                I2 = I2';
            end
            
            I1o = I1; % store original (for comparison)
            
            %         I1 = I1';
            %         I2 = I2';
            
            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,t) = 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,t) =  Csum(:,:,i,j,t)+C; %change this - no averageing % add it to average
                        telC = telC + 1;
                    end % if valid region
                end %j loop
            end %i loop
        end % original timestep loop
    end %loop over files
    for t=1:split
        for i = 1:length(xx)
            for j = 1:length(yy)
                
                dumC = squeeze(Csum(:,:,i,j,t)); % extract correlation from 4D matrix
                dumC = dumC./corf.*validregion; % apply bias correction and valid region matrix
                Cmax = max(dumC(:)); % determine maximum in correlation plane
                if Cmax ~=0
                    abc=1;
                end
                ph1(i,j,t) = 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 = 500; eps_y = 500; % set to extreme value so we will notice bad peak fits
                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,t) = xx(i); yg(i,j,t) = yy(j); vx(i,j,t) = NaN; vy(i,j,t) = NaN; % bad fit = NaN
                else
                    xg(i,j,t) = xx(i); yg(i,j,t) = yy(j); vx(i,j,t) = dx;  vy(i,j,t) = dy; % good fit = store data
                    vx(i,j,t) = vx(i,j,t) + prex(i,j,t);
                    vy(i,j,t) = vy(i,j,t) + prey(i,j,t);
                end % good fit
            end % j
        end% i
    end  % last of times ? (=data processed)
    
    %%%%%%%%% vector validation: we do this after each iteration and when
    %%%%%%%%% we're done processing. See "Universal outlier detection"
    %%%%%%%%% Westerweel and Scarano (Exp. Fluids)
    clear Vx Vy xgo ygo
    for t=1:split
        vxv = vx(:,:,t);
        vyv = vy(:,:,t);
        devx = vxv.*0;
        devy = vyv.*0;
        mex = devx;
        mey = devx;

        tv = 2; % threshold value (1-3 should be OK, depends on flow somewhat)
        
        clear mex mey %vxv vyv
        
        for i = 1:size(vxv,1); % for each vector, we determine its 8 neighbours
            for j = 1:size(vxv,2);
                x0 = max([1 i-1]);
                x1 = min([i+1 size(vxv,1)]);
                y0 = max([1 j-1]);
                y1 = min([j+1 size(vxv,2)]);
                subx = vxv(x0:x1,y0:y1);
                suby = vyv(x0:x1,y0:y1);
                subx(subx==vxv(i,j))=NaN;
                suby(suby==vyv(i,j))=NaN;
                medx = median(subx(isnan(subx)==0)); % median of 8 neighbours
                medy = median(suby(isnan(suby)==0));
                derx = std(subx(isnan(subx)==0)-medx); % standard deviation of neighbours with respect to median
                dery = std(suby(isnan(suby)==0)-medy);
                devx(i,j) = abs(vxv(i,j)-medx)/derx; % normalized deviation
                devy(i,j) = abs(vyv(i,j)-medy)/dery;
                %   mex(i,j) = meanx;
                %   mey(i,j) = meany;
            end
        end
        
        dev = (devx+devy)/2; % average deviation
        dev(isnan(dev)) = tv+1E-3;
        
        vxv(flag(:,:,t)==0) = 0;
        vyv(flag(:,:,t)==0) = 0;
        
        vv = dev<tv | flag(:,:,t) == 0;    % accept low median deviation AND zeros outside ROI

        
        xgx = xg(:,:,t)+rand(size(xg(:,:,t)))./1000; % We add some noise to stabilize Matlab's interpolation algorithm
        ygy = yg(:,:,t)+rand(size(yg(:,:,t)))./1000;
        
        if sum(vv(:))>0
            vxv(vv==0) = griddata(xgx(vv),ygy(vv),vxv(vv),xgx(vv==0),ygy(vv==0),'linear');
            vyv(vv==0) = griddata(xgx(vv),ygy(vv),vyv(vv),xgx(vv==0),ygy(vv==0),'linear');
        end
        
        vv = isnan(vxv); % for bad interpolation results, we replace it with nearest neighbour results
        if sum(vv(:))>0
            vv = ~ vv;
            vxv(vv==0) = griddata(xgx(vv),ygy(vv),vxv(vv),xgx(vv==0),ygy(vv==0),'nearest');
            vyv(vv==0) = griddata(xgx(vv),ygy(vv),vyv(vv),xgx(vv==0),ygy(vv==0),'nearest');
        end
        
        vxv(isnan(vxv)) = 0;
        vyv(isnan(vyv)) = 0;
        
        tx = mean(vxv(:));
        ty = mean(vyv(:));
        
        vxo = vxv; % we copy the data and subtract the mean so we can smooth it without edge effects
        vyo = vyv; %vxo vyo don't seem to be used for anything...?
        vxv = vxv-tx;
        vyv = vyv-ty;
        
        if www==1                           % first iteration: remove aggresively
            vxv = medfilt2(vxv,[3 3],'symmetric') + tx;
            vyv = medfilt2(vyv,[3 3],'symmetric') + ty;
        else                                % second and later iterations: smooth gently
            vxv = conv2(vxv,ones(3,3)./9,'same')+tx; % this is the smoothened
            vyv = conv2(vyv,ones(3,3)./9,'same')+ty; % pre-shift at original locations
        end
        Vx(:,:,t)=vxv;
        Vy(:,:,t)=vyv;
        xgo(:,:,t) = xgx; % original grid location
        ygo(:,:,t) = ygy;
     end
end % iteration loop

clear C Cmax I1 I1o I2 I2i I2o Iavg2 It imx imy shiftx shifty xgo % clean up workspace


xg = round(xg); % remove artificial noise that was used for stability
yg = round(yg); % (positions are in whole pixels)

% save the results;

outfile = file_in;
outfile((end-3):end) = [];
outfile = [filename '_out.mat'];

save(outfile)  % data output

% most relevant parameters: xg,yg
% (location in pixels), and vx,vy (displacements in pixels).