clear

sigma = 0.005; %noise variance
gamma = 2.0;   %Initial value of gamma
b = 0.01;      %Step size

%Read observed image
g = imread('peppers_degraded.tif');

figure(1)
imshow(g,[]);
[height,width] = size(g);

%Defining P matrix

p = [0 -1 0; -1 4 -1; 0 -1 0];

%Define the Butterworth filter H(u,v)

r0 = 36;

u = -(height/2):1:((height/2)-1);
u = (u.*u)';
v = -(width/2):1:((width/2)-1);
v = (v.*v)';
eu = ones(height,1);
ev = ones(width,1);
H = (u*eu' + ev*v')/(r0^2);
H = 1 + H.^5;
H1 = ones(size(H))./H;

%%%%% The following can also be used for creating H matrix %%%%%
%%%%% but it uses for loops and takes considerably longer %%%%%%%
%for u=1:height
%   for v=1:width
%      H(u,v) = 1/(1+ ((((u-1-height/2)^2+(v-1-width/2)^2)/(r0^2))^5));
%   end
%end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Defined filter has origin of frequency axes at the matrix center
%Use fftshift to get the origin to the top-right corner of matrix
H = fftshift(H);

%Convert image data to double and find its DFT
g = im2double(g);
G = fft2(g);

%Find DFT of p matrix. The optional arguments to the 
%fft2 command automatically does the requied zer-padding
P = fft2(p,height,width);

%Noise magnitude = M*N*noise_variance  
%because noise_mean = 0
norm_n = sigma*height*width;
a = norm_n/1000;    %accuracy_factor

for k=1:100
   
   %Restoration filter
   R = conj(H)./(H.*conj(H) + gamma*conj(P).*P);
   Fhat = R.*G;                         %Current estimate Fhat of image DFT
   
   %Find the norm_r = phi(gamma)
   %This can be done using the frequency domain quantities (DFTs) 
   %rather than the spatial domain quantities
   %refer to notes

   norm_r = (G-H.*Fhat).*conj(G-H.*Fhat);    
   norm_r = sum(norm_r(:))/prod(size(norm_r));
   
   %Update gamma based on the value of phi(gamma)
   if (norm_r < norm_n - a)  gamma = gamma + b;
   elseif(norm_r > norm_n + a) gamma = gamma - b;
   else break      %If converged, break
   end
   gamma       %Keep track of variations in gamma
end

%Compute spatial-domain output image. 
%This must be real but there can be small imaginary 
%component due to roun-off

fhat = ifft2(Fhat);
fhat = real(fhat);

figure(2)
imshow(fhat,[]);

%Compare with ideal image
f = imread('\siva\course\cs445\images\peppers_256.tif');
f = im2double(f);

error = f - fhat;
error = error.*error;
sum(error(:))/prod(size(f))
