"""绘制理想轨迹与Detour实际轨迹对比图。

不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
本文件也提供其余三个绘图脚本共用的数据预处理函数。
"""

from __future__ import annotations

import argparse
import re
from pathlib import Path
from typing import Any

import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
from scipy.signal import savgol_filter


SCRIPT_DIR = Path(__file__).resolve().parent
REQUIRED_COLUMNS = {
    "ElapsedSeconds",
    "TrajectoryName",
    "DetourX",
    "DetourY",
    "DetourTheta",
    "CommandSpeed",
    "CommandAngularSpeed",
    "ReferenceStartX",
    "ReferenceStartY",
    "ReferenceEndX",
    "ReferenceEndY",
    "ReferenceSpeed",
}


def configure_matplotlib() -> None:
    """配置中文字体和图片输出风格。"""
    matplotlib.rcParams["font.sans-serif"] = [
        "Microsoft YaHei",
        "SimHei",
        "Arial Unicode MS",
        "DejaVu Sans",
    ]
    matplotlib.rcParams["axes.unicode_minus"] = False
    matplotlib.rcParams["figure.dpi"] = 120


def _odd_window_length(
    sample_count: int,
    sample_interval: float,
    window_seconds: float,
    polynomial_order: int = 2,
) -> int | None:
    """计算不超过数据长度的Savitzky-Golay奇数窗口。"""
    requested = max(
        polynomial_order + 2,
        int(round(window_seconds / sample_interval)),
    )
    if requested % 2 == 0:
        requested += 1

    maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
    window = min(requested, maximum)
    minimum = polynomial_order + 2
    if minimum % 2 == 0:
        minimum += 1

    return window if window >= minimum else None


def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
    """将角度差归一化到[-180°, 180°)。"""
    return (angle_degrees + 180.0) % 360.0 - 180.0


def build_complete_s_curve(
    start: np.ndarray,
    end: np.ndarray,
    offset_mm: float,
    samples_per_segment: int = 120,
) -> tuple[np.ndarray, np.ndarray]:
    """重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。"""
    line = end - start
    length = float(np.linalg.norm(line))
    if length <= 1e-6:
        raise ValueError("S型曲线的起点和终点不能重合。")

    forward = line / length
    left = np.array([-forward[1], forward[0]])
    controls = [
        np.array([
            [0.0, 0.0],
            [length / 12.0, 0.0],
            [length / 6.0, offset_mm],
            [length * 0.25, offset_mm],
        ]),
        np.array([
            [length * 0.25, offset_mm],
            [length / 3.0, offset_mm],
            [length * 2.0 / 3.0, -offset_mm],
            [length * 0.75, -offset_mm],
        ]),
        np.array([
            [length * 0.75, -offset_mm],
            [length * 5.0 / 6.0, -offset_mm],
            [length * 11.0 / 12.0, 0.0],
            [length, 0.0],
        ]),
    ]

    local_parts: list[np.ndarray] = []
    derivative_parts: list[np.ndarray] = []
    for index, points in enumerate(controls):
        t = np.linspace(0.0, 1.0, samples_per_segment + 1)
        if index > 0:
            t = t[1:]
        one_minus_t = 1.0 - t
        local = (
            one_minus_t[:, None] ** 3 * points[0]
            + 3.0
            * one_minus_t[:, None] ** 2
            * t[:, None]
            * points[1]
            + 3.0
            * one_minus_t[:, None]
            * t[:, None] ** 2
            * points[2]
            + t[:, None] ** 3 * points[3]
        )
        derivative = (
            3.0
            * one_minus_t[:, None] ** 2
            * (points[1] - points[0])
            + 6.0
            * one_minus_t[:, None]
            * t[:, None]
            * (points[2] - points[1])
            + 3.0
            * t[:, None] ** 2
            * (points[3] - points[2])
        )
        local_parts.append(local)
        derivative_parts.append(derivative)

    local_points = np.vstack(local_parts)
    local_derivatives = np.vstack(derivative_parts)
    world_points = (
        start
        + local_points[:, 0, None] * forward
        + local_points[:, 1, None] * left
    )
    world_derivatives = (
        local_derivatives[:, 0, None] * forward
        + local_derivatives[:, 1, None] * left
    )
    headings = np.rad2deg(
        np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0])
    )
    return world_points, headings


def segmented_savgol(
    values: np.ndarray,
    sample_interval: float,
    window_seconds: float,
    derivative: int = 0,
    polynomial_order: int = 2,
) -> np.ndarray:
    """对含NaN断点的数据逐段执行SG滤波或求导。"""
    values = np.asarray(values, dtype=float)
    result = np.full_like(values, np.nan)
    finite_indices = np.flatnonzero(np.isfinite(values))
    if finite_indices.size == 0:
        return result

    breaks = np.flatnonzero(np.diff(finite_indices) > 1)
    starts = np.r_[0, breaks + 1]
    ends = np.r_[breaks + 1, finite_indices.size]

    for start_index, end_index in zip(starts, ends):
        indices = finite_indices[start_index:end_index]
        segment = values[indices]
        window = _odd_window_length(
            len(segment),
            sample_interval,
            window_seconds,
            polynomial_order,
        )
        if window is not None:
            result[indices] = savgol_filter(
                segment,
                window,
                polynomial_order,
                deriv=derivative,
                delta=sample_interval,
                mode="interp",
            )
        elif derivative == 0:
            result[indices] = segment
        elif len(segment) >= 2:
            result[indices] = np.gradient(segment, sample_interval)

    return result


def shade_localization_jump_windows(
    axis,
    metadata: dict[str, Any],
) -> None:
    """在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
    for index, (start, end) in enumerate(
        metadata["jump_exclusion_windows"]
    ):
        axis.axvspan(
            start,
            end,
            color="tab:red",
            alpha=0.12,
            label="Detour定位跳变排除窗口" if index == 0 else None,
        )


def load_and_resample(
    csv_path: Path,
    frequency_hz: float = 20.0,
    filter_window_seconds: float = 0.55,
) -> tuple[pd.DataFrame, dict[str, Any]]:
    """压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
    if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
        raise ValueError("重采样频率必须是正有限值。")

    raw = pd.read_csv(csv_path)
    missing = REQUIRED_COLUMNS.difference(raw.columns)
    if missing:
        raise ValueError(
            f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
        )

    numeric_columns = [
        "ElapsedSeconds",
        "DetourX",
        "DetourY",
        "DetourTheta",
        "CommandSpeed",
        "CommandAngularSpeed",
        "ReferenceStartX",
        "ReferenceStartY",
        "ReferenceEndX",
        "ReferenceEndY",
        "ReferenceSpeed",
    ]
    optional_numeric_columns = [
        "CommandAngularSpeedRadPerSecond",
        "ReferenceAngularSpeedRadPerSecond",
        "ReferenceMotionFrameYawDegrees",
    ]
    numeric_columns.extend(
        column
        for column in optional_numeric_columns
        if column in raw.columns
    )
    for column in numeric_columns:
        raw[column] = pd.to_numeric(raw[column], errors="coerce")

    raw = (
        raw.dropna(subset=[
            "ElapsedSeconds",
            "DetourX",
            "DetourY",
            "DetourTheta",
        ])
        .sort_values("ElapsedSeconds")
        .drop_duplicates("ElapsedSeconds", keep="last")
        .reset_index(drop=True)
    )
    if len(raw) < 5:
        raise ValueError(f"{csv_path.name}有效数据不足5行。")

    time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
    time_raw = time_raw - time_raw[0]
    raw["ElapsedSeconds"] = time_raw
    duration = float(time_raw[-1])
    sample_interval = 1.0 / frequency_hz
    time_uniform = np.arange(
        0.0,
        duration + sample_interval * 0.5,
        sample_interval,
    )

    def interpolate_command(column: str) -> np.ndarray:
        values = raw[column].to_numpy(dtype=float)
        return np.interp(time_uniform, time_raw, values)

    # 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
    # 速度估计前先保留真正发生位姿更新的样本。
    x_all = raw["DetourX"].to_numpy(dtype=float)
    y_all = raw["DetourY"].to_numpy(dtype=float)
    theta_all = raw["DetourTheta"].to_numpy(dtype=float)
    position_change = np.hypot(np.diff(x_all), np.diff(y_all))
    heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
    update_mask = np.r_[
        True,
        (position_change > 1e-6) | (heading_change > 1e-6),
    ]
    updates = raw.loc[update_mask].copy().reset_index(drop=True)
    if len(updates) < 3:
        raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")

    update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
    update_x = updates["DetourX"].to_numpy(dtype=float)
    update_y = updates["DetourY"].to_numpy(dtype=float)
    update_theta = updates["DetourTheta"].to_numpy(dtype=float)
    update_command_speed = np.abs(
        updates["CommandSpeed"].to_numpy(dtype=float)
    )
    if "CommandAngularSpeedRadPerSecond" in updates.columns:
        update_command_angular_rad = np.abs(
            updates[
                "CommandAngularSpeedRadPerSecond"
            ].to_numpy(dtype=float)
        )
    else:
        # 旧CSV中的CommandAngularSpeed单位为deg/s。
        update_command_angular_rad = np.deg2rad(
            np.abs(
                updates[
                    "CommandAngularSpeed"
                ].to_numpy(dtype=float)
            )
        )

    # 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
    # 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
    update_dt = np.diff(update_time)
    update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
    expected_distance = (
        0.5 *
        (update_command_speed[1:] + update_command_speed[:-1]) *
        update_dt *
        1000.0
    )
    distance_threshold = np.maximum(
        30.0,
        expected_distance * 3.0 + 15.0,
    )
    update_heading_delta = np.abs(
        wrap_degrees(np.diff(update_theta))
    )
    expected_heading_delta = (
        0.5 *
        (
            update_command_angular_rad[1:] +
            update_command_angular_rad[:-1]
        ) *
        update_dt *
        180.0 / np.pi
    )
    heading_threshold = np.maximum(
        5.0,
        expected_heading_delta * 3.0 + 2.0,
    )
    jump_before_current = (
        (update_distance > distance_threshold) |
        (update_heading_delta > heading_threshold)
    )
    jump_at_update = np.r_[False, jump_before_current]
    segment_ids = np.cumsum(jump_at_update.astype(int))

    jump_events: list[dict[str, float]] = []
    for current_index in np.flatnonzero(jump_at_update):
        previous_index = current_index - 1
        jump_events.append({
            "time_seconds": float(update_time[current_index]),
            "distance_mm": float(update_distance[previous_index]),
            "heading_change_degrees":
                float(update_heading_delta[previous_index]),
            "before_x_mm": float(update_x[previous_index]),
            "before_y_mm": float(update_y[previous_index]),
            "after_x_mm": float(update_x[current_index]),
            "after_y_mm": float(update_y[current_index]),
        })

    # 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
    # 也防止SG滤波把坐标修正涂抹成车辆高速运动。
    x_resampled = np.full_like(time_uniform, np.nan)
    y_resampled = np.full_like(time_uniform, np.nan)
    theta_resampled = np.full_like(time_uniform, np.nan)
    update_theta_unwrapped = np.rad2deg(
        np.unwrap(np.deg2rad(update_theta))
    )
    maximum_segment_id = int(segment_ids[-1])
    for segment_id in range(maximum_segment_id + 1):
        segment_mask = segment_ids == segment_id
        segment_time = update_time[segment_mask]
        if segment_time.size == 0:
            continue

        interval_start = (
            0.0 if segment_id == 0 else float(segment_time[0])
        )
        interval_end = (
            duration
            if segment_id == maximum_segment_id
            else float(segment_time[-1])
        )
        uniform_mask = (
            (time_uniform >= interval_start) &
            (time_uniform <= interval_end)
        )
        x_resampled[uniform_mask] = np.interp(
            time_uniform[uniform_mask],
            segment_time,
            update_x[segment_mask],
        )
        y_resampled[uniform_mask] = np.interp(
            time_uniform[uniform_mask],
            segment_time,
            update_y[segment_mask],
        )
        theta_resampled[uniform_mask] = np.interp(
            time_uniform[uniform_mask],
            segment_time,
            update_theta_unwrapped[segment_mask],
        )

    x_filtered = segmented_savgol(
        x_resampled,
        sample_interval,
        filter_window_seconds,
    )
    y_filtered = segmented_savgol(
        y_resampled,
        sample_interval,
        filter_window_seconds,
    )
    theta_filtered = segmented_savgol(
        theta_resampled,
        sample_interval,
        filter_window_seconds,
    )

    exclusion_half_width = max(
        0.30,
        filter_window_seconds * 0.5,
    )
    jump_exclusion_windows = [
        (
            max(0.0, event["time_seconds"] - exclusion_half_width),
            min(duration, event["time_seconds"] + exclusion_half_width),
        )
        for event in jump_events
    ]
    invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
    for start, end in jump_exclusion_windows:
        invalid_near_jump |= (
            (time_uniform >= start) & (time_uniform <= end)
        )

    if "CommandAngularSpeedRadPerSecond" in raw.columns:
        angular_command_rad = interpolate_command(
            "CommandAngularSpeedRadPerSecond"
        )
    else:
        angular_command_rad = np.deg2rad(
            interpolate_command("CommandAngularSpeed")
        )

    frame = pd.DataFrame({
        "TimeSeconds": time_uniform,
        "DetourXRawMm": x_resampled,
        "DetourYRawMm": y_resampled,
        "DetourXFilteredMm": x_filtered,
        "DetourYFilteredMm": y_filtered,
        "DetourThetaUnwrappedDeg": theta_filtered,
        "DetourThetaDeg": wrap_degrees(theta_filtered),
        "CommandSpeedMps": interpolate_command("CommandSpeed"),
        "CommandAngularSpeedRadPerSec":
            angular_command_rad,
        "InvalidNearLocalizationJump": invalid_near_jump,
    })

    first = raw.iloc[0]
    metadata: dict[str, Any] = {
        "csv_path": csv_path,
        "trajectory_name": str(first["TrajectoryName"]),
        "controller_name": str(first.get("ControllerName", "")),
        "trial_number": str(first.get("TrialNumber", "")),
        # 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。
        # DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。
        "motion_frame_yaw_degrees": float(
            first["ReferenceMotionFrameYawDegrees"]
            if (
                "ReferenceMotionFrameYawDegrees" in raw.columns
                and pd.notna(
                    first["ReferenceMotionFrameYawDegrees"]
                )
            )
            else (
                90.0
                if "crab" in (
                    str(first["TrajectoryName"]) +
                    str(first.get("ControllerName", ""))
                ).lower()
                else 0.0
            )
        ),
        "reference_start_mm": np.array(
            [first["ReferenceStartX"], first["ReferenceStartY"]],
            dtype=float,
        ),
        "reference_end_mm": np.array(
            [first["ReferenceEndX"], first["ReferenceEndY"]],
            dtype=float,
        ),
        "reference_speed_mps": float(first["ReferenceSpeed"]),
        "reference_angular_speed_rad_per_second": float(
            first.get(
                "ReferenceAngularSpeedRadPerSecond",
                0.0,
            )
        ),
        # 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
        "start_heading_degrees": float(first["DetourTheta"]),
        "sample_interval_seconds": sample_interval,
        "filter_window_seconds": filter_window_seconds,
        "raw_sample_count": len(raw),
        "detour_update_count": len(updates),
        "held_sample_count": int(len(raw) - len(updates)),
        "localization_jump_events": jump_events,
        "jump_exclusion_windows": jump_exclusion_windows,
    }
    return frame, metadata


def build_reference(
    frame: pd.DataFrame,
    metadata: dict[str, Any],
) -> dict[str, np.ndarray | float | str]:
    """根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。"""
    trajectory_name = str(metadata["trajectory_name"])
    start = np.asarray(metadata["reference_start_mm"], dtype=float)
    end = np.asarray(metadata["reference_end_mm"], dtype=float)
    motion_frame_yaw_degrees = float(
        metadata.get("motion_frame_yaw_degrees", 0.0)
    )
    actual = frame[
        ["DetourXFilteredMm", "DetourYFilteredMm"]
    ].to_numpy(dtype=float)
    actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)

    radius_match = re.search(
        r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
        trajectory_name,
        flags=re.IGNORECASE,
    )
    if radius_match:
        radius = float(radius_match.group("radius"))
        sweep_degrees = float(radius_match.group("sweep"))
        start_body_heading = float(metadata["start_heading_degrees"])
        start_motion_heading = (
            start_body_heading + motion_frame_yaw_degrees
        )
        heading_radians = np.deg2rad(start_motion_heading)
        center = start + radius * np.array(
            [-np.sin(heading_radians), np.cos(heading_radians)]
        )
        start_radial_degrees = start_motion_heading - 90.0

        radial = actual - center
        distance_to_center = np.linalg.norm(radial, axis=1)
        radial_angle_degrees = np.rad2deg(
            np.arctan2(radial[:, 1], radial[:, 0])
        )
        radial_angle_radians = np.deg2rad(radial_angle_degrees)
        reference_points = center + radius * np.column_stack([
            np.cos(radial_angle_radians),
            np.sin(radial_angle_radians),
        ])
        # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
        lateral_error = radius - distance_to_center
        reference_motion_heading = radial_angle_degrees + 90.0
        reference_heading = (
            reference_motion_heading - motion_frame_yaw_degrees
        )
        heading_error = wrap_degrees(
            actual_heading - reference_heading
        )

        plot_angles = np.deg2rad(
            np.linspace(
                start_radial_degrees,
                start_radial_degrees + sweep_degrees,
                361,
            )
        )
        ideal_plot = center + radius * np.column_stack([
            np.cos(plot_angles),
            np.sin(plot_angles),
        ])
        return {
            "kind": "left_arc",
            "ideal_plot_mm": ideal_plot,
            "reference_points_mm": reference_points,
            "reference_heading_degrees": reference_heading,
            "reference_motion_heading_degrees":
                reference_motion_heading,
            "lateral_error_mm": lateral_error,
            "heading_error_degrees": heading_error,
            "center_mm": center,
            "radius_mm": radius,
        }

    s_curve_match = re.search(
        r"SCurve(?P<length>[0-9.]+)m_A(?P<offset>[0-9.]+)mm",
        trajectory_name,
        flags=re.IGNORECASE,
    )
    if s_curve_match:
        offset_mm = float(s_curve_match.group("offset"))
        ideal_plot, ideal_heading = build_complete_s_curve(
            start,
            end,
            offset_mm,
        )
        delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :]
        nearest_indices = np.argmin(
            np.sum(delta * delta, axis=2),
            axis=1,
        )
        reference_points = ideal_plot[nearest_indices]
        reference_motion_heading = ideal_heading[nearest_indices]
        reference_heading = (
            reference_motion_heading - motion_frame_yaw_degrees
        )
        heading_radians = np.deg2rad(reference_motion_heading)
        left_normals = np.column_stack([
            -np.sin(heading_radians),
            np.cos(heading_radians),
        ])
        lateral_error = np.sum(
            (actual - reference_points) * left_normals,
            axis=1,
        )
        heading_error = wrap_degrees(
            actual_heading - reference_heading
        )
        return {
            "kind": "s_curve",
            "ideal_plot_mm": ideal_plot,
            "reference_points_mm": reference_points,
            "reference_heading_degrees": reference_heading,
            "reference_motion_heading_degrees":
                reference_motion_heading,
            "lateral_error_mm": lateral_error,
            "heading_error_degrees": heading_error,
            "offset_mm": offset_mm,
        }

    line = end - start
    length = float(np.linalg.norm(line))
    if length <= 1e-6:
        rotation_match = re.search(
            r"Rotate(?P<angle>[+-]?[0-9.]+)",
            trajectory_name,
            flags=re.IGNORECASE,
        )
        if rotation_match:
            relative_angle_degrees = float(
                rotation_match.group("angle")
            )
            target_heading_degrees = (
                float(metadata["start_heading_degrees"]) +
                relative_angle_degrees
            )
            reference_points = np.repeat(
                start[np.newaxis, :],
                len(frame),
                axis=0,
            )
            position_drift = np.linalg.norm(
                actual - start,
                axis=1,
            )
            reference_heading = np.full(
                len(frame),
                target_heading_degrees,
            )
            heading_error = wrap_degrees(
                actual_heading - reference_heading
            )
            ideal_plot = np.repeat(
                start[np.newaxis, :],
                2,
                axis=0,
            )
            return {
                "kind": "in_place_rotation",
                "ideal_plot_mm": ideal_plot,
                "reference_points_mm": reference_points,
                "reference_heading_degrees": reference_heading,
                # 对原地自转，该字段表示偏离初始旋转中心的距离。
                "lateral_error_mm": position_drift,
                "heading_error_degrees": heading_error,
                "rotation_center_mm": start,
                "relative_angle_degrees": relative_angle_degrees,
                "target_heading_degrees": target_heading_degrees,
            }

        raise ValueError(
            f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
        )

    tangent = line / length
    left_normal = np.array([-tangent[1], tangent[0]])
    displacement = actual - start
    progress = np.clip(displacement @ tangent, 0.0, length)
    reference_points = start + np.outer(progress, tangent)
    lateral_error = (actual - reference_points) @ left_normal
    reference_motion_heading_scalar = np.rad2deg(
        np.arctan2(tangent[1], tangent[0])
    )
    reference_heading_scalar = (
        reference_motion_heading_scalar -
        motion_frame_yaw_degrees
    )
    reference_heading = np.full(
        len(frame),
        reference_heading_scalar,
    )
    heading_error = wrap_degrees(
        actual_heading - reference_heading
    )
    ideal_plot = np.linspace(start, end, 361)
    return {
        "kind": "line",
        "ideal_plot_mm": ideal_plot,
        "reference_points_mm": reference_points,
        "reference_heading_degrees": reference_heading,
        "reference_motion_heading_degrees": np.full(
            len(frame),
            reference_motion_heading_scalar,
        ),
        "lateral_error_mm": lateral_error,
        "heading_error_degrees": heading_error,
    }


def discover_csv_files(arguments: list[str]) -> list[Path]:
    """解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
    if arguments:
        files = [Path(item).expanduser().resolve() for item in arguments]
    else:
        files = sorted(SCRIPT_DIR.glob("*.csv"))
    if not files:
        raise FileNotFoundError("没有找到可处理的CSV文件。")
    return files


def output_path(
    csv_path: Path,
    output_directory: str | None,
    suffix: str,
) -> Path:
    """构造图片输出路径并创建目录。"""
    directory = (
        Path(output_directory).expanduser().resolve()
        if output_directory
        else csv_path.parent / "plots"
    )
    directory.mkdir(parents=True, exist_ok=True)
    return directory / f"{csv_path.stem}_{suffix}.png"


def plot_trajectory(
    csv_path: Path,
    frequency_hz: float,
    filter_window_seconds: float,
    output_directory: str | None,
    show: bool,
) -> Path:
    """生成单份CSV的理想/实际轨迹对比图。"""
    frame, metadata = load_and_resample(
        csv_path,
        frequency_hz,
        filter_window_seconds,
    )
    reference = build_reference(frame, metadata)

    actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
    actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
    ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0

    fig, ax = plt.subplots(figsize=(8.0, 7.0))
    ax.plot(
        ideal_m[:, 0],
        ideal_m[:, 1],
        "--",
        linewidth=2.2,
        label="理想轨迹",
    )
    ax.plot(
        actual_x_m,
        actual_y_m,
        linewidth=1.8,
        label="Detour实际轨迹（滤波后）",
    )
    if reference["kind"] == "in_place_rotation":
        ax.scatter(
            [ideal_m[0, 0]],
            [ideal_m[0, 1]],
            marker="*",
            s=100,
            label="理想旋转中心",
            zorder=5,
        )
    else:
        ax.scatter(
            [ideal_m[0, 0]],
            [ideal_m[0, 1]],
            marker="o",
            s=55,
            label="起点",
            zorder=5,
        )
        ax.scatter(
            [ideal_m[-1, 0]],
            [ideal_m[-1, 1]],
            marker="x",
            s=65,
            label="终点",
            zorder=5,
        )
    for event_index, event in enumerate(
        metadata["localization_jump_events"]
    ):
        before = np.array([
            event["before_x_mm"],
            event["before_y_mm"],
        ]) / 1000.0
        after = np.array([
            event["after_x_mm"],
            event["after_y_mm"],
        ]) / 1000.0
        ax.scatter(
            [before[0], after[0]],
            [before[1], after[1]],
            marker="x",
            color="tab:red",
            s=55,
            zorder=6,
            label="Detour定位跳变前/后"
            if event_index == 0 else None,
        )
        ax.annotate(
            f"定位跳变 {event['distance_mm']:.1f} mm\n"
            f"t={event['time_seconds']:.2f} s",
            xy=(after[0], after[1]),
            xytext=(8, 8),
            textcoords="offset points",
            color="tab:red",
            fontsize=9,
        )
    ax.set_aspect("equal", adjustable="box")
    ax.set_xlabel("世界坐标 X / m")
    ax.set_ylabel("世界坐标 Y / m")
    ax.set_title(
        f"理想轨迹与实际轨迹对比\n"
        f"{metadata['controller_name']} - "
        f"{metadata['trajectory_name']}"
    )
    ax.grid(True, alpha=0.3)
    ax.legend()
    fig.tight_layout()

    destination = output_path(
        csv_path,
        output_directory,
        "trajectory_comparison",
    )
    fig.savefig(destination, dpi=300, bbox_inches="tight")
    if show:
        plt.show()
    plt.close(fig)
    print(
        f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
        f"有效Detour更新{metadata['detour_update_count']}帧，"
        f"保持重复{metadata['held_sample_count']}帧，"
        f"定位跳变{len(metadata['localization_jump_events'])}次"
    )
    return destination


def main() -> None:
    configure_matplotlib()
    parser = argparse.ArgumentParser(
        description="绘制理想轨迹与Detour实际轨迹对比图。"
    )
    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
    parser.add_argument("--frequency", type=float, default=20.0)
    parser.add_argument("--window", type=float, default=0.55)
    parser.add_argument("--output-dir")
    parser.add_argument("--show", action="store_true")
    args = parser.parse_args()

    for csv_path in discover_csv_files(args.files):
        destination = plot_trajectory(
            csv_path,
            args.frequency,
            args.window,
            args.output_dir,
            args.show,
        )
        print(f"已生成：{destination}")


if __name__ == "__main__":
    main()
