import copy
import datetime
import os
import random
import time
from collections import deque
from pathlib import Path
from typing import Any, Deque, Mapping, TypedDict

import matplotlib
import numpy as np
from scipy.fft import fft, fftfreq
from robot.robot_base import (
    DEFAULT_MAX_DISP,
    DEFAULT_MAX_ACC,
    DEFAULT_ROBOT_FREQ,
    DEFAULT_MAX_VEL,
)
from robot.robot_base import TrajParams, SystemIdParams, Trajectory
from robot.robot_base import store_recorder_data_in_data_folder
from robot.robot_interface import RobotInterface
from reforge_core.util.utility import (
    extract_dynamic_variables,
    read_identification_parameters,
)
from matplotlib import pyplot as plt

from types import SimpleNamespace
from reforge_core.control.python.covalent_wrapper import (
    RobotState as _RobotState,
    ShaperInterface as _ShaperInterface,
)

covalent = SimpleNamespace(
    RobotState=_RobotState,
    ShaperInterface=_ShaperInterface,
)


# =========================
# User-Tunable Plot Options
# =========================
# Turn interactive plotting on/off.
# Set `REFORGE_PLOT_INTERACTIVE=1` to show figures; use `0` for headless/long runs.
PLOT_INTERACTIVE = os.getenv("REFORGE_PLOT_INTERACTIVE", "1") == "1"
if not PLOT_INTERACTIVE:
    matplotlib.use("Agg", force=True)

# Save plots to PNG when enabled.
# Set `REFORGE_PLOT_SAVE=1` to save figures; keep `0` for fastest execution.
PLOT_SAVE = os.getenv("REFORGE_PLOT_SAVE", "0") == "1"
PLOT_DPI = (
    120  # Output image resolution [dots per inch] used when `PLOT_SAVE` is enabled.
)

# ===========================
# User-Tunable Test Execution
# ===========================
# Post-motion hold [s] to capture residual vibration after the move ends.
# Increase for slower-settling systems; decrease to shorten test duration.
DWELL_TIME = 4

# Repetitions per axis when building random pose/axis experiment pairs.
# Increase for testing more poses; decrease for quicker tests.
AXIS_REPETITIONS = 1


# =============================
# User-Tunable Signal Analysis
# =============================
# Butterworth filter order (higher means sharper cutoff, but more phase effect).
BUTTER_ORDER = 4

# Butterworth low-pass cutoff frequency [Hz].
BUTTER_CUTOFF = 40

# Useful lenght of residual vibraiton window [s] used to calcualte FFT.
# Longer windows improve frequency resolution, shorter windows track transients.
# Should be smaller than `DWELL_TIME'
FFT_WINDOW_DURATION_SEC = 3

# FFT window start offset [s] from trajectory start.
# Increase to skip launch transients before frequency analysis.
# Affects plot_fft_summaries()
FFT_WINDOW_START_SEC = 0

# Moving-window lenght [s] for removing trend of rotated accerometer readings.
# Increase for smoother metrics, decrease for more reactive metrics.
# Affects plot_rotated_accelerometer_readings()
MOVING_AVG_WINDOW_SEC = 0.2

# Number of lowest FFT bins to skip to avoid DC/bias dominance in peak searches.
DC_FFT_OFFSET = 5


# ==================================
# Auto-Resolved Paths (Usually Fixed)
# ==================================
# Repository paths are derived from this file location.
# Change these only if project directory layout changes.
THIS_DIR = Path(__file__).resolve().parent
REPO_ROOT = THIS_DIR.parents[2]


def _package_dir(pkg_name: str) -> Path | None:
    """Return the package directory for an importable module.

    Args:
        pkg_name: Module name to import.

    Returns:
        `Path | None` to the module directory, or None if not found.

    Side Effects:
        Attempts to import the module.

    Raises:
        None.

    Preconditions:
        None.
    """
    try:
        mod = __import__(pkg_name)
    except Exception:
        return None
    mod_file = getattr(mod, "__file__", None)
    if not mod_file:
        return None
    return Path(mod_file).resolve().parent


# Installed/project `robot` package directory, if importable.
ROBOT_PKG_DIR = _package_dir("robot")
# Installed/project `util` package directory, if importable.
UTIL_PKG_DIR = _package_dir("util")

MODEL_DIR = (
    (ROBOT_PKG_DIR / "models" / "current")
    if ROBOT_PKG_DIR
    else (REPO_ROOT / "src/robot/models/current")
)
# Python source root used by the shaper wrapper for model-related imports.
PYTHON_SRC_ROOT = UTIL_PKG_DIR.parent if UTIL_PKG_DIR else (REPO_ROOT / "src")
# ===========================
# Output Folder Configuration
# ===========================
# Date-based output folder used by vibration-test CSV and saved figures.
TODAY = datetime.datetime.now()
VIBRATION_DATA_FOLDER = f"src/unit_tests/data/{TODAY.year}-{TODAY.month}-{TODAY.day}"

# Plot output folder used when `PLOT_SAVE` is enabled.
PLOT_OUTPUT_FOLDER = Path(VIBRATION_DATA_FOLDER) / "plots"


class VibrationTestPlots:
    """Generate all vibration-test diagnostic figures.

    Args:
        None.
    Returns:
        None.
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        None.
    """

    @staticmethod
    def finalize_figure(fig: Any, plot_name: str) -> None:
        """Apply common figure finalization behavior for vibration-test plots.

        Args:
            fig: Matplotlib figure object to finalize.
            plot_name: Output filename stem used when saving a PNG.
        Returns:
            `None`.
        Side Effects:
            Writes a PNG file when `PLOT_SAVE` is enabled.
            Displays the figure when `PLOT_INTERACTIVE` is enabled.
            Closes the figure handle.
            Prints a warning line if interactive display fails.
        Raises:
            OSError: If the output directory cannot be created or written.
        Preconditions:
            `fig` is a valid matplotlib figure.
            `plot_name` is a non-empty filename-safe stem.
        """
        # Save to disk first when enabled so figures are preserved in headless runs.
        if PLOT_SAVE:
            PLOT_OUTPUT_FOLDER.mkdir(parents=True, exist_ok=True)
            fig.savefig(PLOT_OUTPUT_FOLDER / f"{plot_name}.png", dpi=PLOT_DPI)
        # Show interactively only when explicitly enabled.
        if PLOT_INTERACTIVE:
            # fig.show()
            plt.show()

    @staticmethod
    def plot_rotated_accelerometer_readings(
        *,
        time_vec: np.ndarray,
        signal: np.ndarray,
        moving_avg: np.ndarray,
        residual: np.ndarray,
        title: str,
        signal_color: str,
        signal_linestyle: str,
        plot_name: str,
    ) -> None:
        """Plot base-frame acceleration components with trend and residual traces.

        Args:
            time_vec: Sample times [s], shape `(N,)`.
            signal: Base-frame acceleration [m/s^2], shape `(N, 3)`.
            moving_avg: Moving-average acceleration [m/s^2], shape `(N, 3)`.
            residual: Residual acceleration [m/s^2], shape `(N, 3)`.
            title: Figure title text.
            signal_color: Matplotlib color for the primary signal trace.
            signal_linestyle: Matplotlib linestyle for the primary signal trace.
            plot_name: Output filename stem used by `finalize_figure`.
        Returns:
            `None`.
        Side Effects:
            Creates a matplotlib figure and delegates save/show/close behavior.
        Raises:
            ValueError: If input arrays are incompatible with plotting operations.
        Preconditions:
            `time_vec`, `signal`, `moving_avg`, and `residual` are time-aligned.
            `signal`, `moving_avg`, and `residual` each contain exactly 3 columns.
        """
        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6

        fig, axes = plt.subplots(3, 1, sharex=True)
        labels = ("Ax [m/s^2]", "Ay [m/s^2]", "Az [m/s^2]")

        # Each iteration renders one acceleration axis with a consistent style.
        for i, axis in enumerate(axes):
            axis.plot(
                time_vec,
                signal[:, i],
                color=signal_color,
                linestyle=signal_linestyle,
                linewidth=2,
                label="Signal",
            )
            axis.plot(
                time_vec,
                moving_avg[:, i],
                color="b",
                linewidth=2,
                label="Moving Avg",
            )
            axis.plot(
                time_vec,
                residual[:, i],
                color="m",
                linewidth=2,
                label="Signal - Avg",
            )
            axis.set_ylabel(labels[i], fontsize=base_font, fontweight="bold")
            axis.set_xlim(0.0, DWELL_TIME)
            axis.legend(prop={"weight": "bold", "size": base_font}, loc="lower right")

        axes[0].set_title(title, fontsize=title_size)
        axes[2].set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        fig.tight_layout()
        VibrationTestPlots.finalize_figure(fig, plot_name)

    @staticmethod
    def plot_encoder_fullacc_residual_vibration(
        *,
        time_u: np.ndarray,
        time_s: np.ndarray,
        encoder_u_axis: np.ndarray,
        encoder_s_axis: np.ndarray,
        mag_acc_u: np.ndarray,
        mag_acc_s: np.ndarray,
        idx_vib_u: int,
        idx_vib_s: int,
        peak_acc_u: int,
        peak_acc_s: int,
        duration_diff_s: float,
        closest_time_abs: float | None,
        pose: int,
        axis: int,
    ) -> None:
        """Plot encoder history, full acceleration norm, and residual-window zoom.

        Args:
            time_u: Unshaped time samples [s], shape `(N_u,)`.
            time_s: Shaped time samples [s], shape `(N_s,)`.
            encoder_u_axis: Unshaped encoder signal [rad], shape `(N_u,)`.
            encoder_s_axis: Shaped encoder signal [rad], shape `(N_s,)`.
            mag_acc_u: Unshaped acceleration norm [m/s^2], shape `(N_u,)`.
            mag_acc_s: Shaped acceleration norm [m/s^2], shape `(N_s,)`.
            idx_vib_u: Start index of unshaped residual-vibration window.
            idx_vib_s: Start index of shaped residual-vibration window.
            peak_acc_u: Peak index within unshaped residual window.
            peak_acc_s: Peak index within shaped residual window.
            duration_diff_s: Shaped minus unshaped trajectory duration [s].
            closest_time_abs: Optional reference timestamp [s] for threshold marker.
            pose: Pose identifier used in the output filename.
            axis: Joint-axis identifier used in the output filename.
        Returns:
            `None`.
        Side Effects:
            Creates a matplotlib figure and delegates save/show/close behavior.
        Raises:
            IndexError: If any provided index is outside the corresponding vector.
            ValueError: If plotted arrays are malformed.
        Preconditions:
            Time and signal arrays are aligned for each trajectory type.
            Residual-window and peak indices are valid for their arrays.
        """
        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6
        legend_size = base_font + 3

        fig, axes = plt.subplots(3, 1, sharex=True)
        axes[0].plot(time_u, encoder_u_axis, color="k", linewidth=2)
        axes[0].plot(time_s, encoder_s_axis, color="r", linestyle="--", linewidth=2)
        axes[0].axvline(time_u[idx_vib_u], color="k", linestyle=":", linewidth=2)
        axes[0].axvline(time_s[idx_vib_s], color="r", linestyle=":", linewidth=2)
        axes[0].annotate(
            f"Added delay ΔT = {duration_diff_s:.2f} [s]",
            xy=(time_s[idx_vib_s], encoder_s_axis[idx_vib_s]),
            xytext=(10, 10),
            textcoords="offset points",
            color="r",
            fontsize=plt.rcParams.get("font.size", 10) + 3,
        )
        axes[0].set_ylabel("Encoder [rad]", fontsize=label_size, fontweight="bold")
        axes[0].set_title("Encoder (Moving Joint)", fontsize=title_size)

        axes[1].plot(time_u, mag_acc_u, color="k", linewidth=2)
        axes[1].plot(time_s, mag_acc_s, color="r", linestyle="--", linewidth=2)
        axes[1].axvline(time_u[idx_vib_u], color="k", linestyle=":", linewidth=2)
        axes[1].axvline(time_s[idx_vib_s], color="r", linestyle=":", linewidth=2)
        # Add the optional threshold crossing marker when available.
        if closest_time_abs is not None:
            y_mid_full = 0.25 * (np.max(mag_acc_u) + np.min(mag_acc_u))
            axes[1].axvline(closest_time_abs, color="blue", linestyle="-.", linewidth=2)
            axes[1].annotate(
                f"  Initial Avg. acceleration \n  reached at t = {closest_time_abs:.2f} [s]",
                xy=(closest_time_abs, y_mid_full),
                xytext=(0, 0),
                textcoords="offset points",
                color="blue",
                fontsize=plt.rcParams.get("font.size", 10) + 3,
            )
        axes[1].set_ylabel("Accel Norm [m/s^2]", fontsize=label_size, fontweight="bold")
        axes[1].set_title("Acceleration Norm (Full Trajectory)", fontsize=title_size)

        axes[2].plot(
            time_u[idx_vib_u:],
            mag_acc_u[idx_vib_u:],
            color="k",
            linewidth=2,
            label="RR control off",
        )
        axes[2].plot(
            time_s[idx_vib_s:],
            mag_acc_s[idx_vib_s:],
            color="r",
            linestyle="--",
            linewidth=2,
            label="RR control on",
        )
        axes[2].scatter(
            [time_u[idx_vib_u + peak_acc_u], time_s[idx_vib_s + peak_acc_s]],
            [mag_acc_u[idx_vib_u + peak_acc_u], mag_acc_s[idx_vib_s + peak_acc_s]],
            s=60,
            c="green",
            zorder=5,
            label="Max acceleration peak",
        )
        axes[2].set_ylabel("Accel Norm [m/s^2]", fontsize=label_size, fontweight="bold")
        axes[2].set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        axes[2].set_title("Residual Vibration Window", fontsize=title_size)
        axes[2].legend(prop={"weight": "bold", "size": legend_size}, loc="upper right")
        axes[0].legend(
            ["RR control off", "RR control on"],
            prop={"weight": "bold", "size": legend_size},
            loc="upper right",
        )
        axes[1].legend(
            ["RR control off", "RR control on"],
            prop={"weight": "bold", "size": legend_size},
            loc="upper right",
        )
        # Apply the same x-axis window to all subplots for visual comparison.
        x_limit = (
            float(closest_time_abs + 1.0)
            if closest_time_abs is not None
            else float(DWELL_TIME)
        )
        for ax in axes:
            ax.set_xlim(0.0, x_limit)
        plt.tight_layout()
        VibrationTestPlots.finalize_figure(
            fig, f"residual_window_pose_{pose}_axis_{axis}"
        )

    @staticmethod
    def plot_fft_summaries(
        *,
        f_u: np.ndarray,
        f_s: np.ndarray,
        fft_u_x: np.ndarray,
        fft_u_y: np.ndarray,
        fft_u_z: np.ndarray,
        fft_s_x: np.ndarray,
        fft_s_y: np.ndarray,
        fft_s_z: np.ndarray,
        fft_u_norm: np.ndarray,
        fft_s_norm: np.ndarray,
        pose: int,
        axis: int,
        plot_fft_axes: bool,
        plot_fft_norm: bool,
    ) -> None:
        """Plot FFT comparisons for per-axis and norm acceleration spectra.

        Args:
            f_u: Unshaped one-sided frequency bins [Hz], shape `(N_f,)`.
            f_s: Shaped one-sided frequency bins [Hz], shape `(N_f,)` or compatible.
            fft_u_x: Unshaped X-axis FFT magnitude.
            fft_u_y: Unshaped Y-axis FFT magnitude.
            fft_u_z: Unshaped Z-axis FFT magnitude.
            fft_s_x: Shaped X-axis FFT magnitude.
            fft_s_y: Shaped Y-axis FFT magnitude.
            fft_s_z: Shaped Z-axis FFT magnitude.
            fft_u_norm: Unshaped norm FFT magnitude.
            fft_s_norm: Shaped norm FFT magnitude.
            pose: Pose identifier used in figure titles/filenames.
            axis: Joint-axis identifier used in output filenames.
            plot_fft_axes: Predicate controlling the per-axis FFT figure.
            plot_fft_norm: Predicate controlling the norm FFT figure.
        Returns:
            `None`.
        Side Effects:
            Creates one or two matplotlib figures and delegates save/show/close.
        Raises:
            ValueError: If frequency and amplitude vectors are incompatible.
        Preconditions:
            Frequency vectors and matching FFT vectors are aligned in length.
            At least one of `plot_fft_axes` or `plot_fft_norm` is `True` to emit plots.
        """
        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6
        legend_size = base_font + 3

        # Plot per-axis FFTs when requested.
        if plot_fft_axes:
            fig, axes = plt.subplots(3, 1, sharex=True)
            axes[0].plot(f_u, fft_u_x, color="k", linewidth=2)
            axes[0].plot(f_s, fft_s_x, color="r", linestyle="--", linewidth=2)
            axes[0].set_ylabel("Ax FFT", fontsize=label_size, fontweight="bold")
            axes[0].set_title("FFT X", fontsize=title_size)
            axes[0].set_xlim(0.0, 30.0)

            axes[1].plot(f_u, fft_u_y, color="k", linewidth=2)
            axes[1].plot(f_s, fft_s_y, color="r", linestyle="--", linewidth=2)
            axes[1].set_ylabel("Ay FFT", fontsize=label_size, fontweight="bold")
            axes[1].set_title("FFT Y", fontsize=title_size)
            axes[1].set_xlim(0.0, 30.0)

            axes[2].plot(f_u, fft_u_z, color="k", linewidth=2)
            axes[2].plot(f_s, fft_s_z, color="r", linestyle="--", linewidth=2)
            axes[2].set_ylabel("Az FFT", fontsize=label_size, fontweight="bold")
            axes[2].set_xlabel("Frequency [Hz]", fontsize=label_size, fontweight="bold")
            axes[2].set_title("FFT Z", fontsize=title_size)
            axes[2].set_xlim(0.0, 30.0)
            axes[0].legend(
                ["RR control off", "RR control on"],
                prop={"weight": "bold", "size": legend_size},
            )
            plt.tight_layout()
            VibrationTestPlots.finalize_figure(fig, f"fft_axes_pose_{pose}_axis_{axis}")

        # Plot FFT of the acceleration norm when requested.
        if plot_fft_norm:
            fig_norm, ax_norm = plt.subplots(1, 1)
            ax_norm.plot(f_u, fft_u_norm, color="k", linewidth=2)
            ax_norm.plot(f_s, fft_s_norm, color="r", linestyle="--", linewidth=2)
            ax_norm.set_title(
                f"FFT of Acceleration Norm at Configuration {pose}",
                fontsize=title_size,
            )
            ax_norm.set_xlabel("Frequency [Hz]", fontsize=label_size, fontweight="bold")
            ax_norm.set_ylabel(
                "Acceleration Norm FFT", fontsize=label_size, fontweight="bold"
            )
            ax_norm.legend(
                ["RR control off", "RR control on"],
                prop={"weight": "bold", "size": legend_size},
            )
            ax_norm.set_xlim(0.0, 30.0)
            ax_norm.grid()
            fig_norm.tight_layout()
            VibrationTestPlots.finalize_figure(
                fig_norm, f"fft_norm_pose_{pose}_axis_{axis}"
            )

    @staticmethod
    def plot_shaped_vs_unshaped_trajectory(
        *,
        unshaped: Mapping[str, np.ndarray],
        shaped: Mapping[str, np.ndarray],
        moved_axis: int | None,
        plot_name: str,
    ) -> None:
        """Plot shaped versus unshaped joint position, velocity, and acceleration.

        Args:
            unshaped: Mapping with keys `time`, `positions`, `velocities`,
                `accelerations` for the unshaped trajectory.
            shaped: Mapping with keys `time`, `positions`, `velocities`,
                `accelerations` for the shaped trajectory.
            moved_axis: Joint index to plot; `None` plots full arrays directly.
            plot_name: Output filename stem used by `finalize_figure`.
        Returns:
            `None`.
        Side Effects:
            Creates a matplotlib figure and delegates save/show/close behavior.
        Raises:
            KeyError: If required trajectory keys are missing.
            IndexError: If `moved_axis` is out of bounds for trajectory arrays.
            ValueError: If provided arrays are not plottable.
        Preconditions:
            Both trajectory mappings contain aligned time and signal arrays.
            `moved_axis` is `None` or a valid joint index.
        """
        fig, axes = plt.subplots(3, 1, sharex=True, figsize=(10, 8))
        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6
        legend_size = base_font + 3
        time_u = unshaped["time"]
        time_s = shaped["time"]

        # Plot either full vectors or one moved axis based on caller intent.
        if moved_axis is None:
            pos_u = unshaped["positions"]
            pos_s = shaped["positions"]
            vel_u = unshaped["velocities"]
            vel_s = shaped["velocities"]
            acc_u = unshaped["accelerations"]
            acc_s = shaped["accelerations"]
        else:
            pos_u = unshaped["positions"][:, moved_axis]
            pos_s = shaped["positions"][:, moved_axis]
            vel_u = unshaped["velocities"][:, moved_axis]
            vel_s = shaped["velocities"][:, moved_axis]
            acc_u = unshaped["accelerations"][:, moved_axis]
            acc_s = shaped["accelerations"][:, moved_axis]

        axes[0].plot(time_u, pos_u, color="k", linewidth=2)
        axes[0].plot(time_s, pos_s, linestyle="--", color="r", linewidth=2)
        axes[0].set_ylabel("Position [rad]", fontsize=label_size, fontweight="bold")
        axes[0].set_title("Position", fontsize=title_size)

        axes[1].plot(time_u, vel_u, color="k", linewidth=2)
        axes[1].plot(time_s, vel_s, linestyle="--", color="r", linewidth=2)
        axes[1].set_ylabel("Velocity [rad/s]", fontsize=label_size, fontweight="bold")
        axes[1].set_title("Velocity", fontsize=title_size)

        axes[2].plot(time_u, acc_u, color="k", linewidth=2)
        axes[2].plot(time_s, acc_s, linestyle="--", color="r", linewidth=2)
        axes[2].set_ylabel(
            "Acceleration [rad/s^2]", fontsize=label_size, fontweight="bold"
        )
        axes[2].set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        axes[2].set_title("Acceleration", fontsize=title_size)

        axes[0].legend(
            ["RR control off", "RR control on"],
            prop={"weight": "bold", "size": legend_size},
        )
        fig.tight_layout()
        VibrationTestPlots.finalize_figure(fig, plot_name)

    @staticmethod
    def plot_mag_acc_residual_and_fft(
        *,
        time_u: np.ndarray,
        time_s: np.ndarray,
        mag_acc_u: np.ndarray,
        mag_acc_s: np.ndarray,
        mag_acc_u_residual_abs: np.ndarray,
        mag_acc_s_residual_abs: np.ndarray,
        sample_time: float,
        pose: int,
        axis: int,
    ) -> None:
        """Plot full-trajectory norm, residual-abs norm, and their FFT comparison.

        Args:
            time_u: Unshaped time vector [s].
            time_s: Shaped time vector [s].
            mag_acc_u: Unshaped acceleration norm [m/s^2].
            mag_acc_s: Shaped acceleration norm [m/s^2].
            mag_acc_u_residual_abs: Absolute residual norm (unshaped) [m/s^2].
            mag_acc_s_residual_abs: Absolute residual norm (shaped) [m/s^2].
            sample_time: Sampling period [s].
            pose: Pose identifier for title/filename.
            axis: Axis identifier for title/filename.
        Returns:
            `None`.
        Side Effects:
            Creates a matplotlib figure and delegates save/show/close behavior.
        Raises:
            ValueError: If `sample_time <= 0` or vectors are malformed.
        Preconditions:
            Time and signal vectors are aligned per trajectory type.
        """
        if sample_time <= 0.0:
            raise ValueError("sample_time must be positive")

        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6
        legend_size = base_font + 3

        def _one_sided_fft(signal: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
            """Compute one-sided FFT magnitude for a scalar signal."""
            n = signal.size
            fft_vals = np.asarray(fft(signal))
            freqs = fftfreq(n, sample_time)
            # `half`: one-sided FFT cutoff index (positive-frequency half-spectrum).
            half = n // 2
            freqs = freqs[:half]
            amps = np.abs(fft_vals[:half]) / float(n)
            if n > 1:
                amps[1:] *= 2.0
            return freqs, amps

        f_u, fft_u = _one_sided_fft(mag_acc_u_residual_abs)
        f_s, fft_s = _one_sided_fft(mag_acc_s_residual_abs)

        fig, axes = plt.subplots(3, 1, sharex=False, figsize=(10, 9))

        axes[0].plot(time_u, mag_acc_u, color="k", linewidth=2, label="RR control off")
        axes[0].plot(
            time_s,
            mag_acc_s,
            color="r",
            linestyle="--",
            linewidth=2,
            label="RR control on",
        )
        axes[0].set_title("Acceleration Norm (Full Trajectory)", fontsize=title_size)
        axes[0].set_ylabel("Norm [m/s^2]", fontsize=label_size, fontweight="bold")
        axes[0].set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        axes[0].legend(prop={"weight": "bold", "size": legend_size}, loc="upper right")

        axes[1].plot(
            time_u,
            mag_acc_u_residual_abs,
            color="k",
            linewidth=2,
            label="RR control off residual abs",
        )
        axes[1].plot(
            time_s,
            mag_acc_s_residual_abs,
            color="r",
            linestyle="--",
            linewidth=2,
            label="RR control on residual abs",
        )
        axes[1].set_title(
            "Acceleration Norm Residual Acceleration (Full Trajectory)",
            fontsize=title_size,
        )
        axes[1].set_ylabel(
            "Residual Abs [m/s^2]", fontsize=label_size, fontweight="bold"
        )
        axes[1].set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        axes[1].legend(prop={"weight": "bold", "size": legend_size}, loc="upper right")

        axes[2].plot(
            f_u,
            fft_u,
            color="k",
            linewidth=2,
            label="FFT RR control off residual abs",
        )
        axes[2].plot(
            f_s,
            fft_s,
            color="r",
            linestyle="--",
            linewidth=2,
            label="FFT RR control on residual abs",
        )
        axes[2].set_title(
            "FFT of Residual Acceleration (Full Trajectory)", fontsize=title_size
        )
        axes[2].set_ylabel("FFT Magnitude", fontsize=label_size, fontweight="bold")
        axes[2].set_xlabel("Frequency [Hz]", fontsize=label_size, fontweight="bold")
        axes[2].legend(prop={"weight": "bold", "size": legend_size}, loc="upper right")

        fig.tight_layout()
        VibrationTestPlots.finalize_figure(
            fig, f"mag_acc_residual_fft_pose_{pose}_axis_{axis}"
        )

    @staticmethod
    def plot_residual_vibration(
        time_unshaped: np.ndarray,
        time_shaped: np.ndarray,
        mag_unshaped: np.ndarray,
        mag_shaped: np.ndarray,
        idx_vib_unshaped: int,
        idx_vib_shaped: int,
        peak_idx_unshaped: int,
        peak_idx_shaped: int,
        sample_time: float,
        pose: int,
        duration_diff_s: float,
        window_sec: float = 0.2,
        tol_rel: float = 0.4,
        min_duration_sec: float = 0.125,
        consecutive_windows: int = 2,
        center_signals_at_zero: bool = False,
    ) -> tuple[float | None, float | None, int | None]:
        """Plot residual vibration metrics and return key detected timestamps.

        Args:
            time_unshaped: Unshaped time samples [s], shape `(N_u,)`.
            time_shaped: Shaped time samples [s], shape `(N_s,)`.
            mag_unshaped: Unshaped acceleration norm [m/s^2], shape `(N_u,)`.
            mag_shaped: Shaped acceleration norm [m/s^2], shape `(N_s,)`.
            idx_vib_unshaped: Start index of unshaped residual-vibration window.
            idx_vib_shaped: Start index of shaped residual-vibration window.
            peak_idx_unshaped: Peak index offset within unshaped residual window.
            peak_idx_shaped: Peak index offset within shaped residual window.
            sample_time: Sample period used for moving-window metrics [s].
            pose: Pose identifier used in title/filename.
            duration_diff_s: Shaped minus unshaped trajectory duration [s].
            window_sec: Moving-window length for RMS/average operations [s].
            tol_rel: Affects blue vertical line placement - Relative tolerance
            for RMS coincidence detection [0..1].
            min_duration_sec: Minimum stable coincidence duration [s].
            consecutive_windows:Affects pink vertical line placement- Number of
            consecutive windows required for the unshaped moving average to stay
            below the shaped initial-window average. This determns the tiem where the
            amplitude match (pink vertical line).
            center_signals_at_zero: If `True`, subtract moving average from
            RR control off/on residual signals before plotting so traces are
            centered around zero.
        Returns:
            `tuple[float | None, float | None, int | None]`:
            (absolute coincidence time [s] or None,
            absolute initial-average reach time [s] or None,
            residual-window index of the blue coincidence line or None).
        Side Effects:
            Creates a matplotlib figure and delegates save/show/close behavior.
        Raises:
            IndexError: If any provided index is outside the corresponding vector.
            ValueError: If window sizes or vector shapes are invalid.
        Preconditions:
            Input vectors are time-aligned within each trajectory.
            `sample_time > 0`.
            Residual-window and peak indices are valid.
        """
        residual_unshaped_raw = mag_unshaped[idx_vib_unshaped:]
        residual_shaped_raw = mag_shaped[idx_vib_shaped:]

        # Center plotted traces around zero when requested, while keeping
        # raw residual signals for timing/metric detection logic below.
        residual_unshaped_plot = residual_unshaped_raw
        residual_shaped_plot = residual_shaped_raw
        if center_signals_at_zero:
            residual_unshaped_plot = residual_unshaped_raw - moving_average_signal(
                signal=residual_unshaped_raw,
                window_sec=window_sec,
                sample_time=sample_time,
            )
            residual_shaped_plot = residual_shaped_raw - moving_average_signal(
                signal=residual_shaped_raw,
                window_sec=window_sec,
                sample_time=sample_time,
            )

        fig, ax = plt.subplots(1, 1)
        ax.plot(
            time_unshaped[idx_vib_unshaped:] - time_unshaped[idx_vib_unshaped],
            residual_unshaped_plot,
            color="k",
            linewidth=2,
            label="RR control off",
        )
        ax.plot(
            time_shaped[idx_vib_shaped:] - time_shaped[idx_vib_shaped],
            residual_shaped_plot,
            color="r",
            linestyle="--",
            linewidth=2,
            label="RR control on",
        )

        # Build moving RMS envelopes and detect earliest stable coincidence region.
        # `window_n`: samples per moving RMS window.
        # `window_n`: samples per moving RMS window.
        window_n = max(1, int(round(window_sec / sample_time)))
        # `min_len`: minimum consecutive samples required for coincidence acceptance.
        min_len = max(1, int(round(min_duration_sec / sample_time)))

        rms_unshaped = np.sqrt(
            np.convolve(
                residual_unshaped_raw**2,
                np.ones(window_n) / window_n,
                mode="same",
            )
        )
        rms_shaped = np.sqrt(
            np.convolve(
                residual_shaped_raw**2,
                np.ones(window_n) / window_n,
                mode="same",
            )
        )
        common_len = min(rms_unshaped.size, rms_shaped.size)
        rms_unshaped = rms_unshaped[:common_len]
        rms_shaped = rms_shaped[:common_len]
        rms_diff = np.abs(rms_unshaped - rms_shaped)
        rms_ref = np.maximum(rms_unshaped, rms_shaped)
        coincidence_mask = rms_diff <= (tol_rel * rms_ref)

        # `coincide_idx`: index inside residual window where blue-line coincidence starts.
        coincide_idx = None
        coincide_time = None
        coincide_time_abs = None
        # Each iteration tests whether a contiguous run satisfies coincidence criteria.
        if np.any(coincidence_mask):
            for i in range(coincidence_mask.size - min_len + 1):
                if np.all(coincidence_mask[i : i + min_len]):
                    coincide_idx = i
                    break
        y_min = float(
            np.min(
                np.concatenate(
                    [
                        residual_unshaped_plot,
                        residual_shaped_plot,
                    ]
                )
            )
        )
        y_max = float(
            np.max(
                np.concatenate(
                    [
                        residual_unshaped_plot,
                        residual_shaped_plot,
                    ]
                )
            )
        )

        if coincide_idx is not None:
            coincide_time_abs = time_unshaped[idx_vib_unshaped + coincide_idx]
            coincide_time = coincide_time_abs - time_unshaped[idx_vib_unshaped]
            ax.axvline(x=coincide_time, color="b", linestyle=":", linewidth=2)
            y_mid = 0.25 * (y_min + y_max)
            ax.annotate(
                f"  Amplitude coincide \n  at t = {coincide_time:.2f} [s]",
                xy=(coincide_time, y_mid),
                xytext=(0, 0),
                textcoords="offset points",
                color="b",
                fontsize=plt.rcParams.get("font.size", 10) + 3,
            )

        # Annotate when unshaped average reaches shaped initial-window amplitude.
        shaped_peak_value = residual_shaped_raw[peak_idx_shaped]
        unshaped_residual = residual_unshaped_raw
        shaped_residual = residual_shaped_raw
        unshaped_ma = np.convolve(
            unshaped_residual,
            np.ones(window_n) / window_n,
            mode="same",
        )
        shaped_window_avg = float(np.mean(shaped_residual[:window_n]))
        reach_mask = unshaped_ma <= shaped_window_avg
        # `start_idx`: first index where pink-line threshold condition is sustained.
        start_idx = 0
        # Each iteration checks for the first stable threshold-reaching segment.
        if np.any(reach_mask):
            window_req = max(1, int(consecutive_windows))
            for i in range(reach_mask.size - window_req + 1):
                if np.all(reach_mask[i : i + window_req]):
                    start_idx = i
                    break
        # `closest_idx`: closest unshaped sample (after `start_idx`) to shaped peak level.
        closest_idx = start_idx + int(
            np.argmin(np.abs(unshaped_residual[start_idx:] - shaped_peak_value))
        )
        closest_time_abs = time_unshaped[idx_vib_unshaped + closest_idx]
        closest_time = closest_time_abs - time_unshaped[idx_vib_unshaped]
        y_mid = 0.5 * (y_min + y_max)
        should_plot_pink = coincide_idx is None or closest_time <= coincide_time
        if should_plot_pink:
            ax.axvline(x=closest_time, color="deeppink", linestyle="-.", linewidth=2)
            ax.annotate(
                f"  Initial Avg. acceleration \n  reached at t = {closest_time:.2f} [s]",
                xy=(closest_time, y_mid),
                xytext=(0, 0),
                textcoords="offset points",
                color="deeppink",
                fontsize=plt.rcParams.get("font.size", 10) + 3,
            )
            ax.scatter(
                [closest_time],
                [residual_unshaped_plot[closest_idx]],
                s=120,
                c="deeppink",
                zorder=5,
            )

        ax.scatter(
            [
                time_unshaped[idx_vib_unshaped + peak_idx_unshaped]
                - time_unshaped[idx_vib_unshaped],
                time_shaped[idx_vib_shaped + peak_idx_shaped]
                - time_shaped[idx_vib_shaped],
            ],
            [
                residual_unshaped_plot[peak_idx_unshaped],
                residual_shaped_plot[peak_idx_shaped],
            ],
            s=120,
            c="green",
            zorder=5,
            label="Max acceleration peak",
        )
        base_font = float(plt.rcParams.get("font.size", 10))
        title_size = base_font + 6
        label_size = base_font + 6
        legend_size = base_font + 3

        # Match the velocity-test title convention: base delay plus marker time
        # (pink preferred, blue fallback when pink is unavailable).
        marker_time_s = (
            float(closest_time)
            if should_plot_pink
            else (float(coincide_time) if coincide_time is not None else 0.0)
        )
        rr_faster_by_s = marker_time_s - float(duration_diff_s)
        ax.set_title(
            (
                f"Residual Vibration at Configuration {pose}\n"
                f"Added delay ΔT = {duration_diff_s:.2f} [s]\n"
                f"RR control on faster by = {rr_faster_by_s:.2f} [s]"
            ),
            fontsize=title_size,
        )
        if coincide_idx is not None:
            ax.legend(prop={"weight": "bold", "size": legend_size})
        else:
            ax.legend(
                ["RR control off", "RR control on", "Max acceleration peak"],
                prop={"weight": "bold", "size": legend_size},
            )
        ax.set_xlabel("Time [s]", fontsize=label_size, fontweight="bold")
        y_label = (
            "Acceleration Norm (detrended) [m/s^2]"
            if center_signals_at_zero
            else "Acceleration Norm [m/s^2]"
        )
        ax.set_ylabel(y_label, fontsize=label_size, fontweight="bold")
        ax.set_xlim(0.0, DWELL_TIME * 0.75)
        fig.tight_layout()
        VibrationTestPlots.finalize_figure(fig, f"residual_vibration_pose_{pose}")
        return (
            coincide_time_abs,
            (closest_time_abs if should_plot_pink else None),
            (int(coincide_idx) if coincide_idx is not None else None),
        )

    @staticmethod
    def plot_velocity_test_generated_trajectories(
        *,
        baseline_unshaped_trajectories: list[Mapping[str, np.ndarray]],
        shaped_only_trajectories: list[Mapping[str, np.ndarray]],
        base_axis: int,
    ) -> None:
        """Summary:
            Plot all generated velocity-test joint trajectories (position, velocity, acceleration) for RR control off/on cases.
        Args:
            baseline_unshaped_trajectories: RR control off generated trajectories with time/position/velocity/acceleration arrays.
            shaped_only_trajectories: RR control on generated trajectories with time/position/velocity/acceleration arrays.
            base_axis: Joint axis index used for plotting [-].
        Returns:
            None: No return value.
        Side Effects:
            Creates a matplotlib figure and saves/displays it using configured plotting settings.
        Raises:
            KeyError: If a required trajectory key is missing.
            IndexError: If `base_axis` is outside trajectory array bounds.
        Preconditions:
            Each trajectory mapping contains `time`, `positions`, `velocities`, and `accelerations`.
        """
        fig_traj, axes_traj = plt.subplots(3, 1, sharex=True, figsize=(10, 9))

        # Each iteration overlays one RR-control-off generated trajectory.
        for idx, traj in enumerate(baseline_unshaped_trajectories):
            t = np.asarray(traj["time"])
            axes_traj[0].plot(
                t,
                np.asarray(traj["positions"])[:, base_axis],
                color="k",
                alpha=0.55,
                linewidth=1.5,
                label="RR control off baseline" if idx == 0 else None,
            )
            axes_traj[1].plot(
                t,
                np.asarray(traj["velocities"])[:, base_axis],
                color="k",
                alpha=0.55,
                linewidth=1.5,
            )
            axes_traj[2].plot(
                t,
                np.asarray(traj["accelerations"])[:, base_axis],
                color="k",
                alpha=0.55,
                linewidth=1.5,
            )

        # Each iteration overlays one RR-control-on generated trajectory.
        for idx, traj in enumerate(shaped_only_trajectories):
            t = np.asarray(traj["time"])
            axes_traj[0].plot(
                t,
                np.asarray(traj["positions"])[:, base_axis],
                color="r",
                linestyle="--",
                alpha=0.65,
                linewidth=1.5,
                label="RR control on additional" if idx == 0 else None,
            )
            axes_traj[1].plot(
                t,
                np.asarray(traj["velocities"])[:, base_axis],
                color="r",
                linestyle="--",
                alpha=0.65,
                linewidth=1.5,
            )
            axes_traj[2].plot(
                t,
                np.asarray(traj["accelerations"])[:, base_axis],
                color="r",
                linestyle="--",
                alpha=0.65,
                linewidth=1.5,
            )

        axes_traj[0].set_title("Velocity Test Generated Trajectories (All Tests)")
        axes_traj[0].set_ylabel("Position [rad]")
        axes_traj[1].set_ylabel("Velocity [rad/s]")
        axes_traj[2].set_ylabel("Acceleration [rad/s^2]")
        axes_traj[2].set_xlabel("Time [s]")
        axes_traj[0].legend(loc="best")
        for axis_plot in axes_traj:
            axis_plot.grid(True)
        fig_traj.tight_layout()
        VibrationTestPlots.finalize_figure(
            fig_traj, "velocity_test_generated_trajectories_all_tests"
        )

    @staticmethod
    def _plot_velocity_std_sweep_axis(
        *,
        ax_obj: plt.Axes,
        x_unshaped: np.ndarray,
        x_shaped: np.ndarray,
        y_unshaped_values: np.ndarray,
        y_shaped_values: np.ndarray,
        x_label_text: str,
        title_text: str,
    ) -> None:
        """Summary:
            Draw one residual-standard-deviation sweep subplot with RR control off/on points and fit lines.
        Args:
            ax_obj: Matplotlib axis receiving the plotted artists [-].
            x_unshaped: RR control off x-series [rad/s].
            x_shaped: RR control on x-series [rad/s].
            y_unshaped_values: RR control off residual standard deviation [m/s^2].
            y_shaped_values: RR control on residual standard deviation [m/s^2].
            x_label_text: X-axis label text [-].
            title_text: Subplot title text [-].
        Returns:
            None: No return value.
        Side Effects:
            Adds markers, lines, labels, and legend to `ax_obj`.
        Raises:
            None.
        Preconditions:
            Input arrays are index-aligned per RR-control dataset.
        """
        ax_obj.plot(
            x_unshaped,
            y_unshaped_values,
            color="k",
            marker="o",
            linestyle="None",
            label="RR controller off",
        )
        ax_obj.plot(
            x_shaped,
            y_shaped_values,
            color="r",
            marker="o",
            linestyle="None",
            label="RR controller on",
        )
        valid_unshaped_exp = (
            np.isfinite(x_unshaped)
            & np.isfinite(y_unshaped_values)
            & (y_unshaped_values > 0.0)
        )
        valid_shaped_lin = np.isfinite(x_shaped) & np.isfinite(y_shaped_values)
        if np.count_nonzero(valid_unshaped_exp) >= 2:
            coeff_u = np.polyfit(
                x_unshaped[valid_unshaped_exp],
                np.log(y_unshaped_values[valid_unshaped_exp]),
                1,
            )
            ax_obj.plot(
                x_unshaped,
                float(np.exp(coeff_u[1])) * np.exp(float(coeff_u[0]) * x_unshaped),
                color="k",
                linewidth=2,
            )
        if np.count_nonzero(valid_shaped_lin) >= 2:
            coeff_s = np.polyfit(
                x_shaped[valid_shaped_lin],
                y_shaped_values[valid_shaped_lin],
                1,
            )
            ax_obj.plot(
                x_shaped,
                np.polyval(coeff_s, x_shaped),
                color="r",
                linewidth=2,
                linestyle="--",
            )
        ax_obj.set_title(title_text)
        ax_obj.set_xlabel(x_label_text)
        ax_obj.set_ylabel("Residual vibration std [m/s^2]")
        ax_obj.legend(loc="best")
        ax_obj.grid(True)

    @staticmethod
    def plot_velocity_test_std_summary(
        *,
        x_unshaped_filtered: np.ndarray,
        x_shaped_filtered: np.ndarray,
        y_unshaped_filtered: np.ndarray,
        y_shaped_filtered: np.ndarray,
        kept_unshaped_idx: np.ndarray,
        kept_shaped_idx: np.ndarray,
        unshaped_max_velocities: list[float],
        shaped_max_velocities: list[float],
        unshaped_residual_stds: list[float],
        shaped_residual_stds: list[float],
        unshaped_residual_candidates: list[dict],
        shaped_residual_candidates: list[dict],
        sample_time: float,
        center_signals_at_zero: bool = False,
    ) -> None:
        """Summary:
            Plot the 3x1 velocity-test summary figure with STD vs max velocity, encoder comparison, and residual-vibration comparison.
        Args:
            x_unshaped_filtered: Outlier-filtered RR control off velocity axis values [rad/s].
            x_shaped_filtered: Outlier-filtered RR control on velocity axis values [rad/s].
            y_unshaped_filtered: Outlier-filtered RR control off residual standard deviation values [m/s^2].
            y_shaped_filtered: Outlier-filtered RR control on residual standard deviation values [m/s^2].
            kept_unshaped_idx: Original-index mask for retained RR control off points [-].
            kept_shaped_idx: Original-index mask for retained RR control on points [-].
            unshaped_max_velocities: Raw RR control off max velocity values [rad/s].
            shaped_max_velocities: Raw RR control on max velocity values [rad/s].
            unshaped_residual_stds: Raw RR control off residual standard deviation values [m/s^2].
            shaped_residual_stds: Raw RR control on residual standard deviation values [m/s^2].
            unshaped_residual_candidates: Per-test RR control off data used for detailed subplot selection.
            shaped_residual_candidates: Per-test RR control on data used for detailed subplot selection.
            sample_time: Analysis sample period [s].
            center_signals_at_zero: If `True`, subtract moving average from
            RR control off/on residual subplot traces so they are centered
            around zero.
        Returns:
            None: No return value.
        Side Effects:
            Creates a matplotlib figure and saves/displays it using configured plotting settings.
        Raises:
            IndexError: If selected candidate indices are out of bounds.
            ValueError: If reduction metric inputs are invalid.
        Preconditions:
            Candidate arrays and raw metric arrays are aligned by test index.
        """
        fig, axes = plt.subplots(3, 1, figsize=(10, 16))
        VibrationTestPlots._plot_velocity_std_sweep_axis(
            ax_obj=axes[0],
            x_unshaped=x_unshaped_filtered,
            x_shaped=x_shaped_filtered,
            y_unshaped_values=y_unshaped_filtered,
            y_shaped_values=y_shaped_filtered,
            x_label_text="Max trajectory velocity [rad/s]",
            title_text="Residual Vibration STD vs Max Trajectory Velocity",
        )

        # Select cases for detailed time-domain comparison.
        if kept_unshaped_idx.size > 0 and kept_shaped_idx.size > 0:
            # Overlay fastest tested points to identify the highest-velocity cases.
            fastest_u_idx = int(
                np.argmax(np.asarray(unshaped_max_velocities, dtype=float))
            )
            fastest_s_idx = int(
                np.argmax(np.asarray(shaped_max_velocities, dtype=float))
            )
            selected_u = unshaped_residual_candidates[fastest_u_idx]
            selected_s = shaped_residual_candidates[fastest_s_idx]
            axes[0].scatter(
                [unshaped_max_velocities[fastest_u_idx]],
                [unshaped_residual_stds[fastest_u_idx]],
                marker="*",
                s=180,
                c="green",
                zorder=6,
                label="Highest Velocity Cases",
            )
            axes[0].scatter(
                [shaped_max_velocities[fastest_s_idx]],
                [shaped_residual_stds[fastest_s_idx]],
                marker="*",
                s=180,
                c="green",
                zorder=6,
                label=None,
            )
            axes[0].legend(loc="best")

            # Subplot 2: encoder comparison with commanded-end markers.
            time_u_full = selected_u["time"] - selected_u["time"][0]
            time_s_full = selected_s["time"] - selected_s["time"][0]
            axes[1].plot(
                time_u_full,
                selected_u["encoder_axis"],
                color="k",
                linewidth=2,
                label="RR control off",
            )
            axes[1].plot(
                time_s_full,
                selected_s["encoder_axis"],
                color="r",
                linestyle="--",
                linewidth=2,
                label="RR control on",
            )
            stop_u_s = (
                float(time_u_full[min(selected_u["idx_vib"], len(time_u_full) - 1)])
                if time_u_full.size
                else 0.0
            )
            stop_s_s = (
                float(time_s_full[min(selected_s["idx_vib"], len(time_s_full) - 1)])
                if time_s_full.size
                else 0.0
            )
            # Signed stop-time difference from encoder subplot:
            # (red vertical line time - black vertical line time).
            red_minus_black_stop_time_s = stop_s_s - stop_u_s
            axes[1].axvline(stop_u_s, color="k", linestyle=":", linewidth=2)
            axes[1].axvline(stop_s_s, color="r", linestyle=":", linewidth=2)
            axes[1].set_title("Encoder Readings (Highest Velocity Cases -> ★)")
            axes[1].set_xlabel("Time [s]")
            axes[1].set_ylabel("Encoder [rad]")
            axes[1].set_xlim(0.0, max(stop_u_s, stop_s_s) + 0.5)
            axes[1].legend(loc="best")
            axes[1].grid(True)

            # Subplot 3: residual-vibration overlay and reduction metric annotation.
            time_u_rel = (
                selected_u["time"][selected_u["idx_vib"] :]
                - selected_u["time"][selected_u["idx_vib"]]
            )
            time_s_rel = (
                selected_s["time"][selected_s["idx_vib"] :]
                - selected_s["time"][selected_s["idx_vib"]]
            )
            mag_u_rel = selected_u["mag"][selected_u["idx_vib"] :]
            mag_s_rel = selected_s["mag"][selected_s["idx_vib"] :]
            # Optional display mode: detrend residual traces for zero-centered plot.
            mag_u_rel_plot = mag_u_rel
            mag_s_rel_plot = mag_s_rel
            if center_signals_at_zero:
                mag_u_rel_plot = mag_u_rel - moving_average_signal(
                    signal=mag_u_rel,
                    window_sec=MOVING_AVG_WINDOW_SEC,
                    sample_time=sample_time,
                )
                mag_s_rel_plot = mag_s_rel - moving_average_signal(
                    signal=mag_s_rel,
                    window_sec=MOVING_AVG_WINDOW_SEC,
                    sample_time=sample_time,
                )
            axes[2].plot(
                time_u_rel,
                mag_u_rel_plot,
                color="k",
                linewidth=2,
                label="RR control off",
            )
            axes[2].plot(
                time_s_rel,
                mag_s_rel_plot,
                color="r",
                linestyle="--",
                linewidth=2,
                label="RR control on",
            )
            axes[2].scatter(
                [
                    time_u_rel[min(selected_u["peak_idx"], len(time_u_rel) - 1)],
                    time_s_rel[min(selected_s["peak_idx"], len(time_s_rel) - 1)],
                ],
                [
                    mag_u_rel_plot[min(selected_u["peak_idx"], len(mag_u_rel) - 1)],
                    mag_s_rel_plot[min(selected_s["peak_idx"], len(mag_s_rel) - 1)],
                ],
                s=80,
                c="green",
                zorder=5,
                label="Peak acceleration",
            )

            pink_rel_idx = _find_pink_line_index_from_initial_average(
                mag_acc_u=selected_u["mag"],
                mag_acc_s=selected_s["mag"],
                idx_vib_u=selected_u["idx_vib"],
                idx_vib_s=selected_s["idx_vib"],
                sample_time=sample_time,
            )
            blue_rel_idx = _find_blue_line_index_from_rms_coincidence(
                mag_acc_u=selected_u["mag"],
                mag_acc_s=selected_s["mag"],
                idx_vib_u=selected_u["idx_vib"],
                idx_vib_s=selected_s["idx_vib"],
                sample_time=sample_time,
            )
            selected_rel_idx = None
            selected_rel_source = None
            if pink_rel_idx is not None and (
                blue_rel_idx is None or pink_rel_idx <= blue_rel_idx
            ):
                selected_rel_idx = int(pink_rel_idx)
                selected_rel_source = "pink"
            elif blue_rel_idx is not None:
                selected_rel_idx = int(blue_rel_idx)
                selected_rel_source = "blue"

            rr_faster_by_s = 0.0
            metric_u_signal = mag_u_rel
            metric_s_signal = mag_s_rel
            if selected_rel_idx is not None and selected_rel_idx < len(time_u_rel):
                selected_time_s = float(time_u_rel[selected_rel_idx])
                # Requested definition:
                # rr_faster_by_s = pink_time - (red_stop - black_stop).
                rr_faster_by_s = selected_time_s - red_minus_black_stop_time_s
                metric_end = int(selected_rel_idx) + 1
                metric_u_signal = mag_u_rel[:metric_end]
                metric_s_signal = mag_s_rel[: min(metric_end, mag_s_rel.size)]
                y_mid = 0.5 * (float(np.max(mag_u_rel)) + float(np.min(mag_u_rel)))
                if selected_rel_source == "pink":
                    axes[2].axvline(
                        selected_time_s, color="deeppink", linestyle="-.", linewidth=2
                    )
                    axes[2].annotate(
                        f"  Initial Avg. acceleration \n  reached at t = {selected_time_s:.2f} [s]",
                        xy=(selected_time_s, y_mid),
                        xytext=(10, 10),
                        textcoords="offset points",
                        color="deeppink",
                    )
                else:
                    axes[2].axvline(
                        selected_time_s, color="blue", linestyle=":", linewidth=2
                    )
                    axes[2].annotate(
                        f"  Amplitude coincide \n  at t = {selected_time_s:.2f} [s]",
                        xy=(selected_time_s, y_mid),
                        xytext=(10, 10),
                        textcoords="offset points",
                        color="blue",
                    )
            reduction_metric_percent = vibration_metric_using_std(
                mag_acc_u=metric_u_signal,
                mag_acc_s=metric_s_signal,
                sample_time=sample_time,
            )
            axes[2].text(
                0.02,
                0.97,
                f"Vibration reduction = {reduction_metric_percent:.2f}%",
                transform=axes[2].transAxes,
                va="top",
                bbox={"facecolor": "white", "alpha": 0.8, "edgecolor": "none"},
            )
            axes[2].set_title(
                f"Residual Vibration (RR control on faster by = {rr_faster_by_s:.2f} [s])"
            )
            axes[2].set_xlabel("Time [s]")
            axes[2].set_ylabel(
                (
                    "Acceleration Norm (detrended) [m/s^2]"
                    if center_signals_at_zero
                    else "Acceleration Norm [m/s^2]"
                )
            )
            axes[2].legend(loc="best")
            axes[2].set_xlim(0.0, DWELL_TIME * 0.75)
            axes[2].grid(True)
        else:
            axes[1].set_title("Encoder Readings unavailable")
            axes[1].set_xlabel("Time [s]")
            axes[1].set_ylabel("Encoder [rad]")
            axes[1].grid(True)
            axes[2].set_title("Residual Vibration unavailable")
            axes[2].set_xlabel("Time [s]")
            axes[2].set_ylabel("Acceleration Norm [m/s^2]")
            axes[2].grid(True)

        fig.tight_layout()
        VibrationTestPlots.finalize_figure(
            fig, "velocity_test_std_sweep_and_worst_case_residual_vibration"
        )


vibration_test_plots = VibrationTestPlots()


def get_pose_pairs(
    local_data_location: str,
    max_disp: float,
    axis_repetitions: int | None = None,
    num_axes_to_test: int | None = None,
    num_shapers: int | None = None,
) -> list[tuple[int, int, list[float], list[float]]]:
    """Select random pose/axis pairs and build target configurations.

    Args:
        local_data_location: Folder containing calibration data CSVs.
        max_disp: Displacement to apply on the commanded axis [rad].
        axis_repetitions: Repetitions per axis for random pose/axis experiments.
            If `None`, defaults to `AXIS_REPETITIONS`.
        num_axes_to_test: Number of leading joints/axes to test. If `None`,
            falls back to `num_shapers`, then `calibration_params.axes`.
        num_shapers: Number of axes to include in the vibration-test axis sweep.
            If `None`, defaults to `calibration_params.axes`.

    Returns:
        `list[tuple[int, int, list[float], list[float]]]` of
        (pose, axis, config2, config1) tuples.

    Side Effects:
        Reads calibration files from disk.

    Raises:
        FileNotFoundError: If required CSV files are missing.

    Preconditions:
        `local_data_location` contains identification parameters and motion files.
    """
    # Find out how many poses and axes there are in the folder by reading the "identification_parameters.csv file"
    calibration_params = read_identification_parameters(data_folder=local_data_location)
    num_poses = calibration_params.nV * calibration_params.nR
    num_axes_available = int(calibration_params.axes)
    num_joints = calibration_params.num_joints

    # Couple axis-testing scope to user input when provided. Backward-compatible
    # fallback keeps current behavior via `num_shapers`, then calibration axes.
    axes_to_test = (
        int(num_axes_to_test)
        if num_axes_to_test is not None
        else (int(num_shapers) if num_shapers is not None else int(num_axes_available))
    )
    if axes_to_test <= 0:
        raise ValueError("num_axes_to_test must be a positive integer when provided")
    if axes_to_test > num_axes_available:
        raise ValueError(
            f"num_axes_to_test ({axes_to_test}) cannot exceed calibration axes "
            f"({num_axes_available})"
        )

    # Select and extract [num_axes * repetitions_per_axis] random pose/axis
    # experiments from the CSVs.
    repetitions_per_axis = (
        int(axis_repetitions)
        if (axis_repetitions is not None and int(axis_repetitions) > 0)
        else AXIS_REPETITIONS
    )

    # Select and extract [axes_to_test * repetitions_per_axis] random pose/axis
    # experiments from the CSVs.
    pose_pair_list = []
    for i in range(axes_to_test * repetitions_per_axis):
        pose_i = random.randint(0, num_poses - 1)
        axis_i = int(i % axes_to_test)
        file_i = f"{local_data_location}/robotData_motion_pose{pose_i}_axis{axis_i}.csv"
        # (TODO: how to bias towards outstretched poses?)

        # Get the initial position of the file
        _, cmd_joints, *_ = extract_dynamic_variables(
            dynamic_file=file_i, num_joints=num_joints
        )

        # Create and store a tuple pair that changes the commanded axes by [max_disp] radians
        # TODO: How do we know config2 is able to be reached?
        config1_i = copy.copy(cmd_joints[0, :])
        config2_i = copy.copy(cmd_joints[0, :])
        config2_i[axis_i] += max_disp

        pose_pair_list.append((pose_i, axis_i, list(config2_i), list(config1_i)))

    # return that list of pair
    return pose_pair_list


def plan_point_to_point_unshaped(
    start_angles: list[float],
    goal_angles: list[float],
    *,
    sample_time: float = 1 / DEFAULT_ROBOT_FREQ,
    dwell: float = DWELL_TIME,
    max_velocity: float = DEFAULT_MAX_VEL,
    max_acceleration: float = DEFAULT_MAX_ACC,
) -> Mapping[str, np.ndarray]:
    """Generate the raw point-to-point trajectory without shaping.

    Args:
        start_angles: Starting joint angles.
        goal_angles: Goal joint angles.
        sample_time: Sampling time [s].
        dwell: Dwell time after motion [s].
        max_velocity: Maximum joint velocity [rad/s].
        max_acceleration: Maximum joint acceleration [rad/s^2].

    Returns:
        `Mapping[str, np.ndarray]` with keys `positions`, `velocities`,
        `accelerations`, and `time`.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `start_angles` and `goal_angles` have the same length.
    """

    displacement = np.max(np.abs(np.array(goal_angles) - np.array(start_angles)))
    t_params = TrajParams(
        max_displacement=float(displacement),
        max_velocity=float(max_velocity),
        max_acceleration=float(max_acceleration),
    )
    s_params = SystemIdParams(nV=1, nR=1)

    # Generate per-axis trajectories using the Python trajectory generator
    trajectory_gen = Trajectory(t_params=t_params, s_params=s_params)
    joint_trajectories = []
    joint_velocities = []
    joint_accelerations = []
    time_vectors = []

    for start_val, goal_val in zip(start_angles, goal_angles):
        # Store directionality, compute point-to-point on the displacement,
        # then add directionality back in after
        direction = np.sign(goal_val - start_val) or 1.0
        displacement_axis = abs(goal_val - start_val)

        qj: np.ndarray
        qj_dot: np.ndarray
        qj_ddot: np.ndarray
        tj: np.ndarray

        if displacement_axis <= 1e-9:
            # Degenerate move: stay at start position.
            qj = np.asarray([start_val])
            tj = np.asarray([0.0])
            qj_dot = np.zeros_like(qj)
            qj_ddot = np.zeros_like(qj)
        else:
            qj, qj_dot, qj_ddot, tj = trajectory_gen.point_to_point_motion(
                feedrate=max_velocity,
                acc_limit=max_acceleration,
                dec_limit=-1 * max_acceleration,
                displacement=displacement_axis,
                Ts=sample_time,
                dwell=dwell,
            )
            qj = start_val + direction * qj
            qj_dot = np.gradient(qj, sample_time)
            qj_ddot = np.gradient(qj_dot, sample_time)

        joint_trajectories.append(qj)
        joint_velocities.append(qj_dot)
        joint_accelerations.append(qj_ddot)
        time_vectors.append(tj)

    max_samples = max(len(q) for q in joint_trajectories)
    common_time = np.linspace(0.0, (max_samples - 1) * sample_time, max_samples)

    def _resample(series_list: list[np.ndarray]) -> np.ndarray:
        """Resample per-axis series onto a common time base.

        Args:
            series_list: One series per joint axis.
        Returns:
            `np.ndarray` stacked as `(N, num_axes)` on `common_time`.
        Side Effects:
            None.
        Raises:
            ValueError: If `series_list` length mismatches `time_vectors`.
        Preconditions:
            Each series aligns with its corresponding entry in `time_vectors`.
        """
        if len(series_list) != len(time_vectors):
            raise ValueError("series_list and time_vectors must have matching length")
        resampled = []
        for arr, t_vec in zip(series_list, time_vectors):
            if len(arr) == max_samples:
                resampled.append(arr)
            else:
                resampled.append(np.interp(common_time, t_vec, arr))
        return np.stack(resampled, axis=1)

    positions = _resample(joint_trajectories)
    velocities = _resample(joint_velocities)
    accelerations = _resample(joint_accelerations)

    return {
        "positions": positions,
        "velocities": velocities,
        "accelerations": accelerations,
        "time": common_time,
    }


def shape_stream(
    shaper: Any,
    commands: np.ndarray,
    times: np.ndarray,
) -> tuple[list, list, list]:
    """Shape a command stream sample-by-sample.

    Args:
        shaper: Shaper instance.
        commands: Commanded joint angles over time.
        times: Time vector aligned with `commands`.

    Returns:
        `tuple[list, list, list]` of (positions, velocities, accelerations).

    Side Effects:
        Advances the shaper state.

    Raises:
        None.

    Preconditions:
        `commands` and `times` have matching length.
    """
    positions = []
    velocities = []
    accelerations = []
    last_time = times[0] if len(times) else 0.0

    for i, cmd in enumerate(commands):
        state = covalent.RobotState()
        state.joint_angles = cmd
        sample = shaper.shape_sample(cmd, state)
        positions.append((times[i], sample.positions))
        velocities.append((times[i], sample.velocities))
        accelerations.append((times[i], sample.accelerations))
        last_time = times[i]

    for tail_idx, tail in enumerate(shaper.finalize()):
        t = last_time + (tail_idx + 1) * shaper.sample_time
        positions.append((t, tail.positions))
        velocities.append((t, tail.velocities))
        accelerations.append((t, tail.accelerations))
    return positions, velocities, accelerations


def shape_batch(
    commands: np.ndarray,
    times: np.ndarray,
    shaper: Any,
    *,
    enable_vibration_control: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """Shape a full trajectory in batch mode.

    Args:
        commands: Commanded joint angles over time.
        times: Time vector aligned with `commands`.
        shaper: Shaper instance.
        enable_vibration_control: Toggle vibration shaping for the full call.

    Returns:
        `tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]` containing time,
        positions, velocities, and accelerations.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `commands` and `times` have matching length.
    """

    states = []
    for cmd in commands:
        st = covalent.RobotState()
        st.joint_angles = cmd
        states.append(st)
    traj = shaper.process_trajectory(
        commands,
        states,
        time_vector=list(times),
        enable_vibration_control=enable_vibration_control,
    )
    return traj.time, traj.positions, traj.velocities, traj.accelerations


def shape_point_to_point_trajectory(
    robot_interface: RobotInterface,
    unshaped_trajectory: Mapping[str, np.ndarray],
    sample_time: float,
    axes_to_shape: int,
    side_length: float,
    initial_height: float,
) -> Mapping[str, np.ndarray]:
    """Compute a shaped trajectory for a point-to-point move.

    Args:
        robot_interface: Robot interface used for kinematics.
        unshaped_trajectory: Raw trajectory mapping.
        sample_time: Sampling time [s].
        axes_to_shape: Number of axes to shape.
        side_length: Side arm length [m].
        initial_height: Initial height [m].

    Returns:
        `Mapping[str, np.ndarray]` containing shaped trajectory fields.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `unshaped_trajectory` contains required keys.
    """
    shaped_positions = np.zeros_like(unshaped_trajectory["positions"])
    shaped_velocities = np.zeros_like(unshaped_trajectory["velocities"])
    shaped_accelerations = np.zeros_like(unshaped_trajectory["positions"])

    shaper_ = covalent.ShaperInterface(
        sample_time=sample_time,
        model_directory=str(MODEL_DIR),
        python_src_root=str(PYTHON_SRC_ROOT),
        urdf_filepath=robot_interface.urdf_path,
        side_length=side_length,
        base_height=initial_height,
        num_axes=axes_to_shape,
        num_joints=robot_interface.num_joints,
    )

    time, shaped_positions, shaped_velocities, shaped_accelerations = shape_batch(
        commands=unshaped_trajectory["positions"],
        times=unshaped_trajectory["time"],
        shaper=shaper_,
    )

    return {
        "positions": shaped_positions,
        "velocities": shaped_velocities,
        "accelerations": shaped_accelerations,
        "time": time,
    }


class CompareResult(TypedDict):
    shaped_duration: float
    unshaped_duration: float
    shaped_log: Deque[dict]
    unshaped_log: Deque[dict]
    shaped_trajectory: Mapping[str, np.ndarray]
    unshaped_trajectory: Mapping[str, np.ndarray]


def generate_stream_shaped_unshaped_trajecotry(
    robot_interface: RobotInterface,
    start_angles: list[float],
    goal_angles: list[float],
    identification_file: str,
    moved_axis: int | None = None,
    *,
    sample_time: float = 1 / DEFAULT_ROBOT_FREQ,
    dwell: float = DWELL_TIME,
    max_velocity: float = DEFAULT_MAX_VEL,
    max_acceleration: float = DEFAULT_MAX_ACC,
    execute_unshaped_on_robot: bool = True,
    execute_shaped_on_robot: bool = True,
    num_shapers: int | None = None,
) -> CompareResult:
    """Summary:
        Build unshaped and optional shaped point-to-point trajectories, stream the selected trajectories to the robot, and return trajectories, durations, and stream logs.
    Args:
        robot_interface: Connected robot interface object [-].
        start_angles: Start joint vector [rad].
        goal_angles: Goal joint vector [rad].
        identification_file: Folder path containing identification parameters [-].
        moved_axis: Joint index used by downstream plotting logic [-].
        sample_time: Trajectory sample period [s].
        dwell: Pause inserted before stream phases [s].
        max_velocity: Trajectory velocity bound [rad/s].
        max_acceleration: Trajectory acceleration bound [rad/s^2].
        execute_unshaped_on_robot: If True, stream unshaped trajectory to hardware [-].
        execute_shaped_on_robot: If True, build and stream shaped trajectory to hardware [-].
        num_shapers: Number of axes/shapers to use when shaping. If `None`, uses
            `calibration_params.axes` from identification parameters [-].
    Returns:
        CompareResult:
        shaped_duration: shaped trajectory duration [s].
        unshaped_duration: unshaped trajectory duration [s].
        shaped_log: shaped stream log entries [-].
        unshaped_log: unshaped stream log entries [-].
        shaped_trajectory: shaped trajectory mapping with time/position/velocity/acceleration.
        unshaped_trajectory: unshaped trajectory mapping with time/position/velocity/acceleration.
    Side Effects:
        Reads identification files, prints status to stdout, moves the robot, and publishes trajectory streams.
    Raises:
        ValueError: If start/goal vectors do not match robot joint count.
    Preconditions:
        Robot connection is active, identification files exist, and angle vectors use robot joint ordering.
    """

    if (
        len(start_angles) != robot_interface.num_joints
        or len(goal_angles) != robot_interface.num_joints
    ):
        raise ValueError(
            f"Expected {robot_interface.num_joints} joint values for start and goal"
        )

    # Load shaping/calibration parameters once per comparison call.
    calibration_params = read_identification_parameters(data_folder=identification_file)
    # Resolve effective shaper count:
    # - explicit CLI override when provided
    # - otherwise use calibration-derived axis count from identification files.
    axes_to_shape = (
        int(num_shapers) if num_shapers is not None else int(calibration_params.axes)
    )
    # Guardrails keep shaping configuration physically meaningful and
    # consistent with the robot model dimensionality.
    if axes_to_shape <= 0:
        raise ValueError("num_shapers must be a positive integer")
    if axes_to_shape > robot_interface.num_joints:
        raise ValueError(
            f"num_shapers ({axes_to_shape}) cannot exceed num_joints "
            f"({robot_interface.num_joints})"
        )
    unshaped = plan_point_to_point_unshaped(
        start_angles=start_angles,
        goal_angles=goal_angles,
        sample_time=sample_time,
        dwell=dwell,
        max_velocity=max_velocity,
        max_acceleration=max_acceleration,
    )

    # Build the shaped trajectory only when it will actually be executed.
    shaped: Mapping[str, np.ndarray]
    if execute_shaped_on_robot:
        print("Completed unshaped trajectory. Building shaped trajectory.")
        shaped = shape_point_to_point_trajectory(
            robot_interface=robot_interface,
            unshaped_trajectory=unshaped,
            sample_time=sample_time,
            axes_to_shape=axes_to_shape,
            side_length=calibration_params.shoulder_len,
            initial_height=calibration_params.base_height,
        )
    else:
        print("Completed unshaped trajectory. Skipping shaped trajectory build.")
        empty_joint_matrix = np.empty((0, robot_interface.num_joints), dtype=float)
        shaped = {
            "positions": empty_joint_matrix,
            "velocities": empty_joint_matrix.copy(),
            "accelerations": empty_joint_matrix.copy(),
            "time": np.empty((0,), dtype=float),
        }

    # Convert ndarray trajectories to list payloads expected by stream publisher.
    unshaped_payload = {
        "time": unshaped["time"].tolist(),
        "positions": unshaped["positions"].tolist(),
        "velocities": unshaped["velocities"].tolist(),
        "accelerations": unshaped["accelerations"].tolist(),
        "Ts": _derive_ts(unshaped["time"], sample_time),
    }
    shaped_payload = {
        "time": shaped["time"].tolist(),
        "positions": shaped["positions"].tolist(),
        "velocities": shaped["velocities"].tolist(),
        "accelerations": shaped["accelerations"].tolist(),
        "Ts": _derive_ts(shaped["time"], sample_time),
    }

    # Implementation note (Codex): print commanded span so "published but no
    # motion" cases can be diagnosed quickly from the terminal output.
    commanded_delta = np.array(goal_angles) - np.array(start_angles)
    print(
        "[StandardRobot] Commanded joint delta [rad]: "
        + np.array2string(commanded_delta, precision=4)
    )

    # Pre-allocate output logs so return type is stable in all execution modes.
    shaped_log: Deque[dict] = deque()
    unshaped_log: Deque[dict] = deque()

    # Here is when the robot is commanded to move

    # Execute requested trajectory streams only on live robot backends.
    if not robot_interface.in_sim_mode and hasattr(robot_interface, "robot"):
        start_tuple = tuple(start_angles)
        print("[StandardRobot] Moving to start configuration before comparison run...")
        robot_interface.move_to_joint(target_joint=start_tuple)

        # Unshaped execution path.
        if execute_unshaped_on_robot and unshaped_payload["positions"]:
            time.sleep(dwell)
            print("[StandardRobot] Streaming unshaped trajectory...")
            unshaped_log = robot_interface.publish_and_record_joint_positions(
                time_data=unshaped_payload["time"],
                position_stream=unshaped_payload["positions"],
                velocity_stream=unshaped_payload["velocities"],
                acceleration_stream=unshaped_payload["accelerations"],
                Ts=unshaped_payload["Ts"],
            )

        # Re-home before shaped run so both streams share the same start state.
        if execute_shaped_on_robot:
            print(
                "[StandardRobot] Returning to start angles before shaped trajectory..."
            )
            robot_interface.move_to_joint(target_joint=start_tuple)
            # Wait after motion to send next command
            time.sleep(dwell)

        # Shaped execution path.
        if execute_shaped_on_robot and shaped_payload["positions"]:
            print("[StandardRobot] Streaming shaped trajectory...")
            shaped_log = robot_interface.publish_and_record_joint_positions(
                time_data=shaped_payload["time"],
                position_stream=shaped_payload["positions"],
                velocity_stream=shaped_payload["velocities"],
                acceleration_stream=shaped_payload["accelerations"],
                Ts=shaped_payload["Ts"],
            )

    # TO-DO: Store information about trajectories that will help us compute the results
    # ex: time when the trajectory ends and the dwell begins, etc.
    return {
        "shaped_duration": float(shaped["time"][-1]) if shaped["time"].size else 0.0,
        "unshaped_duration": float(unshaped["time"][-1]),
        "shaped_log": shaped_log,
        "unshaped_log": unshaped_log,
        "shaped_trajectory": shaped,
        "unshaped_trajectory": unshaped,
    }


def _derive_ts(
    time_vector: np.ndarray,
    fallback: float,
) -> float:
    """Summary:
        Compute a positive sampling period from timestamps and return `fallback` when estimation is invalid.
    Args:
        time_vector: Timestamp vector [s].
        fallback: Fallback sampling period [s].
    Returns:
        float: Estimated sampling period [s].
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        `time_vector` is ordered in time and `fallback > 0`.
    """
    # Use positive deltas only to avoid zero/negative timestamp artifacts.
    if time_vector.size > 1:
        deltas = np.diff(time_vector)
        mean_dt = (
            float(np.mean(deltas[deltas > 0]))
            if np.any(deltas > 0)
            else float(fallback)
        )
        if mean_dt > 0.0:
            return mean_dt
    return float(fallback)


def _build_uniform_time(
    num_points: int,
    sample_time: float,
) -> np.ndarray:
    """Summary:
        Build and return a uniformly sampled time vector `[0, Ts, 2*Ts, ...]`.
    Args:
        num_points: Number of output samples [-].
        sample_time: Sampling period [s].
    Returns:
        np.ndarray: Uniform time vector with shape `(num_points,)` [s].
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        `num_points >= 0` and `sample_time > 0`.
    """
    # Return an empty vector for empty signals to keep caller math safe.
    if num_points <= 0:
        return np.zeros(0, dtype=float)
    return np.arange(num_points, dtype=float) * float(sample_time)


def moving_average_signal(
    signal: np.ndarray,
    window_sec: float,
    sample_time: float,
) -> np.ndarray:
    """Summary:
        Compute and return a moving-average signal for 1D or 2D input samples.
    Args:
        signal: Input samples with shape `(N,)` or `(N, M)` [signal units].
        window_sec: Moving-average window duration [s].
        sample_time: Sampling period [s].
    Returns:
        np.ndarray: Smoothed signal with the same shape as `signal` [signal units].
    Side Effects:
        None.
    Raises:
        ValueError: If `sample_time <= 0` or `signal.ndim` is not 1 or 2.
    Preconditions:
        Samples are ordered in time.
    """
    if sample_time <= 0.0:
        raise ValueError("sample_time must be positive")

    # Normalize input type once so shape checks and convolution are consistent.
    signal_array = np.asarray(signal)
    if signal_array.ndim not in (1, 2):
        raise ValueError("signal must be 1D or 2D")

    # `window_n`: moving-average kernel length in samples.
    window_n = max(1, int(round(window_sec / sample_time)))
    # Keep kernel length <= signal length so `mode="same"` preserves input length.
    max_window = signal_array.shape[0]
    window_n = min(window_n, max_window) if max_window > 0 else 1
    kernel = np.ones(window_n, dtype=float) / float(window_n)

    # For 2D signals, each iteration smooths one axis/column independently.
    if signal_array.ndim == 2:
        return np.vstack(
            [
                np.convolve(signal_array[:, i], kernel, mode="same")
                for i in range(signal_array.shape[1])
            ]
        ).T
    return np.convolve(signal_array, kernel, mode="same")


def vibration_metric_using_std(
    mag_acc_u: np.ndarray,
    mag_acc_s: np.ndarray,
    sample_time: float,
    moving_avg_window_sec: float = MOVING_AVG_WINDOW_SEC,
) -> float:
    """Summary:
        Calculate percent vibration reduction from standard deviation of moving-average residuals.
    Args:
        mag_acc_u: RR control off acceleration norm sequence [m/s^2].
        mag_acc_s: RR control on acceleration norm sequence [m/s^2].
        sample_time: Sampling period [s].
        moving_avg_window_sec: Residual extraction window duration [s].
    Returns:
        float: Vibration reduction percentage [%].
    Side Effects:
        None.
    Raises:
        ValueError: If `sample_time <= 0` or either signal is empty.
    Preconditions:
        Signals are time-aligned over the same interval.
    """
    if sample_time <= 0.0:
        raise ValueError("sample_time must be positive")
    if mag_acc_u.size == 0 or mag_acc_s.size == 0:
        raise ValueError("signals must be non-empty")

    # Compute residuals around local trend before comparing vibration magnitude.
    mag_acc_u_moving_avg = moving_average_signal(
        signal=mag_acc_u,
        window_sec=moving_avg_window_sec,
        sample_time=sample_time,
    )
    mag_acc_s_moving_avg = moving_average_signal(
        signal=mag_acc_s,
        window_sec=moving_avg_window_sec,
        sample_time=sample_time,
    )
    mag_acc_u_residual = mag_acc_u - mag_acc_u_moving_avg
    mag_acc_s_residual = mag_acc_s - mag_acc_s_moving_avg

    std_u = float(np.std(mag_acc_u_residual))
    std_s = float(np.std(mag_acc_s_residual))
    if std_u <= 0.0:
        return 0.0
    return 100.0 * (std_u - std_s) / std_u


def settled_index_from_final_position(
    encoder_axis: np.ndarray,
    position_tolerance: float = 5e-4,
) -> int:
    """Summary:
        Find and return the first index where the encoder remains within final-position tolerance through the last sample.
    Args:
        encoder_axis: Joint encoder trajectory with shape `(N,)` [rad].
        position_tolerance: Allowed deviation from final position [rad].
    Returns:
        int: Settling index in `encoder_axis` [-].
    Side Effects:
        None.
    Raises:
        ValueError: If `encoder_axis` is empty.
    Preconditions:
        `encoder_axis` is one-dimensional and time ordered.
    """
    # Flatten to a single axis signal so subsequent boolean logic is robust.
    axis_values = np.asarray(encoder_axis, dtype=float).reshape(-1)
    if axis_values.size == 0:
        raise ValueError("encoder_axis cannot be empty")

    final_pos = axis_values[-1]
    within_tol = np.abs(axis_values - final_pos) <= float(position_tolerance)
    # `stable_from_here[i]` is True only when all samples from i..end stay in tolerance.
    stable_from_here = np.logical_and.accumulate(within_tol[::-1])[::-1]
    settled_candidates = np.where(stable_from_here)[0]
    if settled_candidates.size == 0:
        return int(axis_values.size - 1)
    return int(settled_candidates[0])


def first_index_of_final_command_value(
    command_position_axis: np.ndarray,
) -> int:
    """Summary:
        Find and return the first index where the commanded trajectory equals its final commanded value.
    Args:
        command_position_axis: Commanded joint position trajectory with shape `(N,)` [rad].
    Returns:
        int: First index where `command_position_axis[i] == command_position_axis[-1]` [-].
    Side Effects:
        None.
    Raises:
        ValueError: If `command_position_axis` is empty.
    Preconditions:
        `command_position_axis` is one-dimensional.
    """
    command_position_values = np.asarray(command_position_axis, dtype=float).reshape(-1)
    if command_position_values.size == 0:
        raise ValueError("command_position_axis cannot be empty")

    final_command_value = command_position_values[-1]
    first_occurrence_indices = np.where(command_position_values == final_command_value)[
        0
    ]
    if first_occurrence_indices.size == 0:
        return int(command_position_values.size - 1)
    return int(first_occurrence_indices[0])


def rotate_and_compensate_acc(
    robot_interface: RobotInterface,
    time_vec: np.ndarray,
    encoder: np.ndarray,
    tcp_acc: np.ndarray,
    sample_time: float,
) -> tuple[np.ndarray, float, np.ndarray]:
    """Summary:
        Convert tool-frame accelerations to base frame and return bias-compensated acceleration with estimated gravity and bias vectors.
    Args:
        robot_interface: Robot interface used to compute kinematic transforms [-].
        time_vec: Timestamp vector with shape `(N,)` [s].
        encoder: Joint trajectory with shape `(N, num_joints)` [rad].
        tcp_acc: Tool-frame acceleration with shape `(N, 3)` [m/s^2].
        sample_time: Sampling period [s].
    Returns:
        tuple[np.ndarray, float, np.ndarray]:
        1) bias_compensated_acc_base: Base-frame acceleration `(N, 3)` [m/s^2].
        2) gravity_z: Estimated gravity component in base Z [m/s^2].
        3) bias_xyz: Estimated static XYZ bias `(3,)` [m/s^2].
    Side Effects:
        Calls robot kinematic model functions.
    Raises:
        RuntimeError: If transformation matrix computation fails in robot model.
    Preconditions:
        `time_vec`, `encoder`, and `tcp_acc` are aligned and share sample count `N`.
    """
    # Allocate output in base frame with same sample count as input.
    acc_base = np.zeros_like(tcp_acc)
    # Each iteration rotates one tool-frame sample into the base frame.
    for i in range(tcp_acc.shape[0]):
        T_tool = robot_interface.model.get_transformation_matrix(encoder[i, :])
        R_base_tool = T_tool[:3, :3]
        acc_base[i, :] = R_base_tool @ tcp_acc[i, :]

    # Estimate static bias from the last second, where motion should be settled.
    last_start = time_vec[-1] - 1.0
    last_mask = time_vec >= last_start
    if not np.any(last_mask):
        # Fallback when timestamps are degenerate: use last ~1 s by sample count.
        last_n = max(1, int(round(1.0 / sample_time)))
        last_mask = np.arange(tcp_acc.shape[0]) >= tcp_acc.shape[0] - last_n
    bias_xyz = np.mean(acc_base[last_mask, :], axis=0)
    gravity_z = bias_xyz[2]
    # Remove estimated static offset (including gravity in base Z).
    acc_base = acc_base - bias_xyz
    return acc_base, gravity_z, bias_xyz


def _one_sided_fft(
    signal: np.ndarray,
    sample_time: float,
) -> tuple[np.ndarray, np.ndarray]:
    """Compute one-sided FFT magnitude with amplitude normalization.

    Args:
        signal: Time-domain signal samples.
        sample_time: Sampling period [s].
    Returns:
        `tuple[np.ndarray, np.ndarray]`:
        (one-sided frequency bins [Hz], one-sided amplitude spectrum).
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        `sample_time > 0` and `signal` is one-dimensional.
    """
    # Compute one-sided magnitude spectrum for positive frequencies only.
    n = signal.size
    fft_vals = fft(signal)
    freqs = fftfreq(n, sample_time)
    # `half`: one-sided FFT cutoff index for positive frequencies.
    half = n // 2
    freqs = freqs[:half]
    amps = np.abs(fft_vals[:half]) / float(n)
    if n > 1:
        amps[1:] *= 2.0
    return freqs, amps


def _find_blue_line_index_from_rms_coincidence(
    mag_acc_u: np.ndarray,
    mag_acc_s: np.ndarray,
    idx_vib_u: int,
    idx_vib_s: int,
    sample_time: float,
    window_sec: float = 0.2,
    tol_rel: float = 0.4,
    min_duration_sec: float = 0.125,
) -> int | None:
    """Find residual-window coincidence index using the same blue-line logic.

    Args:
        mag_acc_u: Unshaped acceleration norm [m/s^2].
        mag_acc_s: Shaped acceleration norm [m/s^2].
        idx_vib_u: Unshaped residual-window start index.
        idx_vib_s: Shaped residual-window start index.
        sample_time: Sampling period [s].
        window_sec: RMS moving-window duration [s].
        tol_rel: Relative RMS coincidence tolerance [0..1].
        min_duration_sec: Minimum consecutive coincidence duration [s].
    Returns:
        `int | None` coincidence index relative to residual window start.
    Side Effects:
        None.
    Raises:
        ValueError: If sample time is invalid.
    Preconditions:
        Input vectors are valid and indices are in-range.
    """
    if sample_time <= 0.0:
        raise ValueError("sample_time must be positive")

    # Mirror run_vibration_test coincidence logic so windowing is consistent.
    window_n = max(1, int(round(window_sec / sample_time)))
    min_len = max(1, int(round(min_duration_sec / sample_time)))

    rms_unshaped = np.sqrt(
        np.convolve(
            mag_acc_u[idx_vib_u:] ** 2,
            np.ones(window_n) / window_n,
            mode="same",
        )
    )
    rms_shaped = np.sqrt(
        np.convolve(
            mag_acc_s[idx_vib_s:] ** 2,
            np.ones(window_n) / window_n,
            mode="same",
        )
    )

    common_len = min(rms_unshaped.size, rms_shaped.size)
    if common_len <= 0:
        return None
    rms_unshaped = rms_unshaped[:common_len]
    rms_shaped = rms_shaped[:common_len]

    rms_diff = np.abs(rms_unshaped - rms_shaped)
    rms_ref = np.maximum(rms_unshaped, rms_shaped)
    coincidence_mask = rms_diff <= (tol_rel * rms_ref)

    # Each iteration checks for the first stable coincidence run.
    for i in range(coincidence_mask.size - min_len + 1):
        if np.all(coincidence_mask[i : i + min_len]):
            return int(i)
    return None


def _find_pink_line_index_from_initial_average(
    mag_acc_u: np.ndarray,
    mag_acc_s: np.ndarray,
    idx_vib_u: int,
    idx_vib_s: int,
    sample_time: float,
    window_sec: float = 0.2,
    consecutive_windows: int = 2,
) -> int | None:
    """Summary:
        Compute and return the residual-window index where RR control off initial-average amplitude matches RR control on reference behavior (pink-line criterion).
    Args:
        mag_acc_u: RR control off acceleration norm sequence [m/s^2].
        mag_acc_s: RR control on acceleration norm sequence [m/s^2].
        idx_vib_u: RR control off residual-window start index [-].
        idx_vib_s: RR control on residual-window start index [-].
        sample_time: Sampling period [s].
        window_sec: Moving-average window length [s].
        consecutive_windows: Required consecutive threshold windows [-].
    Returns:
        int | None: Residual-window index for pink-line placement [-], or `None` when unavailable.
    Side Effects:
        None.
    Raises:
        ValueError: If `sample_time <= 0`.
    Preconditions:
        Residual-window start indices are valid for the provided sequences.
    """
    if sample_time <= 0.0:
        raise ValueError("sample_time must be positive")

    # Extract residual windows beginning at "motion settled" indices.
    unshaped_residual = mag_acc_u[idx_vib_u:]
    shaped_residual = mag_acc_s[idx_vib_s:]
    if unshaped_residual.size == 0 or shaped_residual.size == 0:
        return None

    window_n = max(1, int(round(window_sec / sample_time)))
    window_n = min(window_n, unshaped_residual.size, shaped_residual.size)
    if window_n <= 0:
        return None

    unshaped_ma = np.convolve(
        unshaped_residual,
        np.ones(window_n) / window_n,
        mode="same",
    )
    shaped_window_avg = float(np.mean(shaped_residual[:window_n]))
    reach_mask = unshaped_ma <= shaped_window_avg

    start_idx = 0
    if np.any(reach_mask):
        window_req = max(1, int(consecutive_windows))
        for i in range(reach_mask.size - window_req + 1):
            if np.all(reach_mask[i : i + window_req]):
                start_idx = i
                break

    shaped_peak_idx = int(np.argmax(np.abs(shaped_residual)))
    shaped_peak_value = shaped_residual[shaped_peak_idx]
    if start_idx >= unshaped_residual.size:
        return None

    closest_idx = start_idx + int(
        np.argmin(np.abs(unshaped_residual[start_idx:] - shaped_peak_value))
    )
    return int(closest_idx)


def _remove_outliers_iqr(
    x_values: np.ndarray,
    y_values: np.ndarray,
    series_name: str,
) -> tuple[np.ndarray, np.ndarray]:
    """Summary:
        Remove Interquartile Range (IQR) outliers from paired x/y arrays and return filtered arrays.
    Args:
        x_values: X-axis samples [plot units].
        y_values: Y-axis samples [m/s^2].
        series_name: Series label used in printed messages [-].
    Returns:
        tuple[np.ndarray, np.ndarray]:
        1) x_filtered: Filtered x samples [plot units].
        2) y_filtered: Filtered y samples [m/s^2].
    Side Effects:
        Prints outlier-removal counts to stdout.
    Raises:
        None.
    Preconditions:
        `x_values` and `y_values` are index-aligned.
    """
    x_arr = np.asarray(x_values, dtype=float)
    y_arr = np.asarray(y_values, dtype=float)
    finite_mask = np.isfinite(x_arr) & np.isfinite(y_arr)
    if np.count_nonzero(finite_mask) < 4:
        return x_arr[finite_mask], y_arr[finite_mask]
    y_finite = y_arr[finite_mask]
    q1, q3 = np.percentile(y_finite, [25, 75])
    iqr = q3 - q1
    if iqr <= 0.0:
        return x_arr[finite_mask], y_arr[finite_mask]
    lower = q1 - 1.5 * iqr
    upper = q3 + 1.5 * iqr
    keep_mask = finite_mask & (y_arr >= lower) & (y_arr <= upper)
    removed = int(np.count_nonzero(finite_mask) - np.count_nonzero(keep_mask))
    if removed > 0:
        print(
            f"[StandardRobot] Outlier removal ({series_name}): removed {removed} point(s)."
        )
    return x_arr[keep_mask], y_arr[keep_mask]


def _iqr_keep_indices(
    y_values: np.ndarray,
) -> np.ndarray:
    """Summary:
        Compute and return indices retained by Interquartile Range (IQR) filtering
        on one y-series.
    Args:
        y_values: Y-axis samples [m/s^2].
    Returns:
        np.ndarray: Integer indices retained after filtering [-].
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        `y_values` is one-dimensional.
    """
    y_arr = np.asarray(y_values, dtype=float)
    finite_mask = np.isfinite(y_arr)
    keep_mask = finite_mask.copy()
    if np.count_nonzero(finite_mask) >= 4:
        y_finite = y_arr[finite_mask]
        q1, q3 = np.percentile(y_finite, [25, 75])
        iqr = q3 - q1
        if iqr > 0.0:
            lower = q1 - 1.5 * iqr
            upper = q3 + 1.5 * iqr
            keep_mask &= (y_arr >= lower) & (y_arr <= upper)
    return np.where(keep_mask)[0]


def _residual_std_in_fixed_window(
    mag_acc_norm: np.ndarray,
    idx_vib_start: int,
    window_samples: int,
    sample_time: float,
) -> float:
    """Summary:
        Compute residual-vibration standard deviation on a fixed-length window that starts at a given residual-window index.
    Args:
        mag_acc_norm: Acceleration norm signal [m/s^2].
        idx_vib_start: Residual-window start index [-].
        window_samples: Fixed number of samples in the analysis window [-].
        sample_time: Sampling period used for moving-average detrending [s].
    Returns:
        float: Residual-vibration standard deviation [m/s^2].
    Side Effects:
        None.
    Raises:
        None.
    Preconditions:
        `mag_acc_norm` is one-dimensional and `idx_vib_start` is a valid index.
    """
    idx_window_end_exclusive = min(
        mag_acc_norm.shape[0],
        idx_vib_start + window_samples,
    )
    mag_acc_window = mag_acc_norm[idx_vib_start:idx_window_end_exclusive]
    if mag_acc_window.size == 0:
        return 0.0
    mag_acc_window_moving_avg = moving_average_signal(
        signal=mag_acc_window,
        window_sec=MOVING_AVG_WINDOW_SEC,
        sample_time=sample_time,
    )
    return float(np.std(mag_acc_window - mag_acc_window_moving_avg))


def store_vibration_test_data(
    robot_interface: RobotInterface,
    data_log: Deque[dict],
    pose_number: int,
    axis_number: int,
    label: str,
) -> None:
    """Summary:
        Write recorder motion data to CSV files for one pose-axis experiment label.
    Args:
        robot_interface: Robot interface with active recorder object [-].
        data_log: Recorded motion entries deque [-].
        pose_number: Pose index used in output filename [-].
        axis_number: Axis index used in output filename [-].
        label: Output subfolder label [-].
    Returns:
        None: No return value.
    Side Effects:
        Mutates `robot_interface.recorder`, prints status, and writes CSV files to disk.
    Raises:
        OSError: If output files cannot be written.
    Preconditions:
        Recorder is initialized and `data_log` entries match recorder schema.
    """
    # Skip file generation when no samples were captured for this experiment.
    if not data_log:
        print(
            f"[StandardRobot] Skipping data store for label '{label}' (empty data log)."
        )
        return

    # Rebuild recorder buffers from stream log before writing CSV outputs.
    robot_interface.recorder.reset()

    while data_log:
        log_entry = data_log.popleft()
        robot_interface.process_motion_data(entry=log_entry)

    vibration_data_folder = f"{VIBRATION_DATA_FOLDER}/{label}"
    store_recorder_data_in_data_folder(
        recorder=robot_interface.recorder,
        run_index=pose_number,
        move_axis=axis_number,
        data_folder=vibration_data_folder,
    )


def run_vibration_test(
    robot_interface: RobotInterface,
    local_data_location: str,
    Ts: float,
    max_disp: float = DEFAULT_MAX_DISP,
    axis_repetitions: int | None = None,
    num_axes_to_test: int | None = None,
    num_shapers: int | None = None,
) -> None:
    """Run vibration tests and store shaped/unshaped results.

    Args:
        robot_interface: Robot interface used to execute trajectories.
        local_data_location: Folder containing calibration data.
        Ts: Sampling time [s].
        max_disp: Maximum displacement [rad].
        axis_repetitions: Repetitions per axis for random pose/axis experiments.
            If `None`, defaults to `AXIS_REPETITIONS`.
        num_axes_to_test: Number of leading joints/axes to test. Example:
            `2` tests base+shoulder; `3` tests base+shoulder+elbow.
            If `None`, falls back to `num_shapers`, then `calibration_params.axes`.
        num_shapers: Number of axes/shapers to use when building shaped trajectories.
            If `None`, uses `calibration_params.axes` from identification data.

    Returns:
        `None`.

    Side Effects:
        Commands robot motion and writes test data to disk.

    Raises:
        RuntimeError: If robot motion fails.

    Preconditions:
        Robot connection is active and calibration data exists.
    """

    # Get the random poses that we want to test from the data folder location
    # Compute their pair location based on the axis that was moved in the system ID
    pose_pairs = get_pose_pairs(
        local_data_location=local_data_location,
        max_disp=max_disp * 2,
        axis_repetitions=axis_repetitions,
        num_axes_to_test=num_axes_to_test,
        num_shapers=num_shapers,
    )

    pose_axis_pairs: list[tuple[int, int]] = []

    # Loop 1: execute all experiments and store shaped/unshaped CSV files.
    for _, (pose, axis, start, end) in enumerate(pose_pairs):

        response = generate_stream_shaped_unshaped_trajecotry(
            robot_interface=robot_interface,
            start_angles=start,
            goal_angles=end,
            identification_file=local_data_location,
            moved_axis=axis,
            sample_time=Ts,
            # Propagate optional user override; None preserves calibration default.
            num_shapers=num_shapers,
        )

        store_vibration_test_data(
            robot_interface=robot_interface,
            data_log=response["unshaped_log"],
            pose_number=pose,
            axis_number=axis,
            label="unshaped",
        )
        store_vibration_test_data(
            robot_interface=robot_interface,
            data_log=response["shaped_log"],
            pose_number=pose,
            axis_number=axis,
            label="shaped",
        )
        pose_axis_pairs.append((pose, axis))

    # Loop 2: read saved CSV files and generate all analysis/plots.
    for pose, axis in pose_axis_pairs:
        motion_shaped = (
            f"{VIBRATION_DATA_FOLDER}/shaped/robotData_motion_pose{pose}_axis{axis}.csv"
        )
        motion_unshaped = f"{VIBRATION_DATA_FOLDER}/unshaped/robotData_motion_pose{pose}_axis{axis}.csv"

        # Shaped run signals loaded from CSV:
        # time_s: timeline [s]
        # cmd_s: commanded joint positions [rad]
        # encoder_s: measured joint positions [rad]
        # currents_s: motor current samples
        # acc_t_s: accelerometer timeline [s]
        # tcp_acc_s: TCP/tool-frame linear acceleration [m/s^2]
        # gyro_s: TCP/tool-frame angular velocity [rad/s]
        # quat_t_s: quaternion timeline [s]
        # quat_s: TCP orientation quaternion [w, x, y, z]
        (
            time_s,
            cmd_s,
            encoder_s,
            currents_s,
            acc_t_s,
            tcp_acc_s,
            gyro_s,
            quat_t_s,
            quat_s,
        ) = extract_dynamic_variables(
            dynamic_file=motion_shaped, num_joints=robot_interface.num_joints
        )

        # Unshaped run signals loaded from CSV (same meaning as shaped variables above).
        (
            time_u,
            cmd_u,
            encoder_u,
            currents_u,
            acc_t_u,
            tcp_acc_u,
            gyro_u,
            quat_t_u,
            quat_u,
        ) = extract_dynamic_variables(
            dynamic_file=motion_unshaped, num_joints=robot_interface.num_joints
        )

        # Uniform analysis sample period [s] used to remove host timestamp jitter.
        analysis_Ts = 1.0 / float(DEFAULT_ROBOT_FREQ)
        time_s = _build_uniform_time(time_s.shape[0], analysis_Ts)
        time_u = _build_uniform_time(time_u.shape[0], analysis_Ts)
        acc_t_s = _build_uniform_time(acc_t_s.shape[0], analysis_Ts)
        acc_t_u = _build_uniform_time(acc_t_u.shape[0], analysis_Ts)
        quat_t_s = _build_uniform_time(quat_t_s.shape[0], analysis_Ts)
        quat_t_u = _build_uniform_time(quat_t_u.shape[0], analysis_Ts)

        # Plot 1: trajectory comparison from saved command streams.
        run_vibration_test_plot_trajectory_compare = False
        if run_vibration_test_plot_trajectory_compare:
            vel_u = np.gradient(cmd_u, analysis_Ts, axis=0)
            vel_s = np.gradient(cmd_s, analysis_Ts, axis=0)
            acc_u = np.gradient(vel_u, analysis_Ts, axis=0)
            acc_s = np.gradient(vel_s, analysis_Ts, axis=0)
            vibration_test_plots.plot_shaped_vs_unshaped_trajectory(
                unshaped={
                    "time": time_u,
                    "positions": cmd_u,
                    "velocities": vel_u,
                    "accelerations": acc_u,
                },
                shaped={
                    "time": time_s,
                    "positions": cmd_s,
                    "velocities": vel_s,
                    "accelerations": acc_s,
                },
                moved_axis=axis,
                plot_name=f"trajectory_compare_pose_{pose}_axis_{axis}",
            )

        tcp_acc_s, _, _ = rotate_and_compensate_acc(
            robot_interface=robot_interface,
            time_vec=time_s,
            encoder=encoder_s,
            tcp_acc=tcp_acc_s,
            sample_time=Ts,
        )
        tcp_acc_u, _, _ = rotate_and_compensate_acc(
            robot_interface=robot_interface,
            time_vec=time_u,
            encoder=encoder_u,
            tcp_acc=tcp_acc_u,
            sample_time=Ts,
        )

        # Residual-extraction variables:
        # ma_u/ma_s: moving averages of base-frame acceleration [m/s^2]
        # residual_acc_full_u/residual_acc_full_s: full-trajectory residual
        # acceleration (signal - moving average) [m/s^2]
        ma_u = moving_average_signal(
            signal=tcp_acc_u,
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )
        ma_s = moving_average_signal(
            signal=tcp_acc_s,
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )

        residual_acc_full_u = tcp_acc_u - ma_u
        residual_acc_full_s = tcp_acc_s - ma_s

        run_vibration_test_plot_rotated_acceleration_data = False
        if run_vibration_test_plot_rotated_acceleration_data:
            vibration_test_plots.plot_rotated_accelerometer_readings(
                time_vec=time_u,
                signal=tcp_acc_u,
                moving_avg=ma_u,
                residual=residual_acc_full_u,
                title="RR control off base-frame acceleration for full trajectory (bias/grav removed)",
                signal_color="k",
                signal_linestyle="-",
                plot_name=f"acc_base_unshaped_pose_{pose}_axis_{axis}",
            )

            vibration_test_plots.plot_rotated_accelerometer_readings(
                time_vec=time_s,
                signal=tcp_acc_s,
                moving_avg=ma_s,
                residual=residual_acc_full_s,
                title="RR control on base-frame acceleration for full trajectory (bias/grav removed)",
                signal_color="r",
                signal_linestyle="--",
                plot_name=f"acc_base_shaped_pose_{pose}_axis_{axis}",
            )

        # Residual-vibration metrics:
        # mag_acc_*: acceleration norm [m/s^2]
        # idx_vib_*: index where joint motion is considered settled (end of the motion)
        # peak_acc_*: index of largest residual vibraiton magnitude after end of the motion
        # p2p_*: peak residual amplitude [m/s^2] used for percent reduction metric
        mag_acc_s = np.linalg.norm(tcp_acc_s, axis=1)
        mag_acc_u = np.linalg.norm(tcp_acc_u, axis=1)

        # Residual traces are retained for optional diagnostic plots.
        mag_acc_u_moving_avg = moving_average_signal(
            signal=mag_acc_u,
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )
        mag_acc_s_moving_avg = moving_average_signal(
            signal=mag_acc_s,
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )
        mag_acc_u_residual_abs = mag_acc_u - mag_acc_u_moving_avg
        mag_acc_s_residual_abs = mag_acc_s - mag_acc_s_moving_avg

        run_vibration_test_plot_mag_acc_residual_fft = False
        if run_vibration_test_plot_mag_acc_residual_fft:
            vibration_test_plots.plot_mag_acc_residual_and_fft(
                time_u=time_u,
                time_s=time_s,
                mag_acc_u=mag_acc_u,
                mag_acc_s=mag_acc_s,
                mag_acc_u_residual_abs=mag_acc_u_residual_abs,
                mag_acc_s_residual_abs=mag_acc_s_residual_abs,
                sample_time=Ts,
                pose=pose,
                axis=axis,
            )

        # `idx_vib_*`: first index where commanded trajectory reaches its final
        # commanded value; reused for encoder plots/metrics.
        idx_vib_s = first_index_of_final_command_value(
            command_position_axis=cmd_s[:, axis],
        )
        idx_vib_u = first_index_of_final_command_value(
            command_position_axis=cmd_u[:, axis],
        )

        duration_diff_s = (
            float(time_s[-1] - time_u[-1]) if time_s.size and time_u.size else 0.0
        )

        # `peak_acc_*`: index offset (inside residual window) of max vibration amplitude.
        peak_acc_s = np.argmax(np.abs(mag_acc_s[idx_vib_s:]))
        peak_acc_u = np.argmax(np.abs(mag_acc_u[idx_vib_u:]))

        p2p_s = mag_acc_s[idx_vib_s + peak_acc_s]
        p2p_u = mag_acc_u[idx_vib_u + peak_acc_u]
        percent_decrease = (p2p_u - p2p_s) / p2p_u

        print(
            f"percent reduction in peak-to-peak vibration (residual window): {percent_decrease * 100:.2f}%"
        )

        coincide_time_abs = None
        closest_time_abs = None
        coincide_idx_residual_window = None
        run_vibration_test_plot_residual_window = False
        if run_vibration_test_plot_residual_window:
            vibration_test_plots.plot_encoder_fullacc_residual_vibration(
                time_u=time_u,
                time_s=time_s,
                encoder_u_axis=encoder_u[:, axis],
                encoder_s_axis=encoder_s[:, axis],
                mag_acc_u=mag_acc_u,
                mag_acc_s=mag_acc_s,
                idx_vib_u=idx_vib_u,
                idx_vib_s=idx_vib_s,
                peak_acc_u=int(peak_acc_u),
                peak_acc_s=int(peak_acc_s),
                duration_diff_s=duration_diff_s,
                closest_time_abs=closest_time_abs,
                pose=pose,
                axis=axis,
            )

        run_vibration_test_plot_residual_vibration = True
        if run_vibration_test_plot_residual_vibration:
            # Optional display mode: subtract moving average so residual traces
            # are centered around zero in the residual-vibration plot.
            run_vibration_test_plot_residual_center_zero = True
            coincide_time_abs, closest_time_abs, coincide_idx_residual_window = (
                vibration_test_plots.plot_residual_vibration(
                    time_unshaped=time_u,
                    time_shaped=time_s,
                    mag_unshaped=mag_acc_u,
                    mag_shaped=mag_acc_s,
                    idx_vib_unshaped=idx_vib_u,
                    idx_vib_shaped=idx_vib_s,
                    peak_idx_unshaped=int(peak_acc_u),
                    peak_idx_shaped=int(peak_acc_s),
                    sample_time=Ts,
                    pose=pose,
                    duration_diff_s=duration_diff_s,
                    consecutive_windows=2,
                    center_signals_at_zero=run_vibration_test_plot_residual_center_zero,
                )
            )

        # Select the metric window end index:
        # prefer pink-line index; fallback to blue when pink is missing or blue is earlier.
        pink_idx_residual_window = _find_pink_line_index_from_initial_average(
            mag_acc_u=mag_acc_u,
            mag_acc_s=mag_acc_s,
            idx_vib_u=idx_vib_u,
            idx_vib_s=idx_vib_s,
            sample_time=Ts,
        )
        blue_idx_residual_window = coincide_idx_residual_window
        selected_idx_residual_window = None
        selected_window_source = None
        if pink_idx_residual_window is not None and (
            blue_idx_residual_window is None
            or pink_idx_residual_window <= blue_idx_residual_window
        ):
            selected_idx_residual_window = int(pink_idx_residual_window)
            selected_window_source = "pink"
        elif blue_idx_residual_window is not None:
            selected_idx_residual_window = int(blue_idx_residual_window)
            selected_window_source = "blue"

        # `blue_idx_*`: retained for FFT windowing (blue-line definition).
        blue_idx_u = None
        blue_idx_s = None
        if blue_idx_residual_window is not None:
            blue_idx_u = min(
                idx_vib_u + int(blue_idx_residual_window),
                mag_acc_u.shape[0] - 1,
            )
            blue_idx_s = min(
                idx_vib_s + int(blue_idx_residual_window),
                mag_acc_s.shape[0] - 1,
            )

        if selected_idx_residual_window is not None:
            selected_idx_u = min(
                idx_vib_u + selected_idx_residual_window,
                mag_acc_u.shape[0] - 1,
            )
            selected_idx_s = min(
                idx_vib_s + selected_idx_residual_window,
                mag_acc_s.shape[0] - 1,
            )
            residual_window_percent = vibration_metric_using_std(
                mag_acc_u=mag_acc_u[idx_vib_u : selected_idx_u + 1],
                mag_acc_s=mag_acc_s[idx_vib_s : selected_idx_s + 1],
                sample_time=Ts,
                moving_avg_window_sec=MOVING_AVG_WINDOW_SEC,
            )
            print(
                "percent reduction of vibration (residual vibration window): "
                f"{residual_window_percent:.2f}% "
                f"(window source: {selected_window_source})"
            )
        else:
            print(
                "std-window metrics skipped: neither pink-line nor blue-line index available"
            )

        # FFT window selection: residual-vibration window from end-of-motion
        # index up to the blue-line coincidence index.
        if blue_idx_u is not None and blue_idx_s is not None:
            # `mask_*`: boolean selectors for residual-vibration samples up to blue line.
            mask_u = np.zeros_like(time_u, dtype=bool)
            mask_s = np.zeros_like(time_s, dtype=bool)
            mask_u[idx_vib_u : blue_idx_u + 1] = True
            mask_s[idx_vib_s : blue_idx_s + 1] = True
        else:
            print("FFT skipped: blue-line coincidence index not available")
            continue

        # FFT inputs/outputs:
        # residual_vibration_window_*: residual-vibration time window [m/s^2]
        # f_*: frequency bins [Hz]
        # fft_*_{x,y,z}: one-sided axis FFT magnitudes
        # fft_*_norm: combined axis-norm FFT magnitude

        residual_vibration_window_u = moving_average_signal(
            signal=residual_acc_full_u[mask_u, :],
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )
        residual_vibration_window_s = moving_average_signal(
            signal=residual_acc_full_s[mask_s, :],
            window_sec=MOVING_AVG_WINDOW_SEC,
            sample_time=Ts,
        )

        f_u, fft_u_x = _one_sided_fft(residual_vibration_window_u[:, 0], Ts)
        _, fft_u_y = _one_sided_fft(residual_vibration_window_u[:, 1], Ts)
        _, fft_u_z = _one_sided_fft(residual_vibration_window_u[:, 2], Ts)

        f_s, fft_s_x = _one_sided_fft(residual_vibration_window_s[:, 0], Ts)
        _, fft_s_y = _one_sided_fft(residual_vibration_window_s[:, 1], Ts)
        _, fft_s_z = _one_sided_fft(residual_vibration_window_s[:, 2], Ts)

        fft_u_norm = np.sqrt(fft_u_x**2 + fft_u_y**2 + fft_u_z**2)
        fft_s_norm = np.sqrt(fft_s_x**2 + fft_s_y**2 + fft_s_z**2)

        run_vibration_test_plot_fft_axes = False
        run_vibration_test_plot_fft_norm = False

        if run_vibration_test_plot_fft_axes or run_vibration_test_plot_fft_norm:
            vibration_test_plots.plot_fft_summaries(
                f_u=f_u,
                f_s=f_s,
                fft_u_x=fft_u_x,
                fft_u_y=fft_u_y,
                fft_u_z=fft_u_z,
                fft_s_x=fft_s_x,
                fft_s_y=fft_s_y,
                fft_s_z=fft_s_z,
                fft_u_norm=fft_u_norm,
                fft_s_norm=fft_s_norm,
                pose=pose,
                axis=axis,
                plot_fft_axes=run_vibration_test_plot_fft_axes,
                plot_fft_norm=run_vibration_test_plot_fft_norm,
            )


def run_velocity_test(
    robot_interface: RobotInterface,
    local_data_location: str,
    sample_time: float,
    max_disp: float = DEFAULT_MAX_DISP,
    scale_shaped_speed: float = 1.75,
    scale_unshaped_speed: float = 1.0,
) -> None:
    """Summary:
        Execute velocity-sweep robot experiments, compute residual vibration statistics, and produce sweep and diagnostic figures.
    Args:
        robot_interface: Connected robot interface used for motion and logging [-].
        local_data_location: Folder path containing calibration and source motion CSV files [-].
        sample_time: Analysis sample period [s].
        max_disp: Commanded base-joint displacement [rad].
        scale_shaped_speed: Multiplier applied to shaped velocity/acceleration commands [-].
        scale_unshaped_speed: Multiplier applied to unshaped velocity/acceleration commands [-].
    Returns:
        None: No return value.
    Side Effects:
        Commands robot motion, reads and writes CSV files, prints logs, and renders/saves plots.
    Raises:
        FileNotFoundError: If required CSV files are missing.
        RuntimeError: If robot streaming or model operations fail.
        ValueError: If helper metric/window functions receive invalid inputs.
    Preconditions:
        Robot is connected, calibration files exist, and the base-axis trajectory is valid.
    """
    # Step 1: load calibration metadata and select one known low-V/high-R pose.
    calibration_params = read_identification_parameters(data_folder=local_data_location)

    # Assumption: pose indexing is `(v_idx * n_r_points + r_idx)`, so low-V/high-R is `n_r_points - 1`.
    num_r_points = int(calibration_params.nR)
    num_v_points = int(calibration_params.nV)
    base_axis = 0
    known_pose_idx = min(
        num_r_points - 1,
        num_v_points * num_r_points - 1,
    )

    known_pose_file = f"{local_data_location}/robotData_motion_pose{known_pose_idx}_axis{base_axis}.csv"
    _, known_cmd_joints, *_ = extract_dynamic_variables(
        dynamic_file=known_pose_file,
        num_joints=calibration_params.num_joints,
    )

    start_angles = list(copy.copy(known_cmd_joints[0, :]))
    goal_angles = list(copy.copy(known_cmd_joints[0, :]))
    goal_angles[base_axis] += max_disp

    # Step 2: define start/goal command pair used across sweep conditions.
    # This sclaing is used to increase trajectory lenght ensureing high speeds are reached.
    start_displacement_scale = 1.5
    start_angles = list(copy.copy(known_cmd_joints[0, :]))
    start_angles[base_axis] -= max_disp * start_displacement_scale

    goal_displacement_scale = 1.5
    goal_angles = list(copy.copy(known_cmd_joints[0, :]))
    goal_angles[base_axis] += max_disp * goal_displacement_scale

    # Move robot to start before running sweep, when on hardware.
    if not robot_interface.in_sim_mode and hasattr(robot_interface, "robot"):
        print("[StandardRobot] Moving to known high-R/low-V start position...")
        robot_interface.move_to_joint(target_joint=tuple(start_angles))

    # Step 3: build baseline (off) and shaped-only (on) sweep command sets.
    # Scale factors are applied to both velocity and acceleration limits defaults.
    # Unshaped and shaped trajectories have independent scaling factors so user can
    # set them separately and push the robot to higher speesds with shaper on.
    scale_factors = [0.6, 0.8, 1.0, 1.4, 1.5, 1.6]
    velocity_accel_pairs = [
        (
            DEFAULT_MAX_VEL * scale * scale_unshaped_speed,
            DEFAULT_MAX_ACC * scale * scale_unshaped_speed,
        )
        for scale in scale_factors
    ]

    # Additional shaped-only sweep points (strictly increasing speeds).
    shaped_only_velocity_accel_pairs = [
        (
            DEFAULT_MAX_VEL * scale * scale_shaped_speed,
            DEFAULT_MAX_ACC * scale * scale_shaped_speed,
        )
        for scale in scale_factors
    ]

    # Step 4: initialize output storage for metrics, trajectories, and candidates.
    velocity_test_root = f"{VIBRATION_DATA_FOLDER}/velocity_test"
    unshaped_max_velocities: list[float] = []
    unshaped_residual_stds: list[float] = []
    shaped_max_velocities: list[float] = []
    shaped_residual_stds: list[float] = []
    baseline_unshaped_trajectories: list[Mapping[str, np.ndarray]] = []
    shaped_only_trajectories: list[Mapping[str, np.ndarray]] = []
    unshaped_residual_candidates: list[dict] = []
    shaped_residual_candidates: list[dict] = []
    analysis_sample_time = 1.0 / float(DEFAULT_ROBOT_FREQ)

    # Step 5: run unshaped baseline experiments and store streamed robot logs.
    should_run_unshaped_baseline = True
    if should_run_unshaped_baseline:
        for test_idx, (max_velocity, max_acceleration) in enumerate(
            velocity_accel_pairs
        ):
            print(
                f"[StandardRobot] Velocity test {test_idx + 1}/{len(scale_factors)}: "
                f"max_velocity={max_velocity:.4f} rad/s, max_acceleration={max_acceleration:.4f} rad/s^2"
            )
            response = generate_stream_shaped_unshaped_trajecotry(
                robot_interface=robot_interface,
                start_angles=start_angles,
                goal_angles=goal_angles,
                identification_file=local_data_location,
                moved_axis=base_axis,
                sample_time=sample_time,
                max_velocity=max_velocity,
                max_acceleration=max_acceleration,
                execute_unshaped_on_robot=True,
                execute_shaped_on_robot=False,
            )
            baseline_unshaped_trajectories.append(response["unshaped_trajectory"])
            store_vibration_test_data(
                robot_interface=robot_interface,
                data_log=response["unshaped_log"],
                pose_number=test_idx,
                axis_number=base_axis,
                label="velocity_test/unshaped",
            )
            unshaped_max_velocities.append(
                float(
                    np.max(
                        np.abs(
                            response["unshaped_trajectory"]["velocities"][:, base_axis]
                        )
                    )
                )
            )

    # Step 6: run shaped-only experiments and store streamed robot logs.
    for add_idx, (max_velocity, max_acceleration) in enumerate(
        shaped_only_velocity_accel_pairs
    ):
        print(
            f"[StandardRobot] Velocity shaped-only test {add_idx + 1}/{len(shaped_only_velocity_accel_pairs)}: "
            f"max_velocity={max_velocity:.4f} rad/s, max_acceleration={max_acceleration:.4f} rad/s^2"
        )
        response = generate_stream_shaped_unshaped_trajecotry(
            robot_interface=robot_interface,
            start_angles=start_angles,
            goal_angles=goal_angles,
            identification_file=local_data_location,
            moved_axis=base_axis,
            sample_time=sample_time,
            max_velocity=max_velocity,
            max_acceleration=max_acceleration,
            execute_unshaped_on_robot=False,
            execute_shaped_on_robot=True,
        )
        shaped_only_trajectories.append(response["shaped_trajectory"])
        store_vibration_test_data(
            robot_interface=robot_interface,
            data_log=response["shaped_log"],
            pose_number=len(velocity_accel_pairs) + add_idx,
            axis_number=base_axis,
            label="velocity_test/shaped_only",
        )
        shaped_max_velocities.append(
            float(
                np.max(
                    np.abs(response["shaped_trajectory"]["velocities"][:, base_axis])
                )
            )
        )

    # Step 7: extract unshaped CSVs and compute residual-window standard deviation.
    # Rule: use one fixed residual-window duration for all runs, where duration is
    # the earliest commanded stop time among all unshaped trajectories. This is best
    # than use the window until the end since a long dwell time would affect the
    # standard deviation greatly (too many data near zero) introducing bias on the results.
    unshaped_command_stop_times_sec: list[float] = []
    if should_run_unshaped_baseline:
        for test_idx, _ in enumerate(velocity_accel_pairs):
            motion_unshaped = f"{velocity_test_root}/unshaped/robotData_motion_pose{test_idx}_axis{base_axis}.csv"
            (
                time_u,
                cmd_u,
                encoder_u,
                currents_u,
                acc_t_u,
                tcp_acc_u,
                gyro_u,
                quat_t_u,
                quat_u,
            ) = extract_dynamic_variables(
                dynamic_file=motion_unshaped,
                num_joints=robot_interface.num_joints,
            )
            time_u = _build_uniform_time(time_u.shape[0], analysis_sample_time)
            tcp_acc_u, _, _ = rotate_and_compensate_acc(
                robot_interface=robot_interface,
                time_vec=time_u,
                encoder=encoder_u,
                tcp_acc=tcp_acc_u,
                sample_time=sample_time,
            )
            mag_acc_u = np.linalg.norm(tcp_acc_u, axis=1)
            # Residual window starts at first occurrence of final commanded value;
            # encoder analysis reuses this same index.
            idx_vib_u = first_index_of_final_command_value(
                command_position_axis=cmd_u[:, base_axis],
            )
            peak_idx_u = int(np.argmax(np.abs(mag_acc_u[idx_vib_u:])))
            unshaped_command_stop_times_sec.append(float(time_u[idx_vib_u]))
            unshaped_residual_candidates.append(
                {
                    "time": time_u,
                    "mag": mag_acc_u,
                    "encoder_axis": encoder_u[:, base_axis],
                    "idx_vib": int(idx_vib_u),
                    "peak_idx": peak_idx_u,
                }
            )
    else:
        print(
            "[StandardRobot] Baseline unshaped sweep disabled; skipping unshaped log analysis."
        )

    # Build fixed residual-window duration from earliest unshaped commanded stop.
    if unshaped_command_stop_times_sec:
        fixed_residual_window_sec = float(np.min(unshaped_command_stop_times_sec))
    else:
        # Fallback for unexpected empty unshaped set.
        fixed_residual_window_sec = float(MOVING_AVG_WINDOW_SEC)
    fixed_residual_window_samples = max(
        1,
        int(round(fixed_residual_window_sec / analysis_sample_time)),
    )
    print(
        "[StandardRobot] Fixed residual-window duration for STD analysis: "
        f"{fixed_residual_window_sec:.4f} s "
        f"({fixed_residual_window_samples} samples)"
    )

    # Recompute unshaped residual-window STD using the fixed window length.
    for test_idx, unshaped_candidate in enumerate(unshaped_residual_candidates):
        std_u_window = _residual_std_in_fixed_window(
            mag_acc_norm=unshaped_candidate["mag"],
            idx_vib_start=int(unshaped_candidate["idx_vib"]),
            window_samples=fixed_residual_window_samples,
            sample_time=sample_time,
        )
        unshaped_residual_stds.append(std_u_window)
        print(
            f"[StandardRobot] Velocity test {test_idx + 1}: "
            f"unshaped residual-window std={std_u_window:.6f} [m/s^2]"
        )

    # Step 8: extract shaped CSVs and compute residual-window standard deviation.
    for add_idx, _ in enumerate(shaped_only_velocity_accel_pairs):
        motion_shaped_only = (
            f"{velocity_test_root}/shaped_only/robotData_motion_pose"
            f"{len(velocity_accel_pairs) + add_idx}_axis{base_axis}.csv"
        )
        (
            time_s,
            cmd_s,
            encoder_s,
            currents_s,
            acc_t_s,
            tcp_acc_s,
            gyro_s,
            quat_t_s,
            quat_s,
        ) = extract_dynamic_variables(
            dynamic_file=motion_shaped_only,
            num_joints=robot_interface.num_joints,
        )
        time_s = _build_uniform_time(time_s.shape[0], analysis_sample_time)
        tcp_acc_s, _, _ = rotate_and_compensate_acc(
            robot_interface=robot_interface,
            time_vec=time_s,
            encoder=encoder_s,
            tcp_acc=tcp_acc_s,
            sample_time=sample_time,
        )
        mag_acc_s = np.linalg.norm(tcp_acc_s, axis=1)
        # Residual window starts at first occurrence of final commanded value;
        # encoder analysis reuses this same index.
        idx_vib_s = first_index_of_final_command_value(
            command_position_axis=cmd_s[:, base_axis],
        )
        peak_idx_s = int(np.argmax(np.abs(mag_acc_s[idx_vib_s:])))
        std_s_window = _residual_std_in_fixed_window(
            mag_acc_norm=mag_acc_s,
            idx_vib_start=idx_vib_s,
            window_samples=fixed_residual_window_samples,
            sample_time=sample_time,
        )
        shaped_residual_stds.append(std_s_window)
        shaped_residual_candidates.append(
            {
                "time": time_s,
                "mag": mag_acc_s,
                "encoder_axis": encoder_s[:, base_axis],
                "idx_vib": int(idx_vib_s),
                "peak_idx": peak_idx_s,
            }
        )

    # Step 10: optional diagnostic figure for all generated position/vel/acc profiles.
    should_plot_generated_trajectories = True
    if should_plot_generated_trajectories:
        vibration_test_plots.plot_velocity_test_generated_trajectories(
            baseline_unshaped_trajectories=baseline_unshaped_trajectories,
            shaped_only_trajectories=shaped_only_trajectories,
            base_axis=base_axis,
        )

    # Step 11: outlier-filter sweep data used for trend visualization.
    x_raw = np.asarray(unshaped_max_velocities, dtype=float)
    x_shaped_raw = np.asarray(shaped_max_velocities, dtype=float)
    y_unshaped_raw = np.asarray(unshaped_residual_stds, dtype=float)
    y_shaped_raw = np.asarray(shaped_residual_stds, dtype=float)
    x, y_unshaped = _remove_outliers_iqr(
        x_raw, y_unshaped_raw, "RR controller off (std)"
    )
    x_shaped, y_shaped = _remove_outliers_iqr(
        x_shaped_raw, y_shaped_raw, "RR controller on (std)"
    )
    kept_unshaped_idx = _iqr_keep_indices(y_unshaped_raw)
    kept_shaped_idx = _iqr_keep_indices(y_shaped_raw)

    # Step 12: compose final 3x1 summary (sweep, encoder timing, residual vibration).
    # Optional display mode for velocity-test residual subplot:
    # subtract moving average so traces are centered around zero.
    run_velocity_test_plot_residual_vibration_center_zero = True
    vibration_test_plots.plot_velocity_test_std_summary(
        x_unshaped_filtered=x,
        x_shaped_filtered=x_shaped,
        y_unshaped_filtered=y_unshaped,
        y_shaped_filtered=y_shaped,
        kept_unshaped_idx=kept_unshaped_idx,
        kept_shaped_idx=kept_shaped_idx,
        unshaped_max_velocities=unshaped_max_velocities,
        shaped_max_velocities=shaped_max_velocities,
        unshaped_residual_stds=unshaped_residual_stds,
        shaped_residual_stds=shaped_residual_stds,
        unshaped_residual_candidates=unshaped_residual_candidates,
        shaped_residual_candidates=shaped_residual_candidates,
        sample_time=sample_time,
        center_signals_at_zero=run_velocity_test_plot_residual_vibration_center_zero,
    )
