function [P,C] = calibratedStereoVision(imgR,imgL,stereoParams,filterIntensity,param1,param2)
% calibratedStereoVision - Prostorová rekonstrukce pomocí stereoskopie
%
% Popis funkce:
% Funkce pro výpočet bodů v prostorů ze dvou kalibrovaných pohledů pomocí
% rektifikace snímků
%
% Syntax:  
%        1. Bez textur
%           [P] = calibratedStereoVision(imgR,imgL,stereoParams,filterIntensity)
%        2. Textura z jednoho pohledu
%           [P,C] = calibratedStereoVision(imgR,imgL,stereoParams,filterIntensity,imgColor,mod)
%        3. Textura z obou pohledů
%           [P,C] = calibratedStereoVision(imgR,imgL,stereoParams,filterIntensity,,imgRcolor,imgLcolor)
%
% Vstupní parametry:
%   - 'imgR' = snímek s nasvíceným laserovým vzorem první kamery
%   - 'imgL' = snímek s nasvíceným laserovým vzorem druhé kamery 
%   - 'stereoParams' = parametry získané kalibrací kamer (stereoParameters)
%   - 'filterIntensity' = intenzita filtru, 0 - 100 [%]
% V případě použití textury z jednoho snímku:
%   - 'imgColor' = snímek se zaznamenanou texturou
%   - 'mod' = ze které kamery byl snímek zaznamenán
%           1 = první (z pohledu imgR)
%           2 = druhá (z pohledu imgL)
% V případě použití textur z dvojice snímků:
%   - 'imgRcolor' = snímek první kamery se zaznamenanou texturou
%   - 'imgLcolor' = snímek druhé kamery se zaznamenanou texturou
%
% Výstupní parametry:
%    P - Body v prostoru ve formátu [x,y,z], v případě chyby []
%    C - Barevná informace ve formátu [R,G,B], v případě chyby []
%
% Vytvořeno v rámci bakalářské práce na téma Vytvoření 3D modelu za pomoci 
% laserového vzoru.
% Martin Krátký
% Vysoké učení technické v Brně
% Květen 2019
% Práce je dostupná na webové adrese:
% https://www.vutbr.cz/studenti/zav-prace/detail/119313 
%
% See also: meshGeneration, getIndex
%

f = (stereoParams.CameraParameters1.FocalLength(1)+stereoParams.CameraParameters2.FocalLength(1))/2;
b = sqrt(stereoParams.TranslationOfCamera2(1)^2+stereoParams.TranslationOfCamera2(2)^2+stereoParams.TranslationOfCamera2(3)^2);
laserR = imfill(imgR(:,:,1) >= filterIntensity*255/100, 'holes');
laserL = imfill(imgL(:,:,1) >= filterIntensity*255/100, 'holes');

P = []; C=[]; k = 1; 
[recR,recL] = rectifyStereoImages(laserR,laserL,stereoParams, 'OutputView', 'full');

if nargin == 6
    if isfloat(param2)
        if param2 == 1
            [colorR,~] = rectifyStereoImages(param1,imgL,stereoParams, 'OutputView', 'full');
            mod=1;
        elseif param2 == 2
            [~,colorL] = rectifyStereoImages(imgR,param1,stereoParams, 'OutputView', 'full');
            mod=2;
        end
    else
        [colorR,colorL] = rectifyStereoImages(param1,param2,stereoParams, 'OutputView', 'full');
        mod=3;
    end
else
    mod=0;
end

if exist('colorR','var')
    redR = colorR(:,:,1);
    greenR = colorR(:,:,2);
    blueR = colorR(:,:,3);
end
if exist('colorL','var')
    redL = colorL(:,:,1);
    greenL = colorL(:,:,2);
    blueL = colorL(:,:,3);
end
    
lenR = size(recR,1);
for v=1:lenR
     uR = getIndex(recR(v,:),1);
     uL = getIndex(recL(v,:),1);
     if uR && uL
        P = [P ;zeros(1,3)];
        d = uL - uR;
        P(k,:) = [-b*(uR+uL)/(2*d) b*v/d b*f/d];
        
        switch mod
            case 1
                R = redR(v,uR);
                G = greenR(v,uR);
                B = blueR(v,uR);
            case 2
                R = redL(v,uL);
                G = greenL(v,uL);
                B = blueL(v,uL);
            case 3
                R = int8((redR(v,uR)+redL(v,uL))/2);
                G = int8((greenR(v,uR)+greenL(v,uL))/2);
                B = int8((blueR(v,uR)+blueL(v,uL))/2);
        end
        if mod
            C = [C ;zeros(1,3)];
            C(k,:) = [R G B];
        end
        k=k+1;
    end
end 
end
