#!/usr/bin/env python3
"""底盘标定现场数据配置、检查和数据集索引工具。"""

from __future__ import annotations

import csv
import json
import math
import shutil
import time
from dataclasses import dataclass, field
from pathlib import Path
from typing import Any

import yaml


CHASSIS_TYPES = {"ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"}
CHASSIS_CSV_CONTRACT_VERSION = 1
CHASSIS_TYPE_VALUE_TO_NAME = {
    1: "ackermann",
    2: "differential",
    3: "single_steer_wheel",
    4: "multi_steer_wheel",
}
PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx")

CHASSIS_MOTION_FIELDS = [
    "hardware_timestamp_us",
    "chassis_type",
    "odom_x_m",
    "odom_y_m",
    "odom_yaw_rad",
    "linear_velocity_ms",
    "angular_velocity_rads",
    "estop_engaged",
    "driver_error_code",
    "active_job_id",
    "lateral_slip_estimate",
    "curvature_estimate",
    "module_states_json",
]

ACTUATOR_COMMAND_FIELDS = [
    "command_timestamp_us",
    "command_id",
    "source",
    "target_speed_ms",
    "target_distance_m",
    "target_accel_ms2",
    "target_steering_angle_rad",
    "target_steering_rate_rads",
    "brake_command",
    "throttle_command",
    "command_timeout_sec",
]

TRUTH_TRAJECTORY_FIELDS = [
    "hardware_timestamp_us",
    "pose_valid",
    "x_m",
    "y_m",
    "z_m",
    "roll_rad",
    "pitch_rad",
    "yaw_rad",
    "position_stddev_m",
    "yaw_stddev_rad",
    "tracking_loss_ratio",
    "time_sync_offset_ms",
    "quality_score",
    "observed_target_count",
    "reference_source_name",
    "active_job_id",
]

INTEGER_FIELDS_BY_TABLE = {
    "chassis_motion": {
        "hardware_timestamp_us",
        "driver_error_code",
    },
    "actuator_command": {
        "command_timestamp_us",
    },
    "truth_trajectory": {
        "hardware_timestamp_us",
        "observed_target_count",
    },
}

FLOAT_FIELDS_BY_TABLE = {
    "chassis_motion": {
        "odom_x_m",
        "odom_y_m",
        "odom_yaw_rad",
        "linear_velocity_ms",
        "angular_velocity_rads",
        "lateral_slip_estimate",
        "curvature_estimate",
    },
    "actuator_command": {
        "target_speed_ms",
        "target_distance_m",
        "target_accel_ms2",
        "target_steering_angle_rad",
        "target_steering_rate_rads",
        "brake_command",
        "throttle_command",
        "command_timeout_sec",
    },
    "truth_trajectory": {
        "x_m",
        "y_m",
        "z_m",
        "roll_rad",
        "pitch_rad",
        "yaw_rad",
        "position_stddev_m",
        "yaw_stddev_rad",
        "tracking_loss_ratio",
        "time_sync_offset_ms",
        "quality_score",
    },
}

BOOLEAN_FIELDS_BY_TABLE = {
    "chassis_motion": {"estop_engaged"},
    "truth_trajectory": {"pose_valid"},
}

CSV_CONTRACTS = {
    "chassis_motion": {
        "fields": CHASSIS_MOTION_FIELDS,
        "json_fields": {"module_states_json"},
    },
    "actuator_command": {
        "fields": ACTUATOR_COMMAND_FIELDS,
        "json_fields": set(),
    },
    "truth_trajectory": {
        "fields": TRUTH_TRAJECTORY_FIELDS,
        "json_fields": set(),
    },
}


def now_us() -> int:
    return int(time.time() * 1_000_000)


def load_config(path: Path) -> dict[str, Any]:
    with path.open("r", encoding="utf-8") as stream:
        data = yaml.safe_load(stream) or {}
    if not isinstance(data, dict):
        raise ValueError(f"配置文件必须是 YAML 字典: {path}")
    return data


def write_yaml(path: Path, data: dict[str, Any]) -> None:
    path.parent.mkdir(parents=True, exist_ok=True)
    path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8")


def has_placeholder(value: Any) -> bool:
    return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS)


def resolve_path(root: Path, raw_value: str) -> Path:
    path = Path(raw_value).expanduser()
    if path.is_absolute():
        return path
    return root / path


def relative_to_root(path: Path, root: Path) -> str:
    try:
        return str(path.resolve(strict=False).relative_to(root.resolve(strict=False)))
    except ValueError:
        return str(path.resolve(strict=False))


@dataclass
class ValidationResult:
    errors: list[str] = field(default_factory=list)
    warnings: list[str] = field(default_factory=list)

    @property
    def ok(self) -> bool:
        return not self.errors

    def extend(self, other: "ValidationResult") -> None:
        self.errors.extend(other.errors)
        self.warnings.extend(other.warnings)


def normalize_chassis_type_name(value: Any) -> str:
    if value in (None, ""):
        return ""
    if isinstance(value, int):
        return CHASSIS_TYPE_VALUE_TO_NAME.get(value, "")
    text = str(value).strip()
    if not text:
        return ""
    if text.isdigit():
        return CHASSIS_TYPE_VALUE_TO_NAME.get(int(text), "")
    return text


def is_bool_token(value: Any) -> bool:
    return str(value).strip().lower() in {"0", "1", "true", "false"}


def validate_csv_contract(path: Path, table_name: str) -> ValidationResult:
    result = ValidationResult()
    contract = CSV_CONTRACTS.get(table_name)
    if contract is None:
        result.errors.append(f"未知 CSV 协议名称: {table_name}")
        return result
    if not path.exists():
        result.errors.append(f"{table_name} 文件不存在: {path}")
        return result

    required_fields = list(contract["fields"])
    integer_fields = INTEGER_FIELDS_BY_TABLE.get(table_name, set())
    float_fields = FLOAT_FIELDS_BY_TABLE.get(table_name, set())
    boolean_fields = BOOLEAN_FIELDS_BY_TABLE.get(table_name, set())
    json_fields = contract.get("json_fields", set())

    with path.open("r", newline="", encoding="utf-8") as stream:
        reader = csv.DictReader(stream)
        fieldnames = reader.fieldnames or []
        for field_name in required_fields:
            if field_name not in fieldnames:
                result.errors.append(f"{table_name} 缺少必需列: {field_name}")
        if result.errors:
            return result

        row_count = 0
        for row_number, row in enumerate(reader, start=2):
            row_count += 1
            for field_name in integer_fields:
                try:
                    int(str(row.get(field_name, "")).strip())
                except (TypeError, ValueError):
                    result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是整数。")
            for field_name in float_fields:
                try:
                    float(str(row.get(field_name, "")).strip())
                except (TypeError, ValueError):
                    result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是浮点数。")
            for field_name in boolean_fields:
                if not is_bool_token(row.get(field_name, "")):
                    result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 0/1 或 true/false。")
            for field_name in json_fields:
                try:
                    parsed = json.loads(str(row.get(field_name, "")).strip())
                except json.JSONDecodeError:
                    result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是合法 JSON。")
                    continue
                if field_name == "module_states_json" and not isinstance(parsed, list):
                    result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 JSON 数组。")
            if table_name == "chassis_motion":
                if normalize_chassis_type_name(row.get("chassis_type")) not in CHASSIS_TYPES:
                    result.errors.append(f"{table_name} 第 {row_number} 行 chassis_type 必须是已支持的底盘类型。")
        if row_count == 0:
            result.warnings.append(f"{table_name} 只有表头，没有数据行。")
    return result


def validate_chassis_dataset_files(paths: dict[str, Path]) -> ValidationResult:
    result = ValidationResult()
    checks = [
        ("chassis_motion", "chassis_motion_data_file"),
        ("actuator_command", "actuator_command_file"),
        ("truth_trajectory", "truth_trajectory_file"),
    ]
    for table_name, path_key in checks:
        result.extend(validate_csv_contract(paths[path_key], table_name))
    return result


def validate_config(config: dict[str, Any], config_path: Path | None = None) -> ValidationResult:
    result = ValidationResult()

    csv_contract_version = int(config.get("csv_contract_version", CHASSIS_CSV_CONTRACT_VERSION) or 0)
    if csv_contract_version != CHASSIS_CSV_CONTRACT_VERSION:
        result.errors.append(
            f"csv_contract_version 必须为 {CHASSIS_CSV_CONTRACT_VERSION}。"
        )

    chassis_type = str(config.get("chassis_type", "")).strip()
    if chassis_type not in CHASSIS_TYPES:
        result.errors.append(f"chassis_type 必须是 {sorted(CHASSIS_TYPES)} 之一。")

    for field_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"):
        value = str(config.get(field_name, "")).strip()
        if not value:
            result.errors.append(f"{field_name} 不能为空。")
        elif has_placeholder(value):
            result.errors.append(f"{field_name} 不能保留模板占位符。")

    files = config.get("files", {})
    if not isinstance(files, dict):
        result.errors.append("files 必须是 YAML 字典。")
        files = {}
    for field_name in (
        "chassis_motion_data_file",
        "actuator_command_file",
        "truth_trajectory_file",
        "diagnostics_file",
    ):
        value = str(files.get(field_name, "")).strip()
        if not value:
            result.errors.append(f"files.{field_name} 不能为空。")
        elif has_placeholder(value):
            result.errors.append(f"files.{field_name} 不能保留模板占位符。")

    validation = config.get("validation", {}) or {}
    if not isinstance(validation, dict):
        result.errors.append("validation 必须是 YAML 字典。")
        validation = {}
    for field_name in (
        "min_chassis_motion_samples",
        "min_actuator_command_samples",
        "min_truth_trajectory_samples",
    ):
        if int(validation.get(field_name, 0) or 0) <= 0:
            result.errors.append(f"validation.{field_name} 必须大于 0。")
    if float(validation.get("min_motion_distance_m", 0.0) or 0.0) < 0.0:
        result.errors.append("validation.min_motion_distance_m 不能小于 0。")
    if float(validation.get("max_time_gap_ms", 0.0) or 0.0) <= 0.0:
        result.errors.append("validation.max_time_gap_ms 必须大于 0。")

    if config_path is not None and not config_path.exists():
        result.errors.append(f"配置文件不存在: {config_path}")
    return result


def apply_cli_overrides(config: dict[str, Any], args: Any) -> None:
    for attr_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"):
        value = getattr(args, attr_name, "")
        if value:
            config[attr_name] = value
    if getattr(args, "chassis_type", ""):
        config["chassis_type"] = args.chassis_type


def read_csv_rows(path: Path) -> list[dict[str, str]]:
    with path.open("r", newline="", encoding="utf-8") as stream:
        return list(csv.DictReader(stream))


def timestamp_us(row: dict[str, Any]) -> int:
    for key in ("hardware_timestamp_us", "timestamp_us", "command_timestamp_us"):
        value = row.get(key)
        if value not in (None, ""):
            return int(float(value))
    return 0


def numeric(row: dict[str, Any], key: str, default: float = 0.0) -> float:
    value = row.get(key)
    if value in (None, ""):
        return default
    return float(value)


def trajectory_distance(rows: list[dict[str, str]], x_key: str, y_key: str) -> float:
    distance = 0.0
    previous: tuple[float, float] | None = None
    for row in rows:
        current = (numeric(row, x_key), numeric(row, y_key))
        if previous is not None:
            distance += math.hypot(current[0] - previous[0], current[1] - previous[1])
        previous = current
    return distance


def max_timestamp_gap_ms(rows: list[dict[str, str]]) -> float:
    stamps = sorted(timestamp_us(row) for row in rows if timestamp_us(row) > 0)
    if len(stamps) < 2:
        return 0.0
    return max((b - a) / 1000.0 for a, b in zip(stamps, stamps[1:]))


def write_csv(path: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> None:
    path.parent.mkdir(parents=True, exist_ok=True)
    with path.open("w", newline="", encoding="utf-8") as stream:
        writer = csv.DictWriter(stream, fieldnames=fieldnames)
        writer.writeheader()
        for row in rows:
            writer.writerow(row)


def copy_or_generate_file(source: str, target: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> Path:
    target.parent.mkdir(parents=True, exist_ok=True)
    if source:
        shutil.copyfile(Path(source).expanduser().resolve(strict=False), target)
    else:
        write_csv(target, fieldnames, rows)
    return target


def synthetic_rows(config: dict[str, Any]) -> tuple[list[dict[str, Any]], list[dict[str, Any]], list[dict[str, Any]]]:
    synthetic = config.get("synthetic", {}) or {}
    start_us = now_us()
    period_us = int(float(synthetic.get("sample_period_ms", 100.0) or 100.0) * 1000.0)
    speed = float(synthetic.get("target_speed_ms", 0.1) or 0.1)
    distance = float(synthetic.get("target_distance_m", 0.2) or 0.2)
    sample_count = max(3, int(distance / max(speed, 1e-6) / max(period_us / 1e6, 1e-6)) + 1)

    chassis_rows: list[dict[str, Any]] = []
    truth_rows: list[dict[str, Any]] = []
    for index in range(sample_count):
        stamp = start_us + index * period_us
        x_m = min(distance, index * speed * period_us / 1e6)
        chassis_rows.append({
            "hardware_timestamp_us": stamp,
            "chassis_type": config.get("chassis_type", ""),
            "odom_x_m": x_m,
            "odom_y_m": 0.0,
            "odom_yaw_rad": 0.0,
            "linear_velocity_ms": speed,
            "angular_velocity_rads": 0.0,
            "estop_engaged": 0,
            "driver_error_code": 0,
            "active_job_id": config.get("session_id", ""),
            "lateral_slip_estimate": 0.0,
            "curvature_estimate": 0.0,
            "module_states_json": "[]",
        })
        truth_rows.append({
            "hardware_timestamp_us": stamp,
            "pose_valid": 1,
            "x_m": x_m,
            "y_m": 0.0,
            "z_m": 0.0,
            "roll_rad": 0.0,
            "pitch_rad": 0.0,
            "yaw_rad": 0.0,
            "position_stddev_m": 0.0,
            "yaw_stddev_rad": 0.0,
            "tracking_loss_ratio": 0.0,
            "time_sync_offset_ms": 0.0,
            "quality_score": 1.0,
            "observed_target_count": 0,
            "reference_source_name": "synthetic_chassis_smoke",
            "active_job_id": config.get("session_id", ""),
        })

    command_rows = [
        {
            "command_timestamp_us": start_us,
            "command_id": f"{config.get('session_id', 'session')}_straight_line",
            "source": "synthetic_chassis_smoke",
            "target_speed_ms": speed,
            "target_distance_m": distance,
            "target_accel_ms2": 0.0,
            "target_steering_angle_rad": 0.0,
            "target_steering_rate_rads": 0.0,
            "brake_command": 0.0,
            "throttle_command": 0.0,
            "command_timeout_sec": 0.0,
        }
    ]
    return chassis_rows, command_rows, truth_rows


def build_summary(
    chassis_rows: list[dict[str, str]],
    command_rows: list[dict[str, str]],
    truth_rows: list[dict[str, str]],
    config: dict[str, Any],
) -> dict[str, Any]:
    chassis_distance = trajectory_distance(chassis_rows, "odom_x_m", "odom_y_m")
    truth_distance = trajectory_distance(truth_rows, "x_m", "y_m")
    stamps = [timestamp_us(row) for row in chassis_rows + truth_rows + command_rows if timestamp_us(row) > 0]
    start_us = min(stamps) if stamps else now_us()
    end_us = max(stamps) if stamps else start_us + 1
    return {
        "chassis_csv_contract_version": CHASSIS_CSV_CONTRACT_VERSION,
        "session_id": config.get("session_id", ""),
        "site_id": config.get("site_id", ""),
        "vehicle_id": config.get("vehicle_id", ""),
        "chassis_type": config.get("chassis_type", ""),
        "data_window_start_timestamp_us": int(start_us),
        "data_window_end_timestamp_us": int(max(end_us, start_us + 1)),
        "chassis_motion_sample_count": len(chassis_rows),
        "actuator_command_sample_count": len(command_rows),
        "truth_trajectory_sample_count": len(truth_rows),
        "chassis_odom_distance_m": chassis_distance,
        "truth_distance_m": truth_distance,
        "distance_scale_hint": (truth_distance / chassis_distance) if chassis_distance > 1e-9 else 0.0,
        "max_chassis_timestamp_gap_ms": max_timestamp_gap_ms(chassis_rows),
        "max_truth_timestamp_gap_ms": max_timestamp_gap_ms(truth_rows),
    }


def validate_summary(summary: dict[str, Any], config: dict[str, Any]) -> ValidationResult:
    result = ValidationResult()
    validation = config.get("validation", {}) or {}
    checks = [
        ("chassis_motion_sample_count", "min_chassis_motion_samples", "底盘运动样本数不足"),
        ("actuator_command_sample_count", "min_actuator_command_samples", "执行器命令样本数不足"),
        ("truth_trajectory_sample_count", "min_truth_trajectory_samples", "真值轨迹样本数不足"),
    ]
    for summary_key, config_key, message in checks:
        if int(summary.get(summary_key, 0)) < int(validation.get(config_key, 0) or 0):
            result.errors.append(f"{message}: {summary.get(summary_key, 0)}")
    if float(summary.get("chassis_odom_distance_m", 0.0)) < float(validation.get("min_motion_distance_m", 0.0) or 0.0):
        result.errors.append("底盘运动距离不足。")
    max_gap = float(validation.get("max_time_gap_ms", 200.0) or 200.0)
    if float(summary.get("max_chassis_timestamp_gap_ms", 0.0)) > max_gap:
        result.warnings.append("底盘遥测时间间隔超过配置阈值。")
    if float(summary.get("max_truth_timestamp_gap_ms", 0.0)) > max_gap:
        result.warnings.append("真值轨迹时间间隔超过配置阈值。")
    return result


def build_dataset_index(
    config: dict[str, Any],
    summary: dict[str, Any],
    paths: dict[str, Path],
    config_path: Path,
) -> dict[str, Any]:
    session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False)
    return {
        "schema_version": 1,
        "session": {
            "session_id": str(config["session_id"]),
            "site_id": str(config["site_id"]),
            "vehicle_id": str(config["vehicle_id"]),
            "dataset_root": str(session_dir),
            "data_window_start_timestamp_us": int(summary["data_window_start_timestamp_us"]),
            "data_window_end_timestamp_us": int(summary["data_window_end_timestamp_us"]),
        },
        "data_inputs": {
            "chassis": {
                "chassis_motion_data_files": [
                    relative_to_root(paths["chassis_motion_data_file"], session_dir)
                ],
                "actuator_command_files": [
                    relative_to_root(paths["actuator_command_file"], session_dir)
                ],
                "truth_trajectory_files": [
                    relative_to_root(paths["truth_trajectory_file"], session_dir)
                ],
            }
        },
        "metadata": {
            "chassis_csv_contract_version": CHASSIS_CSV_CONTRACT_VERSION,
            "chassis_data_config_file": str(config_path),
            "chassis_diagnostics_file": relative_to_root(paths["diagnostics_file"], session_dir),
            "chassis_replay_summary": summary,
        },
    }
