%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% dotboard_calibration.m
% find markers in images of calibration plates
% then perform a 2D calibration to dewarp the calibration plate image

addpath(genpath([pwd '/../../minmax/']));
addpath([pwd '/pinhole/']);
addpath([pwd '/../../misc/']);
addpath(genpath([pwd '/../../piv2d']));

%% input parameters
grid_spacing		= 1;		% real units, mm
invert_image		= 0;		% invert image for black dots on white
blur_image			= 0;		% gaussian blur image
LoG_image			= 0;		% apply laplacian of Gaussian filter
checkerboard_image	= 0;		% apply checkerboard image filter
median_filter		= 1;
median_filter_size  = [5 5];
normalise_image     = 1;
default_corner_metric= 0.2;		% threshold for identifying corners
dot_half_window		= [64 64];

output_dir			= 'E:\mixer\lpt\calibration\21-07-12\';
plate_z             = -10 : 4 : 10;
n_planes            = length(plate_z);
plate_origin		= [zeros(1,n_planes); zeros(1,n_planes); plate_z];

camera_index		= 1;
camera_id			= camera_index-1;
calib_idx_offset	= 0;
xy_bl				= [-8 -5];	% coordinate of the bottom left marker on board
input_file_list		= arrayfun(@(n) sprintf('%s/im%02d.cam%d.tif', output_dir, n, camera_index-1), ...
                        [1 3 5 7 9 11], ...
                        'UniformOutput', false);
image_clim			= [0 255];

%% generate calibration for each image

nCalibrations		= length(input_file_list);

calib_idx			= 1;

while calib_idx <= nCalibrations
	%% update user of status
	input_file		= input_file_list{calib_idx};
	output_file		= sprintf('%s/CAM%d-%02d.mat', output_dir, camera_index, calib_idx+calib_idx_offset);
	fprintf('Image %02d of %02d:\n', calib_idx, nCalibrations);
	
	% check if calibration already exists
	if exist(output_file, 'file')
		sInput		= input('Calibration already exists - overwrite? y to overwrite: ', 's');
		if ~strcmpi(sInput, 'y')
			calib_idx = calib_idx + 1;
			continue;
		end
	end
	% check if image input exists
	if ~exist(input_file, 'file')
		fprintf(' - could not find image file\n');
	end
	
	%% load image
	im					= imread(input_file);
    im                  = double((im'));
	Nx					= size(im, 1);
	Ny					= size(im, 2);
	
	% define image coordinate system
	i_axis				= 0 : Nx-1;
	j_axis				= flip(0 : Ny-1);
	i0					= i_axis(1);
	j0					= j_axis(1);
	di					= i_axis(2)-i_axis(1);
	dj					= j_axis(2)-j_axis(1);
	
	%% create filtered image for processing
	im_filt			= im;
	if invert_image
		im_filt		= max(im(:))-im;
	end
	if blur_image
		G				= fspecial('gaussian', [5 5]);
		im_filt		= filter2(G, im_filt);
	end
	if LoG_image
		scale			= norm(dot_half_window)*0.2;
		G				= fspecial('log', 2*dot_half_window, scale);
		im_filt			= scale * conv2(G, im_filt);
	end
	if median_filter
		im_filt			= medfilt2(im_filt, median_filter_size);
	end
	if checkerboard_image
		G				= repelem(eye(2), dot_half_window(1), dot_half_window(2));
		im_filt			= conv2(G, im_filt);
	end
	if normalise_image
		[minv, maxv]	= minmaxfiltnd(single(im_filt), dot_half_window*2+1);
		im_filt			= (im_filt - minv) ./ max(16, maxv-minv);
	end	
	
	%% display image
	figure(1);
	clf;
	hold off;
	ax=gca;
	h_filt = imagesc(i_axis, j_axis, im_filt');
	axis equal tight xy;
	colormap gray;
	hold on;
	colorbar;
	
	%% crop image
	fprintf('Select the bottom left square of the board:\n');
	im_bl				= ginput(1);
	plot(im_bl(1), im_bl(2), 'ro');
	fprintf('Select the bottom right square of the board:\n');
	im_br				= ginput(1);
	plot(im_br(1), im_br(2), 'ro');
	fprintf('Select the top right square of the board:\n');
	im_tr				= ginput(1);
	plot(im_tr(1), im_tr(2), 'ro');
	fprintf('Select the top left square of the board:\n');
	im_tl				= ginput(1);
	plot(im_tl(1), im_tl(2), 'ro');
	% identify corners
	%im_corners			= [im_bl; im_tr];
	%im_corners			= sort(round(im_corners), 1);
	im_corners			= [im_bl(:) im_br(:) im_tr(:) im_tl(:)];
	
	%% coordinate directions
	im_ex				= im_br + im_tr - im_bl - im_tl;
	im_ey				= im_tr + im_tl - im_br - im_bl;
	im_ex				= im_ex' / norm(im_ex);
	im_ey				= im_ey' / norm(im_ey);
	
	%% display cropped and filtered image
	plot(ax(1), im_corners(1,:), im_corners(2,:), 'r-');
	% crop filtered image
	ij_corners			= diag([di dj]) \ (im_corners - [i0;j0]);
	im_crop				= im_filt;
	im_mask				= poly2mask(ij_corners(2,:), ij_corners(1,:), Nx, Ny);
	im_crop(~im_mask)	= 0;
	%im_crop(rx,ry)		= im_filt(rx,ry);
	im_filt				= im_crop;
	set(h_filt, 'cdata', im_filt');	
	drawnow;
	
	%% extract markers using chessboard algorithm
	h_markers			= []; 
	h_text				= [];
    corner_metric       = default_corner_metric;
	while ~isnan(corner_metric)
		%% search for chessboard corners
		fprintf('searching for checkerboard corners with m=%0.3f ...\n', corner_metric);
		[ij_markers, board_size] = detectCheckerboardPoints(im_filt', 'MinCornerMetric', corner_metric);
		
		%% deal with no markers found
		if isempty(ij_markers)
			fprintf('no valid markers found.\n');
			fprintf('try again with a different threshold\n\n');
			ij_markers		= nan(1,2);
		end
		im_markers			= diag([di dj])*ij_markers' + [i0;j0];% 2 x n_markers
		
		%% display
		figure(1);
		hold on;
		delete(h_markers);
		delete(h_text);
		h_markers		= plot(ax(1), im_markers(1,:), im_markers(2,:), 'g+-');
		h_markers(end+1)= plot(im_markers(1,1), im_markers(2,1), 'go');

		%% prompt for new threshold
		ret				= input('enter a new corner metric, delete point [d] or any accept: ', 's');
		corner_metric	= str2double(ret);
    end
    
    %% optionally delete delete marker
    if strcmpi(ret, 'd')
        while true
            %% get location of marker
            fprintf('select a marker to delete. right click to abort.\n');
            [id,jd,but]        = ginput(1);
            if but ~= 1
                break;
            end
            %% mark as NaN
            i_near          = knnsearch(im_markers', [id jd]);
            plot(im_markers(1,i_near), im_markers(2,i_near), 'rx');
            im_markers(:,i_near) = nan;
        end
	end
	
	%% get coordinates of markers
	% orient markers so that fast increasing direction aligns with x
	e_fast			= im_markers(:, 2) - im_markers(:, 1);
	e_fast			= e_fast / norm(e_fast);
	if abs(dot(e_fast, im_ey)) > abs(dot(e_fast, im_ex))
		% the fast increasing direction is y
		% permute orientation so the fast increasing direction is x
		im_markers	= permute(reshape(im_markers, [2 board_size-1]), [1 3 2]);
		im_markers	= im_markers(:,:);
	end
	% order markers so that markers go left to right, bottom to top
	last			= find(~any(isnan(im_markers),1), 1, 'last');
	first			= find(~any(isnan(im_markers),1), 1, 'first');
	im_delta		= im_markers(:, last) - im_markers(:, first);
	grid_dx			= sign(dot(im_ex, im_delta)) * grid_spacing;
	grid_dy			= sign(dot(im_ey, im_delta)) * grid_spacing;
	x_grid			= (0 : board_size(2)-2) * grid_dx;
	y_grid			= (0 : board_size(1)-2) * grid_dy;
	[xm, ym]		= ndgrid(x_grid, y_grid);
	xy_markers		= [xm(:) ym(:)]';
	% define origin in terms of bottom left marker
	[i_bl,d_bl]		= knnsearch(im_markers', im_bl);
	xy_shift		= xy_bl(:) - xy_markers(:,i_bl);
	x_grid			= x_grid + xy_shift(1);
	y_grid			= y_grid + xy_shift(2);
	[xm, ym]		= ndgrid(x_grid, y_grid);
    xy_markers		= [xm(:) ym(:)]';
	
    % eliminate NaN markers
    ok              = ~any(isnan(im_markers), 1);
    n_markers       = nnz(ok);
    im_markers      = im_markers(:, ok);
    xy_markers      = xy_markers(:, ok);
	
	% annotate
	h_text			= [];
	delete(findobj(gcf,'type','text'));
	b_corners		= ismember(xy_markers(1,:), x_grid([1 end]))  ...
					& ismember(xy_markers(2,:), y_grid([1 end]));
	i_corners		= find(b_corners);
	plot(im_markers(1,i_corners), im_markers(2,i_corners), 'rx');
	for k = i_corners
		str			= sprintf('(%0.1f,%0.1f)', xy_markers(:,k));
		h_text(k) = text(im_markers(1,k), im_markers(2,k), str, 'color', 'r', ...
			'fontsize', 14, ...
			'horizontalalignment', 'center', 'verticalalignment', 'middle');
    end

	%% create calibration and dewarp
	xy_tilde					= [xy_markers; ones(1,n_markers)];
	P2D							= dlt33([im_markers;ones(1,n_markers)], xy_tilde);
	im_tilde					= P2D * xy_tilde;
	lambda						= [1; 1] * im_tilde(3,:);
	im_fit						= im_tilde(1:2,:) ./ lambda;
	
	im_error					= im_markers - im_fit;
	px_error					= sqrt(im_error(1,:).^2 + im_error(2,:).^2);
	rms_error					= sqrt(mean(px_error.^2));
	
	fprintf('RMS fit error: %0.2f px\n', rms_error);

	%% work out x and y axes
	x_lim					= sort(x_grid([1 end]));
	y_lim					= sort(y_grid([1 end]));
	x_axis					= linspace(x_lim(1), x_lim(2), Nx);
	y_axis					= linspace(y_lim(1), y_lim(2), Ny);
	% calculate dewarping
	[xmat, ymat]			= ndgrid(x_axis, y_axis);
	im_tilde_dewarped		= P2D * [xmat(:) ymat(:) ones(Nx*Ny,1)]';
	lambda					= im_tilde_dewarped(3,:);
	imat					= reshape(im_tilde_dewarped(1,:) ./ lambda, [Nx Ny]);
	jmat					= reshape(im_tilde_dewarped(2,:) ./ lambda, [Nx Ny]);
	im_xy					= interp2(i_axis, j_axis, im', imat, jmat, 'linear');
	
	%% show dewarped image
	figure(2);
	hold off;
	imagesc(x_axis, y_axis, im_xy');
	xlim(x_lim);
	ylim(y_lim);
	colormap gray;
	axis xy equal tight;
	hold on;
	%set(gca,'Clim', image_clim);
	for x = (round(x_lim(1)/abs(grid_dx)) : round(x_lim(end)/abs(grid_dx))) * abs(grid_dx)
		plot([x x], [y_axis(1) y_axis(end)], 'r-');
	end
	for y = (round(y_lim(1)/abs(grid_dy)) : round(y_lim(end)/abs(grid_dy))) * abs(grid_dy)
		plot([x_axis(1) x_axis(end)], [y y], 'r-');
	end
	
	%% query user to save calibration or repeat
	sInput				= input('Would you like to repeat the calibration? y to repeat: ', 's');
	if strcmpi(sInput, 'y')
		continue; % continue without incrementing the counter
	end
	
	%% save output
	fprintf('Saving ... ');
	fstruct				= struct('source_image', im, ...
										'dewarped_image', im_xy, ...
										'plate_pos',  plate_origin(:, calib_idx), ...
										'P', P2D, ...
										'i_axis', i_axis, 'j_axis', j_axis, ...
										'x_axis', x_axis, 'y_axis', y_axis, 'x_lim', x_lim, 'y_lim', y_lim, ...
										'grid_dx', grid_dx, 'grid_dy', grid_dy, ...
										'ij_markers', im_markers, 'xy_markers', xy_markers, ...
										'camera_index', camera_index, ...
										'id', camera_id);
	save(output_file, '-struct', 'fstruct');
	fprintf('done\n');
	calib_idx			= calib_idx + 1;
end