
%% Funtion to get input from user from original image
% Assume camera intrinsic parameters are already loaded
% 1. Get input on real image
% 2. convert the input coordinates (original -> unwarp -> rectify)

%% Parameters
% Define the size of the blocks for block matching.
halfBlockSize = 9;
blockSize = 2 * halfBlockSize + 1;
disparityRange = 140;
%% Set up
imR = imread('scopeFourundred0_right_rectified.png');
imL = imread('scopeFourundred0_left_rectified.png');

%subplot(2,1,1)
% show the original image
figure, imshow(imL);
hold on

% user input from the left image
[x1, y1] = ginput(1);
plot(x1,y1,'r*')



%[p1, p2] = findCorrespondences(imL, imR);
%plot(p1(:,1),p1(:,2),'r*')
%figure
%imshow(imR)
%hold on
%plot(p2(:,1),p2(:,2),'r*')
%plot(x1,y1,'r*')


%% Compute score around the selected pixel
redImBlock = imL(x1-halfBlockSize:x1+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,1);
greenImBlock = imL(x1-halfBlockSize:x1+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,2);
blueImBlock = imL(x1-halfBlockSize:x1+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,3);

templateBlock = zeros(blockSize,blockSize,3);
templateBlock(:,:,1) = redImBlock;
templateBlock(:,:,2) = greenImBlock;
templateBlock(:,:,3) = blueImBlock;
%% Find correspondence in the other image
% scan the scanline on the right image
[imgHeight, imgWidth] = size(imL);
scanHeight = y1;
startX = max(halfBlockSize+1,min(x1-disparityRange, 1+blockSize));
endX = min(x1+disparityRange, imgWidth-blockSize);
blockR = zeros(blockSize,blockSize,3); 

range = endX - startX;
bestX = 0;
bestScore = Inf;
%for x = startX:endX
for h = 1:range
    x = endX-h;
    redImBlockR = imR(x-halfBlockSize:x+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,1);
    greenImBlockR = imR(x-halfBlockSize:x+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,2);
    blueImBlockR = imR(x-halfBlockSize:x+halfBlockSize,y1-halfBlockSize:y1+halfBlockSize,3);
    blockR(:,:,1) = redImBlockR;
    blockR(:,:,2) = greenImBlockR;
    blockR(:,:,3) = blueImBlockR;
    d = templateBlock - blockR;
    diff = sum(abs(d));
    score = sum(diff(:,:,1))+sum(diff(:,:,2))+sum(diff(:,:,3));
    if score < bestScore
        bestX = x;
        bestScore = score;
    end
end
figure
imshow(imR)
hold on
bestX
plot(bestX,y1,'r*')
%% Compute disparity

%% Compute distance