#!/usr/bin/env python3
"""从现场 profile 生成总控会话配置和 requested_tasks。"""

from __future__ import annotations

import argparse
import importlib.util
import sys
from pathlib import Path
from typing import Any

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


class NoAliasDumper(yaml.SafeDumper):
    def ignore_aliases(self, data: Any) -> bool:
        return True


REPO_ROOT = Path(__file__).resolve().parents[3]

DEFAULT_TASKS = "external,chassis,control,sensor_intrinsic"

STAGE_TYPES = {
    "external": "EXTERNAL_REFERENCE_READY_CHECK_STAGE",
    "chassis": "CHASSIS_CALIBRATION_STAGE",
    "control": "CONTROL_CALIBRATION_STAGE",
    "sensor_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE",
    "sensor_extrinsic": "SENSOR_EXTRINSIC_CALIBRATION_STAGE",
    "hand_eye": "HAND_EYE_CALIBRATION_STAGE",
}

SUPPORTED_CHASSIS_TYPES = {
    "ackermann",
    "differential",
    "single_steer_wheel",
    "multi_steer_wheel",
}

PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx")


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(
        description="根据现场 site profile、底盘动作 profile、运控评估 profile 和传感器标定 profile 生成总控会话配置。"
    )
    parser.add_argument(
        "--site-profile",
        default="src/deployment/profiles/site_template.yaml",
        help="现场部署 profile YAML。",
    )
    parser.add_argument(
        "--tasks",
        default=DEFAULT_TASKS,
        help=f"逗号分隔任务列表，默认: {DEFAULT_TASKS}",
    )
    parser.add_argument("--chassis-type", default="", help="覆盖底盘类型。")
    parser.add_argument("--chassis-action-profile", default="", help="覆盖底盘动作 profile 路径。")
    parser.add_argument("--control-evaluation-profile", default="", help="覆盖运控评估 profile 路径。")
    parser.add_argument("--sensor-calibration-profile", default="", help="覆盖传感器标定 profile 路径。")
    parser.add_argument(
        "--profile-mode",
        choices=["auto", "minimal", "required"],
        default="auto",
        help="profile 任务生成策略：auto 可用则展开，minimal 强制最小任务，required 要求 profile 文件存在。",
    )
    parser.add_argument("--reference-target-id", default="site_reference_target", help="外部真值/传感器默认目标 ID。")
    parser.add_argument("--sensor-id", default="demo_front_camera", help="默认传感器内参任务 sensor_id。")
    parser.add_argument("--sensor-task-subtype", default="front_camera_intrinsic", help="默认传感器内参任务子类型。")
    parser.add_argument("--output", "-o", default="", help="输出 YAML；不填写时输出到标准输出。")
    parser.add_argument(
        "--format",
        choices=["session_config", "requested_tasks"],
        default="session_config",
        help="输出完整 session_config 或只输出 requested_tasks。",
    )
    return parser.parse_args()


def load_yaml(path: Path) -> dict[str, Any]:
    with path.open("r", encoding="utf-8") as stream:
        data = yaml.safe_load(stream)
    if data is None:
        return {}
    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.dump(data, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True),
        encoding="utf-8",
    )


def parse_tasks(raw: str) -> list[str]:
    if raw.strip().lower() == "all":
        raw = DEFAULT_TASKS
    tasks: list[str] = []
    for item in raw.split(","):
        task_name = item.strip()
        if not task_name:
            continue
        if task_name not in STAGE_TYPES:
            raise ValueError(f"不支持的任务名: {task_name}。")
        tasks.append(task_name)
    if not tasks:
        raise ValueError("至少需要指定一个任务。")
    return tasks


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


def read_string(data: dict[str, Any], path: tuple[str, ...], default: str = "") -> str:
    cursor: Any = data
    for key in path:
        if not isinstance(cursor, dict):
            return default
        cursor = cursor.get(key)
    if cursor in (None, "") or has_placeholder(cursor):
        return default
    return str(cursor)


def resolve_path(raw_path: str, site_profile_path: Path) -> Path:
    path = Path(raw_path).expanduser()
    if path.is_absolute():
        return path.resolve(strict=False)
    site_relative = (site_profile_path.parent / path).resolve(strict=False)
    if site_relative.exists():
        return site_relative
    return (REPO_ROOT / path).resolve(strict=False)


def resolve_configured_path(
    override_path: str,
    site_profile: dict[str, Any],
    site_profile_path: Path,
    config_path: tuple[str, ...],
) -> Path | None:
    raw_path = override_path or read_string(site_profile, config_path)
    if not raw_path:
        return None
    return resolve_path(raw_path, site_profile_path)


def load_module(module_name: str, path: Path) -> Any:
    spec = importlib.util.spec_from_file_location(module_name, path)
    if spec is None or spec.loader is None:
        raise RuntimeError(f"无法加载模块: {path}")
    module = importlib.util.module_from_spec(spec)
    spec.loader.exec_module(module)
    return module


def profile_tasks_available(profile_path: Path | None, profile_mode: str, module_name: str) -> bool:
    if profile_mode == "minimal":
        return False
    if profile_path is not None and profile_path.exists():
        return True
    if profile_mode == "required":
        raise FileNotFoundError(f"{module_name} profile 文件不存在: {profile_path}")
    if profile_path is not None:
        print(f"[WARN] {module_name} profile 文件不存在，使用最小任务: {profile_path}", file=sys.stderr)
    return False


def normalize_profile_task(raw_task: dict[str, Any]) -> dict[str, Any]:
    task = {
        "stage_type": str(raw_task["stage_type"]),
        "enabled": bool(raw_task.get("enabled", True)),
        "require_manual_approval": bool(raw_task.get("require_manual_approval", False)),
        "execution_policy": str(raw_task.get("execution_policy", "REQUIRED")),
        "reason": str(raw_task.get("reason", "现场 profile 自动生成")),
        "task_code": str(raw_task.get("task_code", "")),
        "target_id": str(raw_task.get("target_id", "")),
        "task_params": [],
    }
    for item in raw_task.get("task_params", []):
        task["task_params"].append({
            "key": str(item["key"]),
            "value": str(item["value"]),
        })
    if not task["task_code"]:
        raise ValueError("profile 导出的 task_code 不能为空。")
    return task


def export_chassis_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py"
    tool = load_module("chassis_action_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
    exported = tool.export_requested_tasks(profile, chassis_type)
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]


def export_control_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py"
    tool = load_module("control_evaluation_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
    exported = tool.export_requested_tasks(profile, chassis_type)
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]


def export_sensor_profile_tasks(profile_path: Path, task_name: str) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py"
    tool = load_module("sensor_calibration_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
    exported = tool.export_requested_tasks(profile, {task_name})
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]


def task_param(key: str, value: Any) -> dict[str, str]:
    return {
        "key": key,
        "value": str(value),
    }


def minimal_task(
    task_name: str,
    args: argparse.Namespace,
    site_profile: dict[str, Any],
    chassis_type: str,
) -> dict[str, Any]:
    task = {
        "stage_type": STAGE_TYPES[task_name],
        "enabled": True,
        "require_manual_approval": False,
        "execution_policy": "REQUIRED",
        "reason": "site profile 最小总控任务",
        "task_code": task_name,
        "target_id": "",
        "task_params": [],
    }
    if task_name == "external":
        task["target_id"] = args.reference_target_id
        task["task_params"] = [
            task_param("external.static_sample_count", "1"),
            task_param("external.dynamic_sample_count", "1"),
            task_param("external.require_short_motion_segment", "false"),
            task_param("external.max_position_stddev_m", "0.05"),
            task_param("external.max_yaw_stddev_rad", "0.05"),
            task_param("external.max_tracking_loss_ratio", "0.05"),
            task_param("external.max_time_sync_offset_ms", "50.0"),
            task_param("external.timeout_sec", "5.0"),
        ]
    elif task_name == "chassis":
        task["target_id"] = chassis_type
        task["task_params"] = [
            task_param("primitive_type", "straight_line"),
            task_param("straight_line.target_distance_m", "0.2"),
            task_param("straight_line.target_speed_ms", "0.1"),
            task_param("straight_line.reverse", "false"),
            task_param("brake_when_finished", "true"),
            task_param("timeout_sec", "10.0"),
        ]
    elif task_name == "control":
        task["target_id"] = chassis_type
        task["task_params"] = [
            task_param("control.task_type", "velocity_step"),
            task_param("velocity_step.target_velocity_ms", "0.1"),
            task_param("velocity_step.hold_time_sec", "0.5"),
            task_param("velocity_step.settle_before_step_sec", "0.1"),
            task_param("control.timeout_sec", "10.0"),
        ]
    elif task_name == "sensor_intrinsic":
        task["target_id"] = args.sensor_id
        task["task_params"] = [
            task_param("sensor.sensor_id", args.sensor_id),
            task_param("sensor.task_subtype", args.sensor_task_subtype),
            task_param("camera_intrinsic.required_image_count", "1"),
            task_param("camera_intrinsic.target_board_id", args.reference_target_id),
            task_param("camera_intrinsic.timeout_sec", "5.0"),
        ]
    elif task_name == "sensor_extrinsic":
        task["target_id"] = args.sensor_id
        task["task_params"] = [
            task_param("sensor.sensor_id", args.sensor_id),
            task_param("sensor.task_subtype", "front_camera_extrinsic"),
            task_param("sensor_extrinsic.base_frame_id", read_string(site_profile, ("frames", "base_link"), "base_link")),
            task_param("sensor_extrinsic.required_sample_count", "1"),
            task_param("sensor_extrinsic.timeout_sec", "5.0"),
        ]
    elif task_name == "hand_eye":
        task["target_id"] = args.sensor_id
        task["task_params"] = [
            task_param("sensor.sensor_id", args.sensor_id),
            task_param("sensor.task_subtype", "eye_in_hand"),
            task_param("hand_eye.arm_id", "demo_arm"),
            task_param("hand_eye.required_pose_count", "1"),
            task_param("hand_eye.timeout_sec", "5.0"),
        ]
    return task


def resolve_chassis_type(args: argparse.Namespace, site_profile: dict[str, Any]) -> str:
    chassis_type = (
        args.chassis_type
        or read_string(site_profile, ("chassis_calibration", "chassis_type"))
        or read_string(site_profile, ("control_calibration", "chassis_type"))
        or "ackermann"
    )
    if chassis_type not in SUPPORTED_CHASSIS_TYPES:
        raise ValueError(f"chassis_type 不受支持: {chassis_type}")
    return chassis_type


def build_requested_tasks(
    tasks: list[str],
    args: argparse.Namespace,
    site_profile: dict[str, Any],
    site_profile_path: Path,
    chassis_type: str,
) -> list[dict[str, Any]]:
    chassis_profile_path = resolve_configured_path(
        args.chassis_action_profile,
        site_profile,
        site_profile_path,
        ("chassis_calibration", "action_profile_file"),
    )
    control_profile_path = resolve_configured_path(
        args.control_evaluation_profile,
        site_profile,
        site_profile_path,
        ("control_calibration", "evaluation_profile_file"),
    )
    sensor_profile_path = resolve_configured_path(
        args.sensor_calibration_profile,
        site_profile,
        site_profile_path,
        ("sensor_calibration", "profile_file"),
    )

    requested_tasks: list[dict[str, Any]] = []
    for task_name in tasks:
        if task_name == "chassis" and profile_tasks_available(chassis_profile_path, args.profile_mode, "底盘动作"):
            requested_tasks.extend(export_chassis_profile_tasks(chassis_profile_path, chassis_type))
            continue
        if task_name == "control" and profile_tasks_available(control_profile_path, args.profile_mode, "运控评估"):
            requested_tasks.extend(export_control_profile_tasks(control_profile_path, chassis_type))
            continue
        if (
            task_name in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"} and
            profile_tasks_available(sensor_profile_path, args.profile_mode, "传感器标定")
        ):
            requested_tasks.extend(export_sensor_profile_tasks(sensor_profile_path, task_name))
            continue
        requested_tasks.append(minimal_task(task_name, args, site_profile, chassis_type))
    return requested_tasks


def build_session_config(
    site_profile_path: Path,
    site_profile: dict[str, Any],
    requested_tasks: list[dict[str, Any]],
    args: argparse.Namespace,
) -> dict[str, Any]:
    reference_source_name = (
        read_string(site_profile, ("external_pose_bridge", "reference_source_name"))
        or read_string(site_profile, ("external_localization", "reference_source_name"))
        or "workshop_external_truth"
    )
    workcell_zone_id = read_string(site_profile, ("external_localization", "workcell_zone_id"), "default_workcell")
    return {
        "schema_version": 1,
        "source_site_profile": str(site_profile_path),
        "session_config": {
            "auto_commit_parameters": False,
            "require_manual_approval_before_commit": False,
            "run_validation_after_each_stage": False,
            "stop_on_first_failure": True,
            "allow_optional_stage_skip": False,
            "enable_auto_rollback_on_validation_failure": False,
            "allow_rebuild_execution_plan": False,
            "localization_source_id": reference_source_name,
            "workcell_zone_id": workcell_zone_id,
            "reference_target_id": args.reference_target_id,
            "requested_tasks": requested_tasks,
        },
        "requested_tasks": requested_tasks,
    }


def main() -> int:
    args = parse_args()
    site_profile_path = Path(args.site_profile).expanduser().resolve(strict=False)
    site_profile = load_yaml(site_profile_path)
    tasks = parse_tasks(args.tasks)
    chassis_type = resolve_chassis_type(args, site_profile)
    requested_tasks = build_requested_tasks(tasks, args, site_profile, site_profile_path, chassis_type)
    output = (
        {"schema_version": 1, "requested_tasks": requested_tasks}
        if args.format == "requested_tasks"
        else build_session_config(site_profile_path, site_profile, requested_tasks, args)
    )
    if args.output:
        write_yaml(Path(args.output).expanduser().resolve(strict=False), output)
        print(f"[OK] 已写入总控会话配置: {args.output}", file=sys.stderr)
    else:
        print(yaml.dump(output, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True), end="")
    return 0


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