%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimate PSF through blind deconvolution
addpath(genpath([pwd '/../core']));

lambda		= 2;	% regularisation parameter
aa_factor	= 4;
ksize		= 7*aa_factor-1;	% kernel size
sigma_x		= ksize * 0.5;
sigma_y     = ksize * 0.2;
theta		= -pi/4; % angle
	
n_support	= 75;		% number of pixels of support
n_circ		= 20;		% number of samples
R_range		= [25 30]/2;  % range of radii to simulate
support_size= 1;

%% create a sharp calibration image
x_axis		= linspace(-n_support/2, n_support/2, n_support*aa_factor);
y_axis		= linspace(-n_support/2, n_support/2, n_support*aa_factor);
[xm,ym]		= ndgrid(x_axis,y_axis);

% random radii 
R			= rand(n_circ,1)*(R_range(2)-R_range(1)) + R_range(1);
im_sharp	= (xm.^2 + ym.^2) > reshape(R.^2, [1 1 n_circ]);
im_sharp_pad= padarray(im_sharp, floor([ksize ksize]/2), 'replicate', 'both');

%% create a blurry image
% build Gaussian kernel with custom orientation and variance
irel		= -floor(ksize/2) : floor(ksize/2);
[im,jm]		= ndgrid(irel,irel);
im_prime	= cos(theta)*im - sin(theta)*jm;
jm_prime    = sin(theta)*im + cos(theta)*jm;
PSF_ref		= exp(-im_prime.^2/sigma_x^2 - jm_prime.^2/sigma_y^2);
PSF_ref		= PSF_ref/sum(PSF_ref,'all');
%PSF_ref		= fspecial('gaussian', ksize, ksize * 0.2);
im_full		= convn(im_sharp_pad, PSF_ref, 'valid');
% downsample
im_ds		= psf_downsample(im_full, aa_factor);
% create additive Gaussian noise
rng(1);
im_noise	= randn(size(im_ds)) * 0.01;
im          = im_ds + im_noise;

%% estimate PSF
tic;
PSF         = psf_fit_fmincon(im, im_sharp, ksize, lambda);
toc;
im_conv		= psf_downsample(convn(im_sharp_pad, PSF, 'valid'), aa_factor);
im_res		= im - im_conv;
sum(PSF(:))

disp(norm(PSF - PSF_ref))
%% display source images
figure(1);
clf;
n			= 1;

layout		= tiledlayout(2,2);
nexttile;
imagesc(x_axis, y_axis, im(:,:,n).');
axis equal tight xy;
title('Reference');

nexttile
imagesc(x_axis, y_axis, im_sharp(:,:,n).');
axis equal tight xy;
title('Sharp');

nexttile
imagesc(x_axis, y_axis, im_conv(:,:,n).');
axis equal tight xy;
title('Convolved');

nexttile
imagesc(x_axis, y_axis, im_res(:,:,n).');
axis equal tight xy;
title('Residual');

%% compare PSF
figure(2);
clf;

layout		= tiledlayout(1,3);
nexttile;
imagesc(x_axis, y_axis, PSF_ref.');
axis equal tight;
title('Reference PSF');

nexttile;
imagesc(x_axis, y_axis, PSF.');
axis equal tight;
title('Recovered PSF');

nexttile;
% surface plot of PSF
S_ref = surf(PSF_ref);
hold on;
S     = surf(PSF);
set(S_ref, 'EdgeColor', 'r', 'FaceColor', 'none');
title('PSF Comparison');
daspect([1 1 0.5/ksize^2]);


