% WIENFILT
%
% Usage:  newim = wienfilt(im, length, width, ang, order, K)
%
% Arguments:   im      - The image to be processed.
%              length  - The length of the point spread function to use.
%              width   - The width of the PSF
%              ang     - angle of the PSF
%              order   - order of the Butterworth function used to construct
%                        the point spread function. 
%              K       - The estimated noise/signal power ratio.
%
%   As a guide the radius of the point spread function should be somewhere between 2 
%   and 6 pixels (this is affected by the order of filter that you choose). 
%   The value of K should be kept as small as possible. The smaller you can get it 
%   the sharper the reconstruction, but the greater the noise amplification. 
%   You usually have to put up with a reasonable amount of noise amplification to get a 
%   reconstruction you can read. Try a value of K around 0.01 to start with. Later try reducing K to 0 to see what happens. 
%
% Returns:  newim      - The reconstructed image

function newim = wienfilt(im, length, width, ang, order, K)

H = fft2(fftshift(psf2(size(im),order, ang, length, width, 2)));

imwritesc(psf2(size(im),order, ang, length, width, 2), 'motion_blurred_psf.png');
imwritesc(fftshift(real(H)), 'motion_blurred_psf_fft.png');

figure(1);
imagesc(fftshift(real(H))),colormap(gray);;
figure(1), improfile;
print -dpng motion_blurred_psf_fft_profile.png   % Saves current figure at screen resolution

y = abs(H).^2;
% F is the sharp image
% G is the blurred image

G = fft2(im);

invFilt = ((y./(y+K))./H);


imwritesc(fftshift(real(invFilt)), 'motion_blurred_wiener_filter.png');

figure(3);
imagesc(fftshift(real(invFilt))),colormap(gray);;
figure(3), improfile;
print -dpng motion_blurred_wiener_profile.png   % Saves current figure at screen resolution

F = (G).*invFilt;

y = H .* invFilt;


imwritesc(fftshift(real(y)), 'motion_blurred_wiener_product.png');

newim = real(ifft2(F));