clc;clear;close all; %Zdeněk Schoř Diplomová práce 2020
load('..\kalibrace\QaT4body_finalni.mat') %nacteni kalibracnich matic Q a T
% load('..\kalibrace\QaT.mat') % prvotni kalibracni vysledek pro srovnani
%% Změnit pouze tyto parametry
vehicleFolder = 'octavia2'; %zde zadat nazev slozky s vozidly
sens=0.4; %sensitiva imbinarize

%% Výběr slozky a vysky vozidla
switch vehicleFolder %podle nazvu slozky se priradi vyska vozidel
    case 'felicia'
        Z_dolni=0.38;
     case 'fabia1'
        Z_dolni=0.42;
     case 'fabia23'
        Z_dolni=0.40; 
     case 'octavia1'
        Z_dolni=0.38;
     case 'octavia2'
        Z_dolni=0.42;
     case 'octavia3'
        Z_dolni=0.39;
     case 'volks67'
        Z_dolni=0.39;
     case 'nakladni26'
        Z_dolni=0.26;
     case 'nakladni48'
        Z_dolni=0.48;
     case 'nakladni54'
        Z_dolni=0.54;
     case 'nakladni61'
        Z_dolni=0.61;
     case 'nakladni73'
        Z_dolni=0.73;
    otherwise
        Z_dolni=1; %Velke Z pro testovani nahodne vysky
end
%% Read images from Images folder
fullPathImageFolder=strcat(pwd,'\',vehicleFolder);
Imgs = dir(fullfile(fullPathImageFolder,'*.png'));

zapis_uvxyz=zeros(4,5);%zapis vektoru uvxyz
%% Výběr dvojice
for dvojice= 1 : length(Imgs)/2

for poradi_souboru=1:2
    
figure
obr = imread(fullfile(fullPathImageFolder,Imgs(2*dvojice-2+poradi_souboru).name));
Imgs(2*dvojice-2+poradi_souboru).name

if(size(obr,3) == 3) %Prevedeni rgb obrazu na sedotonovy
obr=rgb2gray(obr);
end

BW=imbinarize(obr,'adaptive','Sensitivity',sens);%Binarizace obrazu

indexy=[];
reg=regionprops(BW,'Area','centroid','Circularity','Image','BoundingBox');

i=1; %Vyber pouze vetsich regionu
while i<=length(reg)
    if((reg(i).Area)< 4000)
        reg(i)=[];
    else
        i=i+1;
    end
end

centroids = cat(1, reg.Centroid);
subplot(4,1,2:4);imshow(BW);hold on
plot(centroids(:,1), centroids(:,2), 'b*','MarkerSize',7,'LineWidth',1);
%% Filtrovani regionu
[pocet,~]=size(reg);
for i=1:pocet 
    if(round(size(reg(i).Image,2))>260 && round(size(reg(i).Image,2))<390)
        if (round(size(reg(i).Image,1)))>28 && round(size(reg(i).Image,1))<55
            if reg(i).Circularity>0.04 && reg(i).Circularity <0.35
                    indexy(end+1)=i;
            end
        end 
    end
end
findex=indexy(1); 
RZ=reg(findex); %vytvoreni samostatne struktury RZ

RZ.BoundingBox(1)=round(RZ.BoundingBox(1));
RZ.BoundingBox(2)=round(RZ.BoundingBox(2));
%% Mapovani vpravonahore, vpravodole
roh_nahore=[round(RZ.BoundingBox(1)+RZ.BoundingBox(3)),    round(RZ.BoundingBox(2))];
roh_dole=  [round(RZ.BoundingBox(1)+RZ.BoundingBox(3)), round(RZ.BoundingBox(2)+RZ.BoundingBox(4))];

%% Vykresleni pred upravou rohu
plot(roh_nahore(1)+1, roh_nahore(2),'g+')
plot(roh_dole(1),roh_dole(2),'g+');hold on

%% Zpresneni detekovaného rohu RZ
for k= roh_nahore(2): roh_nahore(2)+round(RZ.BoundingBox(4)/2)
    count=0;%oriznout pravy vrch1
    for i= roh_nahore(1) - round(RZ.BoundingBox(3)/8) : roh_nahore(1)
        if(BW(k,i)==0)
            count=count+1;
        end
    end
    if(count> 0.6*RZ.BoundingBox(3)/8)
        roh_nahore(2)=roh_nahore(2)+1;
    else
        break;
    end
end

pamatovani_bodu=roh_nahore;
for k=roh_nahore(1):-1: roh_nahore(1)- round(RZ.BoundingBox(3)/6)
    
    count=0;%oriznout pravy bok nahore2
    for i= roh_nahore(2): roh_nahore(2)+ round(RZ.BoundingBox(4)/3)
        if(BW(i,k)==0)
            count=count+1;
            plot(k, i,'c+')
            pamatovani_bodu=[k-1,roh_nahore(2)+1];
        end
         
    end
    if(count> round(0.3*RZ.BoundingBox(4)/3) )
        roh_nahore(1)=roh_nahore(1)-1;
    else
        roh_nahore=pamatovani_bodu;
        break;
    end
end
plot(roh_nahore(1), roh_nahore(2),'r+','MarkerSize',12,'LineWidth',3)

%% Uprava rohu dolevpravo
for k=roh_dole(1): -1 : roh_dole(1)- round(RZ.BoundingBox(3)/6)
    count=0;%oriznout pravy bok dole3
    for i= roh_dole(2) - round(RZ.BoundingBox(4)/2) : roh_dole(2)
        if(BW(i,k)==0)
            count=count+1;
        end
    end
    if(count> 0.7*RZ.BoundingBox(4)/2)
        roh_dole(1)=roh_dole(1)-1;
    else
        break;
    end
end
plot(roh_dole(1), roh_dole(2),'g+')

pamatovani_bodu=roh_dole;
for k= roh_dole(2) :-1: roh_dole(2) - round(RZ.BoundingBox(4)/2)     
    count=0;%oriznout pravy spodek4 k=radek
    for i= roh_dole(1)- round(RZ.BoundingBox(3)/8): roh_dole(1)
        if(BW(k,i)==0)
            count=count+1;
            plot(i, k,'c+')
            pamatovani_bodu=[i,k-1];
        end
    end
    if(count> 0.2*20)
        roh_dole(2)=roh_dole(2)-1;
    else
        roh_dole=pamatovani_bodu;
        break;
    end
end
plot(roh_dole(1), roh_dole(2),'m+','MarkerSize',12,'LineWidth',3)
%% 3D rekonstrukce
%% Ulozeni souradnic rohu do spolecne matice
zapis_uvxyz(2*poradi_souboru-1,1:2)=roh_nahore;
zapis_uvxyz(2*poradi_souboru,1:2)=roh_dole;

subplot(4,1,1);hold on
imshow(RZ.Image)
end

%% 2x2 body převést z u,v na XYZ
Z_vysoky= Z_dolni + 0.1 ; %pomer 520x110 - 10mm okraje
for i=1:4 %% uprava výšky Z, 1a3 radek jsou horni, 2a4spodni vysky
if i == 2 ||  i == 4
    Z=Z_dolni;
else
    Z=Z_vysoky;
end

I=T(1:2,1:2);
U=zapis_uvxyz(i,1:2)';
H=[T(1,3)*Z; T(2,3)*Z];
D= (inv(I))*(U-Q-H);

zapis_uvxyz(i,5)=Z;
zapis_uvxyz(i,3:4)=D';
end
%% Každému páru zvlášť vzdálenost X a Y a průměr
vrchni_vzdalenostX=abs( zapis_uvxyz(1,3)-zapis_uvxyz(3,3));
spodni_vzdalenostX= abs(zapis_uvxyz(2,3)-zapis_uvxyz(4,3));
vzdalenostX= (vrchni_vzdalenostX +spodni_vzdalenostX)/2; %prumerX

vrchni_vzdalenostY= abs(zapis_uvxyz(1,4)-zapis_uvxyz(3,4));
spodni_vzdalenostY= abs(zapis_uvxyz(2,4)-zapis_uvxyz(4,4));
vzdalenostY= (vrchni_vzdalenostY+spodni_vzdalenostY)/2; %prumerY

%% Pomoci Pythagorovy vety ziskame vyslednou vzdalenost
s = sqrt(vzdalenostX^2 + vzdalenostY^2);
% Extrakce rozdilu casu mezi snimky
prvni_cas=str2double(Imgs(2*dvojice-1).name(end-7:end-4));
druhy_cas=str2double(Imgs(2*dvojice).name(end-7:end-4));
t= (druhy_cas - prvni_cas)*0.001
%% Konecny vypocet rychlosti
dvojice %znaceni dvojice snimku datasetu do konzole
rychlost_dvojice =(s/t) *3.6
end

