%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Function: my_pseudo_inverse_filter

function [I_restored] = my_pseudo_inverse_filter(I_degraded, psf, beta, const_k)

size_Image=size(I_degraded);
I_restored=zeros(size_Image); %Allocate memory for images
FT_I_restored=zeros(prod(size_Image),1); 
FT_I_degraded=zeros(size_Image); 


I_degraded=double(I_degraded);% Process image in double format


FT_I_degraded=fftn(I_degraded);


vector_index=[1:prod(size_Image)]'; % Useful for indexing the NxMx3 Image matrix as a vector
otf=psf2otf(psf,size_Image);% Compute Optical Transfer Function
min_threshold=0.0162; %To avoid division by 0. Set experimentally
index_otf_bigger_than_thr=find(abs(otf(vector_index)) > min_threshold);

%beta=0.5;
%const_k=50;

FT_I_restored(index_otf_bigger_than_thr)=(FT_I_degraded(index_otf_bigger_than_thr)./otf(index_otf_bigger_than_thr)).*(1-(1-beta.*abs(otf(index_otf_bigger_than_thr)).^2).^const_k);
I_restored=real(ifftn(reshape(FT_I_restored,size_Image)));
I_restored=uint8(I_restored);