close all;
clc;
clear;

%% -------------------- 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);

%% -------------------- Fixed lane pixel measurements -------------------- %%
% These are from your calibration and do NOT change with setup
laneLeft_px_meas  = 900.0;   % one side of lane  (update from your calibration)
laneRight_px_meas = 900.0;   % other side of lane (update from your calibration)

% Ensure left < right logically
laneLeft_px  = min(laneLeft_px_meas,  laneRight_px_meas);
laneRight_px = max(laneLeft_px_meas,  laneRight_px_meas);

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

fprintf('Lane left pixel (stored):   %.1f\n', laneLeft_px);
fprintf('Lane right pixel (stored):  %.1f\n', laneRight_px);
fprintf('Lane center pixel (stored): %.1f\n', laneCenter_px);
fprintf('Pixel span:                 %.3f px\n', pixelSpan);
fprintf('Inches per pixel (fixed):   %.5f in/px\n', inchesPerPixel);

%% expose to base for functions using evalin
assignin('base', 'laneLeft_px', laneLeft_px);
assignin('base', 'laneRight_px', laneRight_px);
assignin('base', 'laneCenter_px', laneCenter_px);
assignin('base', 'inchesPerPixel', inchesPerPixel);

%% -------------------- 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)
evalin('base', 'targetRot = 0;');
evalin('base', 'newTargetRotAvailable = 0;');
evalin('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 Advance dialog) ---------- %%
fprintf("Waiting to capture game state image...\n");
dialogBox("game board");

rawImg = snapshot(cam);
% 
% % ---------- ORIENTATION CHANGE HERE ----------
% % 1) Optional horizontal flip to fix mirroring
% fore = fliplr(rawImg);   % if you do NOT need mirroring, use: fore = rawImg;
% 
% % 2) Rotate so ball travels bottom -> top, pins at the top
% %    90° CCW: original LEFT -> BOTTOM, RIGHT -> TOP
% fore = rot90(fore, 1);   % if upside-down, change 1 to -1
% % ---------------------------------------------

fore = rot90(rawImg, 1);
fore = flipud(fore);    % vertical flip
fore = fliplr(fore);    % mirror horizontally



gameState.foreGnd = fore;

figure;
imshow(gameState.foreGnd);
title('Captured lane image (rotated for bottom-up bowl)');

%% -------------------- 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);
h = hsvImg(:,:,1); %#ok<NASGU>
s = hsvImg(:,:,2);
v = hsvImg(:,:,3);

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

occupiedPins = false(size(pinDotCenters_px,1),1);

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

    % Define a small ROI around the dot
    xRange = round(cx - searchRadius) : round(cx + searchRadius);
    yRange = round(cy - searchRadius) : round(cy + searchRadius);

    % Clamp to image bounds
    xRange = xRange(xRange >= 1 & xRange <= size(s,2));
    yRange = yRange(yRange >= 1 & yRange <= size(s,1));

    % Extract local HSV patch
    sPatch = s(yRange, xRange);
    vPatch = v(yRange, xRange); %#ok<NASGU>

    meanS = mean(sPatch(:));
    meanV = mean(vPatch(:));

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

    % Very simple rule:
% Window radius around each dot
searchRadius = 30;

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');


%% --- Debug visualization of detection windows ---
figure;
imshow(gameState.foreGnd);
title('Pin detection windows (green = detected pin, red = empty)');
hold on;

for i = 1:size(pinDotCenters_px,1)
    cx = pinDotCenters_px(i,1);
    cy = pinDotCenters_px(i,2);

    % Choose color based on detection
    if occupiedPins(i)
        edgeCol = 'g';   % green = pin detected
    else
        edgeCol = 'r';   % red = no pin
    end

    % Draw the search window as a square
    rectangle('Position', [cx-searchRadius, cy-searchRadius, ...
                           2*searchRadius, 2*searchRadius], ...
              'EdgeColor', edgeCol, 'LineWidth', 2);

    % Label each window with its pin number
    text(cx, cy, sprintf('%d', i), ...
        'Color', 'y', 'FontWeight', 'bold', ...
        'HorizontalAlignment', 'center', ...
        'VerticalAlignment', 'middle');
end

hold off;


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);

% We assume 6 pins in a 1–2–3 triangle.
% We now want the triangle to POINT DOWN toward you, so:
%
%   Row (top):    P4  P5  P6  (furthest from you)
%   Row (middle):   P2  P3
%   Row (bottom):      P1     (closest to you)
%
% This matches "pin 1 facing down towards me" with ball from bottom -> top.

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

% Pre-compute the row heights
y_top    = rowY_top;               % top row
y_middle = rowY_top - rowSpacing;  % middle row
y_bottom = rowY_top - 2*rowSpacing;% bottom row (closest to you visually)

if numDots >= 1
    % Pin 1 at the BOTTOM (triangle pointing downwards)
    btnPos(1,:) = [centerX - btnSize/2, y_bottom, btnSize, btnSize];
end
if numDots >= 3
    % Middle row: pins 2 and 3
    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
    % Top row: pins 4, 5, 6
    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 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)
    assignin('base', 'targetRot', targetRot);
    assignin('base', 'newTargetRotAvailable', 1);

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

    startSimulinkModel();
end

function slowMoveToRot(targetRot)
    try
        currentRot = evalin('base','currentRot');
    catch
        currentRot = 0;
    end

    maxStep = 0.05;   % rev per step (smaller = slower)
    dt      = 0.1;    % seconds between steps

    fprintf('Slow move: currentRot = %.3f, targetRot = %.3f\n', currentRot, targetRot);

    while abs(targetRot - currentRot) > 1e-3
        step = maxStep * sign(targetRot - currentRot);

        if abs(targetRot - currentRot) < abs(step)
            currentRot = targetRot;
        else
            currentRot = currentRot + step;
        end

        assignin('base','currentRot', currentRot);
        assignin('base','targetRot', currentRot);
        assignin('base','newTargetRotAvailable', 1);
        set_param('working', 'SimulationCommand', 'update');

        fprintf('  -> commanding %.3f rev\n', currentRot);
        pause(dt);
    end

    fprintf('Reached targetRot = %.3f rev (slow move).\n', targetRot);
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

    % If you want auto-start:
    % set_param(modelName, 'SimulationCommand', 'start');
    % fprintf('Simulation started\n');
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');

    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
    targetRot = 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);

    % Move gently (slowed) to the new rotation
    slowMoveToRot(targetRot);
end