"""
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