# src/util/Utility.py
# This module provides utility objects and functions for other scripts.

from typing import List, Tuple
import csv
import os
from types import SimpleNamespace
import numpy as np
from numpy.typing import NDArray
from scipy.linalg import solve

# Default constants
DEFAULT_CONFIG = "joint"  # control configurationS
DEFAULT_SYSID_TYPE = "sine"  # sine sweep system ID
DEFAULT_SINE_MIN_FREQ = 1  # [Hz] frequency of first sine sweep
DEFAULT_SINE_MAX_FREQ = 20  # [Hz] frequency of first sine sweep
DEFAULT_FREQ_SPACING = (
    0.5  # [Hz] frequency range spacing between first and last sine sweep
)
DEFAULT_SINE_CYCLES = 40  # number of cycles for each sine wave
DEFAULT_DWELL_TIME = 0.5  # [s] number of seconds
DEFAULT_BCB_RUNTIME = (
    6.0  # [s] run time for each system id to get enough data for FFT resolution
)
DEFAULT_OUTPUT_SENSOR = "ToolAcc"  # output sensor defaults to tool accelerometer


def get_default_min_n_points_per_freq(
    sample_freq: float, sine_run_time: float = DEFAULT_BCB_RUNTIME
) -> int:
    """Summary:
    Compute the minimum number of points per frequency window from the active robot frequency.

    Args:
        sample_freq: Sampling frequency (of robot/trajectory) [Hz].
        sine_run_time: Desired run time for sine wave at each frequency [s].


    Returns:
        int: Minimum number of points per sine frequency window [samples].


    Side Effects:
        None.


    Raises:
        None.


    Preconditions:
        `sample_freq` is a positive float.
    """

    return int(np.round(sample_freq * sine_run_time))


DEFAULT_N_SEGMENTS = (
    5  # Number of segments to "avarage the FFTs" of each window for FRF calculation -
)
# Needs to be bigger than 1 (coherence break otherwise) idealy no less than 5.
# Need have at least one full cycle per segment otherwise FFT is no longer valid.

# Default data column locations
DEFAULT_TIMESTAMP_LOCATION = 2
NUM_CARTESIAN_AXES = 3
NUM_QUATERNION_AXES = 4
GRAVITY_ACC = 9.81


class TrajParams:
    """Container for trajectory parameters used in system identification.

    Args:
        max_displacement: Maximum displacement [m] for the commanded motion.
        max_velocity: Maximum velocity [m/s] for the commanded motion.
        max_acceleration: Maximum acceleration [m/s^2] for the commanded motion.
        sysid_type: System identification type (`bcb` or `sine`).
        configuration: Control configuration (`task` or `joint`).
        single_pt_run_time: Run time for a single point [s] in bang-coast-bang.
        output_sensor: Output sensor name (e.g., `ToolAcc`).

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        Units match the control pipeline expectations.
    """

    def __init__(
        self,
        max_displacement: float,
        max_velocity: float,
        max_acceleration: float,
        sysid_type: str = DEFAULT_SYSID_TYPE,
        configuration: str = DEFAULT_CONFIG,
        single_pt_run_time: float = DEFAULT_BCB_RUNTIME,
        output_sensor: str = DEFAULT_OUTPUT_SENSOR,
    ) -> None:
        """Initialize trajectory parameter fields.

        Args:
            max_displacement: Maximum displacement [m].
            max_velocity: Maximum velocity [m/s].
            max_acceleration: Maximum acceleration [m/s^2].
            sysid_type: System identification type.
            configuration: Control configuration.
            single_pt_run_time: Bang-coast-bang runtime [s].
            output_sensor: Output sensor name.
        Returns:
            `None`.
        Side Effects:
            Stores parameter values on `self`.
        Raises:
            None.
        Preconditions:
            None.
        """
        self.max_displacement = max_displacement
        self.max_velocity = max_velocity
        self.max_acceleration = max_acceleration
        self.sysid_type = sysid_type
        self.configuration = configuration
        self.single_pt_run_time = single_pt_run_time
        self.output_sensor = output_sensor


class SystemIdParams:
    """Container for system identification sweep parameters.

    Args:
        nV: Number of angle positions.
        nR: Number of radius positions.
        min_freq: Minimum sweep frequency [Hz].
        max_freq: Maximum sweep frequency [Hz].
        freq_space: Frequency spacing [Hz].
        num_sine_cycles: Number of sine cycles per frequency.
        dwell_time: Post-sweep dwell time [s].

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        Frequency bounds are positive and `min_freq <= max_freq`.
    """

    def __init__(
        self,
        nV: int,
        nR: int,
        min_freq: float = DEFAULT_SINE_MIN_FREQ,
        max_freq: float = DEFAULT_SINE_MAX_FREQ,
        freq_space: float = DEFAULT_FREQ_SPACING,
        num_sine_cycles: int = DEFAULT_SINE_CYCLES,
        dwell_time: float = DEFAULT_DWELL_TIME,
    ) -> None:
        """Initialize system identification sweep parameters.

        Args:
            nV: Number of angle positions.
            nR: Number of radius positions.
            min_freq: Minimum sweep frequency [Hz].
            max_freq: Maximum sweep frequency [Hz].
            freq_space: Frequency spacing [Hz].
            num_sine_cycles: Number of cycles per frequency.
            dwell_time: Dwell time [s].
        Returns:
            `None`.
        Side Effects:
            Stores parameter values on `self`.
        Raises:
            None.
        Preconditions:
            None.
        """
        self.nV = nV
        self.nR = nR
        self.min_freq = min_freq
        self.max_freq = max_freq
        self.freq_space = freq_space
        self.num_sine_cycles = num_sine_cycles
        self.dwell = dwell_time


class StructuredCalibrationData:
    """Structured container for calibration input/output data across poses.

    Args:
        num_input_points: Number of input samples per axis.
        num_output_points: Number of output samples per axis.
        num_output_vars: Number of output variables per sample.
        num_input_vars: Number of input variables per sample.
        num_axes: Number of commanded axes.
        input_begin_end_indices: Begin/end indices for each input window per axis.
        output_begin_end_indices: Begin/end indices for each output window per axis.
        frequency_list: List of sine frequencies per axis.
        numV: Number of angle positions.
        numR: Number of radius positions.

    Side Effects:
        Allocates NumPy arrays sized for the provided dimensions.

    Raises:
        None.

    Preconditions:
        Dimension arguments are positive and consistent with the data arrays.
    """

    def __init__(
        self,
        num_input_points: int,
        num_output_points: int,
        num_output_vars: int,
        num_input_vars: int,
        num_axes: int,
        input_begin_end_indices: List[List[int]] | List[List[List[int]]],
        output_begin_end_indices: List[List[int]] | List[List[List[int]]],
        frequency_list: List[List[float]] = [],
        numV: int = 1,
        numR: int = 1,
    ) -> None:
        """Initialize structured calibration storage arrays.

        Args:
            num_input_points: Input sample count per axis.
            num_output_points: Output sample count per axis.
            num_output_vars: Number of output channels.
            num_input_vars: Number of input channels.
            num_axes: Number of commanded axes.
            input_begin_end_indices: Input segment start/end indices.
            output_begin_end_indices: Output segment start/end indices.
            frequency_list: Frequency list per axis.
            numV: Number of angle positions.
            numR: Number of radius positions.
        Returns:
            `None`.
        Side Effects:
            Allocates NumPy arrays for calibration data storage.
        Raises:
            None.
        Preconditions:
            Dimensions are positive and mutually consistent.
        """
        self.outputData = np.zeros(
            (num_output_points, num_output_vars, num_axes, numV * numR)
        )
        self.inputData = np.zeros(
            ((num_input_points, num_input_vars, num_axes, numV * numR))
        )
        self.encoderInputData = np.zeros_like(self.inputData)
        self.inputAngleFromHorizontal = np.zeros((numV * numR,))
        self.inputRadiusFromBase = np.zeros((numV * numR,))
        self.inputBeginEndIndices = input_begin_end_indices
        self.outputBeginEndIndices = output_begin_end_indices
        self.time = np.zeros((num_input_points, num_axes, numV * numR))
        self.freq_list = frequency_list


# Functions
# ============


# Load data from a file based on the specified format
def load_data_file(fileformat: str, filename: str) -> Tuple[np.ndarray, list[str]]:
    """Load vector data from a file using the specified format.

    Args:
        fileformat: File format identifier (`csv`, `npz`, or `npy`).
        filename: Path to the data file.

    Returns:
        `tuple[np.ndarray, list[str]]` containing the data matrix and headers.

    Side Effects:
        Reads from disk.

    Raises:
        ValueError: If the file format is unsupported.
        OSError: If the file cannot be read.

    Preconditions:
        `filename` exists and matches the declared `fileformat`.
    """
    if fileformat == "csv" or fileformat == ".csv":
        return load_vector_data_from_csv(filename=filename)
    elif (
        fileformat == "npz"
        or fileformat == "npy"
        or fileformat == ".npz"
        or fileformat == ".npy"
    ):
        return load_vector_data_from_npz(filename=filename)
    else:
        raise ValueError(
            f"Unsupported file format '{fileformat}'. Supported formats are 'csv', 'npz', and 'npy'."
        )


# Load from csv
def load_vector_data_from_csv(
    filename: str, delimiter: str = ","
) -> Tuple[np.ndarray, List[str]]:
    """Load numeric vector data from a CSV file.

    Args:
        filename: Path to the CSV file.
        delimiter: Field delimiter used in the CSV.

    Returns:
        `tuple[np.ndarray, list[str]]` containing the data matrix and headers.

    Side Effects:
        Reads from disk.

    Raises:
        OSError: If the file cannot be read.
        ValueError: If any data row cannot be converted to float.

    Preconditions:
        The first row contains headers and remaining rows are numeric.
    """
    with open(filename, "r", newline="") as f:
        reader = csv.reader(f, delimiter=delimiter)
        headers = next(reader)  # read the first line
        rows = [list(map(float, row)) for row in reader]  # convert each cell to float

    data = np.array(rows)  # shape (n_rows, n_cols)
    return data, headers


# Load npz/npy file
def load_vector_data_from_npz(filename: str) -> Tuple[np.ndarray, list[str]]:
    """Load vector data from a NumPy `.npz` file.

    Args:
        filename: Path to the `.npz` file.

    Returns:
        `tuple[np.ndarray, list[str]]` containing the data matrix and headers.

    Side Effects:
        Reads from disk.

    Raises:
        OSError: If the file cannot be read.
        KeyError: If required arrays are missing.

    Preconditions:
        The `.npz` file contains `data` and `headers` arrays.
    """
    npz = np.load(file=filename)
    data = npz["data"]
    headers = npz["headers"]
    npz.close()
    return data, headers


def extract_dynamic_variables(
    dynamic_file: str,
    num_joints: int,
    joint_is_degrees: bool = False,
    acc_is_mps2: bool = True,
    file_format: str = "csv",
) -> Tuple[
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
    np.ndarray,
]:
    """Extract dynamic variables from a motion data file.

    Args:
        dynamic_file: Path to the motion data file.
        num_joints: Number of joints in the robot.
        joint_is_degrees: If True, convert joint positions to radians.
        acc_is_mps2: If True, acceleration is already in m/s^2.
        file_format: Data file format (`csv`, `npz`, or `npy`).

    Returns:
        `tuple[np.ndarray, ...]` containing time, commanded joints, encoder
        joints, joint currents, acceleration time, TCP acceleration,
        TCP gyro, orientation time, and orientation.

    Side Effects:
        Reads from disk.

    Raises:
        FileNotFoundError: If `dynamic_file` does not exist.
        ValueError: If the file format is unsupported.

    Preconditions:
        `dynamic_file` matches the expected format and contains required columns.
    """
    if not os.path.exists(dynamic_file):
        raise Exception(f"Robot dynamic data file ({dynamic_file}) does not exist.")

    # Load the data from the file
    data, _ = load_data_file(fileformat=file_format, filename=dynamic_file)

    # Remove any rows with NaN values from the data
    (cleaned_data,) = remove_nan_rows(data)

    time = (
        cleaned_data[:, DEFAULT_TIMESTAMP_LOCATION]
        - cleaned_data[0, DEFAULT_TIMESTAMP_LOCATION]
    )  # Convert to zero-based time

    # Commanded joint position
    commanded_start = DEFAULT_TIMESTAMP_LOCATION + 1
    commanded_end = commanded_start + num_joints
    # Convert from degrees to radians
    commanded_joint_position = (
        np.deg2rad(cleaned_data[:, commanded_start:commanded_end])
        if joint_is_degrees
        else cleaned_data[:, commanded_start:commanded_end]
    )

    # Encoder joint position
    encoder_start = commanded_end
    encoder_end = encoder_start + num_joints
    # Convert from degrees to radians
    encoder_joint_position = (
        np.deg2rad(cleaned_data[:, encoder_start:encoder_end])
        if joint_is_degrees
        else cleaned_data[:, encoder_start:encoder_end]
    )

    # Joint currents
    joint_currents_start = encoder_end
    joint_currents_end = joint_currents_start + num_joints
    joint_currents = cleaned_data[:, joint_currents_start:joint_currents_end]

    # TCP acceleration
    tcp_acceleration_time_index = joint_currents_end
    tcp_acceleration_time = (
        cleaned_data[:, tcp_acceleration_time_index]
        - cleaned_data[0, tcp_acceleration_time_index]
    )

    tcp_acceleration_start = tcp_acceleration_time_index + 1
    tcp_acceleration_end = tcp_acceleration_start + NUM_CARTESIAN_AXES
    # Convert from G's to m/s^2
    tcp_acceleration = (
        cleaned_data[:, tcp_acceleration_start:tcp_acceleration_end]
        if acc_is_mps2
        else np.array(
            cleaned_data[:, tcp_acceleration_start:tcp_acceleration_end], dtype=float
        )
        * GRAVITY_ACC
    )

    # TCP gyro
    tcp_gyro_start = tcp_acceleration_end
    tcp_gyro_end = tcp_gyro_start + NUM_CARTESIAN_AXES

    tcp_gyro = cleaned_data[:, tcp_gyro_start:tcp_gyro_end]

    # TCP orientation (quaternion)
    tcp_orientation_time_index = tcp_gyro_end
    tcp_orientation_time = (
        cleaned_data[:, tcp_orientation_time_index]
        - cleaned_data[0, tcp_orientation_time_index]
    )

    tcp_orientation_start = tcp_orientation_time_index + 1
    tcp_orientation_end = tcp_orientation_start + NUM_QUATERNION_AXES
    tcp_orientation = cleaned_data[:, tcp_orientation_start:tcp_orientation_end]

    return (
        time,
        commanded_joint_position,
        encoder_joint_position,
        joint_currents,
        tcp_acceleration_time,
        tcp_acceleration,
        tcp_gyro,
        tcp_orientation_time,
        tcp_orientation,
    )


def remove_nan_rows(*arrays: NDArray[np.generic]) -> Tuple[NDArray[np.generic], ...]:
    """Remove rows containing NaNs across multiple arrays.

    Args:
        *arrays: 1D or 2D arrays with the same number of rows.

    Returns:
        `tuple[np.ndarray, ...]` filtered arrays with NaN rows removed.

    Side Effects:
        None.

    Raises:
        ValueError: If arrays have different row counts.

    Preconditions:
        All arrays share the same length on axis 0.
    """
    if not arrays:
        return tuple()

    # 1. Verify they all share the same first dimension
    n_rows = arrays[0].shape[0]
    for arr in arrays:
        if arr.shape[0] != n_rows:
            raise ValueError("All arrays must have the same number of rows")

    # 2. Build a mask of invalid rows (True if that row has any NaN)
    invalid = np.zeros(n_rows, dtype=bool)
    for arr in arrays:
        # For 1D, check each element; for 2D, check any column in each row
        nan_mask = np.isnan(arr) if arr.ndim == 1 else np.isnan(arr).any(axis=1)
        invalid |= nan_mask

    # 3. Invert to get valid rows
    valid = ~invalid

    # 4. Apply mask to each array
    cleaned = tuple(arr[valid] if arr.ndim == 1 else arr[valid, ...] for arr in arrays)
    return cleaned


def invfreqs(
    g: np.ndarray,
    worN: np.ndarray,
    nB: int,
    nA: int,
    wf: np.ndarray | None = None,
    nk: int = 0,
) -> tuple[np.ndarray, np.ndarray]:
    """Fit numerator and denominator coefficients to a complex FRF.

    Args:
        g: Complex frequency response samples.
        worN: Frequency samples [rad/s].
        nB: Numerator order.
        nA: Denominator order.
        wf: Optional weighting vector.
        nk: Number of trailing zeros in the numerator.
    Returns:
        `tuple[np.ndarray, np.ndarray]` containing `(b, a)` polynomial coefficients.
    Side Effects:
        None.
    Raises:
        ValueError: If input lengths mismatch or frequencies are negative.
    Preconditions:
        `len(g) == len(worN)` and all frequencies are non-negative.
    """
    g = np.atleast_1d(g)
    worN = np.atleast_1d(worN)
    if wf is None:
        wf = np.ones_like(worN)
    if len(g) != len(worN) or len(worN) != len(wf):
        raise ValueError("The lengths of g, worN and wf must coincide.")
    if np.any(worN < 0):
        raise ValueError("worN has negative values.")
    # Constraining B(s) with nk trailing zeros
    nm = np.maximum(nA, nB + nk)
    mD = np.vander(1j * worN, nm + 1)
    mH = np.asmatrix(np.diag(g))
    mM = np.asmatrix(
        np.hstack(
            (
                mH * np.asmatrix(mD[:, -nA:]),
                -np.asmatrix(mD[:, -nk - nB - 1 :][:, : nB + 1]),
            )
        )
    )
    mW = np.asmatrix(np.diag(wf))
    Y = solve(
        np.real(mM.H * mW * mM), -np.real(mM.H * mW * mH * np.asmatrix(mD)[:, -nA - 1])
    )
    a = np.ones(nA + 1)
    a[1:] = Y[:nA].flatten()
    b = np.zeros(nB + nk + 1)
    b[: nB + 1] = Y[nA:].flatten()

    return b, a


def deg2rad(degrees: float) -> float:
    """
    Convert degrees to radians.

    Args:
        degrees (float): Angle in degrees.

    Returns:
        float: Angle in radians.
    """
    return np.deg2rad(degrees)


def rad2deg(radians: float) -> float:
    """
    Convert radians to degrees.

    Args:
        radians (float): Angle in radians.

    Returns:
        float: Angle in degrees.
    """
    return np.rad2deg(radians)


def rotation_matrix_to_quaternion(rotation_mat: np.ndarray) -> np.ndarray:
    """Convert a rotation matrix to a quaternion.

    Args:
        rotation_mat: 3x3 rotation matrix.

    Returns:
        `np.ndarray` quaternion as (x, y, z, w).

    Side Effects:
        None.

    Raises:
        ValueError: If `rotation_mat` is not 3x3.

    Preconditions:
        `rotation_mat` is a valid rotation matrix.
    """
    # Check if the input is a 3x3 matrix
    if rotation_mat.shape != (3, 3):
        raise ValueError("Input matrix must be 3x3")

    # Compute the trace of the matrix
    trace = np.trace(rotation_mat)

    # Compute the quaternion
    if trace > 0:
        S = np.sqrt(trace + 1.0) * 2  # S = 4 * qw
        qw = 0.25 * S
        qx = (rotation_mat[2, 1] - rotation_mat[1, 2]) / S
        qy = (rotation_mat[0, 2] - rotation_mat[2, 0]) / S
        qz = (rotation_mat[1, 0] - rotation_mat[0, 1]) / S
    elif (rotation_mat[0, 0] > rotation_mat[1, 1]) and (
        rotation_mat[0, 0] > rotation_mat[2, 2]
    ):
        S = (
            np.sqrt(1.0 + rotation_mat[0, 0] - rotation_mat[1, 1] - rotation_mat[2, 2])
            * 2
        )  # S = 4 * qx
        qw = (rotation_mat[2, 1] - rotation_mat[1, 2]) / S
        qx = 0.25 * S
        qy = (rotation_mat[0, 1] + rotation_mat[1, 0]) / S
        qz = (rotation_mat[0, 2] + rotation_mat[2, 0]) / S
    elif rotation_mat[1, 1] > rotation_mat[2, 2]:
        S = (
            np.sqrt(1.0 + rotation_mat[1, 1] - rotation_mat[0, 0] - rotation_mat[2, 2])
            * 2
        )  # S = 4 * qy
        qw = (rotation_mat[0, 2] - rotation_mat[2, 0]) / S
        qx = (rotation_mat[0, 1] + rotation_mat[1, 0]) / S
        qy = 0.25 * S
        qz = (rotation_mat[1, 2] + rotation_mat[2, 1]) / S
    else:
        S = (
            np.sqrt(1.0 + rotation_mat[2, 2] - rotation_mat[0, 0] - rotation_mat[1, 1])
            * 2
        )  # S = 4 * qz
        qw = (rotation_mat[1, 0] - rotation_mat[0, 1]) / S
        qx = (rotation_mat[0, 2] + rotation_mat[2, 0]) / S
        qy = (rotation_mat[1, 2] + rotation_mat[2, 1]) / S
        qz = 0.25 * S

    return np.array([qx, qy, qz, qw])


def polar_to_cartesian(
    v: float, r: float, side_arm: float, base_height: float, base_angle: float
) -> Tuple[float, float, float]:
    """Convert polar coordinates to Cartesian coordinates for the arm base frame.

    Args:
        v: Angle from horizontal [rad].
        r: Radius from base [m].
        side_arm: Side arm offset [m].
        base_height: Base height offset [m].
        base_angle: Base rotation angle [rad].

    Returns:
        `tuple[float, float, float]` containing (long, width, height) [m].

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        Inputs use consistent units (radians, meters).
    """
    long = r * np.cos(v) * np.cos(base_angle)
    width = side_arm - r * np.cos(v) * np.sin(base_angle)
    height = base_height + r * np.sin(v)
    return long, width, height


def cartesian_to_polar(
    long: float, width: float, height: float, side_arm: float, base_height: float
) -> Tuple[float, float]:
    """Convert Cartesian coordinates to polar coordinates.

    Args:
        long: Longitudinal position [m].
        width: Lateral position [m].
        height: Vertical position [m].
        side_arm: Side arm offset [m].
        base_height: Base height offset [m].

    Returns:
        `tuple[float, float]` containing (v [rad], r [m]).

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        Inputs use consistent units (meters).
    """
    # 1) horizontal projection
    dx = np.sqrt(long**2 + (side_arm - width) ** 2)
    # 2) radial distance
    dz = height - base_height
    r = np.hypot(dx, dz)
    # 3) elevation angle
    v = np.arctan2(dz, dx)

    return v, r


def linspace_step_size(start: float, end: float, step: float) -> np.ndarray:
    """Generate a linspace with a fixed step size.

    Args:
        start: Starting value.
        end: Ending value.
        step: Step size between points.

    Returns:
        `list[float]` of values from start to end inclusive.

    Side Effects:
        None.

    Raises:
        ValueError: If `step` is zero.

    Preconditions:
        `step` is non-zero.
    """
    num_steps = round((end - start) / step) + 1  # Ensure exact number of points
    return np.linspace(start, end, num_steps).tolist()


def normalized_interpolation(
    time_list: List[np.ndarray], to_interp_lists: List[list]
) -> Tuple[List, np.ndarray]:
    """Interpolate trajectories onto a common normalized time base.

    Args:
        time_list: List of time vectors per joint.
        to_interp_lists: Lists of trajectories to interpolate (e.g., positions,
            velocities, accelerations) per joint.

    Returns:
        `tuple[list, np.ndarray]` containing the interpolated lists and the
        common time vector.

    Side Effects:
        Mutates `to_interp_lists` in place.

    Raises:
        None.

    Preconditions:
        Each entry in `time_list` is non-empty and aligned with corresponding
        entries in `to_interp_lists`.
    """
    # Determine the maximum number of trajectory points across joints
    max_traj_pts = max(len(tj) for tj in time_list)

    # Compute normalized time for each joint trajectory (range [0,1])
    normalized_time_trajectories = []
    for tj in time_list:
        if tj[-1] > 0:
            norm_tj = tj / tj[-1]  # normalize each time vector by its final time
            normalized_time_trajectories.append(norm_tj)
        else:
            normalized_time_trajectories.append(tj)

    # Create a common normalized time vector from 0 to 1 with max_traj_pts samples
    common_norm_time = np.linspace(0, 1, max_traj_pts)

    # Optionally, define a common overall duration (e.g., the maximum final time)
    max_duration = max(tj[-1] for tj in time_list)
    path_time = common_norm_time * max_duration

    # Interpolate each joint's trajectory using the normalized time vector
    for (
        _list
    ) in (
        to_interp_lists
    ):  # interp_lists = [joint_trajetories, joint_velocities, joint_accelerations]
        for i in range(len(_list)):  # _list = joint_trajectories list
            arr = np.squeeze(_list[i])
            _list[i] = np.interp(common_norm_time, normalized_time_trajectories[i], arr)

    # Return altered lists
    return to_interp_lists, path_time


def transpose_list(original_list: List) -> List:
    """Transpose a list of lists.

    Args:
        original_list: List of lists to transpose.

    Returns:
        `list` with rows and columns swapped.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        All inner lists have the same length.
    """
    return list(map(list, zip(*original_list)))


def read_identification_parameters(data_folder: str) -> SimpleNamespace:
    """Load and type-cast identification parameters from the data folder.

    Args:
        data_folder: Path containing `identification_parameters.csv`.

    Returns:
        `types.SimpleNamespace` with parameter names as attributes and values
        converted to their expected types.

    Side Effects:
        Reads `identification_parameters.csv` from disk.

    Raises:
        FileNotFoundError: If the parameters file is missing.
        ValueError: If the parameters file has no rows.

    Preconditions:
        `identification_parameters.csv` exists and contains a header row matching
        expected parameter names.
    """
    parameter_filename = os.path.join(data_folder, "identification_parameters.csv")
    if not os.path.exists(parameter_filename):
        raise FileNotFoundError(
            f"Missing identification parameters: {parameter_filename}"
        )

    with open(parameter_filename, mode="r", newline="") as file:
        reader = csv.DictReader(file)
        rows = list(reader)

    if not rows:
        raise ValueError(f"No parameter rows found in {parameter_filename}")

    raw_params = rows[0]
    type_map = {
        "max_disp": float,
        "max_vel": float,
        "max_acc": float,
        "sysid_type": str,
        "ctrl_config": str,
        "single_pt_run_time": float,
        "output_sensor": str,
        "nV": int,
        "nR": int,
        "sine_cycles": int,
        "min_freq": float,
        "max_freq": float,
        "freq_space": float,
        "dwell": float,
        "axes": int,
        "num_joints": int,
        "sample_time": float,
        "start_pose": int,
        "shoulder_len": float,
        "base_height": float,
        "robot_name": str,
        "tcp_payload": float,
        "tcp_payload_com_x": float,
        "tcp_payload_com_y": float,
        "tcp_payload_com_z": float,
        "imu_to_tcp_x": float,
        "imu_to_tcp_y": float,
        "imu_to_tcp_z": float,
    }

    typed_params = {}
    for key, value in raw_params.items():
        converter = type_map.get(key, str)
        typed_params[key] = converter(value)

    # Backward compatibility for older runs without IMU extrinsics.
    typed_params.setdefault("imu_to_tcp_x", 0.0)
    typed_params.setdefault("imu_to_tcp_y", 0.0)
    typed_params.setdefault("imu_to_tcp_z", 0.0)
    typed_params.setdefault("tcp_payload", 0.0)
    typed_params.setdefault("tcp_payload_com_x", 0.0)
    typed_params.setdefault("tcp_payload_com_y", 0.0)
    typed_params.setdefault("tcp_payload_com_z", 0.0)

    return SimpleNamespace(**typed_params)
