%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Dr. Amer
% Concordia Univ.; elec6631 course
%
% Any form of sharing, copying,
% reproduction or other utilization of the content or part
% of it, beyond viewing and testing for strictly
% professional purposes of elec6631 course must only
% be done with the explicit written permission
% of the course instructor Dr. Amer,
% amer@ece
%
%%%%%%%%%%%%%%%%%%%%%
% 
% Write a MATLAB code for implementing an Exhaustive Block Matching Algorithm (EBMA) with integer-pel accuracy.
% Use a block size of 16×16. The program should allow the user to choose the search range, so
% that you can compare the results obtained with different search ranges. (Note that the proper
% search range depends on the extent of the motion in your test images). Apply the program to
% two adjacent frames of a video sequence. Your program should produce and plot the estimated
% motion field, the predicted image and the prediction-error image. It should also calculate the
% PSNR of the predicted frame compared to the original anchor frame. With MATLAB, you can
% plot the motion field using the function quiver.
%%%%%%%%%%%%%%%%%%%%%%%%
%
% Motion estimation and temporal frame prediction using Integer-pel EBMA
%
% Input parameters:
%  TargetImgFileName,AnchorImgFileName, ImgHeight,ImgWidth, MacroBlockSize, SearchRange, PelAccuracy (1 integer pel and  2 half pel)
% 
% Output parameters:
%  TargetImg,AnchorImg,PredictedImg,EstimatedMotionVector location (ox,oy) and direction (px,py), PSNROriginalPredictedImg
%
% Motion_Estimation_2D.m: estimate block wise motion vectors between 
% two video frames using exhaustive search block matching algorithm. 
% The search accuracy can be integer or half pel. This function calls
% EBMA.m() to compute the motion vector for a given block. 
% EBMA.m: estimate the motion vector for a given block
%
% Note that because fread() reads in data from a binary 
% file into an image matrix column wise, whereas the sample 
% image files (foremanX.Y) are written in a binary file row 
% wise, the actual image width is considered ImgHeight, and 
% actual image height ImgWidth.

% Testing with two frames from the sequence foreman (CIF format, 352x288 per frame, Y componenet only)
% Download here ./prelabs
% You can test with any two frames of a video sequence.
%

[Target_Img,Anchor_Img,Predict_Img,ox,oy,px,py,PSNR]=Motion_Estimation_2D('foreman66.Y','foreman69.Y',352,288,[16,16],[-32,-32],[32,32],1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%This function uses the function "EBMA" for motion estimation of every macroblock

function [Target_Img,Anchor_Img,Predict_Img,ox,oy,px,py,PSNR]=Motion_Estimation_2D(TargetName,AnchorName,Img_Height,Img_Width,BlockSize,rangs,range,K)

%Motion_Estimation_2D compute Motion Vectors in Anchor Frame from Target Frame in either integer or half-pel accuracy
%
%	TargetName,AnchorName: 
%		File Names of Target Frame and Anchor Frame
%	Img_Height,Img_Width:  
%		Image Height and Width of a Frame
%  BlockSize:             
%		The size of Macro Block in Frame is BlockSize(1) by BlockSize(2)
%	rangs,range:      
%		The Search Field in Frame A is from (rangs(1),rangs(2)) to (range(1),range(2))
%	K:
%		The search accuracy: 1 integer pel 2 half pel
%  Target_Img,Anchor_Img,Predict_Img:
%		Image Matrix for Target Frame, Anchor Frame, Predicted Frame
%	ox,oy,px,py:
%		The location of Motion vector is (ox,oy), (px,py) for the direction 
%	PSNR
%		The peak signal and noise ratio between original image and predicted image
%
%	Author: Xiaofeng Xu, Polytechnic University  4/21/2002


%Read images from files
fid = fopen(TargetName,'r');
Target_Img= fread(fid,[Img_Height,Img_Width]);
fclose(fid);

fid = fopen(AnchorName,'r');
Anchor_Img= fread(fid,[Img_Height,Img_Width]);
fclose(fid);

figure;
imshow(uint8(Target_Img));

t=0;

Predict_Img=Target_Img;
t0 = clock;
%Upsample the Target_Img for half-pel search
if K==2
   Up_Target_Img=zeros(Img_Height*2,Img_Width*2);
	Up_Target_Img(1:2:Img_Height*2,1:2:Img_Width*2)=Target_Img;
	Up_Target_Img(1:2:Img_Height*2-1,2:2:Img_Width*2-1)=(Target_Img(:,1:Img_Width-1)+Target_Img(:,2:Img_Width))/2;
	Up_Target_Img(2:2:Img_Height*2-1,1:2:Img_Width*2-1)=(Target_Img(1:Img_Height-1,:)+Target_Img(2:Img_Height,:))/2;
   Up_Target_Img(2:2:Img_Height*2-1,2:2:Img_Width*2-1)=(Target_Img(1:Img_Height-1,1:Img_Width-1)+Target_Img(1:Img_Height-1,2:Img_Width)+Target_Img(2:Img_Height,1:Img_Width-1)+Target_Img(2:Img_Height,2:Img_Width))/4;
	Target_Imgbak=Target_Img;
   Target_Img=Up_Target_Img;
   t=1;
end


m=1;

%Search for all the blocks in Anchor Images. 
for i=1:BlockSize(1):Img_Height-BlockSize(1)+1
   %Caculate the search range in Target Images.
   RangeStart(1)=i*K-t+rangs(1)*K;
   RangeEnd(1)=i*K-t+BlockSize(1)*K-1+range(1)*K;
   if RangeStart(1)<1
      RangeStart(1)=1;
   end   
   if RangeEnd(1)>Img_Height*K
      RangeEnd(1)=Img_Height*K;
   end
   for j=1:BlockSize(2):Img_Width-BlockSize(2)+1
      RangeStart(2)=j*K-t+rangs(2)*K;
      RangeEnd(2)=j*K-t+BlockSize(2)*K-1+range(2)*K;
	   if RangeStart(2)<1
   	   RangeStart(2)=1;
	   end   
   	if RangeEnd(2)>Img_Width*K
      	RangeEnd(2)=Img_Width*K;
	   end
      %Get the best estimation from Target Image.
      [px(m), py(m), Predict_Img(i:i+BlockSize(1)-1,j:j+BlockSize(2)-1)]=EBMA(Target_Img,Anchor_Img,BlockSize,[i,j],RangeStart,RangeEnd,K); 
      ox(m)=j+BlockSize(2)/2;
      oy(m)=i+BlockSize(1)/2;
      m=m+1;
      %j
      %i
  end
end

%Caculate the error image
Error_Img=Anchor_Img-Predict_Img;

%Caculate PSNR
PSNR=10*log10(255*255/mean(mean((Error_Img.^2))));
PSNR
etime(clock,t0)

%Display the results
figure;
imshow(uint8(Anchor_Img));

hold on
quiver(ox,oy,px,py);

hold off
axis image

figure;
imshow(uint8(Predict_Img));

figure;
imshow(uint8(Error_Img));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [mv_x,mv_y,predict_img]=EBMA(Target_Img,Anchor_Img,BlockSize,BlockLocation,RangeStart,RangeEnd,accuracy)

%EBMA compute Motion Vector for one block in Anchor Frame from Target Frame
%This function is called by function Motion_Estimation_2D().
%
%	Target_Img,Anchor_Img: 
%		Image Matrixs of Target Frame and Anchor Frame
%	BlockSize:             
%		The size of Macro Block is BlockSize(1) by BlockSize(2)
%	BlcokLocation:
%		The Macro Block Location in Anchor Frame 
%	RangeStart,RangeEnd:      
%		The Search Field in Frame A is from (RangeStart(1),RangeStart(2)) to (RangeEnd(1),RangeEnd(2))
%  Target_Img,Anchor_Img,Predict_Img:
%		Image Matrix for Target Frame, Anchor Frame, Predicted Frame
%	accuracy:
%		Calculation Accuracy: 1 for integer pel; 2 for half pel
%	mv_x,mv_y:
%		The direction of Motion vector is (mv_x,mv_y)
%	predict_img:
%		The best estimation block for given block
%	Author: Xiaofeng Xu, Polytechnic University  4/21/2002

%Set default caculation accuracy 
if nargin<7
   accuracy=1; 
end  

ty=BlockLocation(1);
tx=BlockLocation(2);
Ny=BlockSize(1);
Nx=BlockSize(2);
%Get the anchor macro block
AnchorBlock=Anchor_Img(ty:ty+Ny-1,tx:tx+Nx-1);

%Initial mv_x mv_y
mv_x=0;
mv_y=0;

%Initial error
error=255*Nx*Ny*100;

%Search the best estimation from (RangeStart(1),RangeStart(2)) to (RangeEnd(1),RangeEnd(2))
for y=RangeStart(1):RangeEnd(1)-accuracy*Ny+1
   for x=RangeStart(2):RangeEnd(2)-accuracy*Nx+1
%for y=RangeStart(1):RangeEnd(1)-Ny+1
%   for x=RangeStart(2):RangeEnd(2)-Nx+1
      down_Target_Img=Target_Img(y:accuracy:y+accuracy*Ny-1,x:accuracy:x+accuracy*Nx-1);
      %down_Target_Img=Target_Img(y:y+Ny-1,x:x+Nx-1);
      %caculate the error
      temp_error=sum(sum(abs(AnchorBlock-down_Target_Img)));
		if temp_error < error
			error=temp_error;
			mv_x=x/accuracy-tx;
         mv_y=y/accuracy-ty;
         predict_img=down_Target_Img;
		end;
	end;
end;


