"""
Spline utilities for Catmull-Rom interpolation and trajectory sampling.
"""
import numpy as np

def catmull_rom_spline(points, num_points=100):
    """
    Calcola una Catmull-Rom spline che passa per tutti i punti dati.
    Args:
        points: lista di tuple (x, y) o (x, y, z)
        num_points: numero di punti da campionare lungo la curva
    Returns:
        Lista di punti campionati sulla spline
    """
    points = np.array(points)
    n = len(points)
    if n < 4:
        # Troppo pochi punti per spline: restituisci la polilinea
        return points.tolist()
    # Estendi i punti per garantire la continuità agli estremi
    extended = np.vstack([points[0], points, points[-1]])
    result = []
    # Usa n-1 segmenti validi
    for i in range(1, n):
        p0, p1, p2, p3 = extended[i-1], extended[i], extended[i+1], extended[i+2]
        for t in np.linspace(0, 1, num_points//(n-1)):
            t2 = t*t
            t3 = t2*t
            # Catmull-Rom formula
            pt = (
                0.5 * ((2*p1) +
                (-p0 + p2)*t +
                (2*p0 - 5*p1 + 4*p2 - p3)*t2 +
                (-p0 + 3*p1 - 3*p2 + p3)*t3)
            )
            result.append(pt.tolist())
    return result