clear;clc

% Color threshold
tol = 0.1;

% Get test image
sim('color_sel', 'TimeOut', 3)

% Pixel coordinates field
MN    = [size(I1,1), size(I1,2)];
[X,Y] = meshgrid( 1:size(I1,2), 1:size(I1,1) );

figure(1) % Selecting a pixel manually
subplot(1,2,1); himg1 = imagesc(I1);    % Displaying image Cam-1
pA = drawpoint('Visible','on');         % Selecting a pixel with the mouse
pA = round( pA.Position );              % Pixel coordinates
c1 = I1( pA(2), pA(1), : );             % Selected color
title('Left Camera')

subplot(1,2,2); himg2 = imagesc(I2);    % Displaying image Cam-2
pB = drawpoint('Visible','on');         % Selecting a pixel with the mouse
pB = round( pB.Position );              % Pixel coordinates
c2 = I2( pB(2), pB(1), : );             % Selected color
title('Right Camera')


% Camera parameters
load("params.mat")
cam1.K = K1;
cam1.R = eye(3);
cam1.t = zeros(3,1);
cam1.d = 0;

cam2.K = K2;
cam2.R = R;
cam2.t = t;
cam2.d = 0;

cam1_bus_info = Simulink.Bus.createObject(cam1);
cam2_bus_info = Simulink.Bus.createObject(cam2);

cam1_bus = evalin('base', cam1_bus_info.busName);
cam2_bus = evalin('base', cam2_bus_info.busName);

sim('color_detect', inf)
