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

%% input parameters
subset			= 1:499649;%1:1e5;size(ref.x,2);
cal_file		= 'L:\array\tcf\calib\C30_f20_N4.0_E.mat';
src_dir			= 'L:\array\tcf\30C_E\';
src_pat			= '%s/z001_t001.cam%d.tif';
dst_pat			= '%s/ipr_z001_t001.mat';
ref_pat			= '%s/../rescaled/z001_t001.mat';
b_save			= false;
font_size		= 24;

% particle matching and triangulation options
mopts					= struct();
mopts.img_threshold		= [30 30 30];
mopts.R_search_obj		= 2.5;	% search radius for new particle
mopts.R_search_predictor= 0.3;	% search radius for new particle with predictor field
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.precision		= 'single';
opts.b_debug_plot	= 0;
opts.n_ipr          = 5; % number of iterations
opts.n_shake		= 3; % number of shakes
opts.b_verbose		= true;
% levenberg Marquadt options for shaking
opts.lambda0		= 1;
opts.Lup			= 11;
opts.Ldown			= 9;
opts.lambda_max		= 1E7;
opts.lambda_min		= 1E-7;
opts.b_check_residual=true;
% limits on shake update and velocity
opts.dx_max_step    = 0.3; % largest shake, mm
opts.dx_lim			= [-1 1; -1 1; -1 1]*2.5;% largest displacement, mm
opts.relaxation		= 0.2; % MART relaxation factor
% limits on accepting particles
opts.I_min			= 300;% minimum brightness 
opts.I_max          = 1000;% maximum brightness
opts.snr_min_db		= 0;
% outlier detection, see Schanz et al. (2016) Section 2.4
opts.outlier_nn		= 30;	% number of nearest neighbours for outlier detection
opts.outlier_minnn  = 10;	% minimum number of nearest neighbours
opts.outlier_radius	= 15;	% radius for outlier search
opts.outlier_sigma  = 4;	% outlier standard deviation

%% load calibration
setup			= load(cal_file);
n_cameras		= length(setup.camera);
src_file_ary	= cell(1,n_cameras);
img_ary         = cell(1,n_cameras);
for ic = 1 : n_cameras
	src_file_ary{ic}= sprintf(src_pat, src_dir, ic);
    fprintf('load %s\n', src_file_ary{ic});
    img_ary{ic}     = cast(imread(src_file_ary{ic}), opts.precision);
end
delta_t			= setup.delta_t;

% pad reconstruction limits a bit
setup.re_bb_lim = setup.re_bb_lim + [0 0 -1; 0 0 1];

%% load reference
ref_file		= sprintf(ref_pat, src_dir);
fprintf('reference data %s\n', ref_file);
ref				= load(ref_file);

ref.x			= ref.x(:,subset);
ref.u			= ref.u(:,subset);

%% check visible at all time steps inside measurement volume
ref.x1			= ref.x - ref.u*delta_t;
ref.x2			= ref.x;
ref.x3			= ref.x + ref.u*delta_t;
ok				= all(ref.x2 > setup.re_bb_lim(1,:).', 1) ...
 				& all(ref.x2 < setup.re_bb_lim(2,:).', 1);
% ok				= all(ref.x1 > setup.re_bb_lim(1,:).', 1) ...
% 				& all(ref.x1 < setup.re_bb_lim(2,:).', 1) ...
% 				& all(ref.x2 > setup.re_bb_lim(1,:).', 1) ...
% 				& all(ref.x2 < setup.re_bb_lim(2,:).', 1) ...
% 				& all(ref.x3 > setup.re_bb_lim(1,:).', 1) ...
% 				& all(ref.x3 < setup.re_bb_lim(2,:).', 1);
ref.x(:,~ok)	= [];
ref.u(:,~ok)	= [];
ref.x1(:,~ok)   = [];
ref.x2(:,~ok)	= [];
ref.x3(:,~ok)	= [];
n_ref			= size(ref.x,2);

% project reference to images
ref.im_pos		= cell(1,n_cameras);
for ic = 1 : n_cameras
	im_t1		= pinhole_model(ref.x1, setup.camera(ic).calib, true);
	im_t2		= pinhole_model(ref.x2, setup.camera(ic).calib, true);
	im_t3		= pinhole_model(ref.x3, setup.camera(ic).calib, true);
	ref.im_pos{ic}= cat(2, permute(im_t1,[1 3 2]), permute(im_t2,[1 3 2]), permute(im_t3, [1 3 2]));
end

ref_tracks		= cell(1,n_ref);
for ip = 1 : n_ref
	ref_tracks{ip}.u= ref.u(:,ip);
	ref_tracks{ip}.x= ref.x(:,ip);
	for ic = 1 : n_cameras
		ref_tracks{ip}.im_pos(:,:,ic) = ref.im_pos{ic}(:,:,ip);
	end
end
ref_tracks		= cell2mat(ref_tracks);

%% track!
tracks			= rgblpt(setup, src_file_ary, opts);
n_tracks		= length(tracks);

%% extract first guess
re_fg           = [tracks.x];
dx_fg           = [tracks.u] * delta_t;
s_fg			= struct('x',re_fg,'u',[tracks.u]);

% warning('initialise with reference');
% re_fg=ref.x;
% dx_fg=ref.u*delta_t;
% s_fg=ref;

%% iterative particle reconstruction
ipr             = RGBIPR(setup, opts);
t_start			= tic;
ipr.reconstruct(img_ary, re_fg, dx_fg);
fprintf('took %0.1f seconds\n', toc(t_start));
s_ipr			= struct('x', ipr.re_part, 'u', ipr.dx_part/delta_t);

ipr.project();

%% evaluate statistics
stat_opts		= struct('R_ghost_tol', mopts.R_tolerance_obj);
% error statistics for conventional ptv
stats0			= lpt_error_stats(ref, s_fg, stat_opts);
% error statistics for IPR
stats1			= lpt_error_stats(ref, s_ipr, stat_opts);

[ind,d]		= knnsearch(ipr.re_part.', [ref_tracks.x].', 'K', 1);
b_not_found = d > mopts.R_tolerance_obj;
fprintf('%d particles not found\n', nnz(b_not_found));

%% report statistics
fprintf('particles: %6d LPT, %6d IPR, %6d ref\n', stats0.n_meas, stats1.n_meas, stats1.n_ref);
fprintf('ghosts:    %6d LPT, %6d IPR\n', stats0.n_ghost, stats1.n_ghost);
fprintf('reals:     %6d LPT, %6d IPR\n', stats0.n_real, stats1.n_real);
fprintf('pos error: %6.3f %6.3f %6.3f mm rms LPT\n', stats0.x_res_rms);
fprintf('           %6.3f %6.3f %6.3f mm rms IPR\n', stats1.x_res_rms);
fprintf('           %6.3f %6.3f %6.3f mm med LPT\n', stats0.x_res_med);
fprintf('           %6.3f %6.3f %6.3f mm med IPR\n', stats1.x_res_med);
fprintf('vel error: %6.2f %6.2f %6.2f mm/s rms LPT\n', stats0.u_res_rms);
fprintf('           %6.2f %6.2f %6.2f mm/s rms IPR\n', stats1.u_res_rms);
fprintf('           %6.2f %6.2f %6.2f mm/s med LPT\n', stats0.u_res_med);
fprintf('           %6.2f %6.2f %6.2f mm/s med IPR\n', stats1.u_res_med);

%% save to file
if b_save
	dst_file		= sprintf(dst_pat, src_dir);
	fprintf('save to %s ...', dst_file);
	save(dst_file, 'tracks', 'ipr');
	fprintf('done\n');
end

%% track residual step
[tracks_res,tracks2f_res,re_unmatched]= rgblpt(ipr.setup, ipr.img_res, ipr.opts);     

%% initialise residuals
re_part_new     = cat(2, [tracks_res.x], [tracks2f_res.x]);
dx_part_new     = cat(2, [tracks_res.u], [tracks2f_res.u])*delta_t;
%re_part_new		= cat(2, re_part_new, re_unmatched{2});
%dx_part_new		= cat(2, dx_part_new, delta_t*ipr.opts.matching.v_pred(re_unmatched{2}));
ipr1 = copy(ipr);
ipr1.initialise(ipr.img_meas, re_part_new, dx_part_new);
%% report statistics
fprintf('found %d particles in retriangulation\n', ipr1.count());
s_new			= struct('x', ipr1.re_part, 'u', ipr1.dx_part/delta_t);
s_3f			= struct('x', [tracks_res.x], 'u', [tracks_res.u]);
s_2f			= struct('x', [tracks2f_res.x], 'u', [tracks2f_res.u]);
s_1f			= struct('x', re_unmatched{2}, 'u', 0*re_unmatched{2});
stats2			= lpt_error_stats(ref, s_1f, stat_opts);
fprintf('retriangulated particles:\n')
fprintf('total:     %6d\n', stats2.n_meas);
fprintf('ghosts:    %6d\n', stats2.n_ghost);
fprintf('reals:     %6d\n', stats2.n_real);
fprintf('pos error: %6.3f %6.3f %6.3f mm rms LPT\n', stats2.x_res_rms);
fprintf('           %6.3f %6.3f %6.3f mm med LPT\n', stats2.x_res_med);
fprintf('vel error: %6.2f %6.2f %6.2f mm/s rms LPT\n', stats2.u_res_rms);
fprintf('           %6.2f %6.2f %6.2f mm/s med LPT\n', stats2.u_res_med);


%% convergence
figure(30);
clf;

% number of particles added
layout=tiledlayout(1,2);
nexttile;
riter			= 1 : ipr.riter;
semilogy(riter, ipr.n_part_start, 'o-', 'displayname', 'start');
hold on;
plot(riter, ipr.n_part_remove(end,:), 'x-', 'displayname', 'removed');
plot(riter, ipr.n_part_found, 's-', 'displayname', 'found');
plot(riter, ipr.n_part_merge, '+-', 'displayname', 'accepted');
pbaspect([4/3 1 1]);
xlabel('Iteration', 'fontsize', font_size, 'interpreter', 'latex');
ylabel('\# particles', 'fontsize', font_size, 'interpreter', 'latex');
title('Particle additions/deletions', 'fontsize', font_size, 'interpreter', 'latex');
set(gca,'fontsize',font_size,'TickLabelInterpreter','latex')
set(legend,'interpreter','latex','location','east')

% reason for removing particles
nexttile;
bar(riter, ipr.n_part_remove);
xlabel('Iteration', 'fontsize', font_size, 'interpreter', 'latex');
ylabel('\# particles', 'fontsize', font_size, 'interpreter', 'latex');
title('Reason for removal', 'fontsize', font_size, 'interpreter', 'latex');
legend('SNR', 'Brightness', 'Velocity', 'Outlier', 'Total');
set(legend,'interpreter','latex','location','northwest')
pbaspect([4/3 1 1]);
set(gca,'fontsize',font_size,'TickLabelInterpreter','latex')
%% residuals
%ipr.plotresiduals(ref_tracks(1,b_not_found), 4);
ipr.plotresiduals(tracks2f_res, 4);

%% bar chart comparison
figure(1);
layout=tiledlayout(1,3);

% number of particles reconstructed
nexttile;
bar(categorical({'Real','Ghosts'}), [[stats0.n_real; stats0.n_ghost], [stats1.n_real; stats1.n_ghost]])
hold on;
plot(categorical({'Real','Ghosts'}), size(ref.x,2)*[1 1], 'k--');
ylabel('\# particles', 'interpreter', 'latex');
set(gca,'fontsize',font_size,'ticklabelinterpreter','latex');
pbaspect([1 1 1]);
%set(gca, 'yscale', 'log');
ylim([0 75e3]);

% velocity error
nexttile;
scale_px_to_mm=0.1;
bar(categorical({'u','v','w'}), [stats0.u_res_med, stats1.u_res_med]*delta_t/scale_px_to_mm)
legend('LPT', 'IPR');
set(legend,'location','west');
ylabel('Median velocity error [px]', 'interpreter', 'latex');
set(gca,'fontsize',font_size,'ticklabelinterpreter','latex');
pbaspect([1 1 1]);

% position error
nexttile;
bar(categorical({'u','v','w'}), [stats0.x_res_med, stats1.x_res_med]/scale_px_to_mm)
%legend('LPT', 'IPR');
%set(legend,'location','west');
ylabel('Median position error [px]', 'interpreter', 'latex');
set(gca,'fontsize',font_size,'ticklabelinterpreter','latex');
pbaspect([1 1 1]);

error('stop');

%% ghost particles
figure(20);
clf;
plot3(ref.x(1,b_not_found), ref.x(2,b_not_found), ref.x(3,b_not_found), 'ko');
hold on;
%plot3(ipr.re_part(1,:), ipr.re_part(2,:), ipr.re_part(3,:), 'k.');
cmap_rgb=eye(3);
for rgb=1:3
	plot3(re_unmatched{rgb}(1,:), re_unmatched{rgb}(2,:), re_unmatched{rgb}(3,:), '.', 'color', cmap_rgb(rgb,:));
end

axis equal;
axis(setup.re_bb_lim(:));

%% properties of ghost particles
figure(2);
clf;
layout=tiledlayout(1,2);

% SNR
nexttile
hold on
histogram(ipr.pimg_snr(~stats1.b_ghost), 'Normalization', 'pdf');%, cats(1+stats1.b_ghost));
histogram(ipr.pimg_snr( stats1.b_ghost), 'Normalization', 'pdf');%, cats(1+stats1.b_ghost));
legend('Real','Ghost');
%hist(ipr.pimg_snr(~stats1.b_ghost));%, cats(1+stats1.b_ghost));
xlabel('SNR', 'interpreter', 'latex');
ylabel('PDF', 'interpreter', 'latex');
title('SNR of ghosts', 'interpreter', 'latex');
set(gca,'fontsize',font_size);
pbaspect([4/3 1 1]);

% brightness
nexttile
hold on
histogram(ipr.d_part(~stats1.b_ghost), 'Normalization', 'pdf');%, cats(1+stats1.b_ghost));
histogram(ipr.d_part( stats1.b_ghost), 'Normalization', 'pdf');%, cats(1+stats1.b_ghost));
legend('Real','Ghost');

xlabel('Amplitude', 'interpreter', 'latex');
ylabel('PDF', 'interpreter', 'latex');
title('Brightness of ghosts', 'interpreter', 'latex');
set(gca,'fontsize',font_size);
pbaspect([4/3 1 1]);

%hist(ipr.pimg_snr(~stats1.b_ghost));

%% plot result
figure(10);
clf;
cmap	= {'r','g','b'};
xp		= ipr.re_part;
up		= ipr.dx_part;

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;
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, 'CLim', [0 250*delta_t]*scale);
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);

