%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% qrcode_calibration.m
% find markers in images of calibration plates with QR code fiduciary
% markers
% then perform a 2D calibration to dewarp the calibration plate image

addpath(genpath([pwd '/../core/']));
addpath([pwd '/../../misc/']);

%% input parameters
font_size           = 20;

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

b_skip_owrite_check = true;     % skip checking for overwrite?
b_skip_confirm_save = true;     % skip save confirmation?

output_dir			= 'L:\array\tcf\6C_A\calib\';
n_planes            = 21;

camera_index		= 5;
camera_id			= camera_index;
calib_idx_offset	= 0;

input_file_list		= arrayfun(@(n) sprintf('%s/cal%03d.cam%d.tif', output_dir, n, camera_index), ...
                        1:n_planes, ...
                        'UniformOutput', false);
image_clim			= [0 255];

%% issue warnings to stop user doing stupid things
if b_skip_owrite_check
    fprintf('WARNING: calibration files will be automatically overwritten\n');
    fprintf('are you sure you want to continue? Ctrl+C to stop or any key to continue');
    pause();
end
if b_skip_confirm_save
    warning('Calibration files will be automatically written!');
end

%% 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);
    fprintf('%s\n', input_file);
	
	% check if calibration already exists
	if exist(output_file, 'file') && ~b_skip_owrite_check
		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)/255;
	n_i					= size(im, 1);
	n_j					= size(im, 2);
	
	% define image coordinate system
	i_axis				= (1 : n_i) - (n_i/2 + 1);
	j_axis				= (1 : n_j) - (n_j/2 + 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', [3 3]);
		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;
    % show chessboard image 
	h_filt = imagesc(i_axis, j_axis, im_filt');
    ax=gca;
    hold on;
    axis equal ij;
    xlim(i_axis([1 end]));
    ylim(j_axis([1 end]));
    colormap gray;
    title(input_file, 'interpreter','none','fontsize', font_size);

	%% read QR code calibration target
    target              = qrcode_read_target(im_filt,i_axis,j_axis);
    if target.n_markers == 0
        warning('no markers identified');
    end

    %% plot identified markers
    % show masked regions
    plot(ax, target.roimask);
    plot(ax, target.qrmask);
    
    % show QR code fiduciary markers
    plot(target.im_qrpts(1,:), target.im_qrpts(2,:), 'gs');
    for m = 1 : target.n_qrpts
        str=sprintf('%d) %0.1f, %0.1f', m, target.uv_qrpts(:,m));
        text(ax, target.im_qrpts(1,m), target.im_qrpts(2,m), str, 'color', 'g', 'fontsize', 12, 'rotation', 30);
    end
    
    % show chessboard corner markers
    plot(target.im_markers(1,:), target.im_markers(2,:), 'rx-');
    for m = 1 : target.n_markers
        str=sprintf('(%0.0f, %0.0f)', target.uv_markers(:,m));
        th=text(ax, target.im_markers(1,m), target.im_markers(2,m), str, 'color', 'r', 'fontsize', 12, 'rotation', 30);
    end
    drawnow;

	%% create calibration and dewarp
    % fit 2D pinhole model with direct linear transform
	target.uvt_markers			= [target.uv_markers; ones(1,target.n_markers)];
    target.imt_markers          = [target.im_markers; ones(1,target.n_markers)];
	P2D							= dlt33(target.imt_markers, target.uvt_markers);
    % evaluate fit
	im_fit					    = P2D * target.uvt_markers;
	im_fit						= im_fit(1:2,:) ./ im_fit(3,:);
	% evaluate error
	im_error					= target.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 u and v axes for dewarping calibration plate
	u_axis					= linspace(target.u_lim(1), target.u_lim(2), n_i);
	v_axis					= linspace(target.v_lim(1), target.v_lim(2), n_j);
	% calculate dewarping
	[umat, vmat]			= ndgrid(u_axis, v_axis);
	im_tilde_dewarped		= P2D * [umat(:) vmat(:) ones(n_i*n_j,1)]';
	lambda					= im_tilde_dewarped(3,:);
	imat					= reshape(im_tilde_dewarped(1,:) ./ lambda, [n_i n_j]);
	jmat					= reshape(im_tilde_dewarped(2,:) ./ lambda, [n_i n_j]);
	im_dewarped				= interp2(i_axis, j_axis, im', imat, jmat, 'linear');
	
	%% show dewarped image
	figure(2);
	hold off;
	imagesc(u_axis, v_axis, im_dewarped');
	colormap gray;
	axis xy equal;
    xlim(target.u_lim);
	ylim(target.v_lim);
	hold on;
	%set(gca,'Clim', image_clim);
    % draw gridlines aligned with grid spacing
    ugrid = (-100:100)*target.grid_spacing;
    vgrid = (-100:100)*target.grid_spacing;
    for u = ugrid
        plot(u*[1 1], target.v_lim, 'r-');
    end
    for v = vgrid
        plot(target.u_lim, v*[1 1], 'r-');
    end
    %[~,tstr]=fileparts(input_file);
    title(input_file, 'interpreter','none','fontsize', font_size);
    xlabel('$u$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
    ylabel('$v$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
    set(gca,'ticklabelinterpreter','latex','fontsize',font_size);
    drawnow;

	%% query user to save calibration or repeat
    if ~b_skip_confirm_save
	    sInput				= input('Would you like to repeat the calibration? y to repeat: ', 's');
	    if strcmpi(sInput, 'y')
		    continue; % continue without incrementing the counter
	    end
    end

	%% save output
	fprintf('Saving ... ');
	fstruct				= struct('source_image', im, ...
								'dewarped_image', im_dewarped, ...
                                'target', target, ...
								'P', P2D, ...
								'i_axis', i_axis, 'j_axis', j_axis, ...
								'x_axis', u_axis, 'y_axis', v_axis, 'x_lim', target.u_lim, 'y_lim', target.v_lim, ...
								'grid_dx', target.grid_spacing, 'grid_dy', target.grid_spacing, ...
								'ij_markers', target.im_markers, 'xy_markers', target.uv_markers, ...
                                'units', target.grid_units', ...
								'camera_index', camera_index, ...
								'id', camera_id);
	save(output_file, '-struct', 'fstruct');
	fprintf('done\n');
	calib_idx			= calib_idx + 1;
end