%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test outlier detection

%% input parameters
src_dir			= 'L:\array\tcf\6C_A\';
src_pat			= '%s/z001_t001.cam%d.tif';
ref_pat			= '%s/../rescaled/z001_t001.mat';
cal_file		= 'L:\array\tcf\calib\C6_f20_N4.0_A.mat';
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_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	= 1;
opts.n_ipr          = 3;
opts.n_shake		= 2;
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.1; % largest shake, mm
opts.dx_lim			= [-1 1; -1 1; -1 1]*2.5;% largest displacement, mm
% limits on accepting particles
opts.I_min			= 300;%min(mopts.img_threshold);
opts.snr_min_db		= 8;
% 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	= 16;  % radius for outlier search
opts.outlier_sigma  = 5;  % 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}     = double(imread(src_file_ary{ic}));
end
delta_t			= setup.delta_t;

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

subset			= 1:100000;%size(ref.x,2);
ref.x			= ref.x(:,subset);
ref.u			= ref.u(:,subset);

ok				= all(ref.x > setup.re_bb_lim(1,:).', 1) ...
				& all(ref.x < setup.re_bb_lim(2,:).', 1);
ref.x(:,~ok)	= [];
ref.u(:,~ok)	= [];
n_ref			= size(ref.x, 2);

re_bb_size		= setup.re_bb_lim.'*[-1;1];
re_bb_vol		= prod(re_bb_size);
r_spacing		= (re_bb_vol/n_ref)^(1/3);

%% initialise
ipr				= RGBIPR(setup, opts);
ipr.re_part		= ref.x;
ipr.dx_part		= ref.u*delta_t;
ipr.d_part		= 50*ones(1, n_ref);
ipr.pimg_snr	= 100*ones(1, n_ref);

%% validate
b_reason = ipr.validate();

%% statistics
fprintf('        %d particles\n', n_ref);
fprintf('        %d particles with low SNR\n', nnz(b_reason(1,:)));
fprintf('        %d particles too dim\n', nnz(b_reason(2,:)));
fprintf('        %d particles outside velocity limits\n', nnz(b_reason(3,:)));
fprintf('        %d particles inconsistent with neighbours\n', nnz(b_reason(4,:)));

%% pposition
figure(1);
clf;
hist(ref.x(2,b_reason(4,:)), linspace(-150,150,11));
