close all;
clc;
clear;

motorDirectionSign = -1;   % if left/right are inverted, use -1. If it flips, change to +1.
assignin('base','motorDirectionSign', motorDirectionSign);

assignin('base','servoAngle',0);   % initial rest angle


%% -------------------- Lane & Gear Parameters -------------------- %%
% Lane geometry
laneWidth_in = 6;              % total lane width in inches
halfLane_in  = laneWidth_in/2; % 3 in each side

% Gear / rack parameters (Mod 1, 16-tooth spur gear)
module_mm   = 1;      % metric module (mm per tooth)
gearTeeth   = 16;
mm_per_in   = 25.4;

pitchDiameter_mm = module_mm * gearTeeth;       % d = m*z
travelPerRev_mm  = pi * pitchDiameter_mm;       % linear travel per revolution
travelPerRev_in  = travelPerRev_mm / mm_per_in; % in/rev

maxRotations = halfLane_in / travelPerRev_in;   % max rev from center to side

fprintf('Travel per rev: %.3f in\n', travelPerRev_in);
fprintf('Max rotations from center: %.3f rev\n', maxRotations);

%% -------------------- Initialize Camera & Simulink Vars ---------------- %%
cam = webcam('Brio 100');

% Let the camera handle exposure automatically.
try
    cam.ExposureMode = 'auto';
catch
    disp('Could not set exposure mode (OK to ignore).');
end

% Variables used by Simulink (in base workspace)
assignin('base','targetRot', 0);          % revolutions from center
assignin('base','targetAngle', 0);        % degrees from center
assignin('base','newTargetRotAvailable', 0);
assignin('base','currentRot', 0);

assignin('base', 'travelPerRev_in', travelPerRev_in);
assignin('base', 'maxRotations',    maxRotations);
assignin('base', 'halfLane_in',     halfLane_in);

fprintf('Initialized workspace variables for Simulink.\n');

%% -------------------- Capture Game Image (with dialog) ------------------ %%
fprintf("Waiting to capture game state image...\n");
dialogBox("game board");

rawImg = snapshot(cam);

% Orientation so that ball travels bottom->top and lane left/right match view
fore = rot90(rawImg, 1);
fore = flipud(fore);    % vertical flip
fore = fliplr(fore);    % horizontal mirror

gameState.foreGnd = fore;

%% -------------------- LANE CALIBRATION (LOAD OR CLICK ONCE) ------------ %%
[laneLeft_px, laneRight_px, laneCenter_px, inchesPerPixel] = ...
    getLaneCalibration(gameState.foreGnd, laneWidth_in);

fprintf('\n--- Lane Calibration ---\n');
fprintf('Lane left pixel:          %.1f\n', laneLeft_px);
fprintf('Lane right pixel:         %.1f\n', laneRight_px);
fprintf('Lane center pixel:        %.1f\n', laneCenter_px);
fprintf('Inches per pixel:         %.5f in/px\n', inchesPerPixel);

% Expose to base for moveMotorToPin
assignin('base', 'laneLeft_px',     laneLeft_px);
assignin('base', 'laneRight_px',    laneRight_px);
assignin('base', 'laneCenter_px',   laneCenter_px);
assignin('base', 'inchesPerPixel',  inchesPerPixel);

%% -------------------- Pin Detection Using Fixed Dot Locations ----------- %%
% Get stored dot centers (or calibrate once if file doesn't exist)
pinDotCenters_px = getPinDotCenters(gameState.foreGnd);  % Nx2 [x,y]

% Convert to HSV once
hsvImg = rgb2hsv(gameState.foreGnd);
s = hsvImg(:,:,2);
v = hsvImg(:,:,3);

% Window radius around each dot to "look for pin", in pixels
searchRadius = 30;    % local patch half-size

N = size(pinDotCenters_px,1);
meanS_all = zeros(N,1);
meanV_all = zeros(N,1);

fprintf("\n--- Per-pin saturation debug ---\n");
for i = 1:N
    cx = pinDotCenters_px(i,1);
    cy = pinDotCenters_px(i,2);

    % ROI
    xRange = round(cx - searchRadius) : round(cx + searchRadius);
    yRange = round(cy - searchRadius) : round(cy + searchRadius);
    xRange = xRange(xRange >= 1 & xRange <= size(s,2));
    yRange = yRange(yRange >= 1 & yRange <= size(s,1));

    sPatch = s(yRange, xRange);
    vPatch = v(yRange, xRange);

    meanS_all(i) = mean(sPatch(:));
    meanV_all(i) = mean(vPatch(:));

    fprintf('Pin %d: meanS = %.3f, meanV = %.3f\n', i, meanS_all(i), meanV_all(i));
end

% Use the median saturation as "background paper" level
bgS = median(meanS_all);

occupiedPins = false(N,1);
for i = 1:N
    % pin if it's noticeably more saturated than the typical patch
    if meanS_all(i) > bgS + 0.05 && meanV_all(i) > 0.4
        occupiedPins(i) = true;
    end
end

fprintf('--------------------------------------\n');
numDetected = nnz(occupiedPins);
fprintf('Detected %d occupied pin location(s).\n', numDetected);

%% -------------------- SIMPLE GUI (triangle layout, no image) ----------- %%
% Figure size & centering
figWidth  = 320;
figHeight = 260;

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

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

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

% Title label
uilabel(fig, ...
    'Text', 'Select a pin to target', ...
    'Position', [20, figHeight-40, figWidth-40, 30], ...
    'HorizontalAlignment', 'center', ...
    'FontSize', 14);

numDots = size(pinDotCenters_px,1);

% Triangle layout pointing DOWN toward you:
%   Row (top):    P4  P5  P6  (furthest from you)
%   Row (middle):   P2  P3
%   Row (bottom):      P1     (closest to you)

centerX    = figWidth/2;
rowY_top   = figHeight - 90;
rowSpacing = 45;
btnSize    = 40; % width = height

btnPos = nan(numDots, 4); % [x y w h] for each pin button

y_top    = rowY_top;                % top row
y_middle = rowY_top - rowSpacing;   % middle row
y_bottom = rowY_top - 2*rowSpacing; % bottom row

if numDots >= 1
    % Pin 1 at the bottom
    btnPos(1,:) = [centerX - btnSize/2, y_bottom, btnSize, btnSize];
end
if numDots >= 3
    % Pins 2 and 3 in the middle row
    offsetX = 35;
    btnPos(2,:) = [centerX - offsetX - btnSize/2, y_middle, btnSize, btnSize];
    btnPos(3,:) = [centerX + offsetX - btnSize/2, y_middle, btnSize, btnSize];
end
if numDots >= 6
    % Pins 4, 5, 6 in the top row
    offsetX = 70;
    btnPos(4,:) = [centerX - offsetX - btnSize/2, y_top, btnSize, btnSize];
    btnPos(5,:) = [centerX - btnSize/2,           y_top, btnSize, btnSize];
    btnPos(6,:) = [centerX + offsetX - btnSize/2, y_top, btnSize, btnSize];
end

% Create buttons in the new triangle layout
for i = 1:numDots
    if any(isnan(btnPos(i,:)))
        continue; % in case fewer than 6 dots
    end

    thisPos = btnPos(i,:);

    btn = uibutton(fig, 'push', ...
        'Text', sprintf('%d', i), ...        % label by pin number
        'Position', thisPos, ...
        'ButtonPushedFcn', @(btn, event) moveMotorToPin(pinDotCenters_px(i,:), [], struct()), ...
        'FontSize', 12);

    btn.FontWeight = 'bold';

    if occupiedPins(i)
        % Pin present – enable and green-ish
        btn.Enable          = 'on';
        btn.BackgroundColor = [0.2 0.8 0.2];
    else
        % No pin at this location – disable and grey it out
        btn.Enable          = 'off';
        btn.BackgroundColor = [0.8 0.8 0.8];
    end
end

% Optional note label
if numDetected == 0
    noteTxt = 'No pins detected at any locations.';
else
    noteTxt = sprintf('%d pin(s) detected. Grey pins = empty spots.', numDetected);
end

uilabel(fig, ...
    'Text', noteTxt, ...
    'Position', [20, 15, figWidth-40, 30], ...
    'HorizontalAlignment', 'center', ...
    'FontSize', 10);


%% ========================== LOCAL FUNCTIONS ========================== %%

function dialogBox(currentImage)
    d = dialog('Position',[300 300 260 150],'Name','Image Acquisition');
    textString = "Click Advance when " + currentImage + " is ready";
    uicontrol('Parent',d,...
              'Style','text',...
              'Position',[20 80 220 40],...
              'String',textString);

    uicontrol('Parent',d,...
              'Position',[95 20 70 25],...
              'String','Advance',...
              'Callback','delete(gcf)');

    uiwait(d);
end


function [laneLeft_px, laneRight_px, laneCenter_px, inchesPerPixel] = ...
         getLaneCalibration(foreImg, laneWidth_in)
% GETLANECALIBRATION
%   Load lane calibration from laneCalibration.mat if it exists
%   and matches the current image size + lane width.
%   Otherwise, ask the user to click LEFT and RIGHT lane edges once, then
%   save for future runs.

    fname = 'laneCalibration.mat';
    [hNow, wNow, ~] = size(foreImg);
    imgSizeNow = [hNow, wNow];

    needRecal = true;

    if isfile(fname)
        S = load(fname);
        % Note: we saved laneWidth_in as "laneWidth_in_save"
        if isfield(S, 'laneLeft_px')    && isfield(S, 'laneRight_px') && ...
           isfield(S, 'laneCenter_px')  && isfield(S, 'inchesPerPixel') && ...
           isfield(S, 'imgSize')        && isfield(S, 'laneWidth_in_save')

            if isequal(S.imgSize, imgSizeNow) && S.laneWidth_in_save == laneWidth_in
                laneLeft_px    = S.laneLeft_px;
                laneRight_px   = S.laneRight_px;
                laneCenter_px  = S.laneCenter_px;
                inchesPerPixel = S.inchesPerPixel;
                fprintf('Loaded lane calibration from %s\n', fname);
                needRecal = false;
            else
                fprintf('Lane calibration image size or lane width changed. Recalibrating.\n');
            end
        else
            fprintf('Lane calibration file missing fields. Recalibrating.\n');
        end
    end

    if needRecal
        figure;
        imshow(foreImg);
        title('Click LEFT lane edge, then RIGHT lane edge');
        [xEdge, ~] = ginput(2);
        close;

        laneLeft_px_meas  = xEdge(1);
        laneRight_px_meas = xEdge(2);

        laneLeft_px  = min(laneLeft_px_meas,  laneRight_px_meas);
        laneRight_px = max(laneRight_px_meas, laneLeft_px_meas);

        laneCenter_px   = (laneLeft_px + laneRight_px) / 2;
        pixelSpan       = abs(laneRight_px - laneLeft_px);
        inchesPerPixel  = laneWidth_in / pixelSpan;

        imgSize = imgSizeNow;          %#ok<NASGU>
        laneWidth_in_save = laneWidth_in; %#ok<NASGU>
        save(fname, 'laneLeft_px','laneRight_px','laneCenter_px', ...
                    'inchesPerPixel','imgSize','laneWidth_in_save');
        fprintf('Saved lane calibration to %s\n', fname);
    end
end


function pinDotCenters_px = getPinDotCenters(foreImg)
% GETPINDOTCENTERS  Load or create the list of dot locations (px)
%
%   - If pinDotCenters.mat exists and matches current image size, it loads.
%   - Otherwise, it shows the image and asks you to click each dot center.

    fname = 'pinDotCenters.mat';
    [hNow, wNow, ~] = size(foreImg);
    imgSizeNow = [hNow, wNow];

    needRecal = true;

    if isfile(fname)
        S = load(fname);
        if isfield(S, 'pinDotCenters_px') && isfield(S, 'imgSize')
            if isequal(S.imgSize, imgSizeNow)
                % same orientation/size -> reuse
                pinDotCenters_px = S.pinDotCenters_px;
                fprintf('Loaded %d stored pin dot locations from %s\n', ...
                        size(pinDotCenters_px,1), fname);
                needRecal = false;
            else
                fprintf(['Image size/orientation changed since last ' ...
                         'calibration. Re-calibrating pin dot centers...\n']);
            end
        else
            fprintf('Old calibration file found but missing fields. Re-calibrating.\n');
        end
    end

    if needRecal
        figure;
        imshow(foreImg);
        title('Calibration: click the center of each dot (e.g., 6 clicks)');

        numDots = 6;   % change if you ever use a different number
        [x, y] = ginput(numDots);
        pinDotCenters_px = [x, y];

        imgSize = imgSizeNow; %#ok<NASGU>
        save(fname, 'pinDotCenters_px', 'imgSize');
        fprintf('Saved %d pin dot locations to %s\n', numDots, fname);
        close;
    end
end


function sendRotationsToMotor(targetRot)
    % One-shot command: write to base workspace and poke Simulink
    assignin('base', 'targetRot',   targetRot);
    assignin('base', 'targetAngle', targetRot * 360);  % degrees
    assignin('base', 'newTargetRotAvailable', 1);

    % Let Simulink pull the new value
    set_param('working', 'SimulationCommand', 'update');
    fprintf('targetRot = %.3f rev sent to Simulink workspace\n', targetRot);

    startSimulinkModel();
end


function startSimulinkModel()
    modelName = 'working'; 
    
    if ~bdIsLoaded(modelName)
        try
            load_system(modelName);
            fprintf('Simulink model "%s" loaded\n', modelName);
        catch
            fprintf('Could not load Simulink model "%s". Check model name.\n', modelName);
            return;
        end
    end

    % Auto-start the simulation (comment out if you want manual control)
    try
        set_param(modelName, 'SimulationCommand', 'start');
        fprintf('Simulation "%s" started\n', modelName);
    catch ME
        fprintf('Could not start simulation: %s\n', ME.message);
    end
end


function targetRot = moveMotorToPin(objectLoc, fig, motorPosition) %#ok<INUSD>
    fprintf("Calculating motor rotations for selected pin...\n");    
    
    laneCenter_px    = evalin('base', 'laneCenter_px');
    inchesPerPixel   = evalin('base', 'inchesPerPixel');
    travelPerRev_in  = evalin('base', 'travelPerRev_in');
    maxRotations     = evalin('base', 'maxRotations');
    halfLane_in      = evalin('base', 'halfLane_in');
    motorDirSign     = evalin('base','motorDirectionSign');

    pinX_px = objectLoc(1);
    pinY_px = objectLoc(2); %#ok<NASGU>

    fprintf("Pin centroid (pixels): (%.1f, %.1f)\n", pinX_px, pinY_px);
    fprintf("Lane center (pixels):  %.1f\n", laneCenter_px);

    % Pixels -> inches from lane center
    offset_in = (pinX_px - laneCenter_px) * inchesPerPixel;  % + right, - left
    fprintf("Desired offset from lane center: %.3f in\n", offset_in);

    % Clamp to physical lane edges
    offset_in_clamped = max(min(offset_in, halfLane_in), -halfLane_in);
    if abs(offset_in - offset_in_clamped) > 1e-3
        fprintf("Offset clamped from %.3f in to %.3f in (lane edge)\n", ...
                offset_in, offset_in_clamped);
    end

    % Inches -> rotations, with adjustable direction
    targetRot = motorDirSign * (offset_in_clamped / travelPerRev_in);

    % Clamp rotations to mechanical limit
    targetRot_clamped = max(min(targetRot, maxRotations), -maxRotations);
    if abs(targetRot - targetRot_clamped) > 1e-3
        fprintf("Rotations clamped from %.3f rev to %.3f rev (rack limit)\n", ...
                targetRot, targetRot_clamped);
    end

    targetRot = targetRot_clamped;
    fprintf("Commanded target rotations from center: %.3f rev\n", targetRot);

    % === 1) Move laterally to the pin ===
    sendRotationsToMotor(targetRot);

    % === 2) Wait 3 seconds at that lateral position ===
    pause(3);

    % === 3) Fire the servo motor action ===
    triggerServoMotor();

    % === 4) Wait another 3 seconds after servo action ===
    pause(3);

    % === 5) Return to original lateral position (center = 0 rev) ===
    fprintf("Returning to center (0 rev)...\n");
    sendRotationsToMotor(0);

    
function triggerServoMotor()
    fprintf("Triggering servo via Simulink (servoAngle variable)...\n");

    % Go to 90 degrees
    assignin('base','servoAngle',90);
    set_param('working','SimulationCommand','update');

    pause(2);

    % Back to 0 degrees
    assignin('base','servoAngle',0);
    set_param('working','SimulationCommand','update');

    fprintf("Servo returned to rest (0°).\n");
end






end