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

%% input parameters
src_dir			= 'L:\array\tcf\30C_E\';
src_pat			= '%s/z001_t001.cam%d.tif';
cal_file		= 'L:\array\tcf\calib\C30_f20_N4.0_E.mat';

% particle matching and triangulation options
mopts					= struct();
mopts.img_threshold		= [50 50 40];
mopts.R_search_obj		= 2.5;	% search radius for new particle
mopts.R_tolerance_img	= 1.0;	% triangulation tolerance, px
mopts.R_tolerance_obj	= 0.3;	% tolerance for projection error, mm
mopts.K_feature			= [5 3];% feature size
mopts.n_search			= 3;	% number of candidate particles to consider in search radius

% general options
opts				= struct();
opts.matching		= mopts;
opts.b_debug_plot	= 0;
opts.precision		= 'single';

%% load calibration
setup			= load(cal_file);
n_cameras		= length(setup.camera);
src_file_ary	= cell(1,n_cameras);
for ic = 1 : n_cameras
	src_file_ary{ic} = sprintf(src_pat, src_dir, ic);
end
delta_t			= setup.delta_t;

%% track!
t_start			= tic;
tracks			= rgblpt(setup, src_file_ary, opts);
t_proc			= toc(t_start);

n_tracks		= length(tracks);

fprintf('took %0.1f seconds\n', t_proc)
fprintf('found %d particles\n', n_tracks);

%% plot result
figure(2);
clf;
cmap	= {'r','g','b'};
xp		= [tracks.x];
up		= [tracks.u];

re_pos		= [tracks.re_pos];
re_pos_t1	= re_pos(:, 1:3:end);
re_pos_t2	= re_pos(:, 2:3:end);
re_pos_t3	= re_pos(:, 3:3:end);

hold on;
% plot3(re_pos_t1(1,:), re_pos_t1(2,:), re_pos_t1(3,:), 'r.');
% plot3(re_pos_t2(1,:), re_pos_t2(2,:), re_pos_t2(3,:), 'g.');
% plot3(re_pos_t3(1,:), re_pos_t3(2,:), re_pos_t3(3,:), 'b.');
% 

for it = 1 : n_tracks
	t	= tracks(it);
% 	plot3(t.re_pos(1,:), t.re_pos(2,:), t.re_pos(3,:), 'k-');
end

scale=5*delta_t;
caxis([0 170]*scale);
% quiver3(xp(1,:), xp(2,:), xp(3,:), ...
% 		up(1,:)*scale, up(2,:)*scale, up(3,:)*scale, 0, 'k-');
quiverC3D(xp(1,:), xp(2,:), xp(3,:), ...
	up(1,:)*scale, up(2,:)*scale, up(3,:)*scale, 'scale', 0);
axis equal tight xy;
xlabel('$x$ [mm]', 'interpreter', 'latex', 'fontsize', 20);
ylabel('$y$ [mm]', 'interpreter', 'latex', 'fontsize', 20);
zlabel('$z$ [mm]', 'interpreter', 'latex', 'fontsize', 20);
set(gca, 'ticklabelinterpreter', 'latex', 'fontsize', 20);

axis(setup.re_bb_lim(:));
set(gca,'ytick',-150:50:150);

view(2);

