clear all;

% read in all the images and square them
image1 = double(imread('SubImage1.png'));
image1sq = image1.^2;

image2 = double(imread('SubImage3.png'));
image2sq = image2.^2;

image3 = double(imread('SubImage4.png'));
image3sq = image3.^2;

image4 = double(imread('SubImage2.png'));
image4sq = image4.^2;

% select a sub image from the first image read
subImage1 = image1(224:(224 + 31), 224:(224+31));

% find the sum total squared of the sub image for matching
totsq = sum(sum(subImage1.^2));

%pad all the images
s1_1 = size(image1,1);
s1_2 = size(image2,1);

s2_1 = size(image1,2);
s2_2 = size(image2,2);

subImagePad = zeros(s1_1, s2_1);
subImagePad(1:32, 1:32) = subImage1;

mask = zeros(s1_1, s2_1);
mask(1:32, 1:32) = 1.0;

% subplot(2,2,1); imagesc(image1); axis square; colormap gray;
% subplot(2,2,2); imagesc(subImagePad); axis square; colormap gray;
% subplot(2,2,3); imagesc(mask); axis square; colormap gray;

image1F = fft2(image1);
image2F = fft2(image2);
image3F = fft2(image3);
image4F = fft2(image4);

subImgF = fft2(subImagePad);

image1sqF = fft2(image1sq);
image2sqF = fft2(image2sq);
image3sqF = fft2(image3sq);
image4sqF = fft2(image4sq);

maskF = fft2(mask);

cc1F = image1F.*conj(subImgF);
cc2F = image2F.*conj(subImgF);
cc3F = image3F.*conj(subImgF);
cc4F = image4F.*conj(subImgF);

cc1 = abs(ifft2(cc1F));
cc2 = abs(ifft2(cc2F));
cc3 = abs(ifft2(cc3F));
cc4 = abs(ifft2(cc4F));

cct1 = abs(ifft2(image1sqF.*conj(maskF))) - 2 * cc1 + totsq;
cct2 = abs(ifft2(image2sqF.*conj(maskF))) - 2 * cc2 + totsq;
cct3 = abs(ifft2(image3sqF.*conj(maskF))) - 2 * cc3 + totsq;
cct4 = abs(ifft2(image4sqF.*conj(maskF))) - 2 * cc4 + totsq;

[m1, indx1] = min(cct1(:))
[m2, indx2] = min(cct2(:))
[m3, indx3] = min(cct3(:))
[m4, indx4] = min(cct4(:))

[xoff1, yoff1] = ind2sub(size(cct1), indx1)
[xoff2, yoff2] = ind2sub(size(cct2), indx2) % x and y positions are flipped
[xoff3, yoff3] = ind2sub(size(cct3), indx3) % x and y positions are flipped
[xoff4, yoff4] = ind2sub(size(cct4), indx4)


figure('Name', 'Results');
subplot(2,2,1); imagesc(image1); axis square; colormap gray;
hold on;
plot(yoff1, xoff1, 'rx');
hold off;
subplot(2,2,2); imagesc(image2); axis square; colormap gray;
hold on;
plot(yoff2, xoff2, 'rx');
hold off;
subplot(2,2,3); imagesc(image3); axis square; colormap gray;
hold on;
plot(yoff3, xoff3, 'rx');
hold off;
subplot(2,2,4); imagesc(image4); axis square; colormap gray;
hold on;
plot(yoff4, xoff4, 'rx');

figure('Name', 'Final Image', 'Position', [0 0 512 512]);
finalImage = zeros(512, 512);
finalImage(1:256, 1:256) = image1;
finalImage(xoff1 - xoff2:255 + xoff1 - xoff2, yoff1 - yoff2:255 + yoff1 - yoff2) = image2;
finalImage(xoff1 - xoff3:255 + xoff1 - xoff3, yoff1 - yoff3:255 + yoff1 - yoff3) = image3;
finalImage(xoff1 - xoff4:255 + xoff1 - xoff4, yoff1 - yoff4:255 + yoff1 - yoff4) = image4;

imagesc(finalImage); axis square; colormap gray;