"""
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.asarray(points, dtype=float)
    n = len(points)
    if n < 4:
        # Not enough points for a spline, return a polyline
        return points.tolist()

    # Pad the points to ensure continuity at the ends
    p_start = points[0]
    p_end = points[-1]
    extended_points = np.vstack([p_start, points, p_end])

    # Define the Catmull-Rom matrix
    C = 0.5 * np.array([[0, 2, 0, 0], [-1, 0, 1, 0], [2, -5, 4, -1], [-1, 3, -3, 1]])

    result = []
    total_segments = n - 1
    if total_segments <= 0:
        return points.tolist()

    for k in range(num_points):
        s = (k / (num_points - 1)) * total_segments
        seg = int(np.floor(s))
        if seg >= total_segments:
            seg = total_segments - 1
        t = s - seg

        # Control points for the segment
        # The segment is between P1 and P2 of the control points
        # extended_points is indexed s.t. extended_points[i+1] = points[i]
        P = extended_points[seg : seg + 4]

        # Powers of t
        T = np.array([1, t, t**2, t**3])

        # Calculate the point
        pt = T @ C @ P
        result.append(pt.tolist())

    # Ensure exact endpoints match control points
    result[0] = points[0].tolist()
    result[-1] = points[-1].tolist()
    return result