function [u,v] = motion3wayEstimate(img1, img0, N, srRng)
% function [u,v] = motion3wayEstimate(img1, img0, N, srRng)
% This function implements the 3-step motion estimation algorithm
% the inputs to the function are
% img1 - the frame to be approximated
% img0 - the frame given
% N - block size used for motion estimation (default value 16)
% srRng - search range (default value [8 4 2])
% Outputs
% u and v are the motion estimation parameters. 
% Illustration of usage
% Suppose you want to find the best estimate of the (i,j)th block of img1 interms of 
% some block of img0.. this program gives you a way to do it
% The outputs say that the (i,j)th block of the img1 can be best written as (i+u(i,j)),(j+v(i,j))th block
% of the img0
% Code written by Ashwin Swmaminathan on December 12, 2004
 
if(nargin < 4)
    % default value
    srRng = [ 8 4 2]; 
end

if(nargin < 3)
    % default 
    N = 16;
end

% 3-way motion estimation algorithm
[row,col] = size(img1);
counter = 0;

srRng1 = srRng(1);
srRng2 = srRng(2);
srRng3 = srRng(3);

for i=-srRng1:srRng1:srRng1
    for j=-srRng1:srRng1:srRng1
        imgTmp1 = zeros(size(img1));                
        if and((i <= 0),(j <= 0))
            imgTmp1(1:row+i,1:col+j) = img0(1-i:row,1-j:col);
        end;
        if and((i < 0),(j > 0))
            imgTmp1(1:row+i,1+j:col) = img0(1-i:row,1:col-j);
        end;
        if and((i > 0),(j < 0))
            imgTmp1(1+i:row,1:col+j) = img0(1:row-i,1-j:col);
        end;
        if and((i > 0),(j > 0))
            imgTmp1(1+i:row,1+j:col) = img0(1:row-i,1:col-j);
        end;
        madTmp = abs(img1 - imgTmp1);
        counter = counter + 1;
        umat(counter) = -i;
        vmat(counter) = -j;
        fun = @meamean;
        mad(:,:,counter) = blkproc(madTmp, [ N N ], fun);
    end;
end;


[tmp12343, mmad] = min(mad,[],3);

ushift = umat(mmad); 
vshift = vmat(mmad);

for i=1:row/N
    for j=1:col/N
        maxie = 10000000;        
        for ii=-srRng2:srRng2:srRng2
            for jj=-srRng2:srRng2:srRng2
                blk = findMatch(img0, i,j, ushift(i,j) + ii, vshift(i,j) + jj,N);
                tmpMap = mean(mean(abs(img1(i*N-N+1:i*N,j*N-N+1:j*N) - blk)));
                if (tmpMap < maxie)
                    maxie = tmpMap;
                    umax = ushift(i,j) + ii;
                    vmax = vshift(i,j) + jj;
                end
            end
        end
        ulev2(i,j) = umax;
        vlev2(i,j) = vmax;
        maxie = 10000000;        
        for ii=-srRng3:srRng3:srRng3
            for jj=-srRng3:srRng3:srRng3
                blk = findMatch(img0, i,j, ulev2(i,j)+ii, vlev2(i,j)+jj,N);
                tmpMap = mean(mean(abs(img1(i*N-N+1:i*N,j*N-N+1:j*N) - blk)));
                if (tmpMap < maxie)
                    maxie = tmpMap;
                    umax = ulev2(i,j)+ii;
                    vmax = vlev2(i,j)+jj;
                end;
            end;
        end;
        ulev3(i,j) = umax;
        vlev3(i,j) = vmax;
    end
end
u = ulev3;
v = vlev3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x = meamean(y);
% function x = meamean(y);
% Computes the mean of mean of y
x = mean(mean(y));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function blk = findMatch(img0,  i, j, u, v, N);
% function blk = findMatch(img0,  i, j, u, v, N);
% This function finds the best match for the three step motion estimation algorithm
% this is called by the motion3wayEstimate program
% Inputs img0 is the input image
% i, j are the locators of the block whose best match is to be found
% u, v are the motion estimation vectors
% N - step size
% Written by Ashwin Swaminathan


[row, col] = size(img0);
inirow = i*N - N + 1;
inicol = j*N - N + 1;

finrow = inirow + u;
fincol = inicol + v;

retMatRow = finrow:finrow+N-1;
retMatCol = fincol:fincol+N-1;
retMatRow(find(retMatRow <= 0 )) = 1;
retMatCol(find(retMatCol <= 0 )) = 1;
retMatRow(find(retMatRow > row )) = row;
retMatCol(find(retMatCol > col )) = col;

blk = img0(retMatRow,retMatCol);