% Pseudo-Inverse Filter

N=256;
N2=2*N;
image=imrawread('girl.raw',[N N]);
figure; subplot(1,2,1); imshow(image,[0 255]); title('original');

blurtype='gaussian';
% blurtype='motion';

switch blurtype
    case 'gaussian'
        blurvar=5;
        blurdev=fix(3*sqrt(blurvar));
        blurwidth=2*blurdev+1;
        PSF = fspecial('gaussian',blurwidth,sqrt(blurvar));
    case 'motion'
        blurwidth=15;
        PSF = fspecial('motion',blurwidth,0);
    otherwise
        error('Unknown blur type')
end

%Load a blurred image
blrimage=imfilter(image,PSF,'symmetric');
 
eblrimage=zeros(N2,N2);
eblrimage(1:N,1:N)=blrimage;
eblrimage=imwinext(eblrimage);  % windowed extension
subplot(1,2,2); imshow(eblrimage,[0 255]); title('extended');

% Generate the filter impulse response
switch blurtype
    case 'gaussian'
        h=exp(-[-blurdev:blurdev].^2/(2*blurvar)); % 1-D blur matrix
        h=h/sum(h);
    case 'motion'
        h=PSF;
        blurdev=(blurwidth-1)/2;
end

he=zeros(1,N2);
he(1:blurwidth)=h;
he=circshift(he,[0 -blurdev]);
i=[0:N2-1]; unity=ones(1,N2);
H1=he(mod(i'*unity-unity'*i,N2)+1);
figure; imshow(H1,[]);

figure;
[phi,lambda]=eig(H1'*H1);
phi=fliplr(phi);   % reverse the order of eigenmatrix and eigenvalues
lambda=flipud(fliplr(lambda));
sv=sqrt(diag(lambda));
psi = H1*phi*diag(sv.^-1);
psnr = zeros(1,N);
H1inv=zeros(N2,N2);

switch blurtype
    case 'gaussian'
         Imax=170;
         for k=1: Imax
             H1inv=H1inv+1/sv(k)*phi(:,k)*psi(:,k)';
             recnimage=H1inv*eblrimage*H1inv';
             psnr(k)=10*log10(255^2/mse(image-recnimage(1:N,1:N)));
             if(mod(k,10)==0)
                 subplot(fix(Imax/50)+1,5,ceil(k/10)); imshow(recnimage(1:N,1:N),[0 255]); pause(0.1);
             end
         end
    case 'motion'
         Imax=250;
         for k=1: Imax
             H1inv=H1inv+1/sv(k)*phi(:,k)*psi(:,k)';
             recnimage=eblrimage*H1inv';
             psnr(k)=10*log10(255^2/mse(image-recnimage(1:N,1:N)));
             if(mod(k,10)==0)
                 subplot(fix(Imax/50)+1,5,ceil(k/10)); imshow(recnimage(1:N,1:N),[0 255]); pause(0.1);
             end
         end
end

imagehat=recnimage(1:N,1:N);
figure; plot([1:N],psnr,'bx-'); xlabel('k'); ylabel('dB'); title('PSNR');
figure; imshow(imagehat,[0 255]); title(['restored (PSNR=' sprintf('%5.2f',psnr(Imax)) 'dB)']);