%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%テンプレートマッチング
%
%PART1：グレースケール化&二値化処理
%PART2：SADによるマッチング処理
%PART3：マッチング箇所の描画
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%初期化
clear all;
%マッチングしきい値：これよりもSADの値が小さいときマッチングしているとみなす
thres=500;

%画像の読み込み
imagedata=imread('original.png');
tmpdata=imread('template.png');
imagesize=size(imagedata); 
tmpsize=size(tmpdata);

X=fprintf('SAD threshold is %d\n',thres);
X=fprintf('Original image size is %dx%d\n',imagesize(2),imagesize(1));
X=fprintf('Template image size is %dx%d\n',tmpsize(2),tmpsize(1));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%二値化のMATLAB関数
%これの中身を手で書き直したものがPART1
%
% image_binary = im2bw(imagedata,graythresh(imagedata));
% tmp_binary = im2bw(tmpdata,graythresh(imagedata));
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%--------------------------------------------------------------------------

%PART1：グレースケール化&二値化処理

disp('Part1')
tic;

%ヒストグラム格納配列
histgram=zeros(1,256);

%二値化のしきい値の決定
%全ピクセルの50%が白、50%が黒になるようにする

for height = 1 : imagesize(1)
    for width = 1 : imagesize(2)
        %三次元の配列を1次元に変換(グレースケール変換)
        imagedata_gray(height,width) = floor(0.2989*imagedata(height,width,1)+0.5870*imagedata(height,width,2)+0.1140*imagedata(height,width,3));
        %オリジナル画像のヒストグラムを求める
        histgram(imagedata_gray(height,width)+1)=histgram(imagedata_gray(height,width)+1)+1;
    end
end

%ピクセルの累計数が全ピクセル数(height*width)の半分になる輝度値をしきい値として用いる
i=1;
hist_freq=0;
while(hist_freq<imagesize(1)*imagesize(2)/2)
    hist_freq=hist_freq+histgram(i);
    i=i+1;
end
binarize_thres=i;

%ルックアップテーブルの作成(処理の高速化のために使用)
for i = 1 : 256
    if(binarize_thres<i)
        LUT(i)=1;
    else
        LUT(i)=0;
    end
end


%オリジナル画像
for height = 1 : imagesize(1)
    for width = 1 : imagesize(2)
        %グレースケール化(NTSC 系加重平均法)
        outpixel = floor(0.2989*imagedata(height,width,1)+0.5870*imagedata(height,width,2)+0.1140*imagedata(height,width,3));
        %2値化
        image_binary(height,width)=LUT(outpixel+1);
    end         
end

%テンプレート画像
for height = 1 : tmpsize(1)
    for width = 1 : tmpsize(2)
        %グレースケール化(NTSC 系加重平均法)
        outpixel = floor(0.2989*tmpdata(height,width,1)+0.5870*tmpdata(height,width,2)+0.1140*tmpdata(height,width,3));
        %2値化
        tmp_binary(height,width)=LUT(outpixel+1);
    end         
end

sad=zeros(imagesize(1)-tmpsize(1)+1,imagesize(2)-tmpsize(2)+1);
toc;

%--------------------------------------------------------------------------
%PART2：マッチング処理
disp('Part2')
tic;

for image_height=1:imagesize(1)-tmpsize(1)+1
    for  image_width = 1:imagesize(2)-tmpsize(2)+1        
        for tmp_height = 1 : tmpsize(1)
            for tmp_width = 1 : tmpsize(2)
       %オリジナル画像とテンプレート画像の値が異なる場合SADをインクリメント
                if xor(tmp_binary(tmp_height,tmp_width),image_binary(tmp_height+image_height-1,tmp_width+image_width-1)) == 1
                    sad(image_height,image_width)=sad(image_height,image_width)+1;
                end
            end
        end
    end
end
toc;

%--------------------------------------------------------------------------
%PART3：マッチング箇所の描画

disp('Part3')
tic;

sadsize=size(sad);
for sad_height = 1 : sadsize(1)
    for sad_width = 1 : sadsize(2)
        %SADの値がthresより小さい場合その箇所を赤枠で囲む
        if sad(sad_height,sad_width)<thres
            for tmp_height=1:tmpsize(1)
            %枠線左
            imagedata(sad_height+tmp_height-1,sad_width,:)=[255,0,0];
            %枠線右
            imagedata(sad_height+tmp_height-1,sad_width+tmpsize(2),:)=[255,0,0];   
            end
            
             for tmp_width=1:tmpsize(2)
            %枠線上
            imagedata(sad_height,sad_width+tmp_width-1,:)=[255,0,0];
            %枠線下
            imagedata(sad_height+tmpsize(1),sad_width+tmp_width-1,:)=[255,0,0];
            end
        end
    end
end

toc;

%画像の出力
imwrite(real(imagedata), 'image_result.png');