%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% track particles in shadowgraphic imaging movie using predictor-corrector scheme
%
addpath([pwd '/../../misc/']);

%% input parameters
b_execute				= true;	% start tracking now?

job_dir					= 'L:\shadow\jobs';
src_dir					= 'L:\shadow\images\AR8.00_3C\';
calib_file				= 'L:\shadow\images\AR8.00_3C\setup.mat';
src_file_idx			= 1;

[~,dset]				= fileparts(fileparts(src_dir));
job_file				= sprintf('%s/%s_%04d.mat', job_dir, dset, src_file_idx);

% pattern of source images to load
src_file_pat			= '%s/S0001_F%05d.cam%d.tif';
src_file_fun			= @(frame,cam) sprintf(src_file_pat, src_dir, frame, cam);
% pattern of background image to load
bg_file_pat				= '%s/background.cam%d.tif';
bg_file_fun				= @(cam) sprintf(bg_file_pat, src_dir, cam-1);
% pattern of track files to save
trk_file_pat			= '%s/tracks/S0001.mat';
trk_file				= sprintf(trk_file_pat, src_dir);

scale					= 1/100;			% approximate scale, mm/px

% particle detection and matching options
mopts					= struct();
mopts.algorithm			= 'steerable';
if strcmp(mopts.algorithm, 'steerable')
	% steerable filter matching options
	mopts.steerable				= struct();
	mopts.steerable.r_filter	= 5;			% scale of ridge filter, px
	mopts.steerable.im_threshold= 0.6*255;		% threshold for particle detection
	mopts.steerable.b_overlap   = true;			% enable detection of overlapping particles?
	mopts.steerable.n_theta		= 16;			% angular resolution for multi-particle matching
	mopts.steerable.l_max_px	= 100;			% longest length of rod, px
elseif strcmp(mopts.algorithm, 'template')
	% template matching options
	mopts.template              = struct();
	mopts.template.kernel		= fspecial('disk',2);
	mopts.template.threshold    = 0.25;
elseif strcmp(mopts.algorithm, 'circle')
	% hough transform circle matching options
	mopts.circle				= struct();
	mopts.circle.threshold		= 100;			% edge threshold for circle detection
	mopts.circle.radius			= [6 12];		% radius in px for circular search
	mopts.circle.polarity		= 'bright';       % polarity (for shadowgraph, is dark)
	mopts.circle.binning		= 1;			% factor by which to downsample image in search (helps accelerate search)
else
	error('unrecognised algorithm');
end
% tracking search area
mopts.R_search_img		= 15;           % search distance for new matches, image space, px
mopts.R_search_obj		= scale*mopts.R_search_img;   % search distance, object space, mm
mopts.v_mean			= [0;0;0];		% mean velocity in measurement volume

% triangulation tolerance
triopts					= struct();
triopts.R_tolerance_img = 15;			% tolerance for projection error, px
triopts.R_tolerance_obj	= 2;%scale*triopts.R_tolerance_img;

% tracking options
topts					= struct();
topts.frame_range		= 1 : 100;		%(props.n_frames-1);% index of frames from movie to track
topts.frame_rate		= 500;			% frame rate, Hz
topts.save_threshold	= 5;			% minimum length of track to save
topts.long_threshold	= 10;			% threshold defining long tracks
topts.n_lives_default	= 2;			% default # of frames to follow missing particle for
topts.n_lives_max		= 6;			% max # of frames to follow missing particle for
topts.b_debug_plot		= 1;			% produce a plot for debugging purposes
topts.triangulation		= triopts;		% options for triangulation procedure
topts.matching			= mopts;		% options for matching procedure

% reconnection options
recon.n_truncate		= 2;						% cut this many points off ends of every track
recon.n_min_length		= 7;						% minimum length of tracks after reconnection
recon.n_search			= 60;						% search interval, in frames
recon.r_collision		= 0.050;					% minimum distance between two tracks
recon.C_threshold		= 0.15*[1 1];				% cost threshold for reconnection
recon.tau				= [0 1e6];					% time axis for cost thresold
recon.f_cutoff			= 0.1*topts.frame_rate;		% cutoff frequency for smoothing spline filter
topts.recon				= recon;

% file path
topts.calib_file		= calib_file;	% path to camera calibration
topts.trk_file			= trk_file;

%% define image preprocessing
fprintf('Loading background image ... ');
preproc			= cell(1, 3);
for c = 1 : 3
	preproc{1,c}	= struct('type', 'null');
end
topts.preprocessing	= preproc;
fprintf('done\n');

%% load calibration
setup   = load(calib_file);
setup.gl_bb_lim=setup.re_bb_lim;

%% set up parameters for processing
% define source files
[fidx, cidx] = ndgrid(topts.frame_range, 0:2);
src_ary = arrayfun( ...
		src_file_fun, ...
		fidx, cidx, ...
		'UniformOutput', false);

% get tracker
tracker = ShadowTrack(setup, topts);

%% track!
tracker.track(src_ary);
% reconnect tracks
%tracker.reconnect();

%% fit smoothing spline
tracker.fit_spline(recon.f_cutoff); 

%% save 
tracker.save(trk_file);

%% display result
font_size=36;

figure(1);
clf;
hold on;
plot3([],[],[]);
axis equal tight xy;
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,'TickLabelInterpreter','latex','FontSize',font_size);

n_tracks=numel(tracker.tracks);
tlen = arrayfun(@(t) nnz(~t.ext), tracker.tracks);
[~,longest]=max(tlen);
i_plot=find(tlen>20);
for n = i_plot
	%cla;
	trk			= tracker.tracks(n);
	t			= trk.t;
	% unfiltered position
	re_pos		= squeeze(trk.re_pos);
	re_length   = squeeze(trk.re_length);
	re_endpoints= cat(1, re_pos+re_length/2, re_pos-re_length/2);
	% filtered position
	re_vel		= squeeze(trk.re_vel);
	re_vel_mag  = sqrt(sum(re_vel.^2,1));
	re_acc		= squeeze(trk.re_acc);
	re_acc_mag  = sqrt(sum(re_acc.^2,1));
	fl_pos		= squeeze(trk.re_pred);
	fl_length	= squeeze(trk.fl_length);
	fl_endpoints= cat(1, fl_pos+fl_length/2, fl_pos-fl_length/2);

	% tumbling rate
	e_p			= fl_length ./ sqrt(sum(fl_length.^2,1));
	pdot		= gradient(e_p, t, 1);
	pdot_mag	= sqrt(sum(pdot.^2,1));
	%pdot_mag(isnan(pdot_mag)) = 0;

	%plot3(re_pos(1,:), re_pos(2,:), re_pos(3,:), 'ko');
	%plot3(fl_pos(1,:), fl_pos(2,:), fl_pos(3,:), 'k-', 'linewidth', 2);
	
% 	plot3(re_endpoints([1 4],:), ...
% 		re_endpoints([2 5],:), ...
% 		re_endpoints([3 6],:), ...
% 		'ko', 'linewidth', 1);
	
	%cdata([2],:)=nan;
	rng=find(~trk.ext,1,'first')+1 : find(~trk.ext,1,'last')-1;
	cdata=pdot_mag([1 1],rng);
	hs=surf(fl_endpoints([1 4],rng), ...
		fl_endpoints([2 5],rng), ...
		fl_endpoints([3 6],rng), ...
		cdata, ...
		'linewidth', 2, ...
		'facecolor', 'none', ...
		'edgecolor', 'flat');

% 	plot3(fl_endpoints([1 4],:).', ...
% 		fl_endpoints([2 5],:).', ...
% 		fl_endpoints([3 6],:).', ...
% 		'ko', 'linewidth', 2);
	%drawnow;
	%pause();
end

%caxis([0 120]);
caxis([0 100]);

%cb=colorbar('location','north','TickLabelInterpreter','latex');
%title(cb, 'Tumbling Rate [s$^{-1}$]', 'interpreter', 'latex', 'fontsize', font_size);
axis equal tight xy;
%axis(setup.gl_bb_lim(:));
%colormap(1-summer);
%colormap(brewermap(65,'YlOrRd'));
colormap jet;
view(-90,90);
box on;
