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

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

%% input parameters

grid_dx				= -1;		% real units, mm
grid_dy				= +1;
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;
corner_metric		= 0.1;		% threshold for identifying corners
dot_half_window		= [64 64];


output_dir			= 'E:\mixer\lpt\calibration\21-06-14\';
input_file_list		= {'E:\mixer\lpt\calibration\21-06-14\C15794_im02.tif'};
plate_origin		= [0	0	0]';
camera_index		= 1;
camera_id			= 1;
calib_idx_offset	= 0;
origin_pos			= [0 0];

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					= readimg(input_file);
    im                  = double((im{1}'));
	Nx					= size(im, 1);
	Ny					= size(im, 2);
	i_axis				= 0 : Nx-1;
	j_axis				= 0 : Ny-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(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);
	
	%% 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 2 2 1 1], 1), im_corners([1 1 2 2 1], 2), 'r-');
	rx					= i_axis > im_corners(1,1) & i_axis < im_corners(2,1);
	ry					= j_axis > im_corners(1,2) & j_axis < im_corners(2,2);
	% crop filtered image
	im_crop				= zeros(Nx,Ny);
	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				= [];
	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);
		ij_markers			= ij_markers'; % 2 x n_markers
		
		%% update figure or try again
		if isempty(ij_markers)
			fprintf('no valid markers found.\n');
			fprintf('try again with a different threshold\n\n');
		else
			%% display
			figure(1);
			hold on;
			delete(h_markers);
			delete(h_text);
			h_markers		= plot(ax(1), ij_markers(1,:), ij_markers(2,:), 'g+');
		end
		
		%% prompt for new threshold
		ret				= input('enter a new corner metric or nan to accept: ', 's');
		corner_metric	= str2double(ret);
	end
	
	%% get coordinates of markers
	% create grid
	x_grid			= (1 : board_size(2)-1) * grid_dx;
	y_grid			= (1 : board_size(1)-1) * grid_dy;
	x_grid			= x_grid - round(mean(x_grid));
	y_grid			= y_grid - round(mean(y_grid));
	[xm, ym]		= meshgrid(x_grid, y_grid);
	xy_markers		= [xm(:) ym(:)]';
	n_markers		= prod(board_size-1);
	
	% annotate
	h_text			= [];
	delete(findobj(gcf,'type','text'));
	for k = [1 board_size(1)-1 n_markers n_markers-board_size(1)+2]
		str			= sprintf('(%0.1f,%0.1f)', xy_markers(:,k));
		h_text(k) = text(ij_markers(1,k), ij_markers(2,k), str, 'color', 'b', ...
			'fontsize', 18, ...
			'horizontalalignment', 'center', 'verticalalignment', 'middle');
	end
	
	%% create calibration and dewarp
	xy_tilde					= [xy_markers; ones(1,n_markers)];
	P2D							= dlt33([ij_markers;ones(1,n_markers)], xy_tilde);
	ij_tilde					= P2D * xy_tilde;
	lambda						= [1; 1] * ij_tilde(3,:);
	ij_fit						= ij_tilde(1:2,:) ./ lambda;
	
	ij_error					= ij_markers - ij_fit;
	px_error					= sqrt(ij_error(1,:).^2 + ij_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);
	ij_tilde_dewarped		= P2D * [xmat(:) ymat(:) ones(Nx*Ny,1)]';
	lambda					= ij_tilde_dewarped(3,:);
	imat					= reshape(ij_tilde_dewarped(1,:) ./ lambda, [Nx Ny]);
	jmat					= reshape(ij_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', ij_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