function [x1, x2, tri_pts] = collect_point_matches(im1, im2, x1, x2, tri_pts)
% collect_point_matches(im1, im2)
%
% Allow user to interactively select point correspondances and 
% polygon faces with the mouse.  Pressing 'p' switches from selecting 
% point correspondances to identifying polygon faces, 'z' toggles into
% zoom mode, and <esc> ends the selection process, and exits the function.
%
% Can also be used to append pts and/or polygon faces to an 
% existing set:
%   collect_point_matches(im1, im2, x1, x2) 
%   collect_point_matches(im1, im2, x1, x2, tri_pts)
%
figure(1); clf; image(im1); colormap(gray(256)); hold on; zoom off; axis image; axis equal; ax1 = axis;
figure(2); clf; image(im2); colormap(gray(256)); hold on; zoom off; axis image; axis equal; ax2 = axis;
figure(1);

if (nargin==1 | nargin ==3 | nargin>5),
	error('wrong number of input arguments');
end
npoints = 0;

% if existing points, display them
if nargin > 3,
	npoints = size(x1,1);
    format_figure(x1(:,1), x1(:,2), [0 1 1], [1:npoints]');  
    figure(2); 
    format_figure(x2(:,1), x2(:,2), [0 1 1], [1:npoints]');
else,
	x1 = [];
	x2 = [];	
end

% display existing polygons
if nargin == 5
    for ii = 1:size(tri_pts,1)
        fill(x1(tri_pts(ii,1:3),1), x1(tri_pts(ii,1:3),2), [1 1 0]);
    end; 
    figure(2); 
    for ii = 1:size(tri_pts,1)
        fill(x2(tri_pts(ii,1:3),1), x2(tri_pts(ii,1:3),2), [1 1 0]);
    end;
else,
	tri_pts = [];	
end; 
figure(1);

[m1,n1] = size(im1); 
[m2,n2] = size(im2); 

figure(1)
button = 1;
new_patch = 1;
patch_start_pt = 0;
mode = 'select_points';
new_poly = [];

while (~(button == 120 | button==27))                   % - Exit if ESC or 'x' pressed
    [x,y,button] = ginput(1);                           % - get input from mouse or keyboard
    mode = p_check2(button,mode);
    switch_fig_check(button,mode);
    figure(1); title('Choose a point in this view');
    if button == 1 | button == 2,
        disp('button pressed');
        switch lower(mode)
        case 'select_points'
            [x1,x2] = select_point(x, y, ax1, ax2, [0 1 1], x1, x2);
            title('Point select mode, <p> for polygon, <z> for zoom, or <esc> to exit');        
        case 'select_polygons'
            disp('mode:select_polygons')
            new_poly = select_polygon(x, y, x1, x2, new_poly);
            if length(new_poly)==3
                tri_pts = [tri_pts; new_poly];
                fill(x1(new_poly,1), x1(new_poly,2), [1 1 0]);
                format_figure(x1(new_poly,1), x1(new_poly,2), [0 1 1], new_poly');    
                new_poly = [];
            end; %if
			title('Polygon select mode, <p> for polygon, <z> for zoom, or <esc> to exit');
        end; %switch
    end; %if
    if button==99                                      % - if c, clear the figures
        figure(1); clf; image(im1); colormap(gray(256)); hold on; zoom off; axis image; axis equal; ax1 = axis;
		figure(2); clf; image(im2); colormap(gray(256)); hold on; zoom off; axis image; axis equal; ax2 = axis;
    end
end
hold off


%------------------------------------------------------------------------------------------
function switch_fig_check(button, mode)
if button == 32                                        % - if spacebar pressed, 
    if mode == 'select_polygons'
        if gcf==1 
            figure(2);
        else
            figure(1);
        end  
    else
        disp('Can only switch figures in polygon selection mode.');
    end;
end


% -----------------------------------------------------------------------------------------
function zoomed = checkzoom(button)
zoomed = 0;
if button == 122
	disp('mode:zoom');
	title('zoom select mode, <z> return, <p> for point, or <esc> to exit');
	zoom on;
	keypress = 0; while keypress == 0, keypress = waitforbuttonpress; end
	zoom off;
	zoomed = 1;
end;


%------------------------------------------------------------------------------------------
function mode = p_check2(button, mode);
if button == 112   									% - if 'p' pressed switch modes
	zoom off;
    switch lower(mode)
    case 'select_points'
        disp('switching to polygon selection mode');  
        mode = 'select_polygons';
        xlabel('Select polygon');
    case 'select_polygons'
        disp('switching to point selection mode');
        mode = 'select_points';
        xlabel('Select point correspondance');
    end  
end; %switch
checkzoom(button)

%------------------------------------------------------------------------------------------
function [x1,x2] = select_point(x,y,ax1,ax2,color,x1,x2);
disp('mode:select_points')
x1(end+1,1:2) = [x y];
format_figure(x,y,color,size(x1,1));       

title('Now identify the same point in the other view');
figure(2); title('Choose a point in this view');

[x,y,button] = ginput(1);  
if checkzoom(button),
	[x,y,button] = ginput(1);  
end
x2(end+1,1:2) = [x y];
format_figure(x,y,color,size(x2,1));    
title('');
figure(1);



%------------------------------------------------------------------------------------------
function format_figure(x,y,color,c);
h = plot(x, y,'ro');
set(h,'MarkerSize', 5, 'LineWidth', 2, 'Color', color);
H = text(x+4, y+4,int2str(c));
set(H,'Color',color,'FontSize',10);

%------------------------------------------------------------------------------------------
function new_poly = select_polygon(x, y, x1, x2, new_poly); 
disp('mode:select_polygons')
dist = sqrt((x1(:,1)-x(1)).^2 + (x1(:,2)-y(1)).^2);
[m closest] = min(dist);
xx1 = x1(closest,1);    yy1 = x1(closest,2); 
xx2 = x2(closest,1);    yy2 = x2(closest,2); 
figure(1),
h = plot(xx1,yy1,'ro'); set(h,'MarkerSize', 5); set(h,'LineWidth', 2);  
title('Selected point highlighted in red');
figure(2),
h = plot(xx2,yy2,'ro'); set(h,'MarkerSize', 5); set(h,'LineWidth', 2);  
title('Selected point highlighted in red');
figure(1);
new_poly = [new_poly, closest];

%------------------------------------------------------------------------------------------
function show_planar_image(im1_rect, x_rect_ic);
% display unwarped image and tie points
%
figure, imagesc(im1_rect), hold on,
%plot(x_rect(1,:),x_rect(2,:),'rx'); text(x_rect(1,:),x_rect(2,:),int2str([1:3]'));
h = line([x_rect_ic(1,:), x_rect_ic(1,1)], [x_rect_ic(2,:), x_rect_ic(2,1)]);
%h = plot(x_rect_ic(1,:),x_rect_ic(2,:),'gx'); 
set(h,'MarkerSize', 5); 
set(h,'LineWidth', 2);  
set(h,'Color',[0 1 0])
h = plot(x_rect_ic(1,:),x_rect_ic(2,:),'ko');
set(h,'MarkerSize', 5); 
set(h,'LineWidth', 10);  
h = text(x_rect_ic(1,:),x_rect_ic(2,:),int2str([1:3]'));
set(h,'Color',[1 1 1])
set(h,'FontSize',15)
set(h,'FontWeight','light')
set(h,'HorizontalAlignment','center');
axis image; %axis off; 
colormap(gray(256));
