%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot camera model of synthetic calibration
addpath(genpath([pwd '/../../core/']));

%% input parameters
calib_file		= 'L:/shadow/images/AR8.00/setup.mat';
n_particles     = 100;
particle_length = 0.5; % mm
particle_radius = 0.05; % mm
particle_type   = 'rod';

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

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

%% create a random distribution of rods
re_bb_size      = re_bb_lim(2,:) - re_bb_lim(1,:);
re_pos          = diag(re_bb_size)* rand(3,n_particles) + re_bb_lim(1,:).';
re_length       = [0;1;0]*ones(1,n_particles);%randn(3, n_particles);
re_length       = re_length ./ sqrt(sum(re_length.^2,1)) * particle_length;
re_radius       = particle_radius * ones(1, n_particles);

particles3d     = struct('re_pos', num2cell(re_pos, 1), ...
    're_length', num2cell(re_length, 1), ...
    're_radius', num2cell(re_radius, 1), ...
    'type', particle_type);

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

for ic = 1 : n_cameras
	[M_ary(:,:,ic),U_ary(:,:,ic),xc_ary(:,ic)] = frustum(camera(ic));
	pndist		= M_ary(:,:,ic) * [re_pos; ones(1,n_particles)];
	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);

%% create synthetic image for each camera
img_ary         = cell(1, n_cameras);
p2d_ary         = cell(1, n_cameras);
for c = 1 : n_cameras
    fprintf('projecting %d particles in camera %d\n', n_particles, c);
    [p2d_ary{c}, img_ary{c}] = shad_project(setup.camera(c), particles3d); 
end

%% display projected images
bg_dir='F:\mixer\lpt\run15\';
for c = 1 : 3
	bg_file=sprintf('%s/background.cam%d.tif',bg_dir,c-1);
	bg{c}=double(imread(bg_file,2).');
end

for c = 1 : n_cameras
    figure(100+c);
    clf;
    imagesc(i_axis, j_axis, (bg{c}.*img_ary{c}).');
    axis equal tight xy;
    colormap gray;

	hold on;
	im_pos   = [p2d_ary{c}.im_pos];
	im_length= [p2d_ary{c}.im_length]; 
	im_ends  = [im_pos+im_length/2; im_pos-im_length/2];
	plot(im_pos(1,:), im_pos(2,:), 'ro');
	plot(im_ends([1 3],:), im_ends([2 4],:), 'r-', ...
		'linewidth', 2);
	
end

%% plot particles
re_endpoints = cat(1, re_pos+re_length/2, re_pos-re_length/2);
figure(1);
clf;

% plot rods
plot3(re_endpoints([1 4], :), re_endpoints([2 5], :), re_endpoints([3 6], :), ...
    'r-', 'linewidth', 2);
hold on;

% plot viewing frustums
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
	% point sources
	otf			= camera(ic).otf;
	plot3(otf.re_source(1,:), otf.re_source(2,:), otf.re_source(3,:), '.', 'color', cmap(ic,:));
	% 
end

axis equal tight xy;
axis(setup.re_bb_lim(:)*2);
camup([0 1 0]);
view(0,0);