function [corr] = find_correspondences_SSD(fl_1, fl_2, image1, image2, max_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 sum of square differences.) 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) max_resp = 1000; end
%max_resp is not used right now.  It should somehow 
%dicriminate between good matches and bad matches.
%In ZNCC, it's meaningful since all the responses are
%within [-1,1].

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

image1 = image1/(max(max(image1)));
image2 = image2/(max(max(image2)));

%Equalize the average intensities in the two images.
av1 = mean(mean(image1));
av2 = mean(mean(image2));
image2 = (av1/av2)*image2;


%And mean-zero the two images. 
%SSD is shift-invariant, so nevermind.
%image1 = image1 - av1;
%image2 = image2 - av1;

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

%for every patch in image1
pref12 = 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);
    s = size(tpatch);
    
    %Now we have the relevant patches.
    k = find(m);
    
    resp = sum((fpatch1(:,i)*ones(1, s(2)) - tpatch).^2);
    [minr, ind] = min(resp);
    
    %Condition that there was at least one potential match in the area
    %and that the min response is below some max.
    if nnz(m) > 0 & minr < max_resp  
        pref12(i) = k(ind);
    end
end

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

c = 1;
for i = 1:s1(1)
    if pref12(i) > 0 
        %& 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