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

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

%% input parameters
calib_dir		= '~';
calib_pat		= '%s/C12_f20_N4.0_interpolant_testtest.mat';

% mirror in z axis? (rotate each camera 180deg about y axis)
b_rot180		= false;

% camera model parameters
n_cameras		= 12;					% number of cameras
n_i				= 4056;					% image width [px] 
n_j				= 3040;					% image height [px]
x_spacing		= 134;					% spacing between cams, x, [mm]
fov_height		= 304;					% fov height, mm
f_length		= 20;					% focal length, mm
pixel_pitch		= 1.55e-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			= 10e-3;					% time separation, [s]

font_size		= 24;					

% OTF options, currently only diffraction model is implemented
n_otf_support	= 7;					  % size of OTF support
otf_resolution	= 1/3;					 % sub-pixel resolution for OTF
wavelength_mm	= [630e-6 532e-6 465e-6];% center wavelength of LEDs, mm
otf_type		= 'diffraction_interpolant';		% diffraction or diffraction_bicubic

% sensitivity of [R,G,B] channels to pulses in red, green & blue LEDs
RGB_response	= eye(3);

% additional limits on reconstruction volume
re_z_lim		= [-22.0893   22.0893];

%% 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 * ones(1, n_cameras);
% 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

re_origin		= [	(1:n_cameras)*x_spacing - 3/2*x_spacing;
					zeros(1,n_cameras);
					-d_obj];
if b_rot180
	re_origin(3,:)=-re_origin(3,:);
end

fov_width		= n_i * kappa;
theta			= atan(fov_width / d_obj(1));
fprintf('angular aperture = %0.3f rad\n', theta);
				
%% create bayer pattern
bayer_r			= [1 0; 0 0];
bayer_g			= [0 1; 1 0];
bayer_b			= [0 0; 0 1];
bayer			= cat(3, bayer_r, bayer_g, bayer_b);
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.bayer	= bayer;

	%% pinhole model
	% for meaning of various terms, see Hartley & Zisserman								 
	f			= f_length;				% focal length
	if ~b_rot180
		R		= eye(3);				% rotation matrix
	else
		R		= diag([-1 1 -1]);		% rotate 180deg about y axis
	end
	xc			= re_origin(:,c);			% 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(c)/pixel_pitch 0 ic(1); 
				   0 d_img(c)/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);

	%% save structure
	cam.calib	= calib;
	cam_ary{c}	= cam;
end
camera			= cell2mat(cam_ary);

%% 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);

%% 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');
camera_adjacency    = build_adjacency_matrix(camera, re_bb_lim);

%% optical transfer function model
otf_ary				= cell(3, n_cameras);
for rgb=1:3
	%% diffraction limited imaging model parameters
	otf_opts				= struct();
	otf_opts.n_support		= n_otf_support;
	otf_opts.support		= ones(n_otf_support);
	otf_opts.f				= f_length;				% focal length [mm]
	otf_opts.D				= f_length / f_number;	% aperture diameter
	otf_opts.Z_0			= d_img(1);				% sensor plane position [mm]
	otf_opts.pitch			= pixel_pitch;			% pixel pitch [mm]
	otf_opts.lambda			= wavelength_mm(rgb);	% wavelength [mm]
	otf_opts.resolution		= otf_resolution;		% resolution for integrating profile [px]
	otf_opts.type			= otf_type;
	if strcmp(otf_opts.type, 'diffraction')
		otf					= otf_diffraction_model(otf_opts);
	elseif strcmp(otf_opts.type, 'diffraction_bicubic')
		otf					= otf_diffraction_model_bicubic(otf_opts);
	elseif strcmp(otf_opts.type, 'diffraction_interpolant')
		otf					= otf_diffraction_model_interpolant(otf_opts);
	end
	otf_ary{rgb,1}			= otf;
end
for c = 2 : n_cameras
	otf_ary(:,c) = otf_ary(:,1);
end
otf					 = cell2mat(otf_ary);

%% create camera and laser sheet model structure
setup				= struct();
setup.n_cameras		= n_cameras;
setup.camera		= camera;
setup.otf			= otf;
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');

%% display optical transfer function
figure(3);
clf;
layout					= tiledlayout(1,3);
titstr					= {'red','green','blue'};

for rgb = 1 : 3
	irel				= otf(rgb,1).irel;
	jrel				= otf(rgb,1).jrel;
	phi					= otf(rgb,1).phi;
	irelmat				= ndgrid(irel,jrel,phi);
	[imat,jmat,phimat]	= ndgrid(irel, jrel, phi);
	DeltaZ				= 2*otf(rgb,1).lambda/pi * f_number^2 * phimat;
	Z_img				= d_img(c) + DeltaZ;
	z_obj				= 1./(1/f_length - 1./Z_img) - d_obj(c);

	nexttile;
	
    j0=abs(jrel)<eps;
	S=pcolor(squeeze(irelmat(:,j0,:)), ...
			 squeeze(z_obj(:,j0,:)), ...
			 squeeze(otf(rgb,1).I(:,j0,:)));
	grid on;
	set(S, 'EdgeColor', 'none');
	pbaspect([0.5 1 1]);
	
	chan = zeros(3,3);
	chan(rgb,rgb)=1;
	cmap=gray*chan;
	colormap(gca, cmap);
	
	xlim([-3.5 3.5]);
	xlabel('$i$ [px]', 'interpreter', 'latex', 'fontsize', font_size);
	ylabel('$z$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
	set(gca,'fontsize',font_size,'ticklabelinterpreter','latex');
	title(titstr{rgb}, 'interpreter', 'latex', 'fontsize', font_size);
end