% load('stereoparams.mat'); % zaladowanie automatyczne parametrow
% kalibracyjnych
close all; % zamkniecie okien

% showExtrinsics(stereoParams);  % pokazanie wynikow kalibracji
format long
% przygotowanie zmiennych pod zaladowanie obrazow
mainInputFolder = 'Baza';
leftInputFolder = 'L';
rightInputFolder = 'R';

% wczytanie wszystkich plikow jpg z glownego folderu
filePattern = fullfile(mainInputFolder, leftInputFolder, '*.jpg');
theFiles = dir(filePattern);

% interwaly czasowe do wyswietlania zdjec
time=0.1;
time2 = 0.1;

% parametry wykrywania
blurValue = 3;
BlockSize = 13;
SE = strel('diamond', 1);
SE2 = strel('diamond', 11);
threshold = 25;

% glowna petla, ktora wg ilosci zdjec robi mape dysparycji i przetwarza ja
for number = 40:length(theFiles)

        % wczyanie zdjec wejsciowych
        frameLeft =  imread([mainInputFolder '\' leftInputFolder '\' theFiles(number).name]);
        frameRight =  imread([mainInputFolder '\' rightInputFolder '\' theFiles(number).name]);

        % wyblurowanie zdjec
        frameLeftBlur = imgaussfilt(frameLeft, blurValue);
        frameRightBlur = imgaussfilt(frameRight, blurValue);

        % rektyfikacja
        [frameLeftRect, frameRightRect] = ...
            rectifyStereoImages(frameLeftBlur, frameRightBlur, stereoParams);
        figure;

        imshow(stereoAnaglyph(frameLeftRect, frameRightRect));
        title('Anaglif');

        pause(time2);

        % konwersja zdjec do skali szarosci
        frameLeftGray  = rgb2gray(frameLeftRect);
        frameRightGray = rgb2gray(frameRightRect);

        % metoda wykonujaca metode dysparycji wg zdjec po rektyfikacji

        disparityMap = disparity(frameLeftGray, frameRightGray, 'BlockSize', BlockSize, 'ContrastThreshold', 0.2);
        figure;
        imshow(imdilate(disparityMap, SE), [0, 64]);

        pause(time);

        disparityMapSE = imerode(disparityMap, SE); % wykonanie operacji erozji
%         disparityMapSE8= uint16(disparityMapSE); % konwersja mapy do uint8

        % odciecie disparityMap dla danego progu bez binaryzacji
        disparityMap2 = disparityMapSE
        disparityMap2(disparityMap2 < threshold) = 0;
        figure;
        imshow(disparityMap2, [0, 64])
        title('XDD')
        % progowanie mapy z binaryzacja oraz wykrycie 3 najwiekszych elementow

        disparityMapSEthresh = disparityMapSE > threshold;
        disparityMapSEthresh = bwareafilt(disparityMapSEthresh, 3);

        % wymnozenie maski oraz mapy po odcieciu bez binaryzacji
        disparityTEMP = immultiply(disparityMapSEthresh, disparityMap2)
        figure;
        imshow(disparityTEMP, [0, 64]);


        pause(time);

        figure;
        disparityMapSE2thresh = imdilate(disparityMapSEthresh, SE2); % dylatacja
        imshow(disparityMapSE2thresh);
        title('Disparity Map');
        colorbar

        pause(time);

        figure;
        imgToBB = im2bw(disparityMapSE2thresh) % ponowna konwersja do skali szarosci
        imshow(frameLeft)
        title('Wynik');
        % poszukiwanie bialych regionow na mapie oraz zamykanie ich w
        % prostokaty aby wskazac gdzie znajduje sie przeszkoda
        measurements = regionprops(imgToBB, 'BoundingBox', 'Area');
        for k = 1 : length(measurements)
              thisBB = measurements(k).BoundingBox;
              if(thisBB(3)*thisBB(4) > 10000) % odrzucenie najmniejszych wykryc
                rectangle('Position', [thisBB(1)+54,thisBB(2),thisBB(3)-54,thisBB(4)+20],...
                'EdgeColor','r','LineWidth',4 ) % rysowanie prosotkatu z przesunieciem
                 %wyciecie zawartosci boundingboxa do obliczenia sredniej
                 %wartosci pikseli
                 J1 = imcrop(disparityMap2, [thisBB(1)+54, thisBB(2), thisBB(3)-54, thisBB(4)+20])
%                wyciecie tla z boundinboxa
                 Idx = J1 == 0
                 % obliczenie sredniej wart pikseli bez czarnego tla
                 Mean = uint8(mean(J1(~Idx)));

                 % obliczenie wartosci odleglosci dzieki sredniej oraz
                 % regresji liniowej
                 odleglosc = (double(Mean) - 72.2439)/(-0.36463);
                 disp(odleglosc)
                 % dodanie napisu dla kazdego BB z odlegloscia

                 string = strcat('~', num2str(ceil(odleglosc/10)*10), ' cm')
                 text(thisBB(1)+54, thisBB(2)-15, string, 'Color', 'red', 'FontSize', 18)



              end
        end
end