%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compare particle tracks from simulation agaist reference data
addpath([pwd '/../']);
%% input parameters
src_dir					= 'L:\shadow\images\AR8.00_3C\';
ref_file_idx			= 1;
ref_file_pat			= '%s/S0001_F%05d.mat';
trk_file_pat			= '%s/tracks/S0001.mat';
trk_file				= sprintf(trk_file_pat, src_dir);

ghost_tolerance			= 0.5; % triangulation tolerance for ghosts
n_theta_bins			= 100;

%% load tracks
fprintf('load %s\n', trk_file);
ds			= load(trk_file);
n_tracks	= length(ds.tracks);
fprintf('%d tracks\n', n_tracks);

%% create tracker object
tracker			= ShadowTrack(ds.setup, ds.opts);
tracker.add_track(tracker.pad(ds.tracks));

%% compare against reference
n_t				= tracker.n_t;
err_pos_sum		= zeros(3, 1);
err_len_sum     = zeros(3, 1);
err_theta_mean	= 0;
cost_hist		= zeros(1, n_theta_bins+1);
cost_edges		= linspace(-1, 1, n_theta_bins+1);
cost_bins		= (cost_edges(2:end)+cost_edges(1:end-1))/2;

n_real_sum		= 0;
n_ghost_sum     = 0;
n_ref_sum		= 0;

for it = 1 : n_t
	%% load file
	ref_file	= sprintf(ref_file_pat, src_dir, tracker.frame_range(it));
	fprintf('load %s\n', ref_file);
	ref			= load(ref_file);

	ref_pos		= [ref.particles3d.re_pos];
	ref_length  = [ref.particles3d.re_length];
	n_ref		= size(ref_pos, 2);

	%% get position
	b_ext		= tracker.get_track_property(1:n_tracks, it, 'ext', true);
	meas_pos	= squeeze(tracker.get_track_property(~b_ext, it, 're_pos', true));
	meas_length = squeeze(tracker.get_track_property(~b_ext, it, 're_length', true));
	n_particles = size(meas_pos,2);

	%% find correspondences
	[i_nn, dist] = knnsearch(ref_pos.', meas_pos.', 'K', 1);
	b_real		= dist < ghost_tolerance;
	n_ghost		= nnz(~b_real);
	n_real		= nnz(b_real);
	i_nn		= i_nn(b_real);

	real_pos    = meas_pos(:, b_real);
	real_length = meas_length(:, b_real);

	fprintf('[%4d/%4d] %d ref, %d found, %d ghosts\n', it, n_t, n_ref, n_particles, n_ghost);

	%% measure error
	err_pos		= real_pos - ref_pos(:, i_nn);
	cos_theta   = dot(real_length, ref_length(:,i_nn), 1) ...
		./ sqrt(sum(real_length.^2, 1) .* sum(ref_length(:,i_nn).^2,1));
	cost_samp_hist  = histc(cos_theta, cost_edges);

	%% accumulate errors
	err_pos_sum		= err_pos_sum + sum(err_pos.^2, 2);
	%err_len_sum		= err_len_sum + sum(err_length.^2, 2);
	cost_hist		= cost_hist + cost_samp_hist;
	n_real_sum		= n_real_sum + n_real;
	n_ghost_sum     = n_ghost_sum + n_ghost;
	n_ref_sum		= n_ref_sum + n_ref;
end

err_pos_rms = sqrt(err_pos_sum / n_real_sum);

%% report
fprintf('%5d reference particles\n', n_ref_sum);
fprintf('%5d real  particles (%0.1f%%)\n', n_real_sum, n_real_sum/n_ref_sum*100);
fprintf('%5d ghost particles (%0.1f%%)\n', n_ghost_sum, n_ghost_sum/n_ref_sum*100);

fprintf('RMS position error:\n');
fprintf('%6.3f um\n', err_pos_rms*1e3);

%% histogram of alignment error
figure(1);
clf;
plot(cost_edges, cost_hist, '.-');
pbaspect([4/3 1 1]);
xlabel('Orientation error $\cos(\theta)$', 'interpreter', 'latex');
ylabel('Count', 'interpreter', 'latex');
set(gca,'fontsize',20,'TickLabelInterpreter','latex');

%% plot comparison
figure(10);
clf;
ref_ends = [ref_pos+ref_length/2; ref_pos-ref_length/2];
real_ends= [real_pos+real_length/2; real_pos-real_length/2];

plot3(ref_ends([1 4],:), ref_ends([2 5],:), ref_ends([3 6],:), 'k-', ...
	'linewidth', 2);
hold on;
plot3(real_ends([1 4],:), real_ends([2 5],:), real_ends([3 6],:), 'r-', ...
	'linewidth', 2);

axis equal tight xy;
