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

%% Parameters
% Define the size of the blocks for block matching.
clc
clear
addpath('normxcorr2_mex_ALL')
halfBlockSize = 5;
blockSize = 2 * halfBlockSize + 1;
range = 5;
%disparityRange = 140;
%% Set up
%imR_color = imread('scopeFourundred0_right_rectified.png');% old baseline
%imL_color = imread('scopeFourundred0_left_rectified.png'); % old baseline
%imR_color = imread('throatFour_right_rectified.png');      % old baseline
%imL_color = imread('throatFour_left_rectified.png');       % old baseline
%imR_color = imread('inthroat_right_rectified.png');        % old baseline
%imL_color = imread('inthroat_left_rectified.png');         % old baseline

%imR_color = imread('400move_right.png');   %new baseline
%imL_color = imread('400move_left.png');    %new baseline
%imR_color = imread('throat_right.png');   %new baseline
%imL_color = imread('throat_left.png');    %new baseline
%imR_color = imread('inthroat_check_right.png');   %new baseline
%imL_color = imread('inthroat_check_left.png');    %new baseline
imR_color = imread('Apr06throat_right.png');   %new baseline
imL_color = imread('Apr06throat_left.png');    %new baseline
%imR_color = imread('throat_straw_right.png');   %new baseline with straw
%imL_color = imread('throat_straw_left.png');    %new baseline


figure, imshow(imL_color);
hold on

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

figure, imshow(imR_color);
hold on
% user input from the right image
[xx1, yy1] = ginput(1);
plot(xx1,yy1,'b*')
[xx2, yy2] = ginput(1);
plot(xx2,yy2,'b*')
xx1 = int32(xx1);
yy1 = int32(yy1);
xx2 = int32(xx2);
yy2 = int32(yy2);

imL = (rgb2gray(im2double(imL_color)));
imR = (rgb2gray(im2double(imR_color)));

%% template around points of interest

templateBlock = imL( y1-halfBlockSize:y1+halfBlockSize, x1-halfBlockSize:x1+halfBlockSize);
templateBlock2 = imL(y2-halfBlockSize:y2+halfBlockSize, x2-halfBlockSize:x2+halfBlockSize);

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

%% Find the best correspondence from the scanline
bestCorrelationIndex1 = 0;
bestCorrelationHeight1 = 0;
bestScore1 = -Inf;
for height1 = yy1-range:yy1+range
    for index = max(xx1-range,0):min(xx1+range,size(imR,2))
        if ncc1(height1,index) > bestScore1
            bestScore1 = ncc1(height1,index);
            bestCorrelationIndex1 = index;
            bestCorrelationHeight1 = height1;
        end
    end
end

bestCorrelationIndex2 = 0;
bestCorrelationHeight2 = 0;
bestScore2 = -Inf;
for height2 = yy2-range:yy2+range
    for index2 = max(xx2-range,0):min(xx2+range,size(imR,2))
        if ncc2(height2,index2) > bestScore2
            bestScore2 = ncc2(height2,index2);
            bestCorrelationIndex2 = index2;
            bestCorrelationHeight2 = height2;
        end
    end
end
%% Compute for 3D coordinates and distance

focal_length =  301.8239;
%focal_length =  102.8239;
%focal_length =  11.8239;
%baseline = 84.6102;  % the old

%baseline = 142.1765;  % new sequence, free movement without interference
%baseline = 8.1263 ;   % in throat with checkerboard, with error,
                      % with error baseline =  4.12 ~ 10.22 
%baseline = 10.4094; %  just outside the larynx                      
baseline = 5.8;
                      
angle = pi/10;
x1 = double(x1);
y1 = double(y1);
x2 = double(x2);
y2 = double(y2);
bestCorrelationIndex1 = double(bestCorrelationIndex1);
bestCorrelationHeight1 = double(bestCorrelationHeight1);
bestCorrelationIndex2 = double(bestCorrelationIndex2);
bestCorrelationHeight2 = double(bestCorrelationHeight2);

disparity1 = sqrt((x1 - bestCorrelationIndex1)^2 + (y1-bestCorrelationHeight1)^2)
disparity2 = sqrt((x2 - bestCorrelationIndex2)^2 + (y2-bestCorrelationHeight2)^2)
%disparity2 = x2 - bestCorrelationIndex2;

Z1 = (focal_length*baseline)/(disparity1);
Z2 = (focal_length*baseline)/(disparity2);

X1 = (x1*Z1)/focal_length - (baseline/2);
X2 = (x2*Z2)/focal_length - (baseline/2);

Y1 = (bestCorrelationHeight1/focal_length)*(Z1*cos(angle) + (X1 - baseline/2)*sin(angle));
Y2 = (bestCorrelationHeight2/focal_length)*(Z2*cos(angle) + (X2 - baseline/2)*sin(angle));

%% similar depth assumption
%Z2 = Z1;


point1 = [X1 Y1 Z1]
point2 = [X2 Y2 Z2]

distance = norm(point1-point2)

%% display

figure
hold on
subplot(1,2,1), imshow(imL_color),hold on,plot(x1,y1,'r*'),plot(x2,y2,'r*'),title('Left image')
subplot(1,2,2), imshow(imR_color),hold on,plot(bestCorrelationIndex1,bestCorrelationHeight1,'b*'),plot(bestCorrelationIndex2,bestCorrelationHeight2,'b*'),title('Right image');

text(-150,430  , '3D distance: ', 'Color', 'k');
%hold on
text(-50,430  , num2str(distance), 'Color', 'k', 'FontSize', 15);
text(50,430  , 'mm', 'Color', 'k');