% Senior Design Project 2 Code

close all
clc
clear

% Dialog box prompt for imaging parts
function dialogBox(currentImage)
    d = dialog('Position',[300 300 250 150],'Name','Background Acquisition');
    textString = 'Click advance when ' + currentImage + ' is ready for imaging';
    txt = uicontrol('Parent',d,...
               'Style','text',...
               'Position',[20 80 210 40],...
               'String',textString); %#ok<NASGU>

    btn = uicontrol('Parent',d,...
               'Position',[85 20 70 25],...
               'String','Advance',...
               'Callback','delete(gcf)'); %#ok<NASGU>

    uiwait(d);
end

%% Function for moving motor to selected pin
function angle = moveMotorToPin(objectLoc, fig, backGnd)
    fprintf("Calculating movement angle...\n");    
    
    size_h = size(backGnd, 1);
    
    origin_h = size_h/2;
    origin_w = size(backGnd,2);
    
    dx = objectLoc(1) - origin_w;
    dy = origin_h - objectLoc(2);

    angle = rad2deg(atan2(dy,dx));

    if(angle < 0)
        angle = angle + 360;
    end

    close(fig);

    fprintf("Moving %f degrees\n", angle);

    return;
end

%% Initialize camera
cam = webcam('Brio 100');


%% Background image capture

fprintf("Waiting to capture background image...\n");

dialogBox("background image")
% gameState.backGnd = imread('NoiseBackground.png');
gameState.backGnd = snapshot(cam);

%% Game image capture
fprintf("Waiting to capture game state image...\n")

dialogBox("game board")
% gameState.foreGnd = imread('StickersWithNoise.png');
gameState.foreGnd = snapshot(cam);
[height,width,depth] = size(gameState.foreGnd);

%% Subtract background and game state
gameState.backGndSUBTRACTED = gameState.backGnd - gameState.foreGnd;
% figure();
% imshow(gameState.backGndSUBTRACTED);

%% Fixing image

for i=1:height
    for j=1:width
        if (gameState.backGndSUBTRACTED(i,j,1) > 2) || ...
           (gameState.backGndSUBTRACTED(i,j,2) > 2) || ...
           (gameState.backGndSUBTRACTED(i,j,3) > 2)
            
            %Will Show in Green
            gameState.backGndSUBTRACTED(i,j,:) = [175,200,175];
        end
    end
end

figure();
imshow(gameState.backGndSUBTRACTED);

% convert to 2D using rgb2gray for the imbinarize function
gameState.backGndSUBTRACTED = rgb2gray(gameState.backGndSUBTRACTED);

%% Get foreground binary
gameState.foreGndBINARY = imbinarize(gameState.backGndSUBTRACTED);
% figure();
% imshow(gameState.foreGndBINARY);

%% Erode and dilate to get rid of noise
SE = strel('disk',20);

gameState.foreGndBINARY = imerode(gameState.foreGndBINARY, SE);
gameState.foreGndBINARY = imdilate(gameState.foreGndBINARY, SE);
%foreGndBINARY = imdilate(foreGndBINARY, SE);

figure();
imshow(gameState.foreGndBINARY);

%% Get region information
STATS = regionprops(gameState.foreGndBINARY, 'all');

%% Label Image
% figure();
% imshow(gameState.foreGnd);
% title('Results');

% Note: Thresholds for identifying the shape are tuned for the
% exact distance we have the camera at in the lab. shape identification
% may not work as expected until the conditional values are changed
foreGnd = gameState.foreGnd;
% hold on
% 
% for i = 1 : length(STATS)
%     W(i) = uint8(abs(STATS(i).BoundingBox(3)-STATS(i).BoundingBox(4)) > 1);
%     W(i) = W(i) + 2 * uint8((STATS(i).Extent - 1) == 0 );
%     diameters = mean([STATS(i).MajorAxisLength STATS(i).MinorAxisLength],2);
%     radii = diameters/2;
% 
%     centroid = STATS(i).Centroid;
% 
%     if STATS(i).Area > 200 && STATS(i).Area < 100000
%         gameState.centroidLoc{i} = centroid;
%         radii;
%         if radii > 45 && radii < 60
%             plot(centroid(1),centroid(2),'kO');
%             round(centroid(1));
%             round(centroid(2));
%             foreGnd(round(centroid(2)),round(centroid(1)),1);
%             foreGnd(round(centroid(2)),round(centroid(1)),2);
%             foreGnd(round(centroid(2)),round(centroid(1)),3);
%             if (foreGnd(round(centroid(2)),round(centroid(1)),1) > foreGnd(round(centroid(2)),round(centroid(1)),2)) && (foreGnd(round(centroid(2)),round(centroid(1)),1) > foreGnd(round(centroid(2)),round(centroid(1)),3)) && foreGnd(round(centroid(2)),round(centroid(1)),2) < 150
%                 gameState.color(i) = "RED";
%                 text(centroid(1)+50,centroid(2)+50,'RED');
%             end
%             if (foreGnd(round(centroid(2)),round(centroid(1)),2) > foreGnd(round(centroid(2)),round(centroid(1)),1)) && (foreGnd(round(centroid(2)),round(centroid(1)),2) > foreGnd(round(centroid(2)),round(centroid(1)),3)) && foreGnd(round(centroid(2)),round(centroid(1)),2) < 150
%                 gameState.color(i) = "GREEN";
%                 text(centroid(1)+50,centroid(2)+50,'GREEN');
%             end
%             if (foreGnd(round(centroid(2)),round(centroid(1)),3) > foreGnd(round(centroid(2)),round(centroid(1)),2)) && (foreGnd(round(centroid(2)),round(centroid(1)),3) > foreGnd(round(centroid(2)),round(centroid(1)),1))
%                 gameState.color(i) = "BLUE";
%                 text(centroid(1)+50,centroid(2)+50,'BLUE');
%             end
%             if foreGnd(round(centroid(2)),round(centroid(1)),1) > 150 && foreGnd(round(centroid(2)),round(centroid(1)),2) > 150
%                 gameState.color(i) = "YELLOW";
%                 text(centroid(1)+50,centroid(2)+50,'YELLOW');
%             end
% 
%         end
%     end
% end

%% Acquire Pin Locations
pinPos = [];

for i = 1 : length(STATS)
    if STATS(i).Area > 200 && STATS(i).Area < 100000
        pinPos = [pinPos; STATS(i).Centroid(1), STATS(i).Centroid(2)];
    
    end
end


%% GUI Configure
fig = uifigure('Name', 'Robot Bowling Control', 'Position', [100 100 width height]);

selectionGui = uiimage(fig);
selectionGui.Position = [0 0 width height]; % [x y width height]
selectionGui.ImageSource = gameState.foreGnd; % image or camera frame
selectionGui.ScaleMethod = 'none';

% Overlay buttons only for visible pins
for i = 1:length(pinPos)
        btn = uibutton(fig, 'push', ...
            'Text', sprintf('Select'), ...
            'Position', [pinPos(i,1) pinPos(i,2) 30 30], ...
            'ButtonPushedFcn', @(btn, event) moveMotorToPin(pinPos(i,:), ...
            fig, gameState.backGnd), ...
            'FontSize', 6);
        btn.BackgroundColor = [0.2 0.8 0.2]; % green tint
end