# src/util/Trajectory.py
# Author: Nosa Edoimioya
# Description: Thin Python wrapper around the native trajectory generator.
import copy
import csv
import math
import os
import numpy as np
from typing import List, Tuple

from reforge_core.util.utility import (
    SystemIdParams,
    TrajParams,
    linspace_step_size,
    get_default_min_n_points_per_freq,
    DEFAULT_N_SEGMENTS,
)


def calculate_n_cycles(
    DEFAULT_MIN_N_POINTS_PER_FREQ: int | None = None,
    Ts: float | None = None,
    freq: float | None = None,
    min_points_per_freq: int | None = None,
) -> int:
    """Compute the number of sine cycles needed for a frequency.

    Args:
        DEFAULT_MIN_N_POINTS_PER_FREQ: Minimum points per frequency window.
        Ts: Sampling time [s].
        freq: Frequency [Hz].
        min_points_per_freq: Optional alias for `DEFAULT_MIN_N_POINTS_PER_FREQ`.

    Returns:
        `int` number of cycles to generate.

    Side Effects:
        None.

    Raises:
        ValueError: If required inputs are missing.

    Preconditions:
        `Ts` and `freq` are positive. Assume no more than 50% of time window will be
        discarted to avoid transient.
    """
    # Allow keyword alias used by callers.
    if min_points_per_freq is not None:
        DEFAULT_MIN_N_POINTS_PER_FREQ = min_points_per_freq
    if DEFAULT_MIN_N_POINTS_PER_FREQ is None or Ts is None or freq is None:
        raise ValueError(
            "calculate_n_cycles requires min_points_per_freq, Ts, and freq."
        )

    # Compute cycles from min number of points per frequency to keep FFT resolution.
    num_cycles = int(np.ceil(DEFAULT_MIN_N_POINTS_PER_FREQ * Ts * freq))

    # Num_cycles logic: 2*DEFAULT_N_SEGMENTS+3 so __explicit_FRF_processing has
    # at least one full cycle per segment, after dropping the first 50% window for transients.
    if num_cycles < 2 * DEFAULT_N_SEGMENTS + 3:
        num_cycles = 2 * DEFAULT_N_SEGMENTS + 3

    return num_cycles


class MotionProfile:
    """Container for a generated motion profile.

    Args:
        None.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        None.
    """

    def __init__(
        self,
        t: np.ndarray[float, np.dtype[np.float32]],
        q: np.ndarray[float, np.dtype[np.float32]],
        endIndex: list,
    ) -> None:
        """Initialize motion profile arrays.

        Args:
            t: Time vector.
            q: Position vector.
            endIndex: End indices for each segment.

        Side Effects:
            Stores references to input arrays.

        Raises:
            None.

        Preconditions:
            `t` and `q` have the same length.
        """
        self.t = t
        self.q = q
        self.endIndex = endIndex


class Trajectory:
    """Generate and store system identification trajectories.

    Args:
        None.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        None.
    """

    def __init__(self, t_params: TrajParams, s_params: SystemIdParams) -> None:
        """Initialize trajectory generation parameters and buffers.

        Args:
            t_params: Trajectory parameters.
            s_params: System identification parameters.

        Side Effects:
            Initializes internal buffers for trajectory data.

        Raises:
            None.

        Preconditions:
            `t_params` and `s_params` are fully populated.
        """
        self.configuration = t_params.configuration
        self.max_displacement = t_params.max_displacement
        self.max_velocity = t_params.max_velocity
        self.max_acceleration = t_params.max_acceleration
        self.sysid_type = t_params.sysid_type
        self.single_traj_run_time = t_params.single_pt_run_time

        self.nV = s_params.nV
        self.nR = s_params.nR
        self.freq_range = np.arange(
            s_params.min_freq, s_params.max_freq, s_params.freq_space
        )
        self.dwell = s_params.dwell
        self.num_sine_cycles = s_params.num_sine_cycles

        self.trajectoryTime: List[float] = []
        self.perturbation: List[float] = []
        self.endIndices: List[int] = []
        self.positionTrajectory: List[List[float]] = []
        self.velocityTrajectory: List[List[float]] = []
        self.accelerationTrajectory: List[List[float]] = []
        self._generated_sine_sweep_trajectory = False

    # ------------------------------------------------------------------
    # Public API
    # ------------------------------------------------------------------
    def generate_sine_sweep_trajectory(
        self,
        start_position: List[float],
        move_axis: int,
        Ts: float,
    ) -> None:
        """Generate and store a full sine-sweep trajectory.

        Args:
            start_position: Initial joint positions.
            move_axis: Axis index to excite.
            Ts: Sampling time [s].

        Returns:
            `None`.

        Side Effects:
            Populates trajectory buffers in this instance.

        Raises:
            None.

        Preconditions:
            `start_position` length matches the robot joint count.
        """
        # Generate sine perturbation
        sineProfile = self.sine_sweep(
            Ts, start_position, move_axis, self.freq_range, self.dwell
        )
        self.__store_trajectory_data(sineProfile, start_position, move_axis, Ts)
        self._generated_sine_sweep_trajectory = True

    def sine_sweep(
        self,
        Ts: float,
        initial_position: List[float],
        move_axis: int,
        freq_range: np.ndarray,
        settling_time: float,
    ) -> MotionProfile:
        """Generate a sine sweep motion profile for one axis.

        Args:
            Ts: Sampling time [s].
            initial_position: Initial joint positions.
            move_axis: Axis index to excite.
            freq_range: Array of sweep frequencies [Hz].
            settling_time: Dwell time between frequencies [s].

        Returns:
            `MotionProfile` containing time, position, and segment indices.

        Side Effects:
            None.

        Raises:
            None.

        Preconditions:
            `move_axis` is a valid joint index.
        """
        fs = 1 / Ts
        sin_amp_vector = []
        q = np.array([])
        t = np.array([])
        freq_vec = np.ones(math.ceil(settling_time * fs) + 1) * freq_range[0]

        q0 = initial_position[move_axis]

        for freq in freq_range:

            sin_frequency = freq
            sin_amplitude = min(
                2 * np.pi * sin_frequency * np.sqrt(self.max_displacement),
                self.max_acceleration,
            )

            num_cycles = calculate_n_cycles(
                get_default_min_n_points_per_freq(sample_freq=fs), Ts, sin_frequency
            )

            sin_amp_vector.append(sin_amplitude)
            q_sin, t_sin = self.table_traj_gen_function(
                sin_amplitude, sin_frequency, Ts, num_cycles
            )

            q = np.append(q, q_sin)

            t = np.append(t, t_sin + t[-1]) if len(t) > 0 else np.append(t, t_sin)

            q_dwell = np.ones(math.ceil(settling_time * fs)) * q[-1]
            q = np.append(q, q_dwell)
            freq_temp = np.ones(len(q_sin) + len(q_dwell)) * sin_frequency
            freq_vec = np.append(freq_vec, freq_temp)
            t_dwell = np.arange(0, settling_time, Ts)
            t = np.append(t, t_dwell + t[-1])

        q = q + q0

        return MotionProfile(t, q, [len(q)])

    def table_traj_gen_function(
        self, A: float, freq: float, Ts: float, sine_cycles: int
    ) -> Tuple[np.ndarray, np.ndarray]:
        """Generate a tabulated sine trajectory with ramp-up and return segments.

        Args:
            A: Acceleration amplitude.
            freq: Sine frequency [Hz].
            Ts: Sampling time [s].
            sine_cycles: Number of sine cycles to generate.

        Returns:
            `tuple[np.ndarray, np.ndarray]` containing position and time arrays.

        Side Effects:
            None.

        Raises:
            None.

        Preconditions:
            `Ts` and `freq` are positive.
        """
        p = sine_cycles  # cycles
        q_temp_end = 0

        # ramp up and ramp down point-to-point motion
        p2p_acc = 0.5  # [rad/s^2]
        p2p_vel = 0.05  # [rad/s]

        t_temp: list[float] = []
        for j in range(0, math.ceil(1 / freq * p / Ts)):
            t_temp.append(j * Ts)

        omega = 2 * math.pi * freq
        t_temp_arr = np.array(t_temp, dtype=float)
        v_temp = -(A / (2 * math.pi * freq)) * np.cos(omega * t_temp_arr)
        q_temp = -(A / math.pow(omega, 2)) * np.sin(omega * t_temp_arr)

        ramp_up_steps = range(0, math.ceil(abs(v_temp[0] / 0.5) / Ts))
        ramp_down_steps = range(0, math.ceil(abs(v_temp[-1] / 0.5) / Ts))
        q_ramp_up = np.zeros(ramp_up_steps.stop)
        q_ramp_down = np.zeros(ramp_down_steps.stop)

        for k in ramp_up_steps:
            q_ramp_up[k] = np.sign(v_temp[0]) * 0.5 * 0.5 * math.pow(k * Ts, 2)  # [rad]

        for k in ramp_down_steps:
            q_ramp_down[k] = -np.sign(v_temp[-1]) * 0.5 * 0.5 * math.pow(
                k * Ts, 2
            ) + v_temp[-1] * (
                k * Ts
            )  # [rad]

        q_ramp_up = q_ramp_up - q_ramp_up[-1]
        q_ramp_down = q_ramp_down + q_temp[-1]

        q_ramp_start = q_ramp_up[0]  # [rad]
        q_return = []
        return_time = np.arange(
            0, abs((q_ramp_start - q_temp_end)) / (p2p_vel) + 0.2, Ts
        )

        for t in return_time:
            q_return_temp, _ = self.point_to_point_motion_jerk_limit(
                p2p_acc, q_temp_end, q_ramp_start, p2p_vel, t, Ts
            )  # rad
            q_return.append(q_return_temp)

        q_ramp_up = np.concatenate((np.asarray(q_return), q_ramp_up))  # [rad]
        q_temp = np.concatenate((q_ramp_up[0:-2], q_temp))
        q_temp = np.append(q_temp, q_ramp_down[1:])
        q_temp_end = q_temp[-1]
        del q_ramp_up, q_ramp_down

        q_sin = q_temp

        q_return = []
        return_time = np.arange(
            0, abs((q_ramp_start - q_temp_end)) / (p2p_vel) + 0.2, Ts
        )

        for t in return_time:
            q_return_temp, _ = self.point_to_point_motion_jerk_limit(
                p2p_acc, q_temp_end, 0, p2p_vel, t, Ts
            )  # rad
            q_return.append(q_return_temp)

        q_sin = np.append(q_sin, np.asarray(q_return))
        t_sin = np.array(linspace_step_size(0, ((q_sin.shape[0]) - 1) * Ts, Ts))

        return q_sin, t_sin

    def point_to_point_motion(
        self,
        feedrate: float,
        acc_limit: float,
        dec_limit: float,
        displacement: float,
        init_velocity: float = 0.0,
        end_velocity: float = 0.0,
        Ts: float = 0.001,
        dwell: float = 0.0,
    ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        """Generate a point-to-point motion profile.

        Args:
            feedrate: Commanded feedrate [mm/s].
            acc_limit: Acceleration limit [mm/s^2].
            dec_limit: Deceleration limit [mm/s^2] (negative).
            displacement: Travel length/displacement [mm].
            init_velocity: Initial speed [mm/s].
            end_velocity: End velocity [mm/s].
            Ts: Sampling time [s].
            dwell: Dwell time after motion [s].

        Returns:
            `tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]` containing
            displacement, velocity, acceleration, and time arrays.

        Side Effects:
            None.

        Raises:
            ValueError: If limits are invalid or lead to negative timing.

        Preconditions:
            `Ts` is positive and `dec_limit` is negative.
        """

        # Segment time for accelerating and decelerating segments
        t1 = (feedrate - init_velocity) / acc_limit
        t3 = (end_velocity - feedrate) / dec_limit

        # Calculate the time for t2, constant velocity portion
        t2 = (1 / feedrate) * (
            displacement
            - (1 / (2 * acc_limit) - 1 / (2 * dec_limit)) * feedrate**2
            - (
                (end_velocity**2) / (2 * dec_limit)
                - (init_velocity**2) / (2 * acc_limit)
            )
        )

        # If t2 is less than 0, there's not enough time for a constant velocity motion
        # so we set the time to 0 and recalculate the velocity and the time for the
        # accelerating and decelerating portions.
        if t2 <= 0:
            t2 = 0
            feedrate = np.sqrt(
                ((1 / (2 * acc_limit) - 1 / (2 * dec_limit)) ** -1)
                * (
                    displacement
                    - (
                        (end_velocity**2) / (2 * dec_limit)
                        - (init_velocity**2) / (2 * acc_limit)
                    )
                )
            )
            t1 = (feedrate - init_velocity) / acc_limit
            t3 = (end_velocity - feedrate) / dec_limit

        n1 = int(np.ceil(t1 / Ts))
        n2 = int(np.ceil(t2 / Ts))
        n3 = int(np.ceil(t3 / Ts))

        t1 = n1 * Ts
        t2 = n2 * Ts
        t3 = n3 * Ts

        if (t1 + 2 * t2 + t3) == 0:
            feedrate = init_velocity
            acc = 0.0
            dec = 0.0
        else:
            feedrate = (2 * displacement - init_velocity * t1 - end_velocity * t3) / (
                t1 + 2 * t2 + t3
            )
            acc = (feedrate - init_velocity) / t1
            dec = (end_velocity - feedrate) / t3

        t = np.arange(0, (n1 + n2 + n3 + 1) * Ts, Ts)
        s = np.zeros(len(t))
        s_1e = s[0]
        s_2e = s[0]

        for i in range(1, len(t) + 1):
            if i <= n1 + 1:
                s[i - 1] = init_velocity * t[i - 1] + 0.5 * acc * t[i - 1] ** 2
                if i == n1 + 1:
                    s_1e = s[i - 1]
                    s_2e = s[i - 1]
            elif n1 + 1 < i <= n1 + n2 + 1:
                s[i - 1] = s_1e + feedrate * (t[i - 1] - n1 * Ts)
                if i == n1 + n2 + 1:
                    s_2e = s[i - 1]
            else:
                s[i - 1] = (
                    s_2e
                    + feedrate * (t[i - 1] - (n1 + n2) * Ts)
                    + 0.5 * dec * (t[i - 1] - (n1 + n2) * Ts) ** 2
                )

        # Append dwell time
        if dwell > 0.0:
            dwell_positions = s[-1] * np.ones((int(dwell / Ts)))
            dwell_time = np.arange(0.0, dwell, Ts) + t[-1] + Ts
            s = np.concatenate((s, dwell_positions), axis=0)
            t = np.concatenate((t, dwell_time), axis=0)

        sd = np.diff(s, prepend=s[0]) / Ts
        sdd = np.diff(sd, prepend=sd[0]) / Ts

        return s, sd, sdd, t

    def point_to_point_motion_jerk_limit(
        self,
        acc: float,
        start_point: float,
        end_point: float,
        scan_v: float,
        t: float,
        Ts: float,
    ) -> Tuple[float, int]:
        """Compute a jerk-limited point-to-point position at time t.

        Args:
            acc: Acceleration limit [rad/s^2].
            start_point: Starting position [rad].
            end_point: Ending position [rad].
            scan_v: Scan velocity [rad/s].
            t: Current time [s].
            Ts: Sampling time [s].

        Returns:
            `tuple[float, int]` containing position and end-flag.

        Side Effects:
            None.

        Raises:
            None.

        Preconditions:
            `Ts` is positive.
        """
        dt = Ts
        j_limit = 10 ^ 3  # jerk limit [rad/s^3]
        t_dwell = 0.1  # dwell time [s]

        end_flag = 0

        dist = abs(start_point - end_point)  # distance between two points [rad]
        dir = np.sign(end_point - start_point)  # direction

        tj = acc / j_limit  # jerk limited acceleratio time [s]
        ta = scan_v / acc + tj  # acceleration time [s]

        la = (
            (1 / 6) * j_limit * math.pow(tj, 3)
            + (1 / 2) * acc * math.pow((ta - 2 * tj), 2)
            + (1 / 2) * j_limit * math.pow(tj, 2) * (ta - 2 * tj)
            + (1 / 6) * -j_limit * math.pow(tj, 3)
            + (1 / 2) * acc * math.pow(tj, 2)
            + (acc * (ta - 2 * tj) + (1 / 2) * j_limit * math.pow(tj, 2)) * tj
        )

        td = ta
        ld = la

        tb = (dist - la - ld) / scan_v
        if tb < 0:
            tb = 0  # in case scan_v cannot be reached because of too short stroke

        lb = tb * scan_v  # travel length for constant velocity [s]
        scan_v_p = scan_v  # new scan velocity

        acc_p = acc

        if tb == 0:
            ta = math.sqrt(dist / acc)  # spend half the time accelerating
            na = math.ceil(ta / dt)  # num of acceleration steps
            ta = na * dt  # new acceleration time [s]
            acc_p = dist / (math.pow(ta, 2))  # new acceleration [rad/s^2]

            td = math.sqrt(dist / acc)
            nd = math.ceil(td / dt)
            td = nd * dt

        if tb > 0:
            T = np.zeros(9)
            T[0] = t_dwell
            T[1] = t_dwell + tj
            T[2] = t_dwell + ta - tj
            T[3] = t_dwell + ta
            T[4] = t_dwell + ta + tb
            T[5] = t_dwell + ta + tb + tj
            T[6] = t_dwell + ta + tb + td - tj
            T[7] = t_dwell + ta + tb + td
            T[8] = t_dwell + ta + tb + td + t_dwell

            q_temp = 0.0
            if t <= T[0]:  # dwell
                q_temp = start_point
            elif t > T[0] and t <= T[1]:  # initial acceleration with limited jerk
                q_temp = start_point + (1 / 6) * dir * j_limit * math.pow((t - T[0]), 3)
            elif t > T[1] and t <= T[2]:  # constant accel.
                q_temp = (
                    start_point
                    + (1 / 6) * dir * j_limit * math.pow((T[1] - T[0]), 3)
                    + (1 / 2) * dir * acc * math.pow(t - T[1], 2)
                    + (1 / 2) * dir * j_limit * math.pow(T[1] - T[0], 2) * (t - T[1])
                )
            elif t > T[2] and t <= T[3]:  # reduce accel. w/ limited jerk
                q_temp = (
                    start_point
                    + (1 / 6) * dir * j_limit * math.pow((T[1] - T[0]), 3)
                    + (1 / 2) * dir * acc * math.pow(T[2] - T[1], 2)
                    + (1 / 2) * dir * j_limit * math.pow(T[1] - T[0], 2) * (T[2] - T[1])
                    + (-1 / 6) * dir * j_limit * math.pow(t - T[2], 3)
                    + (1 / 2) * dir * acc * math.pow(t - T[2], 2)
                    + (
                        dir * acc * (T[2] - T[1])
                        + (1 / 2) * dir * j_limit * math.pow((T[1] - T[0]), 2)
                    )
                    * (t - T[2])
                )
            elif t > T[3] and t <= T[4]:  # constant velocity move
                q_temp = (
                    start_point
                    + (1 / 6) * dir * j_limit * math.pow((T[1] - T[0]), 3)
                    + (1 / 2) * dir * acc * math.pow(T[2] - T[1], 2)
                    + (1 / 2) * dir * j_limit * math.pow(T[1] - T[0], 2) * (T[2] - T[1])
                    + (-1 / 6) * dir * j_limit * math.pow(T[3] - T[2], 3)
                    + (1 / 2) * dir * acc * math.pow(T[3] - T[2], 2)
                    + (
                        dir * acc * (T[2] - T[1])
                        + (1 / 2) * dir * j_limit * math.pow((T[1] - T[0]), 2)
                    )
                    * (T[3] - T[2])
                    + dir * scan_v_p * (t - T[3])
                )
            elif t > T[4] and t <= T[5]:  # deceleration with limited jerk
                q_temp = (
                    start_point
                    + dir * (la + lb)
                    + (-1 / 6) * dir * j_limit * math.pow(t - T[4], 3)
                    + dir * scan_v_p * (t - T[4])
                )
            elif t > T[5] and t <= T[6]:  # deceleration with constant deceleration
                q_temp = (
                    start_point
                    + dir * (la + lb)
                    + (-1 / 6) * dir * j_limit * math.pow(T[5] - T[4], 3)
                    + dir * scan_v_p * (T[5] - T[4])
                    + (-1 / 2) * dir * acc * math.pow(t - T[5], 2)
                    + (
                        (-1 / 2) * dir * j_limit * math.pow(T[5] - T[4], 2)
                        + dir * scan_v_p
                    )
                    * (t - T[5])
                )
            elif t > T[6] and t <= T[7]:  # reduce deceleration with limited jerk
                q_temp = (
                    start_point
                    + dir * (la + lb)
                    + (-1 / 6) * dir * j_limit * math.pow(T[5] - T[4], 3)
                    + dir * scan_v_p * (T[5] - T[4])
                    + (-1 / 2) * dir * acc * math.pow(T[6] - T[5], 2)
                    + (
                        (-1 / 2) * dir * j_limit * math.pow(T[5] - T[4], 2)
                        + dir * scan_v_p
                    )
                    * (T[6] - T[5])
                    + (1 / 6) * dir * j_limit * math.pow(t - T[6], 3)
                    + (-1 / 2) * dir * acc * math.pow(t - T[6], 2)
                    + (
                        -dir * acc * (T[6] - T[5])
                        + (-1 / 2) * dir * j_limit * math.pow(T[5] - T[4], 2)
                        + dir * scan_v_p
                    )
                    * (t - T[6])
                )
            elif t > T[7] and t <= T[8]:  # dwell
                q_temp = end_point
            elif t > T[8]:
                q_temp = end_point
                end_flag = 1
        else:
            T = np.zeros(4)
            T[0] = t_dwell
            T[1] = t_dwell + ta  # initial acceleration
            T[2] = t_dwell + ta + tb + td  # slow down
            T[3] = t_dwell + ta + tb + td + t_dwell  # dwell

            q_temp = 0.0
            if t <= T[0]:  # dwell
                q_temp = start_point
            elif t > T[0] and t <= T[1]:  # initial acceleration
                q_temp = start_point + (1 / 2) * dir * acc_p * math.pow((t - T[0]), 2)
            elif t > T[1] and t <= T[2]:  # slow down
                q_temp = (
                    start_point
                    + (1 / 2) * dir * acc_p * math.pow((T[1] - T[0]), 2)
                    + (-1 / 2) * dir * acc_p * math.pow(t - T[1], 2)
                    + (dir * acc_p * (T[1] - T[0])) * (t - T[1])
                )
            elif t > T[2] and t <= T[3]:  # dwell
                q_temp = end_point
            elif t > T[3]:
                q_temp = end_point
                end_flag = 1

        return q_temp, end_flag

    def __store_trajectory_data(
        self,
        motionProfile: MotionProfile,
        start_position: List[float],
        move_axis: int,
        Ts: float,
    ) -> None:
        """Populate trajectory buffers from a motion profile.

        Args:
            motionProfile: Generated motion profile.
            start_position: Initial joint positions.
            move_axis: Axis index to excite.
            Ts: Sampling time [s].

        Returns:
            `None`.

        Side Effects:
            Populates internal trajectory buffers.

        Raises:
            None.

        Preconditions:
            `start_position` length matches the robot joint count.
        """
        self.trajectoryTime = motionProfile.t.tolist()
        self.perturbation = motionProfile.q.tolist()
        self.endIndices = motionProfile.endIndex
        qdot = np.gradient(motionProfile.q, Ts)
        qddot = np.gradient(qdot, Ts)

        # Clear vectors
        self.positionTrajectory.clear()
        self.velocityTrajectory.clear()
        self.accelerationTrajectory.clear()

        # Create current vectors
        currentPosition = copy.copy(start_position)
        currentVelocity = [0.0] * len(start_position)
        currentAcceleration = [0.0] * len(start_position)

        for i in range(len(motionProfile.q)):
            currentPosition[move_axis] = float(motionProfile.q[i])
            currentVelocity[move_axis] = float(qdot[i])
            currentAcceleration[move_axis] = float(qddot[i])

            self.positionTrajectory.append(currentPosition.copy())
            self.velocityTrajectory.append(currentVelocity.copy())
            self.accelerationTrajectory.append(currentAcceleration.copy())

    def write_sine_sweep_to_csv(self, filename: str, init_pos: np.ndarray) -> None:
        """Write the sine sweep trajectory to a CSV file.

        Args:
            filename: Output CSV path.
            init_pos: Initial joint positions offset [rad].

        Returns:
            `None`.

        Side Effects:
            Writes a CSV file to disk.

        Raises:
            AssertionError: If trajectory data are missing or inconsistent.
            OSError: If the file cannot be written.

        Preconditions:
            `generate_sine_sweep_trajectory()` has been called.
        """
        # Ensure the directory exists
        directory = os.path.dirname(filename)
        os.makedirs(directory, exist_ok=True)  # Create directories if they don't exist
        # get data to store
        times = self.trajectoryTime
        positions = self.positionTrajectory
        num_axes = len(positions[0])
        num_indices = len(positions)
        # check assertions
        if not self._generated_sine_sweep_trajectory:
            raise AssertionError(
                "you must have called generate_sine_sweep_trajectory before attempting to save csv."
            )
        for p in positions:
            if len(p) != num_axes:
                raise AssertionError("all positions must have the same length")
        if len(times) != len(positions):
            raise AssertionError(
                f"time and positions vectors must be the same length; len(time) = {len(times)} but len(positions) = {len(positions)}"
            )
        # form header
        header = ["t"] + [f"a{i + 1}" for i in range(num_axes)]
        with open(filename, "w", newline="") as f:
            writer = csv.writer(f)
            writer.writerow(header)
            for i in range(num_indices):
                row = [
                    times[i],
                    *((np.rad2deg(np.array(positions[i])) + init_pos).tolist()),
                ]
                writer.writerow(row)
        return None

    @staticmethod
    def read_times_from_sine_sweep_csv(filename: str) -> List[float]:
        """Read time stamps from a sine sweep CSV file.

        Args:
            filename: Path to the CSV file.

        Returns:
            `list[float]` of time stamps.

        Side Effects:
            Reads from disk.

        Raises:
            OSError: If the file cannot be read.
            ValueError: If time values cannot be parsed.

        Preconditions:
            CSV format matches `write_sine_sweep_to_csv`.
        """
        with open(filename, "r") as f:
            reader = csv.reader(f)
            rows = list(reader)
            data = rows[1:]
            return [float(row[0]) for row in data]

    @staticmethod
    def read_positions_from_sine_sweep_csv(filename: str) -> List[List[float]]:
        """Read joint positions from a sine sweep CSV file.

        Args:
            filename: Path to the CSV file.

        Returns:
            `list[list[float]]` of joint positions per time step.

        Side Effects:
            Reads from disk.

        Raises:
            OSError: If the file cannot be read.
            ValueError: If position values cannot be parsed.

        Preconditions:
            CSV format matches `write_sine_sweep_to_csv`.
        """
        with open(filename, "r") as f:
            reader = csv.reader(f)
            rows = list(reader)
            data = rows[1:]
            return [[float(el) for el in row[1:]] for row in data]
