
%% 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.
addpath('normxcorr2_mex_ALL')
halfBlockSize = 7;
blockSize = 2 * halfBlockSize + 1;
%disparityRange = 140;
%% Set up
imR_color = imread('scopeFourundred0_right_rectified.png');
imL_color = imread('scopeFourundred0_left_rectified.png');


imL = (rgb2gray(im2double(imL_color)));
imR = (rgb2gray(im2double(imR_color)));
disparityMap = zeros(size(imL));
%% template around points of interest
for y1 = halfBlockSize:(size(imR,1)-halfBlockSize)
for x1 = halfBlockSize:(size(imR,2)-halfBlockSize)
    templateBlock = imL( y1-halfBlockSize+1:y1+halfBlockSize, x1-halfBlockSize+1:x1+halfBlockSize);


    %% normalized cross correlation  (computing for the entire image. not necessary)
    ncc1 = normxcorr2_mex(templateBlock, imR, 'same');


    %% Find the best correspondence from the scanline
    bestCorrelationIndex1 = 0;
    bestScore1 = -Inf;
    for index = x1:size(imR,2)
        if ncc1(y1,index) > bestScore1
            bestScore1 = ncc1(y1,index);
            bestCorrelationIndex1 = index;
        end
    end

    disparity1 = x1 - bestCorrelationIndex1;
    disparityMap(y1,x1) = disparity1;
    
end

    if mod(y1, 10) == 0 
        fprintf('  Image row %d / %d (%.0f%%)\n', y1, size(imL,1), (y1 / size(imL,2)) * 100);
    end
end