"""对比原始Detour差分与C#在线车辆状态估计结果。"""

from __future__ import annotations

import argparse
from collections import Counter
from dataclasses import dataclass
from pathlib import Path

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd


REQUIRED_COLUMNS = {
    "ElapsedSeconds",
    "DetourX",
    "DetourY",
    "DetourTheta",
}


def wrap_radians(angle: float | np.ndarray) -> float | np.ndarray:
    """将弧度归一化到[-π, π)区间。"""
    return (angle + np.pi) % (2.0 * np.pi) - np.pi


def angle_difference(target: float, current: float) -> float:
    """计算从当前角到目标角的最短有符号弧度差。"""
    return float(wrap_radians(target - current))


@dataclass(frozen=True)
class Pose:
    """保存世界坐标系中的二维位姿，单位为m和rad。"""

    x: float
    y: float
    yaw: float


@dataclass(frozen=True)
class State:
    """保存脚本复现得到的世界位姿和世界速度。"""

    timestamp: float
    pose: Pose
    vx: float
    vy: float
    omega: float
    velocity_valid: bool


class LowPassFilter:
    """复现FirstOrderLowPassFilter的一阶低通计算。"""

    def __init__(self, time_constant_seconds: float) -> None:
        if not np.isfinite(time_constant_seconds) or time_constant_seconds <= 0:
            raise ValueError("滤波时间常数必须是正有限值。")
        self.time_constant_seconds = float(time_constant_seconds)
        self.initialized = False
        self.value = 0.0

    def update(self, value: float, delta_time_seconds: float) -> float:
        """按照真实采样间隔更新滤波输出。"""
        if not self.initialized:
            self.value = float(value)
            self.initialized = True
            return self.value
        alpha = delta_time_seconds / (
            self.time_constant_seconds + delta_time_seconds
        )
        self.value += alpha * (float(value) - self.value)
        return self.value

    def reset(self) -> None:
        """清除滤波历史。"""
        self.initialized = False
        self.value = 0.0


class VelocityEstimator:
    """复现VelocityEstimator2D的世界速度差分与低通处理。"""

    def __init__(self, linear_tau: float, angular_tau: float) -> None:
        self.vx_filter = LowPassFilter(linear_tau)
        self.vy_filter = LowPassFilter(linear_tau)
        self.omega_filter = LowPassFilter(angular_tau)
        self.previous_pose: Pose | None = None
        self.previous_timestamp = 0.0

    def reset(self, pose: Pose | None = None, timestamp: float = 0.0) -> State | None:
        """清除历史，并可使用当前位姿建立新的零速差分基准。"""
        self.vx_filter.reset()
        self.vy_filter.reset()
        self.omega_filter.reset()
        self.previous_pose = pose
        self.previous_timestamp = float(timestamp)
        if pose is None:
            return None
        return State(timestamp, pose, 0.0, 0.0, 0.0, False)

    def update(self, pose: Pose, timestamp: float) -> State:
        """使用一个新的有效位姿更新速度估计。"""
        if self.previous_pose is None:
            state = self.reset(pose, timestamp)
            assert state is not None
            return state
        delta_time = timestamp - self.previous_timestamp
        if delta_time <= 0.0:
            raise ValueError("新样本时间戳必须严格递增。")
        raw_vx = (pose.x - self.previous_pose.x) / delta_time
        raw_vy = (pose.y - self.previous_pose.y) / delta_time
        raw_omega = angle_difference(
            pose.yaw,
            self.previous_pose.yaw,
        ) / delta_time
        state = State(
            timestamp,
            pose,
            self.vx_filter.update(raw_vx, delta_time),
            self.vy_filter.update(raw_vy, delta_time),
            self.omega_filter.update(raw_omega, delta_time),
            True,
        )
        self.previous_pose = pose
        self.previous_timestamp = timestamp
        return state

    def rebase_preserving_velocity(
        self,
        pose: Pose,
        timestamp: float,
    ) -> State:
        """更新差分基准但保留三个低通滤波器的当前输出。"""
        self.previous_pose = pose
        self.previous_timestamp = timestamp
        velocity_valid = (
            self.vx_filter.initialized
            and self.vy_filter.initialized
            and self.omega_filter.initialized
        )
        return State(
            timestamp,
            pose,
            self.vx_filter.value if velocity_valid else 0.0,
            self.vy_filter.value if velocity_valid else 0.0,
            self.omega_filter.value if velocity_valid else 0.0,
            velocity_valid,
        )


class DetourProviderSimulator:
    """按当前简化版DetourVehicleStateProvider处理离线CSV样本。"""

    def __init__(
        self,
        linear_tau: float = 0.15,
        angular_tau: float = 0.20,
        maximum_linear_speed: float = 1.20,
        maximum_angular_speed: float = np.pi / 4.0,
        position_jump_margin: float = 0.03,
        heading_jump_margin: float = np.deg2rad(5.0),
        stationary_seconds: float = 0.35,
    ) -> None:
        self.estimator = VelocityEstimator(linear_tau, angular_tau)
        self.maximum_linear_speed = maximum_linear_speed
        self.maximum_angular_speed = maximum_angular_speed
        self.position_jump_margin = position_jump_margin
        self.heading_jump_margin = heading_jump_margin
        self.stationary_seconds = stationary_seconds

        self.accepted_pose: Pose | None = None
        self.accepted_timestamp = 0.0
        self.latest_state: State | None = None
        self.stationary_hold = False

    @staticmethod
    def poses_equal(first: Pose, second: Pose) -> bool:
        """判断两次读取是否为Detour保持输出的同一数值帧。"""
        return (
            abs(first.x - second.x) <= 1e-9
            and abs(first.y - second.y) <= 1e-9
            and abs(angle_difference(first.yaw, second.yaw)) <= 1e-8
        )

    def motion_plausible(self, start: Pose, end: Pose, delta_time: float) -> bool:
        """按照车辆绝对运动能力判断两帧是否连续。"""
        if not np.isfinite(delta_time) or delta_time <= 0.0:
            return False
        displacement = np.hypot(end.x - start.x, end.y - start.y)
        heading_change = abs(angle_difference(end.yaw, start.yaw))
        return (
            displacement
            <= self.maximum_linear_speed * delta_time
            + self.position_jump_margin
            and heading_change
            <= self.maximum_angular_speed * delta_time
            + self.heading_jump_margin
        )

    def accept_after_reset(self, pose: Pose, timestamp: float) -> State:
        """接受首帧或确认后的重定位并清除速度历史。"""
        state = self.estimator.reset(pose, timestamp)
        assert state is not None
        self.latest_state = state
        self.accepted_pose = pose
        self.accepted_timestamp = timestamp
        self.stationary_hold = False
        return state

    def accept_continuous(self, pose: Pose, timestamp: float) -> State:
        """接受连续正常位姿并更新速度估计。"""
        state = self.estimator.update(pose, timestamp)
        self.latest_state = state
        self.accepted_pose = pose
        self.accepted_timestamp = timestamp
        self.stationary_hold = False
        return state

    def handle_repeated(self, timestamp: float) -> tuple[State, str]:
        """保留重复帧，并在长期不变后将估计速度归零。"""
        assert self.accepted_pose is not None
        assert self.latest_state is not None
        unchanged = timestamp - self.accepted_timestamp
        if not self.stationary_hold and unchanged >= self.stationary_seconds:
            self.latest_state = State(
                timestamp,
                self.accepted_pose,
                0.0,
                0.0,
                0.0,
                True,
            )
            self.stationary_hold = True
            return self.latest_state, "stationary_zero"
        return self.latest_state, "duplicate"

    def process(
        self,
        pose: Pose,
        timestamp: float,
        velocity_innovation_abnormal: bool = False,
    ) -> tuple[State | None, str]:
        """处理一帧CSV中的Detour读取结果。"""
        if self.accepted_pose is None:
            return self.accept_after_reset(pose, timestamp), "initialized"
        if self.poses_equal(pose, self.accepted_pose):
            return self.handle_repeated(timestamp)
        if self.stationary_hold:
            return self.accept_after_reset(pose, timestamp), "restart_after_stationary"

        elapsed = timestamp - self.accepted_timestamp
        if not self.motion_plausible(self.accepted_pose, pose, elapsed):
            assert self.latest_state is not None
            return self.latest_state, "physical_anomaly"
        if velocity_innovation_abnormal:
            self.latest_state = self.estimator.rebase_preserving_velocity(
                pose,
                timestamp,
            )
            self.accepted_pose = pose
            self.accepted_timestamp = timestamp
            self.stationary_hold = False
            return self.latest_state, "velocity_rebase"
        return self.accept_continuous(pose, timestamp), "accepted"


def segmented_unwrap_degrees(values_radians: np.ndarray) -> np.ndarray:
    """分别展开由NaN分隔的有效航向区间。"""
    result = np.full(values_radians.shape, np.nan, dtype=float)
    finite = np.isfinite(values_radians)
    indices = np.flatnonzero(finite)
    if not indices.size:
        return result
    starts = np.r_[0, np.flatnonzero(np.diff(indices) > 1) + 1]
    ends = np.r_[starts[1:], indices.size]
    for start, end in zip(starts, ends):
        segment_indices = indices[start:end]
        result[segment_indices] = np.rad2deg(
            np.unwrap(values_radians[segment_indices])
        )
    return result


def calculate_naive_derivatives(
    time: np.ndarray,
    x: np.ndarray,
    y: np.ndarray,
    yaw: np.ndarray,
) -> tuple[np.ndarray, np.ndarray]:
    """直接逐记录帧差分，保留重复帧造成的零值和更新尖峰。"""
    speed = np.full(time.shape, np.nan, dtype=float)
    omega = np.full(time.shape, np.nan, dtype=float)
    delta_time = np.diff(time)
    valid = np.isfinite(delta_time) & (delta_time > 0.0)
    delta_x = np.diff(x)
    delta_y = np.diff(y)
    delta_yaw = wrap_radians(np.diff(yaw))
    speed_values = np.full(delta_time.shape, np.nan, dtype=float)
    omega_values = np.full(delta_time.shape, np.nan, dtype=float)
    speed_values[valid] = (
        np.hypot(delta_x[valid], delta_y[valid])
        / delta_time[valid]
    )
    omega_values[valid] = delta_yaw[valid] / delta_time[valid]
    speed[1:] = speed_values
    omega[1:] = omega_values
    return speed, omega


def configure_matplotlib() -> None:
    """配置常见中文字体和负号显示。"""
    plt.rcParams["font.sans-serif"] = [
        "Microsoft YaHei",
        "SimHei",
        "Arial Unicode MS",
        "DejaVu Sans",
    ]
    plt.rcParams["axes.unicode_minus"] = False


def load_csv(csv_path: Path) -> pd.DataFrame:
    """读取并校验状态估计对比所需的CSV字段。"""
    frame = pd.read_csv(csv_path)
    missing = REQUIRED_COLUMNS.difference(frame.columns)
    if missing:
        raise ValueError(
            f"{csv_path.name}缺少字段：{', '.join(sorted(missing))}"
        )
    for column in REQUIRED_COLUMNS:
        frame[column] = pd.to_numeric(frame[column], errors="coerce")
    frame = (
        frame.dropna(subset=list(REQUIRED_COLUMNS))
        .sort_values("ElapsedSeconds")
        .drop_duplicates("ElapsedSeconds", keep="last")
        .reset_index(drop=True)
    )
    if len(frame) < 3:
        raise ValueError(f"{csv_path.name}有效数据不足3行。")
    frame["ElapsedSeconds"] -= frame["ElapsedSeconds"].iloc[0]
    return frame


def detect_visual_anomalies(
    time: np.ndarray,
    x: np.ndarray,
    y: np.ndarray,
    yaw: np.ndarray,
    linear_tau: float,
    angular_tau: float,
    position_residual_meters: float,
    heading_residual_radians: float,
    stationary_seconds: float,
) -> np.ndarray:
    """用恒速预测残差标注可疑跳变，不修改任何状态估计数据。"""
    anomalies = np.zeros(time.shape, dtype=bool)
    estimator = VelocityEstimator(linear_tau, angular_tau)
    previous_pose: Pose | None = None
    previous_update_time = 0.0
    latest_state: State | None = None
    stationary = False

    for index, timestamp in enumerate(time):
        pose = Pose(
            float(x[index]),
            float(y[index]),
            float(wrap_radians(yaw[index])),
        )

        if previous_pose is None:
            latest_state = estimator.reset(pose, float(timestamp))
            previous_pose = pose
            previous_update_time = float(timestamp)
            continue

        if DetourProviderSimulator.poses_equal(pose, previous_pose):
            if (
                not stationary
                and timestamp - previous_update_time >= stationary_seconds
            ):
                stationary = True
            continue

        # 静止后的第一个新定位只重新建立差分基准，避免把起步误标为跳变。
        if stationary:
            latest_state = estimator.reset(pose, float(timestamp))
            previous_pose = pose
            previous_update_time = float(timestamp)
            stationary = False
            continue

        delta_time = float(timestamp) - previous_update_time
        if (
            latest_state is not None
            and latest_state.velocity_valid
            and delta_time > 0.0
        ):
            predicted_x = previous_pose.x + latest_state.vx * delta_time
            predicted_y = previous_pose.y + latest_state.vy * delta_time
            predicted_yaw = float(
                wrap_radians(
                    previous_pose.yaw + latest_state.omega * delta_time
                )
            )
            position_residual = np.hypot(
                pose.x - predicted_x,
                pose.y - predicted_y,
            )
            heading_residual = abs(
                angle_difference(pose.yaw, predicted_yaw)
            )

            if (
                position_residual > position_residual_meters
                or heading_residual > heading_residual_radians
            ):
                anomalies[index] = True
                # 标注后从当前观测重新开始，避免一个跳变引发连续误标。
                latest_state = estimator.rebase_preserving_velocity(
                    pose,
                    float(timestamp),
                )
                previous_pose = pose
                previous_update_time = float(timestamp)
                continue

        latest_state = estimator.update(pose, float(timestamp))
        previous_pose = pose
        previous_update_time = float(timestamp)

    return anomalies


def simulate(
    frame: pd.DataFrame,
    args: argparse.Namespace,
) -> tuple[pd.DataFrame, Counter]:
    """使用当前C#参数处理整份Detour记录。"""
    time = frame["ElapsedSeconds"].to_numpy(float)
    raw_x = frame["DetourX"].to_numpy(float) / 1000.0
    raw_y = frame["DetourY"].to_numpy(float) / 1000.0
    raw_yaw = np.deg2rad(frame["DetourTheta"].to_numpy(float))
    raw_speed, raw_omega = calculate_naive_derivatives(
        time,
        raw_x,
        raw_y,
        raw_yaw,
    )
    visual_anomalies = detect_visual_anomalies(
        time,
        raw_x,
        raw_y,
        raw_yaw,
        args.linear_tau,
        args.angular_tau,
        args.annotation_position_residual_mm / 1000.0,
        np.deg2rad(args.annotation_heading_residual_deg),
        args.stationary_seconds,
    )

    simulator = DetourProviderSimulator(
        linear_tau=args.linear_tau,
        angular_tau=args.angular_tau,
        maximum_linear_speed=args.maximum_linear_speed,
        maximum_angular_speed=np.deg2rad(args.maximum_angular_speed_deg),
        position_jump_margin=args.position_jump_margin_mm / 1000.0,
        heading_jump_margin=np.deg2rad(args.heading_jump_margin_deg),
        stationary_seconds=args.stationary_seconds,
    )

    processed_x = np.full(time.shape, np.nan)
    processed_y = np.full(time.shape, np.nan)
    processed_yaw = np.full(time.shape, np.nan)
    processed_speed = np.full(time.shape, np.nan)
    processed_omega = np.full(time.shape, np.nan)
    events: list[str] = []

    for index, timestamp in enumerate(time):
        pose = Pose(
            raw_x[index],
            raw_y[index],
            float(wrap_radians(raw_yaw[index])),
        )
        state, event = simulator.process(
            pose,
            float(timestamp),
            bool(visual_anomalies[index]),
        )
        events.append(event)
        if state is None:
            continue
        processed_x[index] = state.pose.x
        processed_y[index] = state.pose.y
        processed_yaw[index] = state.pose.yaw
        if state.velocity_valid:
            processed_speed[index] = np.hypot(state.vx, state.vy)
            processed_omega[index] = state.omega

    result = pd.DataFrame(
        {
            "TimeSeconds": time,
            "RawX": raw_x,
            "RawY": raw_y,
            "RawYawRadians": raw_yaw,
            "RawSpeed": raw_speed,
            "RawOmegaRadiansPerSecond": raw_omega,
            "ProcessedX": processed_x,
            "ProcessedY": processed_y,
            "ProcessedYawRadians": processed_yaw,
            "ProcessedSpeed": processed_speed,
            "ProcessedOmegaRadiansPerSecond": processed_omega,
            "VisualAnomaly": visual_anomalies,
            "Event": events,
        }
    )
    counts = Counter(events)
    counts["visual_anomaly"] = int(visual_anomalies.sum())
    return result, counts


def plot_comparison(
    csv_path: Path,
    frame: pd.DataFrame,
    result: pd.DataFrame,
    event_counts: Counter,
    output_directory: str | None,
    show: bool,
) -> Path:
    """生成位置、航向、线速度和角速度处理前后对比图。"""
    time = result["TimeSeconds"].to_numpy(float)
    anomalous = result["VisualAnomaly"].to_numpy(bool)
    raw_yaw_degrees = segmented_unwrap_degrees(
        result["RawYawRadians"].to_numpy(float)
    )
    processed_yaw_degrees = segmented_unwrap_degrees(
        result["ProcessedYawRadians"].to_numpy(float)
    )

    fig, axes = plt.subplots(
        5,
        1,
        figsize=(13.0, 16.0),
        sharex=True,
    )

    series = [
        ("RawX", "ProcessedX", "世界坐标X / m"),
        ("RawY", "ProcessedY", "世界坐标Y / m"),
    ]
    for axis, (raw_name, processed_name, ylabel) in zip(axes[:2], series):
        axis.plot(time, result[raw_name], color="0.65", linewidth=1.0, label="原始Detour")
        axis.plot(time, result[processed_name], color="tab:blue", linewidth=1.5, label="在线处理后")
        axis.scatter(
            time[anomalous],
            result.loc[anomalous, raw_name],
            color="tab:red",
            marker="x",
            s=26,
            label="异常位置",
            zorder=3,
        )
        axis.set_ylabel(ylabel)
        axis.grid(True, alpha=0.3)
        axis.legend(loc="best")

    axes[2].plot(time, raw_yaw_degrees, color="0.65", linewidth=1.0, label="原始Detour")
    axes[2].plot(time, processed_yaw_degrees, color="tab:blue", linewidth=1.5, label="在线处理后")
    axes[2].scatter(
        time[anomalous],
        raw_yaw_degrees[anomalous],
        color="tab:red",
        marker="x",
        s=26,
        label="异常位置",
        zorder=3,
    )
    axes[2].set_ylabel("展开航向角 / deg")
    axes[2].grid(True, alpha=0.3)
    axes[2].legend(loc="best")

    axes[3].plot(time, result["RawSpeed"], color="0.65", linewidth=1.0, label="逐记录帧直接差分")
    axes[3].plot(time, result["ProcessedSpeed"], color="tab:green", linewidth=1.5, label="去重、跳变保护和低通后")
    if "CommandSpeed" in frame.columns:
        command_speed = pd.to_numeric(
            frame["CommandSpeed"], errors="coerce"
        ).to_numpy(float)
        axes[3].plot(time, command_speed, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令线速度")
    axes[3].set_ylabel("合线速度 / (m/s)")
    axes[3].grid(True, alpha=0.3)
    axes[3].legend(loc="best")

    axes[4].plot(
        time,
        np.rad2deg(result["RawOmegaRadiansPerSecond"]),
        color="0.65",
        linewidth=1.0,
        label="逐记录帧最短角差",
    )
    axes[4].plot(
        time,
        np.rad2deg(result["ProcessedOmegaRadiansPerSecond"]),
        color="tab:purple",
        linewidth=1.5,
        label="去重、跳变保护和低通后",
    )
    if "CommandAngularSpeedRadPerSecond" in frame.columns:
        command_omega = np.rad2deg(
            pd.to_numeric(
                frame["CommandAngularSpeedRadPerSecond"],
                errors="coerce",
            ).to_numpy(float)
        )
        axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
    elif "CommandAngularSpeed" in frame.columns:
        # 旧版CSV只有CommandAngularSpeed列，该列历史单位是deg/s；
        # 新版CSV另增RadPerSecond列，不能把旧列再次按rad/s换算。
        command_omega = pd.to_numeric(
            frame["CommandAngularSpeed"],
            errors="coerce",
        ).to_numpy(float)
        axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
    axes[4].set_ylabel("角速度 / (deg/s)")
    axes[4].set_xlabel("时间 / s")
    axes[4].grid(True, alpha=0.3)
    axes[4].legend(loc="best")

    controller = (
        str(frame["ControllerName"].iloc[0])
        if "ControllerName" in frame.columns
        else "UnknownController"
    )
    trajectory = (
        str(frame["TrajectoryName"].iloc[0])
        if "TrajectoryName" in frame.columns
        else csv_path.stem
    )
    anomaly_count = int(anomalous.sum())
    fig.suptitle(
        "Detour状态估计处理前后对比\n"
        f"{controller} - {trajectory}，"
        f"标注异常位置{anomaly_count}帧",
        fontsize=14,
    )
    fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.965))

    if output_directory:
        destination_directory = Path(output_directory)
    else:
        destination_directory = csv_path.parent / "state_estimation_plots"
    destination_directory.mkdir(parents=True, exist_ok=True)
    destination = destination_directory / (
        csv_path.stem + "_state_estimation_comparison.png"
    )
    fig.savefig(destination, dpi=220, bbox_inches="tight")
    if show:
        plt.show()
    plt.close(fig)
    return destination


def discover_csv_files(arguments: list[str]) -> list[Path]:
    """解析文件或目录；目录会被递归展开为全部轨迹CSV。"""
    input_paths = (
        [Path(argument).resolve() for argument in arguments]
        if arguments
        else [Path(__file__).resolve().parent]
    )
    csv_files: set[Path] = set()

    for input_path in input_paths:
        if input_path.is_file():
            if input_path.suffix.lower() == ".csv":
                csv_files.add(input_path)
            continue

        if input_path.is_dir():
            csv_files.update(
                path.resolve()
                for path in input_path.rglob("*.csv")
                if not any(
                    part.startswith("state_estimation_plots")
                    for part in path.parts
                )
            )
            continue

        raise FileNotFoundError(
            f"输入文件或目录不存在：{input_path}"
        )

    return sorted(csv_files)


def main() -> None:
    """解析命令行并批量生成Detour状态估计对比图。"""
    configure_matplotlib()
    parser = argparse.ArgumentParser(
        description="比较原始Detour与当前C#在线状态估计算法。"
    )
    parser.add_argument(
        "files",
        nargs="*",
        help="一个或多个轨迹实验CSV或包含CSV的目录",
    )
    parser.add_argument("--output-dir")
    parser.add_argument("--show", action="store_true")
    parser.add_argument("--linear-tau", type=float, default=0.15)
    parser.add_argument("--angular-tau", type=float, default=0.20)
    parser.add_argument("--maximum-linear-speed", type=float, default=1.20)
    parser.add_argument("--maximum-angular-speed-deg", type=float, default=45.0)
    parser.add_argument("--position-jump-margin-mm", type=float, default=30.0)
    parser.add_argument("--heading-jump-margin-deg", type=float, default=5.0)
    parser.add_argument(
        "--annotation-position-residual-mm",
        type=float,
        default=40.0,
        help="只用于图中红色异常位置标注的预测位置残差阈值",
    )
    parser.add_argument(
        "--annotation-heading-residual-deg",
        type=float,
        default=5.0,
        help="只用于图中红色异常位置标注的预测航向残差阈值",
    )
    parser.add_argument("--stationary-seconds", type=float, default=0.35)
    args = parser.parse_args()

    processed_count = 0
    for csv_path in discover_csv_files(args.files):
        try:
            frame = load_csv(csv_path)
            result, counts = simulate(frame, args)
            destination = plot_comparison(
                csv_path,
                frame,
                result,
                counts,
                args.output_dir,
                args.show,
            )
            print(
                f"{csv_path.name}: "
                f"重复帧={counts['duplicate']}，"
                f"异常位置={counts['visual_anomaly']}"
            )
            print(f"已生成：{destination}")
            processed_count += 1
        except Exception as exception:
            print(f"跳过{csv_path.name}：{exception}")

    if processed_count == 0:
        raise SystemExit("没有找到包含有效Detour字段的轨迹实验CSV。")


if __name__ == "__main__":
    main()