%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Function: my_wiener_filter

function [I_restored] = my_wiener_filter(I_degraded, psf, NSR)


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=1e-1; %To avoid division by 0. Set experimentally
index_otf_bigger_than_thr=find(abs(otf(vector_index)) > min_threshold);


Nom=conj(otf(index_otf_bigger_than_thr)) .* FT_I_degraded(index_otf_bigger_than_thr);
Den= abs(otf(index_otf_bigger_than_thr)).^2 + NSR;

FT_I_restored(index_otf_bigger_than_thr)= Nom ./ Den;
I_restored=real(ifftn(reshape(FT_I_restored,size_Image)));
I_restored=uint8(I_restored);