function [corr] = find_correspondences_ZNCC(fl_1, fl_2, image1, image2, min_resp, patch_size)
%Joshua Stough
%COMP 290-89, Pollefeys
%Assignment 2, Computing F
%This function takes two feature lists (from corner_detector) and
%the two images, and finds correspondences between the two.
%A 4 x n list of these is returned (n is the number of good ones).

%We have to compare the neighborhood of a point in fl_1 to those
%of nearby (in pixel coords) points in fl_2.  The highest correlated
%neighborhoods (using zero-mean normalized cross-correlation) imply 
%the actual point correspondences to use.

s1 = size(fl_1);
s2 = size(fl_2);

if (nargin < 6) patch_size = 7; end
if (nargin < 5) min_resp = .7; end

%Size of the patches: plus and minus 7 of the feature point, so 15x15 
ps = (2*patch_size+1)^2;

window_size = size(image1)/10;

%First, turn the images to grayscale;
image1 = double(rgb2gray(image1));
image2 = double(rgb2gray(image2));


%Next, pad the images for getting patches around the border.
image1 = padarray(image1,[patch_size patch_size],'replicate','both');
image2 = padarray(image2,[patch_size patch_size],'replicate','both');

fl_1 = fl_1 + patch_size;
fl_2 = fl_2 + patch_size;

%Next, perhaps organize the feature lists so that nearby point in
%the two images are nearby in their index in the feature list. This
%may help speed up comparisons of patches.  Otherwise, it may be 
%just as well to compare every feature with every feature, given
%matlab's quirkiness.



%Next, turn all feature points in both images into their respective 
%column vectors in two large matrices.  So, each feature point 
%corresponds to a patch, and each patch is reshaped as a column 
%vector.
fpatch1 = zeros(ps, s1(1));
fpatch2 = zeros(ps, s2(1));

c = 1;
for i = fl_1'  %corresponding to an x,y feature point.
    curpatch = image1(i(1)-patch_size:i(1)+patch_size , i(2)-patch_size:i(2)+patch_size);
    fpatch1(:,c) = reshape(curpatch, ps, 1);
    c = c + 1;
end

c = 1;
for i = fl_2'  %corresponding to an x,y feature point.
    curpatch = image2(i(1)-patch_size:i(1)+patch_size , i(2)-patch_size:i(2)+patch_size);
    fpatch2(:,c) = reshape(curpatch, ps, 1);
    c = c + 1;
end

%We want zero-mean and normalized, so do that.
fpatch1 = fpatch1 - ones(ps, s1(1))*diag(mean(fpatch1));
fpatch2 = fpatch2 - ones(ps, s2(1))*diag(mean(fpatch2));

fpatch1 = fpatch1./(ones(ps, s1(1))*diag(sqrt(sum(fpatch1.*fpatch1))));
fpatch2 = fpatch2./(ones(ps, s2(1))*diag(sqrt(sum(fpatch2.*fpatch2))));

%Every patch is now zero-mean and normalized. 
%Now, we compare patches within comparable regions of the image, looking
%for cases where patches are each others' best matches.  

%dp = fpatch1'*fpatch2;

%A max along a row corresponds to the best match over image2 of a patch 
%in image1, and a max for a column is vice versa.  If j is the column 
%containing the max for a row i, and the max along the j'th column
%happens to be the i'th row, then the patch i in image1 and the patch j
%in image2 are specified a correspondence.


%The matches that this comes up with are not usually local, and so
%should be discounted. We need to sort by distances between the 
%putative correspondences and look for a max within the proper 
%window.

%For each point in fl_1, we'll build temporary patch matrices by
%sampling from fpatch1 and 2.  Then look at max as before.  So dp
%isn't used, but every temp product will be a subset of it.

%for every patch in image1
pref12 = zeros(1,s1(1));
%bests = zeros(1,s1(1));
for i = 1:s1(1)
    %for every feature in image2, is it close enough to consider.
    df(:,1) = fl_2(:,1) - fl_1(i,1);
    df(:,2) = fl_2(:,2) - fl_1(i,2);
    df = abs(df);
    %df is pixel distance to every feature in image2 to the feature 
    %in image1
    m = df(:,1) < window_size(1) & df(:,2) < window_size(2);
    %m represents the columns of fpatch2 that are for close enough
    %features to consider.
    tpatch = fpatch2(:,m > 0);
    %Now we have the relevant patches to test against in one direction.
    k = find(m);
    resp = fpatch1(:,i)'*tpatch;
    [maxr, ind] = max(resp);
    %Condition that there was at least one potential match in the area
    %and that the maximum response is above some minimum.
    if nnz(m) > 0 & maxr > min_resp  
        pref12(i) = k(ind);
        %bests(i) = maxr;
    end
end

pref21 = zeros(1,s2(1));
for i = 1:s2(1)
    %for every feature in image1, is it close enough to consider.
    df(:,1) = fl_1(:,1) - fl_2(i,1);
    df(:,2) = fl_1(:,2) - fl_2(i,2);
    df = abs(df);
    %df is pixel distance to every feature in image1 to the feature 
    %in image2
    m = df(:,1) < window_size(1) & df(:,2) < window_size(2);
    %m represents the columns of fpatch2 that are for close enough
    %features to consider.
    tpatch = fpatch1(:,m > 0);
    %Now we have the relevant patches to test against in one direction.
    k = find(m);
    resp = fpatch2(:,i)'*tpatch;
    [maxr, ind] = max(resp);
    if nnz(m) > 0 & maxr > min_resp 
        pref21(i) = k(ind);
    end
end

%So now, pref12(i) is the choice of the i'th feature point in image1 
%for the best relevant match in image2.  And vice versa for pref21.

%These patch matches correspond to point matches in the original
%feature lists.  Reshift the feature lists then.
fl_1 = fl_1 - patch_size;
fl_2 = fl_2 - patch_size;

%[b, index] = sort(bests);
%desired_num = s1(1)/4;
%the last desired_num of index are the best.
%temp = s1(1) - desired_num;

c = 1;
for i = 1:s1(1)
    if pref12(i) > 0 & pref21(pref12(i)) == i 
        %& find(index == i) > temp 
        %the correspondence is i in image1 and pref12(i) in image2.
        corr(1:2,c) = fl_1(i,:)';
        corr(3:4,c) = fl_2(pref12(i),:)';
        c = c + 1;
    end
end

number_of_correspondences = c-1