%!!!The function also use the function "EBMA" for motion estimation of every macroblock
% An example of main function calling this function is "Prob6_12.m" and "Prob6_13.m", which can be entered on the command window.

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));