generate_paths.m

function paths = generate_paths(T, arenaSize, waypoints, startPhases)
% GENERATE_PATHS  Pre-compute deterministic paths for all 6 robots over T steps.
%
%   paths = generate_paths(T, arenaSize)
%   paths = generate_paths(T, arenaSize, waypoints)
%   paths = generate_paths(T, arenaSize, waypoints, startPhases)
%
%   Paths fill the full arenaSize x arenaSize space with natural overlap.
%   All dimensions scale proportionally with arenaSize.
%
%   INPUTS:
%     T           scalar    number of time steps
%     arenaSize   scalar    arena side length (m)
%     waypoints   [W x 2]   detector waypoints (optional, arena coords).
%                           If empty, default 6-point perimeter route used.
%     startPhases [6 x 1]   fractional start offset [0,1) per robot:
%                           [detector, H1, H2, H3, H4, H5].
%                           If empty/omitted, all start at 0.
%
%   PATH LAYOUT (all scaled to arenaSize):
%     Detector — perimeter-hugging waypoint route (single pass / looping)
%     H1 — large circle, centre-left,  r = 0.30*A,  2.0 laps
%     H2 — large circle, centre-right, r = 0.30*A,  1.5 laps, offset phase
%     H3 — lawnmower, full width, bottom 60% of arena, horizontal stripes
%     H4 — lawnmower, full height, left  60% of arena, vertical stripes
%     H5 — lawnmower, full width, top    60% of arena, horizontal stripes
%           (overlaps H3 in the middle band — intentional)

    if nargin < 3, waypoints   = []; end
    if nargin < 4, startPhases = zeros(6,1); end
    if isempty(startPhases), startPhases = zeros(6,1); end
    startPhases = startPhases(:);

    A = arenaSize;
    m = 0.05 * A;   % small wall margin (5% of arena)

    N = 5;
    paths.detector = make_detector_path(T, A, m, waypoints, startPhases(1));
    paths.helpers  = zeros(T, 2, N);

    % H1: large circle, left-of-centre, 2.0 laps
    paths.helpers(:,:,1) = make_circle(T, ...
        0.35*A, 0.50*A,  0.28*A,  2.0,  startPhases(2));

    % H2: large circle, right-of-centre, 1.5 laps, opposite phase start
    paths.helpers(:,:,2) = make_circle(T, ...
        0.65*A, 0.50*A,  0.28*A,  1.5,  startPhases(3) + 0.5);

    % H3: lawnmower, full width, lower 60%  [m .. A-m] x [m .. 0.62*A]
    paths.helpers(:,:,3) = make_lawnmower(T, ...
        m, A-m, m, 0.62*A, 'horizontal', 6, startPhases(4));

    % H4: lawnmower, left 60%, full height  [m .. 0.62*A] x [m .. A-m]
    paths.helpers(:,:,4) = make_lawnmower(T, ...
        m, 0.62*A, m, A-m, 'vertical', 6, startPhases(5));

    % H5: lawnmower, full width, upper 60%  [m .. A-m] x [0.38*A .. A-m]
    %     Overlaps H3 in the central band [0.38*A .. 0.62*A]
    paths.helpers(:,:,5) = make_lawnmower(T, ...
        m, A-m, 0.38*A, A-m, 'horizontal', 6, startPhases(6));
end

% =========================================================================
function path = make_detector_path(T, A, m, waypoints, startFrac)
% Perimeter-hugging waypoint route, arc-length parameterised, looping.
    if isempty(waypoints)
        waypoints = [
            m,     m;        % SW
            0.50*A, m;       % S mid
            A-m,   m;        % SE
            A-m,   0.50*A;   % E mid
            A-m,   A-m;      % NE
            0.50*A, A-m;     % N mid
            m,     A-m;      % NW
            m,     0.50*A;   % W mid
        ];
    end
    path = interp_loop(T, waypoints, startFrac);
    path = clamp(path, 0, A);
end

% =========================================================================
function path = make_circle(T, cx, cy, r, periods, startFrac)
    ph0   = startFrac * 2 * pi;
    theta = ph0 + linspace(0, 2*pi*periods, T)';
    path  = [cx + r*cos(theta),  cy + r*sin(theta)];
end

% =========================================================================
function path = make_lawnmower(T, xlo, xhi, ylo, yhi, orient, nStripes, startFrac)
    pts = [];
    if strcmp(orient, 'horizontal')
        ys = linspace(ylo, yhi, nStripes);
        for k = 1:nStripes
            if mod(k,2)==1; pts = [pts; xlo,ys(k); xhi,ys(k)]; %#ok<AGROW>
            else;           pts = [pts; xhi,ys(k); xlo,ys(k)]; end %#ok<AGROW>
        end
    else
        xs = linspace(xlo, xhi, nStripes);
        for k = 1:nStripes
            if mod(k,2)==1; pts = [pts; xs(k),ylo; xs(k),yhi]; %#ok<AGROW>
            else;           pts = [pts; xs(k),yhi; xs(k),ylo]; end %#ok<AGROW>
        end
    end
    path = interp_loop(T, pts, startFrac);
end

% =========================================================================
function path = interp_loop(T, pts, startFrac)
% Interpolate T equally-spaced positions along a closed polyline,
% starting at fractional arc-length position startFrac in [0,1).
    nPts    = size(pts, 1);
    ptsLoop = [pts; pts(1,:)];

    dists = zeros(nPts, 1);
    for k = 1:nPts
        dists(k) = norm(ptsLoop(k+1,:) - ptsLoop(k,:));
    end
    cumDist   = [0; cumsum(dists)];
    totalDist = cumDist(end);

    s0     = startFrac * totalDist;
    sQuery = mod(s0 + (0:T-1)' * (totalDist / T), totalDist);

    path = zeros(T, 2);
    for t = 1:T
        s   = sQuery(t);
        seg = find(cumDist <= s, 1, 'last');
        seg = min(seg, nPts);
        segLen = cumDist(seg+1) - cumDist(seg);
        if segLen < 1e-9; frac = 0;
        else;             frac = (s - cumDist(seg)) / segLen;
        end
        path(t,:) = ptsLoop(seg,:) + frac*(ptsLoop(seg+1,:) - ptsLoop(seg,:));
    end
end

% =========================================================================
function p = clamp(p, lo, hi)
    p = max(lo, min(hi, p));
end