function path_smooth = smooth_path_spline(path_in, ds_out, ~)
%SMOOTH_PATH_SPLINE  Smoothing molto aggressivo via Laplacian + resampling.
%
%   Pipeline:
%     1. Resample uniforme a L_resample (es. 6 m) -> punti distribuiti
%     2. Laplacian smoothing iterativo (10 iter) -> curva quasi-gaussiana
%        con primo e ultimo punto FISSI
%     3. Resample finale a ds_out
%
%   Risultato: curva liscia priva di spigoli locali, raggio min curvatura
%   garantito > L_resample (in pratica > 6 m).

    if size(path_in, 1) < 3
        path_smooth = path_in;
        return;
    end

    % --- Parametri smoothing ---------------------------------------
    L_resample = 6.0;     % [m] passo di resampling intermedio
    n_iter     = 10;      % iterazioni Laplacian smoothing

    % --- 1. Resample uniforme --------------------------------------
    p = resample_uniform(path_in, L_resample);
    if size(p, 1) < 3
        path_smooth = resample_uniform(path_in, ds_out);
        return;
    end

    % --- 2. Laplacian smoothing (anchor primo/ultimo) -------------
    %  Per ogni iterazione, ogni punto interno diventa una media
    %  pesata del vicinato (filtro gaussiano discreto).
    n = size(p, 1);
    for it = 1:n_iter
        p_new = p;
        for i = 2:(n-1)
            p_new(i, :) = 0.50 * p(i, :) ...
                        + 0.25 * p(i-1, :) ...
                        + 0.25 * p(i+1, :);
        end
        p = p_new;
    end

    % --- 3. Resample finale ----------------------------------------
    path_smooth = resample_uniform(p, ds_out);
end


% =====================================================================
function path_out = resample_uniform(path_in, ds)
    diffs   = diff(path_in);
    seg_len = sqrt(sum(diffs.^2, 2));
    s_cum   = [0; cumsum(seg_len)];
    L_tot   = s_cum(end);
    if L_tot < ds
        path_out = [path_in(1, :); path_in(end, :)];
        return;
    end
    s_new = (0:ds:L_tot)';
    if s_new(end) < L_tot - 0.5*ds
        s_new(end+1) = L_tot;
    end
    x_new = interp1(s_cum, path_in(:,1), s_new, 'linear');
    y_new = interp1(s_cum, path_in(:,2), s_new, 'linear');
    path_out = [x_new, y_new];
end



% function path_smooth = smooth_path_spline(path_in, ds_out, psi0)
% %SMOOTH_PATH_SPLINE  Resample + (Laplacian opzionale) + CONDIZIONE INIZIALE:
% %   il tratto iniziale parte con heading = psi0 (der1) e CURVATURA LIMITATA
% %   (der2 <= 1/R_min), e sovrascrive l'inizio del path liscio. Cosi' al replan
% %   non c'e' lo scalino di sterzo, e la spline non "si chiude" subito su se' stessa.
% 
%     if size(path_in, 1) < 3
%         path_smooth = path_in;
%         return;
%     end
% 
%     % --- Parametri smoothing (i tuoi) ----------------------------------
%     L_resample = 2.0;   % era 6
%     n_iter     = 2;     % era 30
% 
%     % --- 1. Resample uniforme ------------------------------------------
%     p = resample_uniform(path_in, L_resample);
%     if size(p, 1) < 3
%         path_smooth = resample_uniform(path_in, ds_out);
%         return;
%     end
% 
%     % --- 2. Laplacian smoothing (anchor primo/ultimo) -----------------
%     n = size(p, 1);
%     for it = 1:n_iter
%         p_new = p;
%         for i = 2:(n-1)
%             p_new(i, :) = 0.50 * p(i, :) ...
%                         + 0.25 * p(i-1, :) ...
%                         + 0.25 * p(i+1, :);
%         end
%         p = p_new;
%     end
% 
%     % --- 2b. CONDIZIONE INIZIALE (sovrascrive il tratto iniziale) ------
%     if nargin >= 3 && isnumeric(psi0) && isscalar(psi0) && isfinite(psi0)
%         p = lead_in_heading(p, psi0, ds_out);
%     end
% 
%     % --- 3. Resample finale --------------------------------------------
%     path_smooth = resample_uniform(p, ds_out);
% end
% 
% % =====================================================================
% function p = lead_in_heading(p, psi0, ds)
% %  Raccordo iniziale a curvatura limitata: parte da p(1) ESATTAMENTE con
% %  heading psi0, insegue il path (pure-pursuit) ma limita la variazione di
% %  heading per passo (= curvatura max = 1/R_min), poi si ricongiunge al path.
%     R_min  = 6.0;            % [m] raggio di sterzata minimo  -> LIMITE der2 = 1/R_min
%     L_lead = 8.0;            % [m] lunghezza del raccordo iniziale
%     Ld     = 4.0;            % [m] lookahead durante il raccordo
%     if ds <= 0, ds = 1.0; end
%     dpsi_max = ds / R_min;   % max variazione heading per passo [rad]
% 
%     pos  = p(1, :);
%     th   = psi0;
%     lead = pos;
%     s = 0;  guard = 0;
%     while s < L_lead && guard < 1000
%         guard = guard + 1;
%         % passo nella direzione CORRENTE (il 1o tratto e' esattamente psi0)
%         pos = pos + ds * [cos(th), sin(th)];
%         lead(end+1, :) = pos;            %#ok<AGROW>
%         s = s + ds;
%         % heading desiderato verso il path, con LIMITE di curvatura
%         d   = sqrt(sum((p - pos).^2, 2));
%         idx = find(d >= Ld, 1, 'first');
%         if isempty(idx), tgt = p(end, :); else, tgt = p(idx, :); end
%         th_des = atan2(tgt(2)-pos(2), tgt(1)-pos(1));
%         dth    = atan2(sin(th_des - th), cos(th_des - th));   % wrap [-pi,pi]
%         dth    = max(min(dth, dpsi_max), -dpsi_max);          % <-- LIMITE der2
%         th     = th + dth;
%     end
%     % ricongiunzione al path nel punto piu' vicino alla fine del raccordo
%     d = sqrt(sum((p - pos).^2, 2));
%     [~, j] = min(d);
%     j = min(max(j, 2), size(p, 1));
%     p = [lead; p(j:end, :)];
% end
% 
% % =====================================================================
% function path_out = resample_uniform(path_in, ds)
%     diffs   = diff(path_in);
%     seg_len = sqrt(sum(diffs.^2, 2));
%     s_cum   = [0; cumsum(seg_len)];
%     L_tot   = s_cum(end);
%     if L_tot < ds
%         path_out = [path_in(1, :); path_in(end, :)];
%         return;
%     end
%     s_new = (0:ds:L_tot)';
%     if s_new(end) < L_tot - 0.5*ds
%         s_new(end+1) = L_tot;
%     end
%     x_new = interp1(s_cum, path_in(:,1), s_new, 'linear');
%     y_new = interp1(s_cum, path_in(:,2), s_new, 'linear');
%     path_out = [x_new, y_new];
% end