function H = calcH(p1, p2) 
% Assume we have two images called '1' and '2' 
% p1 is an n x 2 matrix containing n feature points, where each row 
% holds the coordinates of a feature point in image '1' 
% p2 is an n x 2 matrix where each row holds the coordinates of a 
% corresponding point in image '2' 
% H is the homography matrix, such that 
% p1_homogeneous = H * [p2 ones(size(p2, 1), 1)]' 
% p1_homogeneous contains the transformed homogeneous coordinates of 
% p2 from image �2� to image �1�. CS 534 Computational Photography Fall 2013


% NOTE: this file was modified as per Zach's recommendations

n = size(p1, 1); 
if n < 4 
 error('Not enough points'); 
end 

max = 0;
inlier_count_Thresh = 0;

    [n, m] = size(p1);
    pt_Count = size(p1, 1);
    
    assert(size(p2, 1) == pt_Count);
    
    homog_ImgLoc1 = [p1'; ones(1, pt_Count)];
    bestHomog = [];

A = zeros(n*3,9); 
b = zeros(n*3,1); 

% Generate random points for ransac
for i=1:100
	randomPoints = datasample(1:1:size(p1,1),4); 
    
for j = 1:4
	i = randomPoints(j);
	A(3*(i-1)+1,1:3) = [p2(i,:),1]; 
	A(3*(i-1)+2,4:6) = [p2(i,:),1]; 
	A(3*(i-1)+3,7:9) = [p2(i,:),1]; 
	b(3*(i-1)+1:3*(i-1)+3) = [p1(i,:),1]; 
end 
x = (A\b)'; 

% Ransac implementation
H = [x(1:3); x(4:6); x(7:9)]; 

 temp = H * homog_ImgLoc1;
 temp = [temp(1, :) ./ temp(3, :); temp(2, :) ./ temp(3, :)];
    for ndx =1:n
       diff = sqrt(sum((temp - p2.').^2, 1));
       
       % This value seems to be off, but it works for current images
        if (diff <= 550)
            inlier_count_Thresh = inlier_count_Thresh +1;
        end
    end
       if (inlier_count_Thresh > max)
           max = inlier_count_Thresh;
          % Set the best homography here
           bestHomog = H;
       end
end
    
    H = bestHomog;
end
