clc
close all

cam = webcam('USB Camera'); 
laser = 100; %% citlivost lokalizace laserové èáry (0;255)
for i = 1:1:100
          
rgbim = snapshot(cam); %% snímek v RGB
clear rows columns D A sirka vyska OUTPUT sloupce radky L x Vzdalenost
IMG = rgb2gray(rgbim); %% pøevedení RGB snímku na èernobílý

[rows, columns] = find(IMG > laser); %% lokalizace èáry - vytvoøení dvou vektorù
x = numel(columns);
L = columns(240,1);
H(1:x)= 60; %% vzdálenost h

%% Zvolení oblasti
if(484.8 > L)&&( L > 374.1) %% blízká oblast (10;20) cm
A(1:x) = (columns - 320)*(0.0072)+0.203;
D = H./(tan(A));
elseif(620 > L)&&(L > 484.9) %% støední oblast (20;100) cm
A(1:x) = (columns - 320)*(0.0014)+1.016;
D = H./(tan(A));
elseif(360 < L)&&(L < 374.1) %% vzdálená oblast (100;130) cm
A(1:x) = (columns - 320)*(0.0149)-0.2081;
D = H./(tan(A));
else %% mimo aktivní rozsah
    disp('ERROR'); 
end 

%% Výpoèet výšky a šíøky bodu ze vzdálenosti
 %% Kamera má pøibližné stejnì široké i vysoké zorné pole, kde jeho velikost
 %% pøibližnì odpovídá i vzdálenosti clonky od kamery
Vzdalenost = D';
vyska (1:x) = 480;
radky = (Vzdalenost./vyska').*(240-rows);

sirka (1:x) = 640;
sloupce = (Vzdalenost./sirka').*(columns-320);

OUTPUT = horzcat(radky, sloupce, Vzdalenost); 
TF = OUTPUT(:,3) < 10 | OUTPUT(:,3) > 130; %% kontrola aktivního rozsahu
OUTPUT(TF,:)=[]; %% matice bodù
%% levý sloupec - výška bodu od osy kamery
%% støední sloupec - šíøka bodu od osy kamery
%% pravý sloupec - vzdálenost bodu od èela kamery

% plot3(OUTPUT(:,2),OUTPUT(:,3),OUTPUT(:,1),'+') %% mapa bodù
% grid on
pause(1)
end

