#!/usr/bin/env python3
"""验证传感器标定车间电脑侧数据、交接和应用链路。"""

from __future__ import annotations

import argparse
import shutil
import subprocess
import sys
import tempfile
from pathlib import Path
from typing import Any

try:
    import yaml
except ImportError as exc:
    raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc


SCRIPT_DIR = Path(__file__).resolve().parent
REPO_ROOT = Path(__file__).resolve().parents[3]
DEPLOYMENT_TOOLS = REPO_ROOT / "src" / "deployment" / "tools"


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="本地验证 sensor 数据回放、待提交包、审批交接、参数配置落盘链路。")
    parser.add_argument("--session-dir", default="", help="smoke 会话目录；为空时使用临时目录。")
    parser.add_argument("--keep-session-dir", action="store_true", help="使用临时目录时保留输出。")
    parser.add_argument("--vehicle-id", default="agv_sensor_smoke_001")
    parser.add_argument("--site-id", default="site_a")
    parser.add_argument("--operator-id", default="sensor_smoke_operator")
    return parser.parse_args()


def run_command(args: list[str], cwd: Path) -> None:
    result = subprocess.run(args, cwd=cwd, text=True, capture_output=True, check=False)
    if result.returncode != 0:
        if result.stdout:
            print(result.stdout, file=sys.stderr)
        if result.stderr:
            print(result.stderr, file=sys.stderr)
        raise RuntimeError(f"命令执行失败：{' '.join(args)}")


def load_yaml(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"{path} 顶层必须是 YAML map。")
    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 copy_examples(session_dir: Path) -> None:
    source_dir = SCRIPT_DIR / "config" / "data_examples"
    shutil.copytree(source_dir, session_dir, dirs_exist_ok=True)


def make_estimated_params(
    session_dir: Path,
    vehicle_id: str,
    selected_task: str,
    task_subtype: str,
    sensor_id: str,
) -> Path:
    parameter_version = f"{sensor_id}_{task_subtype}_smoke_v1"
    if selected_task == "camera_intrinsic":
        estimated_params = {
            "camera_intrinsics": {
                "image_width": 1920,
                "image_height": 1080,
                "camera_model": "pinhole",
                "distortion_model": "plumb_bob",
                "fx": 1000.0,
                "fy": 1001.0,
                "cx": 960.0,
                "cy": 540.0,
                "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0],
            }
        }
        validation_summary = {
            "reprojection_error_px": 0.25,
            "sample_count": 20,
            "auto_acceptance_passed": True,
        }
    elif selected_task == "sensor_extrinsic":
        estimated_params = {
            "sensor_extrinsics": {
                "parent_frame_id": "base_link",
                "child_frame_id": "lidar_3d_link",
                "translation_xyz_m": [0.5, 0.0, 1.2],
                "rotation_xyzw": [0.0, 0.0, 0.0, 1.0],
            }
        }
        validation_summary = {
            "translation_residual_m": 0.01,
            "rotation_residual_rad": 0.002,
            "plane_residual_m": 0.01,
            "sample_count": 10,
            "auto_acceptance_passed": True,
        }
    elif selected_task == "hand_eye":
        estimated_params = {
            "hand_eye": {
                "base_frame_id": "base_link",
                "tool_frame_id": "tool0",
                "camera_frame_id": "front_camera_link",
                "translation_xyz_m": [0.1, 0.0, 0.2],
                "rotation_xyzw": [0.0, 0.0, 0.0, 1.0],
            }
        }
        validation_summary = {
            "reprojection_error_px": 0.3,
            "translation_residual_m": 0.012,
            "rotation_residual_rad": 0.003,
            "repeatability_error_m": 0.004,
            "sample_count": 10,
            "auto_acceptance_passed": True,
        }
    else:
        raise ValueError(f"不支持的 selected_task：{selected_task}")

    output_path = session_dir / "sensor" / f"estimated_params_{task_subtype}.yaml"
    output = {
        "schema_version": 1,
        "session_id": session_dir.name,
        "vehicle_id": vehicle_id,
        "sensor_id": sensor_id,
        "selected_task": selected_task,
        "task_subtype": task_subtype,
        "parameter_version": parameter_version,
        "estimated_params": estimated_params,
        "quality": {
            "data_quality_passed": True,
            "suitable_for_commit": True,
            "validation_summary": validation_summary,
        },
    }
    write_yaml(output_path, output)
    return output_path


def stage_approve_apply(
    session_dir: Path,
    dataset_index: Path,
    estimated_params: Path,
    task_subtype: str,
    operator_id: str,
) -> dict[str, str]:
    pending = session_dir / "sensor" / f"pending_sensor_commit_{task_subtype}.yaml"
    approved = session_dir / "sensor" / f"approved_sensor_parameter_handoff_{task_subtype}.yaml"
    applied_dir = session_dir / "sensor" / "applied_parameters" / task_subtype

    run_command(
        [
            sys.executable,
            str(SCRIPT_DIR / "stage_sensor_parameter_commit.py"),
            "--dataset-index",
            str(dataset_index),
            "--estimated-params",
            str(estimated_params),
            "--output",
            str(pending),
            "--operator-id",
            operator_id,
        ],
        REPO_ROOT,
    )
    run_command(
        [
            sys.executable,
            str(SCRIPT_DIR / "approve_sensor_pending_parameters.py"),
            str(pending),
            "--operator-id",
            operator_id,
            "--output",
            str(approved),
        ],
        REPO_ROOT,
    )
    run_command(
        [
            sys.executable,
            str(SCRIPT_DIR / "apply_sensor_parameter_handoff.py"),
            str(approved),
            "--output-dir",
            str(applied_dir),
            "--operator-id",
            operator_id,
        ],
        REPO_ROOT,
    )
    return {
        "estimated_params": str(estimated_params),
        "pending_commit": str(pending),
        "approved_handoff": str(approved),
        "applied_dir": str(applied_dir),
    }


def run_smoke(session_dir: Path, args: argparse.Namespace) -> dict[str, Any]:
    copy_examples(session_dir)
    dataset_index = session_dir / "dataset_index.yaml"
    site_data_input = session_dir / "site_data_input.yaml"
    preprocess_report = session_dir / "sensor" / "preprocess_quality_report.json"

    run_command(
        [
            sys.executable,
            str(SCRIPT_DIR / "sensor_dataset_manifest.py"),
            "--dataset-root",
            str(session_dir),
            "--dataset-index",
            str(dataset_index),
            "--session-id",
            session_dir.name,
            "--site-id",
            args.site_id,
            "--vehicle-id",
            args.vehicle_id,
            "--image-index",
            "images.yaml",
            "--target-detections",
            "charuco_detections.json",
            "--imu-samples",
            "imu_samples.csv",
            "--imu-segments",
            "imu_segments.yaml",
            "--pointcloud-index",
            "pointclouds.yaml",
            "--laser-scan-index",
            "laser_scans.yaml",
            "--robot-poses",
            "robot_poses.csv",
            "--synchronized-dataset",
            "synchronized_sensor_dataset.yaml",
            "--overwrite",
        ],
        REPO_ROOT,
    )
    run_command(
        [
            sys.executable,
            str(SCRIPT_DIR / "sensor_preprocess_pipeline.py"),
            "--dataset-root",
            str(session_dir),
            "--dataset-index",
            str(dataset_index),
            "--image-index",
            "images.yaml",
            "--target-detections",
            "charuco_detections.json",
            "--imu-samples",
            "imu_samples.csv",
            "--imu-segments",
            "imu_segments.yaml",
            "--pointcloud-index",
            "pointclouds.yaml",
            "--laser-scan-index",
            "laser_scans.yaml",
            "--robot-poses",
            "robot_poses.csv",
            "--output-report",
            str(preprocess_report),
            "--max-image-missing-ratio",
            "1.0",
        ],
        REPO_ROOT,
    )
    run_command(
        [
            sys.executable,
            str(DEPLOYMENT_TOOLS / "dataset_index_to_site_data_input.py"),
            str(dataset_index),
            "-o",
            str(site_data_input),
            "--strict",
        ],
        REPO_ROOT,
    )
    run_command(
        [
            sys.executable,
            str(DEPLOYMENT_TOOLS / "validate_dataset_index.py"),
            str(dataset_index),
            "--tasks",
            "sensor_intrinsic,sensor_extrinsic,hand_eye",
        ],
        REPO_ROOT,
    )

    tasks = [
        ("camera_intrinsic", "front_camera_intrinsic", "demo_front_camera"),
        ("sensor_extrinsic", "lidar_3d_extrinsic", "demo_lidar_3d"),
        ("hand_eye", "eye_in_hand", "demo_front_camera"),
    ]
    generated: list[dict[str, str]] = []
    for selected_task, task_subtype, sensor_id in tasks:
        estimated_params = make_estimated_params(
            session_dir,
            args.vehicle_id,
            selected_task,
            task_subtype,
            sensor_id,
        )
        generated.append(
            stage_approve_apply(
                session_dir,
                dataset_index,
                estimated_params,
                task_subtype,
                args.operator_id,
            )
        )

    report = {
        "schema_version": 1,
        "success": True,
        "session_dir": str(session_dir),
        "dataset_index": str(dataset_index),
        "site_data_input": str(site_data_input),
        "preprocess_report": str(preprocess_report),
        "generated": generated,
    }
    report_path = session_dir / "sensor" / "sensor_smoke_report.yaml"
    write_yaml(report_path, report)
    return report


def main() -> int:
    args = parse_args()
    if args.session_dir:
        session_dir = Path(args.session_dir).expanduser().resolve(strict=False)
        session_dir.mkdir(parents=True, exist_ok=True)
        report = run_smoke(session_dir, args)
        print(f"[OK] sensor 标定本地 smoke 通过：{report['session_dir']}")
        return 0

    with tempfile.TemporaryDirectory(prefix="agv_sensor_smoke_") as temp_dir:
        session_dir = Path(temp_dir).resolve(strict=False)
        report = run_smoke(session_dir, args)
        print(f"[OK] sensor 标定本地 smoke 通过：{report['session_dir']}")
        if args.keep_session_dir:
            keep_dir = Path("/tmp") / f"{session_dir.name}_kept"
            if keep_dir.exists():
                shutil.rmtree(keep_dir)
            shutil.copytree(session_dir, keep_dir)
            print(f"[OK] 已保留 smoke 输出：{keep_dir}")
    return 0


if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except Exception as exc:
        print(f"[错误] {exc}", file=sys.stderr)
        raise SystemExit(1)
