%%
% clear all
% close all
% clc

%% CAMERA SETTING
cameraName = 'winvideo';
cameraId = 1;
format= 'RGB24_640x360';

vid = videoinput(cameraName, cameraId, format);

set(vid, 'FramesPerTrigger', Inf);
set(vid, 'ReturnedColorspace', 'rgb')
vid.FrameGrabInterval = 5;

%% START ACQUISITION
start(vid)

%% IMAGE PROCESSING

% Parameters
rMin = 20;
rMax = 60;
centers = [];
radii = [];
metric = [];
rMin_str = num2str(rMin);
rMax_str = num2str(rMax);

% Iterate for 200 frames
while(vid.FramesAcquired<=200)
    
    % Get screenshot
    data = getsnapshot(vid);
    figure(2), subplot(3,2,1), imshow(data), title('1: original image')
    
    % Extract blue component
	blueImg = imsubtract(data(:,:,3), rgb2gray(data));
    figure(2), subplot(3,2,2), imshow(blueImg), title('2: blue component')
    
    % Apply median filter
    blueImg = medfilt2(blueImg);
    figure(2), subplot(3,2,3), imshow(blueImg), title('3: filtered blue component')
    
    % Apply threshold
    blueImg = imadjust(blueImg);
    blueImg = im2bw(blueImg, 0.3);
    figure(2), subplot(3,2,4), imshow(blueImg), title('4: after threshold')
    
    % Apply morphological open
    blueImg = bwareaopen(blueImg,300);
    figure(2), subplot(3,2,5), imshow(blueImg), title('5: fter open')
    
    % Find circles
    [centers, radii, metric] = imfindcircles(blueImg, [rMin rMax]); 
    
    % Select best circle
    if((numel(centers)>0 && numel(radii)>0))
        [max_metric, index_metric] = max(metric);
        
        centers = centers(index_metric, :);
        radii = radii(index_metric, :);
    else
        centers = [];
        radii = [];
    end
    
    % Show tracking image
    % figure(1), imshow(data), title(strcat('Circle detection r=', rMin_str,'--', rMax_str))
    figure(2), subplot(3,2,6), imshow(data), title(strcat('6: Circle detection r=', rMin_str,'--', rMax_str))

    % Insert radius, center and outline
    if(numel(centers)>0 && numel(radii)>0)
        hold on
        
        % Create outline
        viscircles(centers(1,:),radii(1), 'color', 'yellow', 'lineStyle', '-', 'lineWidth', 0.3);
        % Add center and radius
        plot(centers(1,1), centers(1,2), '-m+', 'color', 'y')
        a = text(centers(1,1)+10, centers(1,2), strcat('X: ', num2str(round(centers(1,1))), '    Y: ', num2str(round(centers(1,2))), '    R: ', num2str(round(radii(1)))));
        set(a, 'FontName', 'Cambria', 'FontWeight', 'bold', 'FontSize', 11, 'Color', 'yellow');
        
        hold off
        
        % Print center
        % xCoord = centers(1,1)
        % yCoord = centers(1,2)
    end
    
end

%% STOP ACQUISITION
stop(vid);
flushdata(vid);


