clear all;

I1=imread('susan11.bmp');  %read the first frame
I2=imread('susan33.bmp');  %read the second frame

dm=7;
%給圖像擴邊，每個邊都擴dm大小
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         |-----|-------------------------------|------|
%         |  B  |              C                |  D   |
%         |-----|-------------------------------|------|
%         |     |                               |      |
%         |     |                               |      |
%         |  E  |               A               |  F   |   ----> 圖像
%         |     |                               |      |
%         |     |                               |      |
%         |     |                               |      |
%         |-----|-------------------------------|------|
%         |  G  |              H                |  I   |
%         |-----|-------------------------------|------|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

I1=double(I1);
I2=double(I2);
[rownum colnum] = size(I1);
II=zeros(rownum+2*dm,colnum+2*dm);
for x=1:rownum
    for y=1:colnum
        II(x+dm,y+dm)=I1(x,y);
    end
end
  
%下面進行全搜索算法
 blocksize=16;
 rowblocks =rownum/blocksize;
 colblocks =colnum/blocksize;
 A=99999999999999999999;
 Eij=0;
 xrecord=ones(rowblocks,colblocks);
 xrecord1=ones(rowblocks,colblocks);
 yrecord=ones(rowblocks,colblocks);
 yrecord1=ones(rowblocks,colblocks);
 diff=zeros(288,352);
 dif=zeros(rowblocks,colblocks);
 tic
                for x=0:(rowblocks-1)
                    row=x*blocksize;
                   for y=0:(colblocks-1)
                       col=y*blocksize;
                        for tempx=x*blocksize+1:(x+1)*blocksize;
                            for tempy=y*blocksize+1:(y+1)*blocksize;
                                 for p=-dm:dm
                                    for q=-dm:dm
                                        Eij=0;
                                        Eij=Eij+(I2(tempx,tempy)-II(tempx+dm+p,tempy+dm+q))^2;
                                            if Eij<A
                                                    A=Eij;
                                                    xrecord(x+1,y+1)=p;
                                                    yrecord(x+1,y+1)=q;
                                            end   
                                    end
                                 end
                             end
                         end
                                          A=999999999999999999;
                                          for mx=1:blocksize
                                            for ny=1:blocksize
                                            diff(row+mx,col+ny)=I2(row+mx,col+ny)-II(row+mx+dm+xrecord(x+1,y+1),col+ny+dm+yrecord(x+1,y+1));
%                                             dfd(x+1,y+1)=dfd(x+1,y+1)+diff(row+mx,col+ny);
                                            end
                                          end
                                        
                       end
                end
                time1=toc;
                time1
                         figure,imshow(I1,[]);
                         title('the first frame');
                         figure,imshow(I2,[]);
                         title('the second frame');
                         IIII=I2-I1;
                         figure,imshow(IIII,[]);
                         title('幀間差值');
                         figure,imshow(diff,[]);
                         title('利用全搜索算法塊匹配后的幀間差');
                         III=I1+abs(diff);
                         figure,imshow(III,[]);
                         title('全搜索算法恢復后的第二幀圖像');
                         
                         
diff=abs(diff);                  
numberarray=0:1:255;
 for n=1:256
     numberarray(n)=0;
 end;
 zeronumber=0;
  for x=0:(rowblocks-1)                
          for y=0:(colblocks-1)
               for tempx=x*blocksize+1:(x+1)*blocksize;
                    for tempy=y*blocksize+1:(y+1)*blocksize;
                        dif(x+1,y+1)=dif(x+1,y+1)+diff(tempx,tempy);
                    end
               end
            dif(x+1,y+1)=round(dif(x+1,y+1)/(blocksize*blocksize));
            numberarray(dif(x+1,y+1)+1)=numberarray(dif(x+1,y+1)+1)+1;
        end
    end


 figure;plot(numberarray,'r*');hold off;
                                 
%   for x=0:(rowblocks-1)
%                     row=x*blocksize;
%                    for y=0:(colblocks-1)
%                        col=y*blocksize;
%                        a=[row+blocksize/2];b=[col+blocksize/2];
%                        c=[row+blocksize/2+xrecord(x+1,y+1)];d=[col+blocksize/2];
%                        quiver(a,b,c,d,2.5);hold
%                    end
%                end
 
  dif=zeros(rowblocks,colblocks);  
  tic
  for x=0:(rowblocks-1)
                    row=x*blocksize;
                   for y=0:(colblocks-1)
                       col=y*blocksize;
                           for m=-4:4:4
                                 for n=-4:4:4 
                                     Eij=0;
                                     for     tempx=x*blocksize+1:(x+1)*blocksize;
                                         for tempy=y*blocksize+1:(y+1)*blocksize;
                                             Eij=Eij+(I2(tempx,tempy)-II(tempx+dm+p,tempy+dm+q))^2;
                                         end
                                     end
%                                      Eij=Eij/(blocksize*blocksize);
                                            if Eij/(blocksize*blocksize)<A
                                                    A=Eij/(blocksize*blocksize);
                                                    xrecord1(x+1,y+1)=m;
                                                    yrecord1(x+1,y+1)=n;   
                                   end
                               end
                          end
                          for m=xrecord1(x+1,y+1)-2:2:xrecord1(x+1,y+1)+2
                                 for n=yrecord1(x+1,y+1)-2:2:yrecord1(x+1,y+1)+2 
                                     Eij=0;
                                     for     tempx=x*blocksize+1:(x+1)*blocksize;
                                         for tempy=y*blocksize+1:(y+1)*blocksize;
                                             Eij=Eij+(I2(tempx,tempy)-II(tempx+dm+m,tempy+dm+n))^2;
                                         end
                                     end
%                                      Eij=Eij/(blocksize*blocksize);
                                            if Eij/(blocksize*blocksize)<A
                                                    A=Eij/(blocksize*blocksize);
                                                    xrecord1(x+1,y+1)=m;
                                                    yrecord1(x+1,y+1)=n;   
                                   end
                               end
                          end   
                          for m=xrecord1(x+1,y+1)-1:1:xrecord1(x+1,y+1)+1
                                 for n=yrecord1(x+1,y+1)-1:1:yrecord1(x+1,y+1)+1
                                     Eij=0;
                                     for     tempx=x*blocksize+1:(x+1)*blocksize;
                                         for tempy=y*blocksize+1:(y+1)*blocksize;
                                             Eij=Eij+(I2(tempx,tempy)-II(tempx+dm+m,tempy+dm+n))^2;
                                         end
                                     end
%                                      Eij=Eij/(blocksize*blocksize);
                                            if Eij/(blocksize*blocksize)<A
                                                    A=Eij/(blocksize*blocksize);
                                                    xrecord1(x+1,y+1)=m;
                                                    yrecord1(x+1,y+1)=n;   
                                   end
                               end
                          end   
                          A=999999999999999999;
                                          for mx=1:blocksize
                                            for ny=1:blocksize
                                            diff(row+mx,col+ny)=I2(row+mx,col+ny)-II(row+mx+dm+xrecord1(x+1,y+1),col+ny+dm+yrecord1(x+1,y+1));
                                            end
                                          end
                                        
                  end
              end
              time2=toc;
              time2
                        
                         figure,imshow(diff,[]);
                         title('利用三步搜索算法塊匹配后的幀間差');
                         III=I1+abs(diff);
                         figure,imshow(III,[]);
                         title('三步法恢復后的第二幀圖像');
                       diff=abs(diff);                  
diff=abs(diff);                  
numberarray=0:1:255;
 for n=1:256
     numberarray(n)=0;
 end;
 zeronumber=0;
  for x=0:(rowblocks-1)                
          for y=0:(colblocks-1)
               for tempx=x*blocksize+1:(x+1)*blocksize;
                    for tempy=y*blocksize+1:(y+1)*blocksize;
                        dif(x+1,y+1)=dif(x+1,y+1)+diff(tempx,tempy);
                    end
               end
            dif(x+1,y+1)=round(dif(x+1,y+1)/(blocksize*blocksize));
            numberarray(dif(x+1,y+1)+1)=numberarray(dif(x+1,y+1)+1)+1;
        end
    end
  figure;plot(numberarray,'r*');hold off;