function obstacle_brake_flag = obstacle_brake_from_base(x, y, psi, vel)
%OBSTACLE_BRAKE_FROM_BASE
% Emergency obstacle brake simulation-only.
%
% Legge dal base workspace:
%   - map.known
%   - map.obstacle
%   - scenario
%
% Ritorna:
%   obstacle_brake_flag = 1 se esiste un ostacolo noto nel corridoio
%                         frontale del veicolo (oltre la sagoma del veicolo stesso).
%
% FIX 2026-06-01:
%   - Dead zone: la scansione parte dal naso del veicolo (d_nose_m), non dal centro.
%     Questo evita che il checker scatti al momento del lancio se l'ostacolo e'
%     gia' noto nella zona subito davanti (ma fuori dalla sagoma).
%   - Out-of-bounds: celle fuori dalla mappa non sono piu' trattate come ostacoli.
%   - D_check: ridimensionato. La formula e' d_nose + d_safe_buffer + d_brake.
%     d_safe_base_m ora rappresenta il margine oltre la distanza di frenata,
%     non la distanza minima assoluta di check. Abbassarlo a 2m in init_swarm_scenario.
%
% NOTA:
% Questa funzione e' pensata per simulazione desktop.
% Non e' una soluzione codegen/real-time.

obstacle_brake_flag = 0.0;

%% Recupero dati dal base workspace
if ~evalin('base', 'exist(''map'',''var'') && exist(''scenario'',''var'')')
    return;
end

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

%% Controlli minimi
if ~isfield(map, 'known') || ~isfield(map, 'obstacle')
    return;
end

if isempty(map.known) || isempty(map.obstacle)
    return;
end

if ~isequal(size(map.known), size(map.obstacle))
    return;
end

%% Breakpoints mappa
if ~isfield(map, 'x') || ~isfield(map, 'y')
    return;
end
map_x = map.x;
map_y = map.y;
if numel(map_x) < 2 || numel(map_y) < 2
    return;
end

%% Parametri geometria veicolo
veh_width_m = scenario.vehicle.width_m;

% Corridoio: larghezza veicolo + piccolo margine laterale.
% Si usa track_update_width_m solo se e' sensato (non troppo diverso da width_m).
corridor_width_m = max(scenario.vehicle.track_update_width_m, veh_width_m);
half_corridor_m  = 0.5 * corridor_width_m;

% Dead zone: dalla sagoma del veicolo (dal centro al naso).
% Usa track_update_length_m se disponibile, altrimenti stima con wheelbase.
if isfield(scenario.vehicle, 'track_update_length_m')
    d_nose_m = 0.5 * scenario.vehicle.track_update_length_m;
elseif isfield(scenario.vehicle, 'wheelbase_m')
    d_nose_m = 0.6 * scenario.vehicle.wheelbase_m;  % stima naso da assi
else
    d_nose_m = 2.5;  % fallback conservativo
end

%% Parametri brake
d_safe_buffer_m = scenario.safety.d_safe_base_m;  % margine oltre distanza frenata
d_safe_max_m    = scenario.safety.d_safe_max_m;
a_brake = scenario.control.max_brake_mps2; %CONTROLLARE SE COERENTE CON 750NM PER RUOTA
%FORSE MEGLIO CONSIDERARLO COME MU*G COSì SI TIENE CONTO ANCHE DELL'ABS
if a_brake <= 0
    a_brake = 1.0;
end

% Passo longitudinale di scansione
if isfield(scenario.control, 'ray_sample_step_m') && scenario.control.ray_sample_step_m > 0
    ds_long = scenario.control.ray_sample_step_m;
else
    ds_long = map.params.res_m;
end

% Passo laterale: risoluzione mappa
ds_lat = max(map.params.res_m, 0.1);

%% Distanza di controllo
v_abs   = abs(vel);
d_brake = (v_abs * v_abs) / (2.0 * a_brake);  % distanza di frenata cinematica

% Distanza totale da controllare (misurata dal CENTRO del veicolo):
%   d_nose_m       = zona gia' occupata dalla sagoma (non scansionata)
%   d_safe_buffer  = margine di sicurezza oltre la distanza di frenata
%   d_brake        = distanza necessaria per fermarsi a questa velocita'
D_check = d_nose_m + d_safe_buffer_m + d_brake;
D_check = min(D_check, d_safe_max_m);

% Se la distanza di check e' minore della dead zone, non c'e' nulla da scansionare.
if D_check <= d_nose_m
    return;
end

%% Scansione corridoio frontale
cpsi = cos(psi);
spsi = sin(psi);

% Indice di partenza: primo campione oltre il naso del veicolo.
il_start = max(1, ceil(d_nose_m / ds_long));
n_long   = floor(D_check / ds_long);
n_lat    = floor(half_corridor_m / ds_lat);

for il = il_start : n_long
    xb = il * ds_long;
    for jl = -n_lat : n_lat
        yb = jl * ds_lat;
        % Body frame -> global frame
        xg = x + xb * cpsi - yb * spsi;
        yg = y + xb * spsi + yb * cpsi;
        if is_known_obstacle(xg, yg, map_x, map_y, map.known, map.obstacle)
            obstacle_brake_flag = 1.0;
            return;
        end
    end
end

end

%% ========================================================================
function flag = is_known_obstacle(x, y, map_x, map_y, known_map, obstacle_map)

flag = false;

nx = numel(map_x);
ny = numel(map_y);

if nx < 2 || ny < 2
    return;
end

% FIX: fuori mappa = ignora (non e' un ostacolo conosciuto).
% Prima era 'flag = true', che causava false positivi ogni volta che
% il look-ahead superava i bordi della mappa.
if x < map_x(1) || x > map_x(end) || y < map_y(1) || y > map_y(end)
    flag = false;
    return;
end

dx_map = map_x(2) - map_x(1);
dy_map = map_y(2) - map_y(1);

if abs(dx_map) < 1e-12 || abs(dy_map) < 1e-12
    return;
end

j = floor((x - map_x(1)) / dx_map) + 1;
i = floor((y - map_y(1)) / dy_map) + 1;

if i < 1 || i > ny || j < 1 || j > nx
    return;  % double-check indici: ignora
end

% Frena solo su ostacoli gia' conosciuti.
if known_map(i, j) && obstacle_map(i, j)
    flag = true;
end

end
