#!/usr/bin/env python3
"""校验传感器标定现场任务配置，并导出总控任务片段。"""

from __future__ import annotations

import argparse
import sys
from pathlib import Path
from typing import Any

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


TASK_GROUPS = {
    "camera_intrinsic",
    "imu_intrinsic",
    "sensor_extrinsic",
    "hand_eye",
}

TASK_GROUP_TO_STAGE_TYPE = {
    "camera_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE",
    "imu_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE",
    "sensor_extrinsic": "SENSOR_EXTRINSIC_CALIBRATION_STAGE",
    "hand_eye": "HAND_EYE_CALIBRATION_STAGE",
}

TASK_GROUP_TO_REQUESTED_TASK = {
    "camera_intrinsic": "sensor_intrinsic",
    "imu_intrinsic": "sensor_intrinsic",
    "sensor_extrinsic": "sensor_extrinsic",
    "hand_eye": "hand_eye",
}

SUPPORTED_SUBTYPES = {
    "camera_intrinsic": {
        "front_camera_intrinsic",
        "downward_camera_intrinsic",
    },
    "imu_intrinsic": {
        "imu_intrinsic",
    },
    "sensor_extrinsic": {
        "front_camera_extrinsic",
        "downward_camera_extrinsic",
        "imu_extrinsic",
        "lidar_2d_extrinsic",
        "lidar_3d_extrinsic",
    },
    "hand_eye": {
        "eye_in_hand",
        "eye_to_hand",
    },
}


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="校验并导出传感器标定现场 profile。")
    parser.add_argument(
        "--profile",
        default="src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml",
        help="传感器标定 profile YAML 路径。",
    )
    parser.add_argument(
        "--tasks",
        default="sensor_intrinsic,sensor_extrinsic,hand_eye",
        help="只导出指定总控任务，逗号分隔：sensor_intrinsic,sensor_extrinsic,hand_eye。",
    )
    parser.add_argument(
        "--format",
        choices=["summary", "requested_tasks"],
        default="summary",
        help="输出格式：summary 为摘要，requested_tasks 为总控任务片段。",
    )
    parser.add_argument("-o", "--output", help="输出文件路径；不填写时输出到标准输出。")
    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 require_map(value: Any, field_name: str) -> dict[str, Any]:
    if not isinstance(value, dict):
        raise ValueError(f"{field_name} 必须是 YAML map。")
    return value


def require_list(value: Any, field_name: str) -> list[Any]:
    if not isinstance(value, list):
        raise ValueError(f"{field_name} 必须是 YAML list。")
    return value


def require_string(value: Any, field_name: str) -> str:
    if not isinstance(value, str) or not value.strip():
        raise ValueError(f"{field_name} 必须是非空字符串。")
    return value.strip()


def as_int(value: Any, field_name: str) -> int:
    try:
        return int(value)
    except (TypeError, ValueError) as exc:
        raise ValueError(f"{field_name} 必须是整数，当前值为 {value!r}。") from exc


def as_float(value: Any, field_name: str) -> float:
    try:
        return float(value)
    except (TypeError, ValueError) as exc:
        raise ValueError(f"{field_name} 必须是数字，当前值为 {value!r}。") from exc


def validate_positive_int(value: Any, field_name: str) -> int:
    number = as_int(value, field_name)
    if number <= 0:
        raise ValueError(f"{field_name} 必须大于 0。")
    return number


def validate_positive_float(value: Any, field_name: str) -> float:
    number = as_float(value, field_name)
    if number <= 0.0:
        raise ValueError(f"{field_name} 必须大于 0。")
    return number


def stringify_value(value: Any) -> str:
    if isinstance(value, bool):
        return "true" if value else "false"
    if isinstance(value, list):
        return ",".join(stringify_value(item) for item in value)
    return str(value)


def parse_requested_tasks(raw_tasks: str | list[str] | None) -> set[str]:
    if raw_tasks is None:
        return set()
    if isinstance(raw_tasks, list):
        items = raw_tasks
    else:
        items = raw_tasks.split(",")
    result: set[str] = set()
    for item in items:
        task = str(item).strip()
        if not task:
            continue
        if task == "all":
            return {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"}
        if task not in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"}:
            raise ValueError(f"不支持的传感器总控任务：{task}")
        result.add(task)
    return result


def validate_camera_intrinsic(task: dict[str, Any], task_name: str) -> None:
    payload = require_map(task.get("camera_intrinsic"), f"{task_name}.camera_intrinsic")
    validate_positive_int(
        payload.get("required_image_count"),
        f"{task_name}.camera_intrinsic.required_image_count",
    )
    if "target_board_id" in payload and payload["target_board_id"] not in (None, ""):
        require_string(payload["target_board_id"], f"{task_name}.camera_intrinsic.target_board_id")
    if "timeout_sec" in payload:
        validate_positive_float(payload["timeout_sec"], f"{task_name}.camera_intrinsic.timeout_sec")


def validate_imu_intrinsic(task: dict[str, Any], task_name: str) -> None:
    payload = require_map(task.get("imu_intrinsic"), f"{task_name}.imu_intrinsic")
    validate_positive_int(
        payload.get("required_static_segment_count"),
        f"{task_name}.imu_intrinsic.required_static_segment_count",
    )
    validate_positive_int(
        payload.get("required_motion_segment_count"),
        f"{task_name}.imu_intrinsic.required_motion_segment_count",
    )
    if "timeout_sec" in payload:
        validate_positive_float(payload["timeout_sec"], f"{task_name}.imu_intrinsic.timeout_sec")


def validate_sensor_extrinsic(task: dict[str, Any], task_name: str) -> None:
    payload = require_map(task.get("sensor_extrinsic"), f"{task_name}.sensor_extrinsic")
    require_string(payload.get("base_frame_id"), f"{task_name}.sensor_extrinsic.base_frame_id")
    validate_positive_int(
        payload.get("required_sample_count"),
        f"{task_name}.sensor_extrinsic.required_sample_count",
    )
    if "timeout_sec" in payload:
        validate_positive_float(payload["timeout_sec"], f"{task_name}.sensor_extrinsic.timeout_sec")


def validate_hand_eye(task: dict[str, Any], task_name: str) -> None:
    payload = require_map(task.get("hand_eye"), f"{task_name}.hand_eye")
    require_string(payload.get("arm_id"), f"{task_name}.hand_eye.arm_id")
    validate_positive_int(
        payload.get("required_pose_count"),
        f"{task_name}.hand_eye.required_pose_count",
    )
    if "timeout_sec" in payload:
        validate_positive_float(payload["timeout_sec"], f"{task_name}.hand_eye.timeout_sec")


def validate_task(task: dict[str, Any], index: int) -> None:
    task_name = f"tasks[{index}]"
    require_string(task.get("task_code"), f"{task_name}.task_code")
    selected_task = require_string(task.get("selected_task"), f"{task_name}.selected_task")
    if selected_task not in TASK_GROUPS:
        raise ValueError(f"{task_name}.selected_task 必须是 {sorted(TASK_GROUPS)} 之一。")

    require_string(task.get("sensor_id"), f"{task_name}.sensor_id")
    task_subtype = require_string(task.get("task_subtype"), f"{task_name}.task_subtype")
    if task_subtype not in SUPPORTED_SUBTYPES[selected_task]:
        raise ValueError(
            f"{task_name}.task_subtype={task_subtype} 不适用于 selected_task={selected_task}。"
        )

    if selected_task == "camera_intrinsic":
        validate_camera_intrinsic(task, task_name)
    elif selected_task == "imu_intrinsic":
        validate_imu_intrinsic(task, task_name)
    elif selected_task == "sensor_extrinsic":
        validate_sensor_extrinsic(task, task_name)
    elif selected_task == "hand_eye":
        validate_hand_eye(task, task_name)


def validate_profile(profile: dict[str, Any]) -> dict[str, Any]:
    schema_version = int(profile.get("schema_version", 0) or 0)
    if schema_version != 1:
        raise ValueError("schema_version 必须为 1。")

    data_capture = require_map(profile.get("data_capture", {}), "data_capture")
    required_inputs = require_list(
        data_capture.get("required_data_inputs", []),
        "data_capture.required_data_inputs",
    )
    for item in required_inputs:
        require_string(item, "data_capture.required_data_inputs[]")

    tasks = require_list(profile.get("tasks"), "tasks")
    if not tasks:
        raise ValueError("tasks 不能为空。")
    seen_task_codes: set[str] = set()
    for index, task_value in enumerate(tasks):
        task = require_map(task_value, f"tasks[{index}]")
        task_code = require_string(task.get("task_code"), f"tasks[{index}].task_code")
        if task_code in seen_task_codes:
            raise ValueError(f"重复的 task_code：{task_code}")
        seen_task_codes.add(task_code)
        validate_task(task, index)
    return profile


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


def task_matches_requested(task: dict[str, Any], requested_tasks: set[str]) -> bool:
    if not requested_tasks:
        return True
    selected_task = str(task["selected_task"])
    return TASK_GROUP_TO_REQUESTED_TASK[selected_task] in requested_tasks


def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]:
    selected_task = str(task["selected_task"])
    params = [
        make_task_param("sensor.sensor_id", task["sensor_id"]),
        make_task_param("sensor.task_subtype", task["task_subtype"]),
    ]

    if selected_task == "camera_intrinsic":
        payload = task["camera_intrinsic"]
        params.append(make_task_param("camera_intrinsic.required_image_count", payload["required_image_count"]))
        if payload.get("target_board_id") not in (None, ""):
            params.append(make_task_param("camera_intrinsic.target_board_id", payload["target_board_id"]))
        if payload.get("timeout_sec") not in (None, ""):
            params.append(make_task_param("camera_intrinsic.timeout_sec", payload["timeout_sec"]))
    elif selected_task == "imu_intrinsic":
        payload = task["imu_intrinsic"]
        params.append(
            make_task_param(
                "imu_intrinsic.required_static_segment_count",
                payload["required_static_segment_count"],
            )
        )
        params.append(
            make_task_param(
                "imu_intrinsic.required_motion_segment_count",
                payload["required_motion_segment_count"],
            )
        )
        if payload.get("timeout_sec") not in (None, ""):
            params.append(make_task_param("imu_intrinsic.timeout_sec", payload["timeout_sec"]))
    elif selected_task == "sensor_extrinsic":
        payload = task["sensor_extrinsic"]
        params.append(make_task_param("sensor_extrinsic.base_frame_id", payload["base_frame_id"]))
        params.append(make_task_param("sensor_extrinsic.required_sample_count", payload["required_sample_count"]))
        if payload.get("timeout_sec") not in (None, ""):
            params.append(make_task_param("sensor_extrinsic.timeout_sec", payload["timeout_sec"]))
    elif selected_task == "hand_eye":
        payload = task["hand_eye"]
        params.append(make_task_param("hand_eye.arm_id", payload["arm_id"]))
        params.append(make_task_param("hand_eye.required_pose_count", payload["required_pose_count"]))
        if payload.get("timeout_sec") not in (None, ""):
            params.append(make_task_param("hand_eye.timeout_sec", payload["timeout_sec"]))
    return params


def export_requested_tasks(
    profile: dict[str, Any],
    requested_tasks: set[str] | list[str] | str | None = None,
) -> dict[str, Any]:
    selected_requested_tasks = (
        requested_tasks
        if isinstance(requested_tasks, set)
        else parse_requested_tasks(requested_tasks)
    )
    tasks: list[dict[str, Any]] = []
    for task in profile["tasks"]:
        if not bool(task.get("enabled", True)):
            continue
        if not task_matches_requested(task, selected_requested_tasks):
            continue
        selected_task = str(task["selected_task"])
        tasks.append({
            "stage_type": TASK_GROUP_TO_STAGE_TYPE[selected_task],
            "enabled": True,
            "require_manual_approval": bool(task.get("require_manual_approval", False)),
            "execution_policy": str(task.get("execution_policy", "REQUIRED")),
            "reason": str(task.get("reason", "现场传感器标定 profile")),
            "task_code": str(task["task_code"]),
            "target_id": str(task["sensor_id"]),
            "task_params": flatten_task_metadata(task),
        })
    return {
        "schema_version": 1,
        "requested_tasks": tasks,
    }


def build_summary(profile: dict[str, Any]) -> dict[str, Any]:
    return {
        "schema_version": profile["schema_version"],
        "profile_name": profile.get("profile_name", ""),
        "task_count": len(profile["tasks"]),
        "tasks": [
            {
                "task_code": task["task_code"],
                "enabled": bool(task.get("enabled", True)),
                "selected_task": task["selected_task"],
                "sensor_id": task["sensor_id"],
                "task_subtype": task["task_subtype"],
                "display_name": task.get("display_name", ""),
            }
            for task in profile["tasks"]
        ],
    }


def render_yaml(data: dict[str, Any]) -> str:
    return yaml.safe_dump(data, sort_keys=False, allow_unicode=True)


def main() -> int:
    args = parse_args()
    profile_path = Path(args.profile).expanduser().resolve(strict=False)
    profile = validate_profile(load_yaml(profile_path))

    if args.format == "requested_tasks":
        output = export_requested_tasks(profile, parse_requested_tasks(args.tasks))
    else:
        output = build_summary(profile)

    rendered = render_yaml(output)
    if args.output:
        output_path = Path(args.output).expanduser().resolve(strict=False)
        output_path.parent.mkdir(parents=True, exist_ok=True)
        output_path.write_text(rendered, encoding="utf-8")
        print(f"[OK] 已写入传感器标定 profile 输出：{output_path}", file=sys.stderr)
    else:
        print(rendered, end="")
    return 0


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