%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot camera model of synthetic calibration

%% input parameters
calib_file		= 'test\C3_f20_N4.0.mat';
% example source data
src_file		= 'L:\array\tcf\rescaled\z001_t001.mat';
font_size		= 24;
fig_offset		= 00;

%% load setup
fprintf('loading camera setup from %s\n', calib_file);
setup			= importdata(calib_file);
camera			= setup.camera;
n_cameras		= setup.n_cameras;
delta_t			= setup.delta_t;

i_axis			= camera(1).i_axis;
j_axis			= camera(1).j_axis;
di				= i_axis(2)-i_axis(1);
dj				= j_axis(2)-j_axis(1);
n_i				= length(i_axis);
n_j				= length(j_axis);
i_lim			= [min(i_axis) max(i_axis)];
j_lim			= [min(j_axis) max(j_axis)];
t				= (-1 : 1) * delta_t;

im_corners		= [i_lim([1 1 2 2]); j_lim([1 2 2 1])];

fprintf('# cameras:    %d\n', n_cameras);
fprintf('resolution:   %d x %d\n', n_i, n_j);
fprintf('delta t:      %0.3f ms\n', delta_t*1e3);

%% calculate particle displacement
fprintf('loading sample data...\n');
fs				= load(src_file);
re_part			= fs.x;
u_part			= fs.u;
re_part_t0		= re_part - delta_t * u_part;
re_part_t1		= re_part;
re_part_t2		= re_part + delta_t * u_part;

re_lim_data		= [min(fs.x, [], 2), max(fs.x, [], 2)];
n_part			= fs.n_part;

%% evaluate frustum
M_ary			= zeros(4, 4, n_cameras);
U_ary			= zeros(3, 4, n_cameras);
xc_ary			= zeros(3, n_cameras);
b_visible		= false(n_cameras, n_part);

for ic = 1 : n_cameras
	[M_ary(:,:,ic),U_ary(:,:,ic),xc_ary(:,ic)] = frustum(camera(ic));
	pndist		= M_ary(:,:,ic) * [re_part_t1; ones(1,n_part)];
	ok			= all(pndist>=0,1);
	b_visible(ic,:)=ok;
end
n_min_cams		= 3;
b_visible3		= sum(b_visible,1)>=n_min_cams;
n_visible3		= nnz(b_visible3);
fprintf('%d particles visible in at least %d cameras\n', n_visible3, n_min_cams);

%% 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));

%% plot particle distribution
figure(fig_offset+11);
clf;

cmap			= hsv(n_cameras);

for ic = 1 : n_cameras
	xc			= camera(ic).calib.xc;
	plot3(xc(1), xc(2), xc(3), 'ko');
	re_corners	= back_project_1c(camera(ic).calib, [0 0 -1 re_bb_lim(2,3)], im_corners, true);
	P			= patch(re_corners(1,:), re_corners(2,:), re_corners(3,:), cmap(ic,:));
	set(P, 'FaceAlpha', 0.5, 'edgecolor', cmap(ic,:));
	hold on;
	for k = 1 : 4
		plot3([xc(1) re_corners(1,k)], [xc(2) re_corners(2,k)], [xc(3) re_corners(3,k)], '-', 'color', cmap(ic,:));
	end
	% check...
	dx			= re_corners-xc;
	dx_len		= sqrt(sum(dx.^2,1));
	gamma		= dot(U_ary(:,:,ic), dx, 1) ./ dx_len;

	% point sources
	otf			= camera(ic).otf;
	plot3(otf.re_source(1,:), otf.re_source(2,:), otf.re_source(3,:), '.', 'color', cmap(ic,:));
	
	% show particles in this camera
	%plot3(re_part_t1(1,b_visible(ic,:)), re_part_t1(2,b_visible(ic,:)), re_part_t1(3,b_visible(ic,:)), '.');
end

subset=1:10000;
%plot3(re_part_t0(1,subset), re_part_t0(2,subset), re_part_t0(3,subset), 'r.');
%hold on;
%plot3(re_part_t1(1,subset), re_part_t1(2,subset), re_part_t1(3,subset), 'k.');
%plot3(re_part_t2(1,subset), re_part_t2(2,subset), re_part_t2(3,subset), 'b.');


axis equal tight xy;
%xlim(re_bb_lim(:,1));
%xlim(re_lim_data(1,:));
%ylim(re_lim_data(2,:));
camup([0 1 0]);

xlabel('$x$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
ylabel('$y$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
zlabel('$z$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
set(gca,'fontsize',font_size,'TickLabelInterpreter','latex');