clc
%clear all

I=imread('sablona_prah135.png');
I=rgb2gray(I);

stred_roi=[1242 1666];%%stred okna sablony -> vzoroveho prvniho snimku
okno=400; %%1/2 okna sablony - velikost regionu of interest vzoroveho prvniho snimku

okno_shape=400;%velikost vstupniho obrazu ktery srovnavam se vzorem, stred ma stejny jako okno

uhel=4;
interval=4;
time = cputime;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%VYTVARIM VSTUPNI SNIMEK
%%pitlovinka na to abych urcil +- stred sablony na vstupnim snimku
%template=imread('1.JPG');
% template=imread('vyrez2_rot3.png');
% template=rgb2gray(template);

%imshow(I);
%[p2 p1]=ginput(1);
%template=I;
%p1=stred_roi(1);
%p2=stred_roi(2);
%template=template(p1-okno_shape:p1+okno_shape,p2-okno_shape:p2+okno_shape);

%%%toto bylo pokud template byl i referencni snimek
posun=[-100 10];%umele posunu obraz, tak abych nasel nejaky rozdil
rotace=-2;

template=GenTemplate(I,stred_roi+posun,2*okno_shape,rotace);


%%%%%%%%%%%HARRIS ZJISTENI ZDA V BODU ZAJMU JSOU NEJAKE BODY
if exist('P','var')==0
    Iroi=I(stred_roi(1)-okno:stred_roi(1)+okno-1,stred_roi(2)-okno:stred_roi(2)+okno-1);
    H=harrisn(Iroi);
    if(sum(H(:))>10000)
        pozice_kamery_OK=1
    end
    
end

%%%%%Vytvoreni meta sablony
if exist('P','var') == 0
    okno2=ceil(okno*1.4142);%zvetsim Iroi kvuli rotaci (mrtve rohy)
    Iroi=I(stred_roi(1)-okno2:stred_roi(1)+okno2-1,stred_roi(2)-okno2:stred_roi(2)+okno2-1);%zvetsim Iroi kvuli rotaci (mrtve rohy)
    uhel1=uhel*-1;
    uhel2=uhel;
    P=metaShapeRot(Iroi,uhel1,uhel2,interval);
    Pf=fft2(P);
%%%odsud to staci odkomentovat a funguje pyramida
    interval=interval/2;
    uhel2=uhel1+(2*interval);
    for i=1:3
        uhel1
        p=metaShapeRot(Iroi,uhel1,uhel2,interval);%Pyramida
        PP(i,:,:)=p;
        PPf(i,:,:)=fft2(p);
        uhel1=uhel1+interval;
        uhel2=uhel2+interval;
    end
    interval=interval/2;
    uhel1=-1*uhel;
    uhel2=uhel1+(2*interval);
    for i=1:8
        uhel1
        p=metaShapeRot(Iroi,uhel1,uhel2,interval);%Pyramida
        PPP(i,:,:)=p;
        PPPf(i,:,:)=fft2(p);
        uhel1=uhel1+interval;
        uhel2=uhel2+interval;
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Odkomentuj potud 


 %Vracim Iroi na spravnou velikost
  Iroi=I(stred_roi(1)-okno:stred_roi(1)+okno,stred_roi(2)-okno:stred_roi(2)+okno);

    end

%%vytvorim FFT z template
templateF=rot90(template,2);%(Convolution is equivalent to correlation if you rotate the convolution kernel by 180o.) 
templateF=fft2(templateF,size(Pf,1),size(Pf,2));

[j,i,uhel]=pozice(Pf,templateF)



sprintf('LEVEL 2 !!!!')
next_angle=uhel+2

[j,i,uhel]=pozice (squeeze(PPf(next_angle,:,:)),templateF)

sprintf('LEVEL 3 !!!!')

switch next_angle
    case 1,
        next_angle=uhel+2
        if(next_angle==0)next_angle=1;end
    case 2,
        next_angle=uhel+4
    case 3,
        next_angle=7
end


[doprava,dolu,uhel]=pozice (squeeze(PPPf(next_angle,:,:)),templateF)
sprintf('vysledny uhel je :')
switch uhel
    case -1
        vysledek=next_angle-5
        %%seen s PPPf dany obrazek:
        Vf=squeeze(PPPf(next_angle,:,1:size(templateF,1)));
        V=squeeze(PPP(next_angle,:,1:size(templateF,1)));
    case 0
        vysledek=next_angle-4
        %%seen s PPPf dany obrazek:
        Vf=squeeze(PPPf(next_angle,:,size(templateF,1)+1:size(templateF,2)-size(templateF,1)));
        V=squeeze(PPP(next_angle,:,size(templateF,1)+1:size(templateF,2)-size(templateF,1)));
    case 1
        vysledek=next_angle-3
        %%seen s PPPf dany obrazek:
        Vf=squeeze(PPPf(next_angle,:,size(templateF,2)-size(templateF,1):size(templateF,2)));
        V=squeeze(PPP(next_angle,:,size(templateF,2)-size(templateF,1):size(templateF,2)));
end


%%korelace vysledku s template

%[dolu,doprava,uhel]=pozice(Vf,fft2(rot90(template,2))) %%melo by fungovat, proc
%nefunguje? proc vaci jinyhodnoty nez pozice o par radku nahore??


%%Tvoreni rozdioveho snimku = rozdil mezi referencnim snimkem a vstupnim
%%posunutym, v idelanim pripade to bude nula (pokud se provede spravne
%%korelace a vypocet)
R=uint8(zeros(size(I,1),size(I,2),3));

w2=stred_roi(1)+size(V,1)/2-1;
w1=stred_roi(1)-size(V,1)/2;
q2=stred_roi(2)+size(V,2)/2-1;
q1=stred_roi(2)-size(V,2)/2;

R(w1:w2,q1:q2,3)=V;%vykresli vysledny obraz do prostred platna 
R(:,:,2)=R(:,:,3);

w2=stred_roi(1)+size(template,1)/2+dolu-1;
w1=(stred_roi(1)-size(template,1)/2+dolu);
q2=stred_roi(2)+size(template,2)/2+doprava-1;
q1=stred_roi(2)-size(template,2)/2+doprava;

R(w1:w2,q1:q2,1)=template;%nakresli posunutou sablonu (radovy vstupni snimek)
figure(2);
imshow(R);




