function [corr] = guided_matching_ZNCC(image1, image2, F, fl_1, fl_2, min_resp, patch_size, how_close)
%Joshua Stough
%COMP 290-89, Pollefeys
%Assignment 2, Computing F

%This performs the guided matching using provided 
%feature lists and the provided fundamental matrix
%F.

%A lot of this code comes from find_correspondences_ZNCC.

if (nargin < 8) how_close = 1; end
if (nargin < 7) patch_size = 7; end
if (nargin < 6) min_resp = .7; end
if (nargin < 5) fl_2 = corner_detector(image2, 500); end
if (nargin < 4) fl_1 = corner_detector(image1, 500); 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, 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.
s1 = size(fl_1);
s2 = size(fl_2);

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))));

%For each point in fl_1, we'll build temporary patch matrices by
%sampling from fpatch1 and 2, along the epipole of the feature. 
%Pick the ones that match in both directions.

%compute ahead of the for loops all the epipolar line equations
%and distances.

X = fl_1';
X(3,:) = ones(1,s1(1));
XP = fl_2';
XP(3,:) = ones(1,s2(1));

lx = F*X;
lxp = F'*XP;
%normalize the lines.
lxnorms = sqrt(sum(lx(1:2,:).^2));
lxpnorms = sqrt(sum(lxp(1:2,:).^2));
lxnorms(lxnorms == 0) = 1e-15;
lxpnorms(lxpnorms == 0) = 1e-15;

lx = lx./(ones(3,1)*lxnorms);
lxp = lxp./(ones(3,1)*lxpnorms);

%lx and lxp are the normalized epipolar lines in image2 and image1
%respectively.

DIST_X_LXP = abs(X'*lxp);
DIST_XP_LX = abs(XP'*lx);
%the two matrices above contain info for all the feature points.
%For example, the i'th row of the first matrix gives the 
%perendicular distance of the i'th feature point in image1 to 
%the epipolar lines of all the feature points in image2.

%One drawback of my first implementation is that I look along
%the whole line.  But I can also build distance arrays and pick
%closer ones.



%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 the
    %epipolar line from the feature 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.
    %With this next line, the potential matches becomes much smaller,
    %with so few feature points near the epipolar line of the current
    %point.
    
    m = m & (DIST_XP_LX(:,i) < how_close);
    
    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.
    
    m = m & (DIST_X_LXP(:,i) < how_close);
    
    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