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 to send angle to Simulink via base workspace
function sendAngleToMotor(angle)
    % Send angle to Simulink via base workspace
    assignin('base', 'targetAngle', angle);
    
    % Set a flag to indicate new angle is available
    assignin('base', 'newAngleAvailable', 1);
    
    % Display confirmation
    fprintf('Angle %.2f° sent to Simulink workspace\n', angle);
    
    % Optional: If you want to automatically start/update Simulink model
    startSimulinkModel(); % Uncomment if needed
end

%% Optional: Function to automatically control Simulink model
function startSimulinkModel()
    modelName = 'motor_control_model'; % Replace with your Simulink model name
    
    % Check if model is loaded
    if ~bdIsLoaded(modelName)
        try
            load_system(modelName);
            fprintf('Simulink model "%s" loaded\n', modelName);
        catch
            fprintf('Could not load Simulink model "%s". Please check the model name.\n', modelName);
            return;
        end
    end
    
    % Start simulation (optional - uncomment if you want auto-start)
    % set_param(modelName, 'SimulationCommand', 'start');
    % fprintf('Simulation started\n');
end

%% Function for moving motor to selected pin
function angle = moveMotorToPin(objectLoc, fig, backGnd, motorPosition)
    fprintf("Calculating movement angle...\n");    
    
    % Get image dimensions
    imgHeight = size(backGnd, 1);
    imgWidth = size(backGnd, 2);
    
    % Motor is at the bowler's position: far left middle of image
    % 0° = straight right (east), positive = counterclockwise (up), negative = clockwise (down)
    
    % Define motor position (bowler's position)
    motorX = motorPosition.x;  % Far left (small X value)
    motorY = motorPosition.y;  % Middle of image height
    
    % Pin position in image coordinates
    pinX = objectLoc(1);
    pinY = objectLoc(2);
    
    % Calculate vector from motor to pin
    dx = pinX - motorX;  % Horizontal distance (positive = pin is to the right of motor)
    dy = motorY - pinY;  % Vertical distance (positive = pin is above motor)
    
    % Calculate angle using atan2
    % atan2(dy, dx) gives angle relative to positive X-axis (east)
    % This matches: 0° = east, 90° = north, -90° = south
    angle = rad2deg(atan2(dy, dx));
    
    fprintf("Motor position: (%.1f, %.1f)\n", motorX, motorY);
    fprintf("Pin position: (%.1f, %.1f)\n", pinX, pinY);
    fprintf("Vector to pin: dx=%.1f, dy=%.1f\n", dx, dy);
    fprintf("Calculated angle: %.2f degrees\n", angle);
  
    close(fig);
    
    % Send the angle to your motor controller via Simulink
    sendAngleToMotor(angle);

    % pause(30);
    % 
    % sendAngleToMotor(-1*angle);
    
    return;
end

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

% Initialize variables in base workspace for Simulink
evalin('base', 'targetAngle = 0;'); % Initial angle
evalin('base', 'newAngleAvailable = 0;'); % Flag to indicate new angle

fprintf('Initialized workspace variables for Simulink:\n');
fprintf('  targetAngle = 0\n');
fprintf('  newAngleAvailable = 0\n');

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

dialogBox("background image")
rawBackGnd = snapshot(cam);
gameState.backGnd = rot90(rawBackGnd, 2);

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

dialogBox("game board")
rawForeGnd = snapshot(cam);
gameState.foreGnd = rot90(rawForeGnd, 2);
[height,width,~] = size(gameState.foreGnd);

%% Motor position (bowler's position - far left middle)
motorPosition.x = width * 0.2;  % 20% from left edge (adjust as needed)
motorPosition.y = height * 0.5; % 50% from top (middle vertically)

%% Subtract background and game state
gameState.backGndSUBTRACTED = rawBackGnd - rawForeGnd;
[rawHeight, rawWidth, ~] = size(rawBackGnd);

%% Fixing image
for i=1:rawHeight
    for j=1:rawWidth
        if (gameState.backGndSUBTRACTED(i,j,1) > 2) || ...
           (gameState.backGndSUBTRACTED(i,j,2) > 2) || ...
           (gameState.backGndSUBTRACTED(i,j,3) > 2)
            gameState.backGndSUBTRACTED(i,j,:) = [175,200,175];
        end
    end
end

figure();
imshow(gameState.backGndSUBTRACTED);

gameState.backGndSUBTRACTED = rgb2gray(gameState.backGndSUBTRACTED);

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

%% Erode and dilate to get rid of noise
SE = strel('disk',20);
gameState.foreGndBINARY = imerode(gameState.foreGndBINARY, SE);
gameState.foreGndBINARY = imdilate(gameState.foreGndBINARY, SE);

figure();
imshow(gameState.foreGndBINARY);

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

%% Acquire Pin Locations and transform coordinates for rotated display
pinPos = [];

for i = 1 : length(STATS)
    if STATS(i).Area > 200 && STATS(i).Area < 100000
        centroid_x = STATS(i).Centroid(1);
        centroid_y = STATS(i).Centroid(2);
        
        % Transform coordinates for 180-degree rotated display
        rotated_x = rawWidth - centroid_x;
        rotated_y = rawHeight - centroid_y;
        
        pinPos = [pinPos; rotated_x, rotated_y];
    end
end

%% GUI Configure
screenSize = get(0, 'ScreenSize');
screenWidth = screenSize(3);
screenHeight = screenSize(4);

scaleFactor = 0.8;
maxFigHeight = screenHeight * scaleFactor;
maxFigWidth = screenWidth * scaleFactor;

if height > maxFigHeight || width > maxFigWidth
    heightRatio = maxFigHeight / height;
    widthRatio = maxFigWidth / width;
    scale = min(heightRatio, widthRatio);
    figWidth = width * scale;
    figHeight = height * scale;
else
    figWidth = width;
    figHeight = height;
end

figX = (screenWidth - figWidth) / 2;
figY = (screenHeight - figHeight) / 2;

fig = uifigure('Name', 'Robot Bowling Control', ...
               'Position', [figX, figY, figWidth, figHeight]);

ax = uiaxes(fig, 'Position', [0, 0, figWidth, figHeight]);
imshow(gameState.foreGnd, 'Parent', ax);
title(ax, 'Select a Pin to Target');

% Draw motor position on the image for visualization
hold(ax, 'on');
plot(ax, motorPosition.x, motorPosition.y, 'ro', 'MarkerSize', 10, 'LineWidth', 2);
hold(ax, 'off');

ax.DataAspectRatio = [1 1 1];
ax.XLim = [1 width];
ax.YLim = [1 height];

xScale = figWidth / width;
yScale = figHeight / height;

% Overlay buttons only for visible pins
for i = 1:size(pinPos, 1)
    scaledX = pinPos(i,1) * xScale;
    scaledY = pinPos(i,2) * yScale;
    buttonY = figHeight - scaledY - 15;
    
    btn = uibutton(fig, 'push', ...
        'Text', sprintf('Select'), ...
        'Position', [scaledX-15, buttonY, 30, 30], ...
        'ButtonPushedFcn', @(btn, event) moveMotorToPin([pinPos(i,1), pinPos(i,2)], ...
        fig, gameState.backGnd, motorPosition), ...
        'FontSize', 6);
    btn.BackgroundColor = [0.2 0.8 0.2];
end