function [rep_motion motion_vector] = fastMV(im1, im2, w)

% Author:    Arash Jalalian

% E-mail:    arash.jalalian@gmail.com

% Arguments: [rep_motion motion_vector] = fastMV(im1, im2, w)

% This is the simplified block matching algorithm(BMA) which is proposed by

% L. Hao. I've simplified this algorithm for use it in a real-time problem.

% We have a dynamic search pattern during finding the motion vector. 

% 1. check with big diamond.

% 2. check with one of the hexagon subject to previous results.

% 3. check with small diamond.

% 'im1' is base frame of a video and 'im2' is the second frame. and 'w' is 

% the window size. 'rep_motion' is the representative motion vector and

% 'motion_vector' declare motion vectors for each block of image.

% Example:

% im1 = imread('frame001.jpg');

% im2 = imread('frame002.jpg');

% w = 16;

% [rep_m m_vector] = fastMV(im1, im2, w);

%   clear all

%   close all

%   clc

%   im1 = imread('img_1057.pgm');

%   im2 = imread('img_1058.pgm');

%  % dis = 2;

%   w = 8;

% initialization

[r1 c1] = size(im1);

[r2 c2] = size(im2);

if r1 ~= r2 || c1 ~= c2 

    error('The images must be in a same size')

end

pat.org    = [0; 0];

pat.diam1  = [2 0 -2 0; 0 2 0 -2];%big diamond

pat.diam2  = [1 0 -1 0; 0 1 0 -1];%small diamond

pat.hexver = [2 1  -1 -2 -1 1; 0 2 2 0 -2 -2];

pat.hexhor = [2 0 -2 -2 0 2 ;1 2 1 -1 -2 -1];

r_time = int16(floor(r1 ./ w));

c_time = int16(floor(c1 ./ w));

% for preventing index exceeding from end of image. 4 is the maximum

% Magnitude of the motion vector

if mod(r1, w) < 4 

    r_time = r_time -1;

end

if mod(c1, w) < 4

    c_time = c_time -1;

end

slice1 = cell(r_time, c_time);

slice2 = cell(r_time, c_time);

motion_vector = cell(r_time, c_time);

% creating slices with cell array of slices

for i = 1:r_time

    for j = 1:c_time

        slice1{i, j} = im1(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);

        slice2{i, j} = im2(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);

    end

end

% first step checking with 4 points of big diamond and the origin

mad_org = zeros(r_time, c_time);

min_mad = zeros(r_time, c_time);

idx_min_mad = zeros(r_time, c_time);

for i = 1:r_time

    for j = 1:c_time       

        s_h_r = ((i-1) * w) + 1;%slice head row

        s_h_c = ((j-1) * w) + 1;%slice head column

        diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});

        [r_diam c_diam] = size(pat.diam1);

        for k = 1:c_diam

            if s_h_r + pat.diam1(1, k) > 0 && s_h_c + pat.diam1(2, k) > 0

                diff.diam1{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam1(1, k):...

                    i *w + pat.diam1(1, k),...

                    s_h_c + pat.diam1(2, k):...

                    j * w + pat.diam1(2, k)));

                % calculating MAD = Mean Absolute Difference

                diff.diam1{2, k} = sum(sum(diff.diam1{1,k})) / (w ^ 2);% insert each MAD below each Pat

                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);

            else

                diff.diam1{1, k} = 9999999999999;

                diff.diam1{2, k} = 9999999999999;

                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);

            end

        end

        % calculating minimum of mad in 4 pat and the origin and inserting

        % them into a min_mad(i, j). where i, j decler the position of the

        % slice in the image

        mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);

        mad = [mad mad_org(i, j)];

        [min_mad(i, j) idx_min_mad(i, j)] = min(mad);

        clear mad

    end

end 

% if the idx_min_mad == 5 it means that the origin has the minimum value of

% mad (between 4 big diamond points and the origin the orogin has the

% minimum value) otherwise the index in idx_min_mad shows the pat index that

% has the min mad value.

idx_min_mad2 = zeros(r_time, c_time);

min_mad2 = zeros(r_time, c_time);

for i = 1:r_time

    for j = 1:c_time

        clear mad

        if idx_min_mad(i, j) == 5

            % it means this point didn't moved and it's better to do not use

            % any motion vectors so we must put [0; 0] for these points as a

            % motion vector.accourding to the paper for any slices that 

            % idx_min_mad(i, j) ==5 we must use anothe four points as new

            % pat and check the slices with these i and j for mad. and the

            % final solution for these slices is these mad. here we must

            % use pat.diam2 as new patterns and repeat thise process again.

            s_h_r = ((i-1) * w) + 1;%slice head row

            s_h_c = ((j-1) * w) + 1;%slice head column

%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});

            [r_diam c_diam] = size(pat.diam1);

            for k = 1:c_diam

                if s_h_r + pat.diam2(1, k) > 0 && s_h_c + pat.diam2(2, k) > 0

                    diff.diam2{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam2(1, k):...

                        i *w + pat.diam2(1, k),...

                        s_h_c + pat.diam2(2, k):...

                        j * w + pat.diam2(2, k)));

                    % calculating MAD = Mean Absolute Diffrence

                    diff.diam2{2, k} = sum(sum(diff.diam2{1,k})) / (w ^ 2);% insert each MAD below each Pat

                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);

                else

                    diff.diam2{1, k} = 9999999999999;

                    diff.diam2{2, k} = 9999999999999;

                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);

                end

            end

            % calculating minimum of mad in 4 pat and the origin and inserting

            % them into a min_mad(i, j). where i, j decler the position of the

            % slice in the image

%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);

%            mad = [mad mad_org(i, j)];

            [min_mad2(i, j) idx_min_mad2(i, j)] = min(mad);

        end

    end

end

for i = 1:r_time

    for j = 1:c_time

        if idx_min_mad2(i, j) ~= 0

            motion_vector{i , j} = pat.diam2(:, idx_min_mad2(i, j));

        end

    end

end

% other points that their minimum mads idx ~= 5. it means that these slices

% are closer to farder slices.

idx_min_mad3 = zeros(r_time, c_time);

min_mad3 = zeros(r_time, c_time);

for i = 1:r_time

    for j = 1:c_time

        clear mad

        if idx_min_mad(i, j) ~= 5

            s_h_r = ((i-1) * w) + 1;%slice head row

            s_h_c = ((j-1) * w) + 1;%slice head column

%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});

            [r_hex c_hex] = size(pat.hexhor);

            for k = 1:c_hex

                if s_h_r + pat.hexhor(1, k) > 0 && s_h_c + pat.hexhor(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)

                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor(1, k):...

                        i *w + pat.hexhor(1, k),...

                        s_h_c + pat.hexhor(2, k):...

                        j * w + pat.hexhor(2, k)));

                    % calculating MAD = Mean Absolute Diffrence

                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                elseif s_h_r + pat.hexver(1, k) > 0 && s_h_c + pat.hexver(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)

                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver(1, k):...

                        i *w + pat.hexver(1, k),...

                        s_h_c + pat.hexver(2, k):...

                        j * w + pat.hexver(2, k)));

                    % calculating MAD = Mean Absolute Diffrence

                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                else

                    diff.hex{1, k} = 9999999999999;

                    diff.hex{2, k} = 9999999999999;

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                end

            end

            % calculating minimum of mad in 4 pat and inserting

            % them into a min_mad(i, j). where i, j decler the position of the

            % slice in the image

%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);

%            mad = [mad mad_org(i, j)];

            [min_mad3(i, j) idx_min_mad3(i, j)] = min(mad);

        end

    end

end

% org_mov declare the movement vector for new pattern origins.

org_mov = cell(r_time, c_time);

for i = 1:r_time

    for j = 1:c_time

        if idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)

            org_mov{i , j} = pat.hexhor(:, idx_min_mad3(i, j));

        elseif idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)

            org_mov{i , j} = pat.hexver(:, idx_min_mad3(i, j));

        end

    end

end

% now we must go the theird step. the minimum mad point found in the

% previous search step is repositioned as the center point to form a new

% hexagon nad only three new non overlaped points will be checked as

% candidates each time. so we must define new pattern. this time our

% patterns must have a dynamic behavior. and also these pattern must be

% created based on the old pat.hexver and pat.hexhor

[r_mov c_mov] = size(org_mov);

idx_min_mad4 = zeros(r_mov, c_mov);

min_mad4 = zeros(r_mov, c_mov);

for i = 1:r_mov

    for j = 1:c_mov

        clear mad

        nu = sparse(org_mov{i, j});

        [r_nu c_nu] = size(nu);

        if r_nu ~= 0 || c_nu ~= 0

            for z = 1:c_hex

                pat.hexver_new(:, z) = pat.hexver(:, z) + org_mov{i, j};

                pat.hexhor_new(:, z) = pat.hexhor(:, z) + org_mov{i, j};

            end

            s_h_r = ((i-1) * w) + 1;%slice head row

            s_h_c = ((j-1) * w) + 1;%slice head column

%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});

            [r_hex c_hex] = size(pat.hexhor_new);

            for k = 1:c_hex

%                clear mad

                if s_h_r + pat.hexhor_new(1, k) > 0 && s_h_c + pat.hexhor_new(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)

                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor_new(1, k):...

                        i *w + pat.hexhor_new(1, k),...

                        s_h_c + pat.hexhor_new(2, k):...

                        j * w + pat.hexhor_new(2, k)));

                    % calculating MAD = Mean Absolute Diffrence

                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                elseif s_h_r + pat.hexver_new(1, k) > 0 && s_h_c + pat.hexver_new(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)

                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver_new(1, k):...

                        i *w + pat.hexver_new(1, k),...

                        s_h_c + pat.hexver_new(2, k):...

                        j * w + pat.hexver_new(2, k)));

                    % calculating MAD = Mean Absolute Diffrence

                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                else

                    diff.hex{1, k} = 9999999999999;

                    diff.hex{2, k} = 9999999999999;

                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);

                end

            end

            % calculating minimum of mad in 4 pat and inserting

            % them into a min_mad(i, j). where i, j declare the position of the

            % slice in the image

%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);

            mad = [mad mad_org(i, j)];

            [min_mad4(i, j) idx_min_mad4(i, j)] = min(mad);

%              clear mad

        end

    end

end

% org_mov declare the movement vector for new pattern origins.

% final motion vector calculation

for i = 1:r_mov

    for j = 1:c_mov

        if idx_min_mad4(i, j) == 7 

            motion_vector{i, j} = pat.org;

        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)

            motion_vector{i , j} = pat.hexhor_new(:, idx_min_mad4(i, j));

        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)

            motion_vector{i , j} = pat.hexver_new(:, idx_min_mad3(i, j));

        end

    end

end

% now we want to find rep_motion vector

motion_vectors = cat(2, motion_vector{:});

most_motion_vector = zeros(1, r_time .* c_time);

clear temp

temp = motion_vectors;

for i=1:r_time .* c_time

    my_count = 0;

    element(:, 1) = temp(:, i);

    for k = i+1:r_time .* c_time

        if temp(:, k) == element(:, 1)

            my_count = my_count+1;

%             temp(:, k) = [];

        end

    end

    most_motion_vector(i) = my_count;

end

[value, idx] = max(most_motion_vector);

rep_motion= motion_vectors(:, idx);