clear all;
close all;
f = imread('Cameraman.tif'); figure; imshow(f); f = double(f); title('Original image');
h = [1 1 1 1 1]/5;
p = [1 -2 1]; 
g = imfilter(f,h);
g = g + 1*randn(size(g)); 
figure; imshow(uint8(g)); title('Blurred and noisy image');

% Note that the filter is blurring the rows only. 

% Extend the signals along the rows
ge = zeros(256,260);
ge(:,1:256) = g(:,1:256);
%PSF = fspecial('gaussian',60,10); ge = edgetaper(ge,PSF); % TEST THIS


% APPROACH I: Create the H matrix

% -- Create the first row
FirstRow = zeros(1,260); 
FirstRow(1) = h(1);
FirstRow(257:260) = h(2:5);

% -- Create the whole matrix
H = zeros(260,260);
for i=1:260,
    H(i,:) = circshift(FirstRow,[0 i-1]);
end

% % - UNCONSTRAINED RESTORATION: f_est = inv(H'*H)*H'*ge
% -- Reconstruct each row separately
for i=1:256,
    %row_est = inv(H'*H)*H'*ge(i,:)';
    %row_est = inv(H'*H+exp(-5))*H'*ge(i,:)';
    row_est = pinv(H'*H)*H'*ge(i,:)';
    f_est(i,:) = row_est';
end
figure; imshow(uint8(f_est)); title('Unconstrained Restoration');


% % - CONSTRAINED RESTORATION: f_est = inv(H'*H+gamma*Q'*Q)*H'*g
% -- Create the first row
FirstRow = zeros(1,260); 
FirstRow(1) = p(1);
FirstRow(259:260) = p(2:3);

% -- Create the whole matrix
Q = zeros(260,260);
for i=1:260,
    Q(i,:) = circshift(FirstRow,[0 i-1]);
end

% -- Reconstruct each row separately
gamma = 0.001; % TEST DIFFERENT GAMMA VALUES
for i=1:256,
    row_est = inv(H'*H+gamma*Q'*Q)*H'*ge(i,:)';
    f_est1(i,:) = row_est';
end
figure; imshow(uint8(f_est1)); title('Constrained Restoration');


% % APPROACH II: Iterative Restoration
%
% % 1. Unconstrained Restoration
f_est = g;
alpha = 0.1;
for i=1:20,
    residual = f_est - filter2(h,f_est);
    f_est = f_est + 2*alpha*filter2(h,residual);
    %figure; imshow(uint8(f_est)); title('Iterative restoration');
end
figure; imshow(uint8(f_est)); title('Iterative Unconstrained');

% % 2. Constrained Restoration
f_est = g;
alpha = 0.1;
gamma = 1;
for i=1:20,
    residual = f_est - filter2(h,f_est);
    Q_f_est = filter2(p,f_est);
    f_est = f_est - alpha*(gamma*filter2(p,Q_f_est) - 2*filter2(h,residual) );
    %figure; imshow(uint8(f_est)); title('Iterative restoration');
end
figure; imshow(uint8(f_est)); title('Iterative Constrained');


% APPROACH III: DFT-domain solution

% 1. Unconstrained Restoration
DFT_h = fft(h,260);
PSF = fspecial('gaussian',60,10); ge = edgetaper(ge,PSF);
for i=1:256,
    DFT_g_row = fft(ge(i,:));
    DFT_f_est_row = (conj(DFT_h).*DFT_g_row)./((conj(DFT_h).*DFT_h)+exp(-10));
    f_est_row = real(ifft(DFT_f_est_row));
    f_est2(i,:) = f_est_row;
end
figure; imshow(uint8(f_est2)); title('DFT domain unconstrained restoration');
% 
% 2. Constrained Restoration
DFT_h = fft(h,260);
DFT_p = fft(p,260);
gamma = 0.001;
for i=1:256,
    DFT_g_row = fft(ge(i,:));
    DFT_f_est_row = (conj(DFT_h).*DFT_g_row)./((conj(DFT_h).*DFT_h)+gamma*(conj(DFT_p).*DFT_p));
    f_est_row = real(ifft(DFT_f_est_row));
    f_est3(i,:) = f_est_row;
end
figure; imshow(uint8(f_est3)); title('DFT domain constrained restoration');

% 
% % APPROACH IV: WIENER FILTER SOLUTION
 f_est4 = deconvwnr(g,h);
 figure; imshow(uint8(f_est4)); title('Wiener filter solution');