%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate synthetic calibration images
%
% based on a known calibration model, generate synthetic images of a
% calibration target, which can be used to test automatic checkerboard
% calibration
%
% calibration target images have a QR code to mark the origin that
% specifies the dimension of the grid in (u,v)

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

%% input parameters
setup_file		= 'L:\array\tcf\calib\C6_f20_N4.0_A.mat';
cimg_dir        = 'L:\array\tcf\6C_A\calib\';
cimg_pat        = '%s/cal%03d.cam%d.tif';
plate_pat       = '%s/target.png';

n_target_pos    = 7;               % number of calibration target positions
theta_range     = [-30 0 30]/180*pi;% orientations of target wrt x axis 
font_size       = 24;
b_debug         = false;            % include debugging plots?
u_lim = [-199.945946 199.945946];
v_lim = [-119.945946 119.945946];

%% load
fprintf('loading calibration from %s\n', setup_file);
setup           = load(setup_file);

%% report size of measurable volume
re_bb_lim		= setup.re_bb_lim;
fprintf('reconstruction limits are:\n');
fprintf('  x = %+6.1f - %+6.1f mm\n', re_bb_lim(:,1));
fprintf('  y = %+6.1f - %+6.1f mm\n', re_bb_lim(:,2));
fprintf('  z = %+6.1f - %+6.1f mm\n', re_bb_lim(:,3));

%% load image of calibration target
plate_file      = sprintf(plate_pat, cimg_dir);
fprintf('loading calibration plate image from %s\n', plate_file);
img_plate       = double(imread(plate_file));
u_axis          = linspace(u_lim(1), u_lim(2), size(img_plate,1));
v_axis          = linspace(v_lim(1), v_lim(2), size(img_plate,2));
poly_plate      = polyshape(u_axis([1 end end 1]), v_axis([1 1 end end]));

%% draw calibration target
figure(1);
clf;
imagesc(u_axis, v_axis, img_plate');
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);

%% define positions and orientations of target
% (u,v,w) are coordinates of plate
% (x,y,z) are coordinates of world
% plate maps to world coordinate system via
%   x = x0 + u cos(theta)
%   y = y0 - v
%   z = z0 + u sin(theta)
% 
% theta is angle of u-axis with respect to x axis 
% (x0,y0,z0) is origin of calibration target
% 
n_theta         = length(theta_range);
x_range         = linspace(re_bb_lim(1,1), re_bb_lim(2,1), n_target_pos);
y_range         = mean(re_bb_lim(:,2));
z_range         = mean(re_bb_lim(:,3));
[x_target,theta_target]= ndgrid(x_range, theta_range);
n_target        = numel(x_target);
theta_target    = theta_target(:);

% origin of calibration plate in reconstruction coordinate system
re_target       = [x_target(:).'; ones(1,n_target)*y_range; ones(1,n_target)*z_range];
% coordinate system transformation
% re = pl_to_re*pl + re_origin
pl_to_re_target = zeros(3,3,n_target);
for n = 1 : n_target
    theta       = theta_target(n);
    pl_to_re_target(:,:,n) = [cos(theta) 0 sin(theta); 0 -1 0; sin(theta) 0 -cos(theta)];
end
% equation of plane for each calibration plate
% M			equation of plane. M(1:3).x + M(4) = 0
M_ary = zeros(4,n_target);
for n = 1 : n_target
    e_normal     = pl_to_re_target(:,3,n);
    M_ary(1:3,n) = e_normal;
    M_ary(4,n) = -dot(re_target(:,n), e_normal);
end

%% save ground truth
gt_file = sprintf('%s/ground_truth.mat', cimg_dir);
fprintf('saving ground truth to %s\n', gt_file);
gt = struct();
% the camera model
gt.setup        = setup;
% the configuration of the calibration plate
gt.theta_range  = theta_range;
gt.x_range      = x_range;
gt.y_range      = y_range;   
gt.z_range      = z_range;
gt.grid_spacing = grid_spacing;
gt.grid_dimension=grid_dimension;
% orientation and position of plate
gt.pl_to_re     = pl_to_re_target;
gt.re_origin    = re_target;

%% create synthetic images of calibration target
% create directory for calibration images
if exist(cimg_dir, 'dir') == 0
    mkdir(cimg_dir);
end

% iterate over calibration target positions
for n = 1 : n_target
    for c = 1 : setup.n_cameras
        %% check visibility of target in (u,v) plane
        cmodel  = setup.camera(c);
        i_axis  = setup.camera(c).i_axis;
        j_axis  = setup.camera(c).j_axis;
        b_distort=any(cmodel.calib.kr ~= 0);% do we have to apply distortion model?
        
        % check visibility of planar target
        im_corner=[i_axis([1 end end 1]); j_axis([1 1 end end])];
        re_corner=back_project_1c(cmodel.calib, M_ary(:,n), im_corner, b_distort);
        pl_corner= pl_to_re_target(:,:,n) \ (re_corner - re_target(:,n));
        % check intersection is non-zero
        poly_visible  = polyshape(pl_corner(1,:), pl_corner(2,:));
        poly_intersect= intersect(poly_visible, poly_plate);
        b_visible     = area(poly_intersect) > 0;

        %% interpolate to make an image 
        cimg     = ones(length(i_axis),length(j_axis));
        if b_visible
            % back project
            [im,jm] = ndgrid(i_axis, j_axis);
            im_pts  = [im(:) jm(:)].';        
            re_pts  = back_project_1c(cmodel.calib, M_ary(:,n), im_pts, b_distort);
            pl_pts  = pl_to_re_target(:,:,n) \ (re_pts - re_target(:,n));
            % interpolate
            cimg    = interpn(u_axis, v_axis, img_plate, pl_pts(1,:), pl_pts(2,:), 'linear', 1);
            cimg    = reshape(cimg, size(im));
        end

        %% save
        cimg_file=sprintf(cimg_pat, cimg_dir, n, cmodel.id);
        fprintf('writing camera %d for target %d to %s\n', c, n, cimg_file);
        imwrite(cimg, cimg_file, 'tif');

        %% debug plot
        if b_debug
            figure(2);
            clf;
            imagesc(i_axis,j_axis,cimg.');
            axis equal tight ij;
            colormap gray;
            titstr=sprintf('camera %d, plate position %d', c, n);
            title(titstr, 'interpreter', 'latex', 'fontsize', font_size);
            drawnow;
        end
    end
end
