%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate a chessboard calibration target with a QR code fiduciary marker
%
% calibration target images have a QR code to mark the origin that
% specifies the dimension of the grid in (u,v) as well as the chessboard
% dimension and units
%
% the QR code is placed in the bottom left corner of the chessboard

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

%% input parameters
cimg_dir        = 'L:\array\tcf\6C_A\calib\';
cimg_pat        = '%s/target_asdf.png';

grid_units      = 'mm';
grid_spacing    = 30;               % dimension of grid spacing
grid_dimension  = [9 5];          % dimensions of checkerboard grid
font_size       = 24;

%% generate chessboard target
[img_plate,u_axis,v_axis] = qrcode_target(grid_dimension, grid_spacing, grid_units);

img_resolution   = length(u_axis) / (grid_spacing*grid_dimension(1)*1e-3); % pixels per m
img_size_mm      = size(img_plate)/img_resolution *1e3;

%% write calibration plate image
cimg_file       = sprintf(cimg_pat, cimg_dir);
fprintf('saving to %s\n', cimg_file);
imwrite(img_plate, cimg_file, ...
    'ResolutionUnit', 'meter', 'XResolution', img_resolution, 'YResolution', img_resolution);
fprintf('print this calibration image at full size: %0.1f x %0.1f mm\n', img_size_mm);
fprintf('u_lim = [%f %f];\n', u_axis([1 end]));
fprintf('v_lim = [%f %f];\n', v_axis([1 end]));

%% extract qr code
[msg,fmt,loc] = readBarcode(img_plate.', 'QR-CODE');
code = qrcode_read_string(msg);
du=mean(diff(u_axis));
dv=mean(diff(v_axis));
qr_corners = loc(1:3,:)*du + [u_axis(1) v_axis(1)];

%% display image 
figure(1);
clf;
imagesc(u_axis, v_axis, img_plate');
hold on;
plot(qr_corners(1,1), qr_corners(1,2), 'ro');
plot(qr_corners(2,1), qr_corners(2,2), 'go');
plot(qr_corners(3,1), qr_corners(3,2), 'bo');
str=sprintf('%0.1f,%0.1f', code.origin.*code.spacing);
text(mean(qr_corners(:,1)), mean(qr_corners(:,2)), str, 'color', 'r', 'fontsize', font_size);

axis equal tight xy;
colormap gray;
xlabel('$u$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
ylabel('$v$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
set(gca,'ticklabelinterpreter', 'latex','fontsize',font_size);
title('calibration target', 'interpreter', 'latex', 'fontsize', font_size);