clear
close all

load('foreman.mat');

MB_size = 8;
search_width = 8; 

I = crop_pic(  grayscale( frames(1).cdata ), MB_size );
P = crop_pic(  grayscale( frames(3).cdata ), MB_size  );
%%
Nmb = size(I)/MB_size;
I_mb = mat2cell(I, repmat(MB_size,1,Nmb(1)), repmat(MB_size,1,Nmb(2)));
P_mb = mat2cell(P, repmat(MB_size,1,Nmb(1)), repmat(MB_size,1,Nmb(2)));
Pred_mb = cell(size(I_mb));
Pred_mb_hp = cell(size(I_mb));

[ u , v ]       = get_frame_motion_vectors( P_mb, I_mb, MB_size, search_width );
%
for n = 1 : size(P_mb,1)
   for m = 1 : size(P_mb,2)
        Pred_mb{n,m} = motion_prediction( P_mb{n,m}, I, u(n,m), v(n,m), n, m );
   end
   
end

P_pred = cell2mat(Pred_mb);
%%
figure('units','normalized','outerposition',[0 0 1 1])

max_I = max(max(I));
PSNR_orig = 10*log10( max_I^2/(sum( sum(  (P-I).^2 ) ) /numel(P).^2 ));
PSNR_pred = 10*log10( max_I^2/(sum( sum(  P_pred.^2 ) ) /numel(P).^2 ));

subplot(1,2,1);
pcolor(flipud(P-I));shading interp;colormap gray
caxis([-1e1,1e1]*10)
axis equal tight
title( sprintf( 'PSNR = %f', PSNR_orig  ) );
subplot(1,2,2);
pcolor(flipud(P_pred));shading interp;colormap gray
caxis([-1e1,1e1]*10)
axis equal tight
title( sprintf( 'PSNR = %f', PSNR_pred) );
%%
figure
[X,Y] = meshgrid((MB_size/2:MB_size:size(I,2)),(MB_size/2:MB_size:size(I,1)));

quiver(X,Y,flipud(u),flipud(-v),'-k','LineWidth',1.5);
hold on
plot(X,Y,'ok','MarkerSize',3)
hold on
for n = 1:size(P,1)/MB_size
plot( linspace(1,size(P,2),100),(size(P,1)-n*MB_size)*ones(100,1) ,'-k') 
end
for n = 1:size(P,2)/MB_size
plot( n*MB_size*ones(100,1), linspace(1,size(P,1),100) ,'-k')
end
xlim([1,size(I,2)]);
ylim([1,size(I,1)]);
axis equal tight
title('P picture motion vectors')