function out = swarm_planner_helper2(xself,yself,psiself, x2,y2,psi2, x3,y3,psi3, ...
                                    MAX_PATH_LEN, replan_steps)
%%
%SWARM_PLANNER_HELPER  Esegue per il leader: cost map + A* + spline.
%   Per i follower: path lineare (interpolato a 1 m) verso target di
%   formazione (rotazione del leader). Aggiorna anche map.known con i 3
%   veicoli (visione condivisa).
%
%   Output flat (lunghezza totale 6*L + x):
%     [1 .. L]               path_x_1
%     [L+1 .. 2L]            path_y_1
%     [2L+1]                 n_1
%     [2L+2 .. 3L+1]         path_x_2
%     [3L+2 .. 4L+1]         path_y_2
%     [4L+2]                 n_2
%     [4L+3 .. 5L+2]         path_x_3
%     [5L+3 .. 6L+2]         path_y_3
%     [6L+3]                 n_3
%e poi ci sono le mu, unknown ratio per ogni veicolo, pct known e goal flag (non usato, diventa 1 quando si arriva) 
%dovrei mettere un flag che viaggia sia tra il planner che tra veicoli in
%caso un veicolo fallisca, così  se è il leader qualcuno deu 2 follower
%prende il posto suo
%   Replan A* avviene ogni 'replan_steps' chiamate (gestito con persistent).
%   Le altre chiamate riutilizzano l'ultimo path noto.
% il vantaggio di persistent è che al rilancio della funzione conserva in
% memoria il valore di prima, anche se si fa clear. lo 'svantaggio' appunto è
% che bisogna chiamare un clear functions all'inizio della demo che a
% matlab per qualche motivo non piace
L = MAX_PATH_LEN;
out = zeros(6*L + 11, 1);
% 6 perchè 2 x 3 veicoli
% --- Sanity check sul base workspace
has_data = evalin('base', ...
    'exist(''map'',''var'') && exist(''scenario'',''var'') && exist(''vehicles'',''var'')');
if ~has_data
    return; %esce dal programma
end

map = evalin('base', 'map');
scenario = evalin('base', 'scenario');
vehicles = evalin('base', 'vehicles');

% Stati arrivati da Simulink

states = [xself yself psiself; x2 y2 psi2; x3 y3 psi3];

% Protezione contro sample iniziale a coordinate nulle.
% Serve a evitare cerchio map.known e compaction in origine.
tol0 = 1e-9;

for i = 1:3
    bad_xy = abs(states(i,1)) < tol0 && abs(states(i,2)) < tol0;
    start_not_zero = abs(scenario.start(i,1)) > tol0 || abs(scenario.start(i,2)) > tol0;

    if bad_xy && start_not_zero
        states(i,1) = scenario.start(i,1);
        states(i,2) = scenario.start(i,2);
        states(i,3) = scenario.start(i,3);
    end
end

for i = 1:3
    vehicles(i).state.x   = states(i,1);
    vehicles(i).state.y   = states(i,2);
    vehicles(i).state.psi = states(i,3);
end

%% =================================================================
%  1. AGGIORNA map.known (visione condivisa dei 3 veicoli)
%% =================================================================
if ~isfield(map, 'known') || isempty(map.known) || ...
   ~isequal(size(map.known), size(map.X))
    map.known = false(size(map.X));
end

if isfield(scenario, 'sensor') && isfield(scenario.sensor, 'vision_radius_m')
    R_vis = scenario.sensor.vision_radius_m;
else
    R_vis = 10.0;
end

res = map.params.res_m;
sz  = size(map.X);
for i = 1:3
    xi = states(i,1); yi = states(i,2);
    i_min = max(1, floor((yi - R_vis) / res) + 1);
    i_max = min(sz(1), ceil ((yi + R_vis) / res) + 1);
    j_min = max(1, floor((xi - R_vis) / res) + 1);
    j_max = min(sz(2), ceil ((xi + R_vis) / res) + 1);
    if i_min > i_max || j_min > j_max, continue; end
    Xb = map.X(i_min:i_max, j_min:j_max);
    Yb = map.Y(i_min:i_max, j_min:j_max);
    in_disc = (Xb - xi).^2 + (Yb - yi).^2 <= R_vis^2; %crea il cerchio
    map.known(i_min:i_max, j_min:j_max) = ...
        map.known(i_min:i_max, j_min:j_max) | in_disc;
    map = update_friction_map(map, ... %da qui
    [states(i,1), states(i,2), states(i,3)], ...
    scenario.vehicle.track_update_width_m, ...
    scenario.vehicle.track_update_length_m, ...
    scenario.terrain.compaction_factor, ...
    scenario.terrain.mu_max); %a qui si sta aggiornando la mappa per il multipass
end

%% =================================================================
%  2. PATH PLANNING LEADER (A* + spline) ogni replan_steps step
%  =================================================================
persistent leader_path step_counter
if isempty(step_counter), step_counter = 0; end %primo step
step_counter = step_counter + 1;

do_replan = isempty(leader_path) || (mod(step_counter-1, replan_steps) == 0);

if do_replan
     try
        %% Cost map percepita dal planner
        % map.cost_all è la cost map completa precalcolata in init_swarm_scenario.
        % Qui ne creo una copia locale e ci sovrappongo la conoscenza corrente.

        if isfield(map, 'cost_all') && ~isempty(map.cost_all)
            cost_map = map.cost_all;
        else
            % Fallback di sicurezza: utile se per errore lanci vecchie demo.
            cost_map = build_cost_map(map, scenario);
            map.cost_all = cost_map;
        end

        % Overlay dinamico della mappa non nota.
        % Le celle non conosciute vengono viste da A* con costo cost_unknown.
        % Questa operazione NON modifica map.cost_all.
        if isfield(map, 'known') && ~isempty(map.known) && ...
                isequal(size(map.known), size(cost_map))

            unknown_mask = ~logical(map.known);
            cost_map(unknown_mask) = scenario.planning.cost_unknown;
        end

        leader_xy = [states(1,1), states(1,2)];
        path_raw  = astar_path(leader_xy, scenario.goal, cost_map, map, scenario);
        if ~isempty(path_raw) && size(path_raw,1) >= 2
            path_smooth = smooth_path_spline(path_raw, 1.0, 'pchip');
            leader_path = path_smooth;

            t_now = (step_counter - 1) * scenario.sim.dtastar;

            % --- Inline log paths_raw_history ---------------------------
            if evalin('base', 'exist(''paths_raw_history'',''var'')')
                hist_r = evalin('base', 'paths_raw_history');
            else
                hist_r = struct('t', {}, 'path', {});
            end
            hist_r(end+1).t    = t_now;
            hist_r(end).path   = path_raw;
            assignin('base', 'paths_raw_history', hist_r);

            % --- Inline log paths_smooth_history ------------------------
            if evalin('base', 'exist(''paths_smooth_history'',''var'')')
                hist_s = evalin('base', 'paths_smooth_history');
            else
                hist_s = struct('t', {}, 'path', {});
            end
            hist_s(end+1).t    = t_now;
            hist_s(end).path   = path_smooth;
            assignin('base', 'paths_smooth_history', hist_s);

            % --- Inline log known_history -------------------------------
            if evalin('base', 'exist(''known_history'',''var'')')
                hist_k = evalin('base', 'known_history');
            else
                hist_k = struct('t', {}, 'mask', {});
            end
            hist_k(end+1).t      = t_now;
            hist_k(end).mask     = map.known;
            assignin('base', 'known_history', hist_k);
         end
     catch ME
         warning('swarm_planner_helper:replan_failed', ...
             'A* fallito allo step %d: %s', step_counter, ME.message);
     end
end

if isempty(leader_path)
    % Fallback: linea retta start→goal
    leader_path = [states(1,1), states(1,2); scenario.goal];
end

% Pack leader path
n_lead = min(size(leader_path, 1), L);
p_x_1 = zeros(L, 1); p_y_1 = zeros(L, 1);
p_x_1(1:n_lead) = leader_path(1:n_lead, 1);
p_y_1(1:n_lead) = leader_path(1:n_lead, 2);
%Simulink vuole dimensioni fisse. Quindi anche se il path ha, per esempio,
%237 punti, tu devi comunque dare fuori vettori lunghi L = 1000.
%p_x_1 = [x1, x2, ..., x237, 0, 0, 0, ...]
%p_y_1 = [y1, y2, ..., y237, 0, 0, 0, ...]
%n_1   = 237 così il track usa solo i primi 237 punti
% %% Aggiorna leader_history
% persistent leader_history
% if isempty(leader_history)
%     leader_history = [states(1,1), states(1,2)];
% end
% 
% % Aggiungi punto solo se ti sei mosso abbastanza (downsampling a 1 m)
% last_pt = leader_history(end, :);
% if hypot(states(1,1)-last_pt(1), states(1,2)-last_pt(2)) > 1.0
%     leader_history(end+1, :) = [states(1,1), states(1,2)];
% end
% 
% % Path completo per i follower: passato + futuro
% % Evita di duplicare la posizione attuale del leader: leader_path inizia da lì
% if size(leader_history,1) >= 1 && size(leader_path,1) >= 1
%     path_full = [leader_history(1:end-1, :); leader_path];
% else
%     path_full = leader_path;
% end
% %% =================================================================
% %  3. PATH FOLLOWER DERIVATO DAL PATH SMOOTHED DEL LEADER
% %  =================================================================
% 
% leader_path_use = [p_x_1(1:n_lead), p_y_1(1:n_lead)];
% 
% [p_x_2, p_y_2, n_2] = make_follower_path_from_leader_path( ...
%     states(2,1), states(2,2), ...
%     states(1,1), states(1,2), states(1,3), ...
%     leader_path_use, 2, scenario, map, L);
% 
% [p_x_3, p_y_3, n_3] = make_follower_path_from_leader_path( ...
%     states(3,1), states(3,2), ...
%     states(1,1), states(1,2), states(1,3), ...
%     leader_path_use, 3, scenario, map, L);
%% =================================================================
%  3. PATH FOLLOWER (linea retta verso target di formazione)
%  =================================================================
psiL = states(1,3);
R    = [cos(psiL), -sin(psiL); sin(psiL), cos(psiL)];%matrice rotaz leader
%qui sotto se si vuole tornare come prima mettere di nuovo solo
%make_follower_path e togli map dall'input
[p_x_2, p_y_2, n_2] = make_follower_path(states(2,1), states(2,2), 2, ...
    states(1,1), states(1,2), R, scenario, map,L);
[p_x_3, p_y_3, n_3] = make_follower_path(states(3,1), states(3,2), 3, ...
     states(1,1), states(1,2), R, scenario, map, L);

%make follower path dà delle coordinate relative al leader, ad esempio 5
%dietro 10 a sx, ma deve essere orientata con il leader
%% =================================================================
%  4. Goal flag e % mappa scoperta
%% =================================================================

% Goal raggiunto se QUALSIASI veicolo entra nel cerchio r_goal
r_goal = scenario.control.goal_reached_radius_m;
any_in_goal = false;
for i = 1:size(states, 1)
    d = hypot(scenario.goal(1) - states(i,1), ...
              scenario.goal(2) - states(i,2));
    if d < r_goal
        any_in_goal = true;
        break;
    end
end
goal_flag = double(any_in_goal);
pct_known = nnz(map.known) / numel(map.known);

%% =================================================================
%  5. Pack output flat
%% =================================================================
out(1:L)         = p_x_1;
out(L+1:2*L)     = p_y_1;
out(2*L+1)       = n_lead;
out(2*L+2:3*L+1) = p_x_2;
out(3*L+2:4*L+1) = p_y_2;
out(4*L+2)       = n_2;
out(4*L+3:5*L+2) = p_x_3;
out(5*L+3:6*L+2) = p_y_3;
out(6*L+3)       = n_3;
out(6*L+4)       = goal_flag;
out(6*L+5)       = pct_known;

%% mu locale per ogni veicolo (da map.mu nella posizione corrente)
%questa serve per la funzione di costo, non per controllare il veicolo.
%già usata in un'altra funzione, si può cancellare
for i = 1:3
    local_i = get_local_terrain(map, states(i,1), states(i,2));
    out(6*L + 5 + i) = double(local_i.mu);
end

    %% Calcolo mu_next e unknown_ratio_ahead

R_vis   = scenario.sensor.vision_radius_m;
d_look  = 0.9 * R_vis; %più piccolo del raggio noto per stare sicuri che è vista questa mu
L_sq    = R_vis * sqrt(pi / 0.60);   % lato quadrato: circa 2.29 * R_vis

paths_x = {p_x_1, p_x_2, p_x_3};
paths_y = {p_y_1, p_y_2, p_y_3};
paths_n = {n_lead, n_2, n_3};

for i = 1:3
    xi_r = states(i,1);
    yi_r = states(i,2);
    psi_r = states(i,3);
    cp = cos(psi_r); sp = sin(psi_r);

    % --- mu_next ---
    mu_next_i = 0.3; %metto 0.3 così per sicurezza se non riesce a vederla
%almeno ha un valore basso
    path_xi = paths_x{i};
    path_yi = paths_y{i};
    n_i     = paths_n{i};

    for kk = 1:n_i
        d_kk = sqrt((path_xi(kk)-xi_r)^2 + (path_yi(kk)-yi_r)^2);
        if d_kk >= d_look
            ix = round((path_xi(kk) - map.x(1)) / res) + 1;
            iy = round((path_yi(kk) - map.y(1)) / res) + 1;
            ix = max(1, min(ix, size(map.X,2)));
            iy = max(1, min(iy, size(map.X,1)));
            if map.known(iy, ix)
                mu_val = map.mu(iy, ix);
                if ~isnan(mu_val) && mu_val > 0 && mu_val ~= -1
                    mu_next_i = mu_val;
                end
            end
            break;
        end
    end
    out(6*L + 5 + i) = mu_next_i;

    % --- unknown_ratio_ahead ---
    % Bounding box conservativa intorno al quadrato
    half_diag = L_sq * sqrt(2) / 2;
    ix_min = max(1,        round((xi_r - half_diag - map.x(1)) / res) + 1);
    ix_max = min(size(map.X,2), round((xi_r + half_diag - map.x(1)) / res) + 1);
    iy_min = max(1,        round((yi_r - half_diag - map.y(1)) / res) + 1);
    iy_max = min(size(map.X,1), round((yi_r + half_diag - map.y(1)) / res) + 1);

    Xsub = map.X(iy_min:iy_max, ix_min:ix_max);
    Ysub = map.Y(iy_min:iy_max, ix_min:ix_max);
    Ksub = map.known(iy_min:iy_max, ix_min:ix_max);

    % Trasformazione in body frame
    dx = Xsub - xi_r;
    dy = Ysub - yi_r;
    u_body =  dx*cp + dy*sp;
    v_body = -dx*sp + dy*cp;

    in_sq = (u_body >= 0) & (u_body <= L_sq) & (abs(v_body) <= L_sq/2);
    n_sq  = nnz(in_sq);

    if n_sq > 0
        unknown_ratio_i = nnz(in_sq & ~Ksub) / n_sq;
    else
        unknown_ratio_i = 0;
    end
    out(6*L + 8 + i) = unknown_ratio_i;   % indici 6L+9, 6L+10, 6L+11
end
    %% =================================================================
    %  6. Risalva map e vehicles
    %% =================================================================
assignin('base', 'map',      map);
assignin('base', 'vehicles', vehicles);
% Log traiettoria per debug (aggiornato ad ogni step) TOGLIERE QUESTE RIGHE
% ALLA FINE
t_now_log = (step_counter - 1) * scenario.sim.dtastar;
if evalin('base', 'exist(''traj_log'',''var'')')
    tl = evalin('base', 'traj_log');
    tl.x(end+1, :) = states(:, 1)';   % x dei 3 veicoli
    tl.y(end+1, :) = states(:, 2)';   % y dei 3 veicoli
    tl.t(end+1, 1) = t_now_log;
    assignin('base', 'traj_log', tl);
end
end


% =========================================================================
%  Helper: path lineare follower → target_formazione, sampled a ~1 m
% QUESTA NON VIENE PIù USATA
% =========================================================================
function [px, py, n] = make_follower_path(xf, yf, idx, xL, yL, R, scenario, map, L)

    px = zeros(L, 1);
    py = zeros(L, 1);

    offsetBody   = scenario.formation.offsets_body(idx, :).';
    offsetGlobal = R * offsetBody;
    target = [xL, yL] + offsetGlobal.';

    dx = target(1) - xf;
    dy = target(2) - yf;
    d  = hypot(dx, dy);

    if d < 0.5
        px(1) = xf;        py(1) = yf;
        px(2) = target(1); py(2) = target(2);
        n = 2;
        return;
    end

    %% Check veloce: linea retta libera?
    direct_heading = atan2(dy, dx);
    lookahead = min(d, scenario.control.local_lookahead_m);

    % Campionamento più largo: serve solo per capire se la linea è bloccata
    ds_check = 4.0;

    straight_safe = true;

    for r = ds_check:ds_check:lookahead
        xs = xf + r*cos(direct_heading);
        ys = yf + r*sin(direct_heading);

        % Accesso diretto alla griglia, molto più leggero di get_local_terrain
        res = map.params.res_m;
        ii = floor(ys / res) + 1;
        jj = floor(xs / res) + 1;

        if ii < 1 || ii > size(map.traversable,1) || ...
           jj < 1 || jj > size(map.traversable,2)
            straight_safe = false;
            break;
        end

        if ~map.traversable(ii,jj) || map.obstacle(ii,jj)
            straight_safe = false;
            break;
        end
    end

    %% Se linea libera: usa la linea retta
    if straight_safe
        n_seg = max(2, min(L, floor(d) + 1));
        s = linspace(0, 1, n_seg).';

        px(1:n_seg) = xf + s * dx;
        py(1:n_seg) = yf + s * dy;
        n = n_seg;
        return;
    end

    %% Se linea ostruita: usa select_local_heading
    vehicleF.state.x = xf;
    vehicleF.state.y = yf;
    vehicleF.state.psi = 0;

    desired_heading = select_local_heading(vehicleF, target, map, scenario);

    local_len = min(d, scenario.control.local_lookahead_m);

    ds_path = 1.0;
    n_seg = max(2, min(L, floor(local_len/ds_path) + 1));

    s = linspace(0, local_len, n_seg).';

    px(1:n_seg) = xf + s*cos(desired_heading);
    py(1:n_seg) = yf + s*sin(desired_heading);

    n = n_seg;
end

function [px, py, n] = make_follower_path2(xf, yf, idx, xL, yL, R, scenario, L)
    px = zeros(L, 1);
    py = zeros(L, 1);

    offsetBody   = scenario.formation.offsets_body(idx, :).';
    offsetGlobal = R * offsetBody;
    target = [xL, yL] + offsetGlobal.';

    dx = target(1) - xf; dy = target(2) - yf;
    d  = sqrt(dx^2 + dy^2);

    if d < 0.5
        % Veicolo gia' al target: path "nullo" di 2 punti coincidenti
        px(1) = xf; py(1) = yf;
        px(2) = target(1); py(2) = target(2);
        n = 2;
        return;
    end

    n_seg = max(2, min(L, floor(d) + 1));
    s = linspace(0, 1, n_seg).';
    px(1:n_seg) = xf + s * dx;
    py(1:n_seg) = yf + s * dy;
    n = n_seg;
end
%% =========================================================================
%  Helper: log path history nel base workspace
% =========================================================================

%NUOVA FCN PER IL PATH DEI FOLLOWER
function [px, py, n] = make_follower_path_from_leader_path( ...
    xf, yf, xL, yL, psiL, leader_path, idx, scenario, map, L)

    px = zeros(L,1);
    py = zeros(L,1);

    if isempty(leader_path) || size(leader_path,1) < 2
        px(1:2) = xf;
        py(1:2) = yf;
        n = 2;
        return;
    end

    % Curvilinea del path leader
    s = path_arclength(leader_path);

    % Proiezione del leader sul suo path
    s_leader = project_point_on_path_s([xL, yL], leader_path, s);

    % Nome formazione
    formation_name = 'triangle';
    if isfield(scenario, 'formation') && isfield(scenario.formation, 'name')
        formation_name = scenario.formation.name;
    end

    % Parametri formazione
    line_spacing = 10.0;
    if isfield(scenario.formation, 'line_spacing_m')
        line_spacing = scenario.formation.line_spacing_m;
    end

    tri_back = 10.0;
    if isfield(scenario.formation, 'triangle_back_m')
        tri_back = scenario.formation.triangle_back_m;
    end

    tri_lat = 12.0;
    if isfield(scenario.formation, 'triangle_lat_m')
        tri_lat = scenario.formation.triangle_lat_m;
    end

    % Definizione distanza dietro e offset laterale
    switch lower(formation_name)
        case 'line'
            d_back  = (idx - 1) * line_spacing;
            lat_des = 0.0;

        case 'triangle'
            d_back = tri_back;

            if idx == 2
                lat_des = -tri_lat;
            elseif idx == 3
                lat_des = tri_lat;
            else
                lat_des = 0.0;
            end

        otherwise
            d_back  = (idx - 1) * line_spacing;
            lat_des = 0.0;
    end

    % Target dietro al leader: intersezione path-smoothed / cerchio
    % centro = posizione leader, raggio = d_back.
    [p_center, s_target] = find_path_circle_target_behind_leader( ...
        [xL, yL], psiL, leader_path, s, s_leader, d_back);

    % Applica offset laterale, se triangle.
    % Se l'offset finisce in ostacolo, viene ridotto verso il centro.
    p_target = apply_triangle_offset_safe( ...
        p_center, leader_path, s, s_target, lat_des, map, scenario);

    % Proiezione follower sul path leader
    s_follower = project_point_on_path_s([xf, yf], leader_path, s);

    % Se il follower è già oltre il target dietro al leader,
    % non gli diamo un path verso avanti: lo facciamo puntare al target locale.
    if s_follower >= s_target
        px(1) = xf;
        py(1) = yf;
        px(2) = p_target(1);
        py(2) = p_target(2);
        n = 2;
        return;
    end

    % Costruzione path dal follower fino al target dietro al leader
    ds = 1.0;
    s_vec = (s_follower:ds:s_target).';

    if isempty(s_vec)
        s_vec = s_target;
    elseif s_vec(end) < s_target
        s_vec(end+1,1) = s_target;
    end

    path_f = zeros(numel(s_vec)+1, 2);

    % Primo punto = posizione attuale follower
    path_f(1,:) = [xf, yf];

    for k = 1:numel(s_vec)
        p_c = interp_path_by_s(leader_path, s, s_vec(k));

        p_o = apply_triangle_offset_safe( ...
            p_c, leader_path, s, s_vec(k), lat_des, map, scenario);

        path_f(k+1,:) = p_o;
    end

    n = min(size(path_f,1), L);
    px(1:n) = path_f(1:n,1);
    py(1:n) = path_f(1:n,2);
end
%funzione per trovare intersezione circonferenza-path follower
function [p_target, s_target] = find_path_circle_target_behind_leader( ...
    pL, psiL, path, s, s_leader, R)

    % Se R è zero o quasi, target = leader sul path
    if R < 1e-6
        s_target = s_leader;
        p_target = interp_path_by_s(path, s, s_target);
        return;
    end

    % Direzione asse x leader
    exL = [cos(psiL), sin(psiL)];

    best_score = Inf;
    best_s = max(0, s_leader - R);

    % Campionamento tratto dietro al leader
    s_min = max(0, s_leader - 2.5*R);
    s_max = s_leader;

    ds = 0.5;
    s_vec = (s_min:ds:s_max).';

    if isempty(s_vec)
        s_vec = s_min;
    end

    for k = 1:numel(s_vec)
        p = interp_path_by_s(path, s, s_vec(k));

        rel = p - pL;
        dist_err = abs(norm(rel) - R);

        % Deve stare dietro al leader: proiezione sull'asse x leader negativa
        u_body = dot(rel, exL);

        if u_body > 0
            continue;
        end

        % Preferisci:
        % 1) distanza dal cerchio corretta
        % 2) punto il più possibile dietro, non laterale
        v_body = abs(rel(1)*(-sin(psiL)) + rel(2)*cos(psiL));

        score = dist_err + 0.05*v_body;

        if score < best_score
            best_score = score;
            best_s = s_vec(k);
        end
    end

    s_target = best_s;
    p_target = interp_path_by_s(path, s, s_target);
end
function p_safe = apply_triangle_offset_safe( ...
    p_center, leader_path, s, s_query, lat_des, map, scenario)

    if abs(lat_des) < 1e-9
        p_safe = p_center;
        return;
    end

    r_safe = 4.0;
    if isfield(scenario, 'formation') && isfield(scenario.formation, 'r_safe_obs_m')
        r_safe = scenario.formation.r_safe_obs_m;
    end

    t_hat = path_tangent_by_s(leader_path, s, s_query);
    n_hat = [-t_hat(2), t_hat(1)];

    % Prova offset pieno, poi riduce progressivamente verso il centro
    alpha_vec = linspace(1, 0, 11);

    best_p = p_center;
    best_d = -Inf;

    for kk = 1:numel(alpha_vec)
        a = alpha_vec(kk);

        p_try = p_center + (a * lat_des) * n_hat;

        [ok_cell, d_obs] = point_clearance_from_obstacles(p_try, map);

        % Caso ottimo: cella libera e distanza >= r_safe
        if ok_cell && d_obs >= r_safe
            p_safe = p_try;
            return;
        end

        % Se non esiste punto a r_safe, scegli quello con clearance massima.
        if ok_cell && d_obs > best_d
            best_d = d_obs;
            best_p = p_try;
        end
    end

    % Se non trova offset sicuro, usa il miglior punto libero trovato.
    % Se nemmeno quello esiste, best_p rimane p_center.
    p_safe = best_p;
end
function [ok_cell, d_obs] = point_clearance_from_obstacles(p, map)

    xq = p(1);
    yq = p(2);

    % Fuori mappa = non valido
    if xq < min(map.x) || xq > max(map.x) || yq < min(map.y) || yq > max(map.y)
        ok_cell = false;
        d_obs = 0;
        return;
    end

    obs_q = interp2(map.X, map.Y, double(map.obstacle), xq, yq, 'nearest', 1);
    trav_q = interp2(map.X, map.Y, double(map.traversable), xq, yq, 'nearest', 0);

    if isnan(obs_q) || isnan(trav_q) || obs_q > 0.5 || trav_q < 0.5
        ok_cell = false;
        d_obs = 0;
        return;
    end

    obs = map.obstacle;

    % Usa solo ostacoli noti, se presente map.known
    if isfield(map, 'known') && ~isempty(map.known) && isequal(size(map.known), size(obs))
        obs = obs & map.known;
    end

    if ~any(obs(:))
        ok_cell = true;
        d_obs = Inf;
        return;
    end

    d_map = bwdist(obs) * map.params.res_m;

    d_obs = interp2(map.X, map.Y, d_map, xq, yq, 'linear', 0);

    ok_cell = true;
end
function s = path_arclength(path)

    if size(path,1) < 2
        s = 0;
        return;
    end

    ds = sqrt(sum(diff(path,1,1).^2, 2));
    s = [0; cumsum(ds)];
end
function s_proj = project_point_on_path_s(p, path, s)

    best_d2 = Inf;
    s_proj = s(1);

    for i = 1:size(path,1)-1
        a = path(i,:);
        b = path(i+1,:);
        ab = b - a;
        ab2 = dot(ab, ab);

        if ab2 < 1e-12
            continue;
        end

        u = dot(p - a, ab) / ab2;
        u = max(0, min(1, u));

        q = a + u * ab;
        d2 = sum((p - q).^2);

        if d2 < best_d2
            best_d2 = d2;
            s_proj = s(i) + u * (s(i+1) - s(i));
        end
    end
end
function p = interp_path_by_s(path, s, s_query)

    s_query = max(s(1), min(s_query, s(end)));

    x = interp1(s, path(:,1), s_query, 'linear', 'extrap');
    y = interp1(s, path(:,2), s_query, 'linear', 'extrap');

    p = [x, y];
end
function t_hat = path_tangent_by_s(path, s, s_query)

    if size(path,1) < 2
        t_hat = [1, 0];
        return;
    end

    s_query = max(s(1), min(s_query, s(end)));

    idx = find(s <= s_query, 1, 'last');

    if isempty(idx)
        idx = 1;
    end

    if idx >= size(path,1)
        idx = size(path,1) - 1;
    end

    dp = path(idx+1,:) - path(idx,:);

    nrm = norm(dp);

    if nrm < 1e-9
        t_hat = [1, 0];
    else
        t_hat = dp / nrm;
    end
end
% =========================================================================
% Helper: path follower locale con obstacle avoidance reattivo
% =========================================================================
function [px, py, n] = make_follower_path_local_heading( ...
    xf, yf, idx, xL, yL, R, scenario,map, L)

%MAKE_FOLLOWER_PATH_LOCAL_HEADING
% Genera un path locale per il follower verso il target di formazione,
% usando select_local_heading per evitare ostacoli e celle non attraversabili.
%
% Firma volutamente uguale alla vecchia make_follower_path:
%   xf, yf, idx, xL, yL, R, scenario, L

    px = zeros(L, 1);
    py = zeros(L, 1);

    % Recupera la mappa corrente dal base workspace.
    % Coerente con swarm_planner_helper, che già lavora con evalin/assignin.


    % Struct minimale per select_local_heading.
    % La funzione legge solo vehicle.state.x e vehicle.state.y.
    vehicleF.state.x = xf;
    vehicleF.state.y = yf;
    vehicleF.state.psi = 0;

    % Target di formazione nel frame globale
    offsetBody   = scenario.formation.offsets_body(idx, :).';
    offsetGlobal = R * offsetBody;
    target = [xL, yL] + offsetGlobal.';

    dx = target(1) - xf;
    dy = target(2) - yf;
    d  = hypot(dx, dy);

    % Se il follower è già vicino al target, path corto
    if d < 0.5
        px(1) = xf;
        py(1) = yf;
        px(2) = target(1);
        py(2) = target(2);
        n = 2;
        return;
    end

    % Heading locale con avoidance reattivo
    desired_heading = select_local_heading(vehicleF, target, map, scenario);

    % Lunghezza path locale.
    % Non serve arrivare fino al target: il path viene rigenerato continuamente.
    local_len = min(d, scenario.control.local_lookahead_m);

    % Campionamento path locale
    ds_path = 1.0;
    n_seg = max(2, min(L, floor(local_len/ds_path) + 1));

    s = linspace(0, local_len, n_seg).';

    px(1:n_seg) = xf + s*cos(desired_heading);
    py(1:n_seg) = yf + s*sin(desired_heading);

    n = n_seg;
end