%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate a synthetic camera model and optical transfer function model

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

%% input parameters
calib_dir		= 'L:/shadow/images/AR8.00/';
calib_pat		= '%s/setup.mat';

% camera model parameters
n_cameras		= 3;					% number of cameras
theta_y			= [-30 0 30]/180*pi;	% rotation about y axis
n_i				= 2560;					% image width [px] 
n_j				= 1600;					% image height [px]
n_source_linear = 8;					% number of point sources per side length
W_source		= 4;					% apparent size of light source, [mm]
x_spacing		= 0;					% spacing between cams, x, [mm]
fov_height		= 16;					% fov height, mm
f_length		= 200;					% focal length, mm
pixel_pitch		= 5.5e-3;				% pixel pitch [mm]
kappa			= fov_height/n_j;		% digital resolution [mm/px]
M				= pixel_pitch/kappa;	% optical magnification
f_number		= 4;					% f# of imaging
delta_t			= 2e-3;					% time separation, [s]
n_support       = 65;                   % number of pixels per side of support

font_size		= 24;					

% additional limits on reconstruction volume
re_z_lim		= [-1 1]*100;

%% construct pinhole camera model
cam_ary			= cell(1,n_cameras);
% distance of image to object plane
l_img_obj		= f_length * (M+1)^2/M;
% distance to object plane from lens plane
% l = d_img + d_obj = (M+1)*d_obj
% d_img = M*d_obj
d_obj			= l_img_obj/(M+1);
% distance to image plane from lens plane
d_img			= M * d_obj;			   % distance to image plane

% 
% distance of image of light source to object plane from pinhole
d_source		= 0;
x_source		= linspace(-W_source/2, W_source/2, n_source_linear);
[xs,ys,zs]		= ndgrid(x_source, x_source, -d_source);
re_source		= [xs(:) ys(:) zs(:)].';
n_source		= n_source_linear^2;

fov_width		= n_i * kappa;
theta			= atan(fov_width / d_obj(1));
fprintf('angular aperture = %0.3f rad\n', theta);
				
%% create image axes
i_axis			= (0 : n_i-1) - (n_i)/2;
j_axis			= (0 : n_j-1) - (n_j)/2;

%% create camera model parameters
for c = 1 : n_cameras
	%% basic parameters
	cam			= struct();
	cam.id		= c;
	cam.i_axis	= i_axis;
	cam.j_axis	= j_axis;
    cam.pitch   = pixel_pitch;

	%% pinhole model
	% for meaning of various terms, see Hartley & Zisserman								 
	f			= f_length;				% focal length
	R			= vrrotvec2mat([0 1 0 theta_y(c)]);
	xc			= R.'*[0;0;-d_obj];		% pinhole position
	kr			= [0;0];					% radial distortion coefficients
	ic			= [mean(cam.i_axis);mean(cam.j_axis)];		  % image centre (also centre of distortion)

	G			= [d_img/pixel_pitch 0 ic(1); 
				   0 d_img/pixel_pitch ic(2); 0 0 1];
	P			= G * R * [eye(3) -xc];
	calib		= struct('G', G, 'R', R, 'xc', xc, 'P', P, 'kr', kr, 'ic', ic);

	%% optical transfer function
	% defined in terms of a focal plane and light source
	
	% define normal of focal plane, parallel to principal ray
	e_n			= (G*R)\[ic(1);ic(2);1];
	e_n			= e_n / norm(e_n);
	% define a point on the focal plane
	re_focus	= xc + e_n * d_obj;
	M_focus		= [e_n; -dot(e_n,re_focus)];
	otf			= struct();
	otf.type	= 'shadow';
	otf.M_focus = M_focus;

	% define point light sources along optical axis
	otf.re_source = R.' * (re_source) + xc;
	otf.n_source  = n_source;
	otf.W_source  = W_source;
	% relative intensity of point sources
	otf.weights   = ones(1,n_source) / n_source;
	% supported area for reprojection
	otf.n_support = n_support;

	%% save structure
	cam.calib	= calib;
	cam.otf		= otf;
	cam_ary{c}	= cam;
end
camera			= cell2mat(cam_ary);
calibs			= [camera.calib];
re_origin		= [calibs.xc];

%% build bounding box
% add additional constraints 
%	z_lim(1) <= x(3,:) <= z_lim(2)
A_additional	= [0 0 -1; 0 0 1];
b_additional	= [-re_z_lim(1); re_z_lim(2)];
%[re_bb_lim,re_A,re_b]= bounding_box3(camera, A_additional, b_additional);
[re_bb_lim,re_A,re_b]= bounding_box3(camera, [], []);

%% report size of measurable volume
fprintf('reconstruction limits are:\n');
fprintf('  x = %+6.1f - %+6.1f mm\n', re_bb_lim(:,1));
fprintf('  y = %+6.1f - %+6.1f mm\n', re_bb_lim(:,2));
fprintf('  z = %+6.1f - %+6.1f mm\n', re_bb_lim(:,3));

%% test adjacency between camera pairs
% a pair of cameras can be said to be adjacent 
fprintf('building adjacency matrix...\n');
pairs			= nchoosek(1:n_cameras, 2);
n_pairs			= size(pairs,1);
camera_adjacency= false(n_cameras, n_cameras);
for ip = 1 : n_pairs
	iA			= pairs(ip,1);
	iB			= pairs(ip,2);
	is_adjacent = check_adjacency(camera(iA), camera(iB), re_bb_lim);
	camera_adjacency(iA,iB) = is_adjacent;
	camera_adjacency(iB,iA) = is_adjacent;
end

%% create camera model structure
setup				= struct();
setup.n_cameras		= n_cameras;
setup.camera		= camera;
setup.delta_t		= delta_t;
setup.re_A			= re_A;
setup.re_b			= re_b;
setup.re_bb_lim		= re_bb_lim;
setup.adjacency		= camera_adjacency;

%% save
calib_file			  = sprintf(calib_pat, calib_dir);
fprintf('saving to %s\n', calib_file);
save(calib_file, '-struct', 'setup');

