%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POCS Image Reconstruction
% -------------------------
%  AUTHOR: Stephen Rose, Maher Khoury
%    DATE: March 1, 1999
% PURPOSE: Generates SR frame using the POCS method
%
%
%
% Notes:
%   -init.m contains the affine transformation parameters
%   -Assuming a gaussian PSF
%   -u,v are affine transformation vectors for (x,y)
%   -mcX,mcY are transformed coordines in SR frame
%
% Variables:
%   -ref            = LR reference frame
%   -upref          = HR reference frame
%   -NumberOfFrames = Number of pixel frames to consider
%   -frame          = LR frame currently being examined
%   -weights        = weights based on Gaussian PSF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initialization

%init;
NumberOfFrames = 5;
k = zeros(1,4);

%%% Create the high-resolution reference frame
ref = pgmRead('frames/frame00.pgm');
ref = ref(1:size(ref,1)./2,1:size(ref,2)./2);

%%%Interpolate values at inbetween points
[x, y] = meshgrid(1:size(ref,2), 1:size(ref,1));
[X, Y] = meshgrid(1:2.*size(ref,2), 1:2.*size(ref,1));
upref = interp2(x,y,ref,X./2,Y./2,'linear');
upref(isnan(upref)) = 0;

figure;
showIm(upref);
drawnow;

%%% Iterate the entire process
for iter=1:10,
  disp(iter);
  %%% Iterate over the frames
  for num = 1:NumberOfFrames,

    %%% Read in the frame
    if (num < 10);
      frame = pgmRead(strcat('frames/frame0',num2str(num),'.pgm'));
    else
      frame = pgmRead(strcat('frames/frame',num2str(num),'.pgm'));
    end
    frame = frame(1:size(frame,1)./2,1:size(frame,2)./2);

    %%%Calculate the affine motion parameters for this frame
    k = affine(frame,ref);
    u =  k(1).*X + k(2).*Y + 2.*k(3);
    v = -k(2).*X + k(1).*Y + 2.*k(4);

    %%% Calculate the coordinates of the motion compensated pixels
    mcX = X + u;
    mcY = Y + v;

    %%% Loop over entire (low-res) frame
    for m2 = 1:size(frame,2),
      for m1 = 1:size(frame,1),

        %%% Get high-resolution coordinates
        n1 = 2*m1;
        n2 = 2*m2;

        %%% Get coordinates of the motion compensated pixel
        N2 = mcX(n1,n2);
        N1 = mcY(n1,n2);

        %%% If not a border pixel
        if ( N1>3 & N1<size(upref,1)-2 & N2>3 & N2<size(upref,2)-2 )

        %%% Find center of the window where the PSF will be applied
        rN1 = round(N1);
        rN2 = round(N2);

        %%% Calculate the effective window
        windowX = Y(rN1-2:rN1+2,rN2-2:rN2+2);
        windowY = X(rN1-2:rN1+2,rN2-2:rN2+2);

        %%% Find the value of the gaussian at these points and normalize
        weights = exp(-((N1-windowX).^2+(N2-windowY).^2)./2);
        weights = weights./sum(sum(weights));

        %%% Calculate the value of the estimate Ihat
        Ihat = sum(sum(weights.*upref(rN1-2:rN1+2,rN2-2:rN2+2)));

        %%% Calculate the residual
        R = frame(m1,m2) - Ihat;

        temp = 0;

        %%% Calculate new values for the reference frame
        if (R>1)
          upref(rN1-2:rN1+2,rN2-2:rN2+2) = upref(rN1-2:rN1+2,rN2-2:rN2+2) + ...
              (weights.*(R-1))./sum(sum(weights.^2));
        elseif (R<-1)
          upref(rN1-2:rN1+2,rN2-2:rN2+2) = upref(rN1-2:rN1+2,rN2-2:rN2+2) + ...
              (weights.*(R+1))./sum(sum(weights.^2));
        end
      end
    end
  end

  upref(upref<0) = 0;
  upref(upref>255) = 255;

end
end

%%% Display the image %%%
pgmWrite(upref,'SRframe.pgm');

figure;
showIm(upref);
drawnow;