#!/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


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

PRIMITIVE_REQUIRED_KEYS = {
    "straight_line": [
        "straight_line.target_distance_m",
        "straight_line.target_speed_ms",
    ],
    "arc": [
        "arc.target_speed_ms",
        "arc.radius_m",
        "arc.sweep_angle_deg",
    ],
    "in_place_rotation": [
        "in_place_rotation.target_yaw_deg",
        "in_place_rotation.target_angular_vel_deg_s",
    ],
    "steering_sweep": [
        "steering_sweep.target_angle_deg",
        "steering_sweep.sweep_amplitude_deg",
        "steering_sweep.sweep_frequency_hz",
        "steering_sweep.duration_sec",
    ],
    "lateral_translation": [
        "lateral_translation.target_speed_ms",
        "lateral_translation.target_distance_m",
    ],
    "diagonal_motion": [
        "diagonal_motion.target_speed_ms",
        "diagonal_motion.target_distance_m",
        "diagonal_motion.heading_deg",
    ],
    "module_alignment": [
        "module_alignment.module_ids",
        "module_alignment.target_zero_deg",
        "module_alignment.tolerance_deg",
    ],
    "coordinated_steering": [
        "coordinated_steering.module_ids",
        "coordinated_steering.target_angle_deg",
        "coordinated_steering.hold_time_sec",
    ],
}

COMMON_REQUIRED_KEYS = [
    "brake_when_finished",
    "timeout_sec",
]

POSITIVE_VALUE_KEYS = {
    "straight_line.target_distance_m",
    "straight_line.target_speed_ms",
    "arc.target_speed_ms",
    "arc.radius_m",
    "in_place_rotation.target_angular_vel_deg_s",
    "steering_sweep.sweep_amplitude_deg",
    "steering_sweep.sweep_frequency_hz",
    "steering_sweep.duration_sec",
    "lateral_translation.target_speed_ms",
    "lateral_translation.target_distance_m",
    "diagonal_motion.target_speed_ms",
    "diagonal_motion.target_distance_m",
    "module_alignment.tolerance_deg",
    "coordinated_steering.hold_time_sec",
    "timeout_sec",
}

NON_ZERO_VALUE_KEYS = {
    "arc.sweep_angle_deg",
    "in_place_rotation.target_yaw_deg",
}

LINEAR_SPEED_KEYS = {
    "straight_line.target_speed_ms",
    "arc.target_speed_ms",
    "lateral_translation.target_speed_ms",
    "diagonal_motion.target_speed_ms",
}


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="校验并导出底盘标定现场动作 profile。")
    parser.add_argument(
        "--profile",
        default="src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml",
        help="底盘动作 profile YAML 路径。",
    )
    parser.add_argument(
        "--chassis-type",
        choices=sorted(SUPPORTED_CHASSIS_TYPES),
        help="只导出指定底盘类型；不填写时只做整表校验和摘要输出。",
    )
    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_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 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 read_max_linear_speed(profile: dict[str, Any]) -> float:
    safety = profile.get("safety", {})
    if safety is None:
        return 0.0
    safety_map = require_map(safety, "safety")
    raw_value = safety_map.get("max_linear_speed_ms", 0.0)
    if raw_value in (None, ""):
        return 0.0
    value = as_float(raw_value, "safety.max_linear_speed_ms")
    if value < 0.0:
        raise ValueError("safety.max_linear_speed_ms 不能小于 0。")
    return value


def validate_metadata_value(
    key: str,
    value: Any,
    action_name: str,
    max_linear_speed_ms: float,
) -> None:
    if key in POSITIVE_VALUE_KEYS and as_float(value, f"{action_name}.{key}") <= 0.0:
        raise ValueError(f"{action_name}.{key} 必须大于 0。")
    if key in NON_ZERO_VALUE_KEYS and as_float(value, f"{action_name}.{key}") == 0.0:
        raise ValueError(f"{action_name}.{key} 不能为 0。")
    if key in LINEAR_SPEED_KEYS and max_linear_speed_ms > 0.0:
        speed = as_float(value, f"{action_name}.{key}")
        if speed > max_linear_speed_ms:
            raise ValueError(
                f"{action_name}.{key}={speed} 超过安全上限 {max_linear_speed_ms}。"
            )
    if key.endswith("module_ids"):
        if isinstance(value, list):
            if not value:
                raise ValueError(f"{action_name}.{key} 不能为空。")
            for item in value:
                require_string(item, f"{action_name}.{key}[]")
            return
        require_string(value, f"{action_name}.{key}")


def validate_action(
    action: dict[str, Any],
    action_index: int,
    profile_name: str,
    max_linear_speed_ms: float,
) -> None:
    action_name = f"{profile_name}.actions[{action_index}]"
    task_code = require_string(action.get("task_code"), f"{action_name}.task_code")
    primitive_type = require_string(action.get("primitive_type"), f"{action_name}.primitive_type")
    if primitive_type not in PRIMITIVE_REQUIRED_KEYS:
        raise ValueError(f"{action_name}.primitive_type 不受支持：{primitive_type}")

    metadata = require_map(action.get("metadata"), f"{action_name}.metadata")
    missing_keys = [
        key
        for key in ["primitive_type", *PRIMITIVE_REQUIRED_KEYS[primitive_type], *COMMON_REQUIRED_KEYS]
        if key not in metadata and key != "primitive_type"
    ]
    if missing_keys:
        raise ValueError(f"{task_code} 缺少 metadata 字段：{', '.join(missing_keys)}")

    for key, value in metadata.items():
        validate_metadata_value(key, value, task_code, max_linear_speed_ms)


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

    read_max_linear_speed(profile)
    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[]")

    chassis_profiles = require_map(profile.get("chassis_profiles"), "chassis_profiles")
    missing_profiles = sorted(SUPPORTED_CHASSIS_TYPES - set(chassis_profiles))
    if missing_profiles:
        raise ValueError(f"缺少底盘类型配置：{', '.join(missing_profiles)}")

    max_linear_speed_ms = read_max_linear_speed(profile)
    for chassis_type, section_value in chassis_profiles.items():
        if chassis_type not in SUPPORTED_CHASSIS_TYPES:
            raise ValueError(f"未知底盘类型：{chassis_type}")
        section = require_map(section_value, f"chassis_profiles.{chassis_type}")
        require_list(section.get("calibration_targets"), f"chassis_profiles.{chassis_type}.calibration_targets")
        actions = require_list(section.get("actions"), f"chassis_profiles.{chassis_type}.actions")
        if not actions:
            raise ValueError(f"chassis_profiles.{chassis_type}.actions 不能为空。")
        seen_task_codes: set[str] = set()
        for index, action_value in enumerate(actions):
            action = require_map(action_value, f"chassis_profiles.{chassis_type}.actions[{index}]")
            task_code = require_string(
                action.get("task_code"),
                f"chassis_profiles.{chassis_type}.actions[{index}].task_code",
            )
            if task_code in seen_task_codes:
                raise ValueError(f"重复的 task_code：{task_code}")
            seen_task_codes.add(task_code)
            validate_action(action, index, f"chassis_profiles.{chassis_type}", max_linear_speed_ms)
    return profile


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


def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]:
    section = profile["chassis_profiles"][chassis_type]
    tasks: list[dict[str, Any]] = []
    for action in section["actions"]:
        metadata = dict(action["metadata"])
        metadata["primitive_type"] = action["primitive_type"]
        task_params = [
            make_task_param(key, metadata[key])
            for key in sorted(metadata)
        ]
        tasks.append({
            "stage_type": "CHASSIS_CALIBRATION_STAGE",
            "enabled": True,
            "require_manual_approval": False,
            "execution_policy": "REQUIRED",
            "reason": "现场底盘标定动作 profile",
            "task_code": action["task_code"],
            "target_id": chassis_type,
            "task_params": task_params,
        })
    return {
        "schema_version": 1,
        "chassis_type": chassis_type,
        "requested_tasks": tasks,
    }


def build_summary(profile: dict[str, Any]) -> dict[str, Any]:
    summary: dict[str, Any] = {
        "schema_version": profile["schema_version"],
        "profile_name": profile.get("profile_name", ""),
        "chassis_profiles": {},
    }
    for chassis_type, section in profile["chassis_profiles"].items():
        summary["chassis_profiles"][chassis_type] = {
            "action_count": len(section["actions"]),
            "actions": [
                {
                    "task_code": action["task_code"],
                    "primitive_type": action["primitive_type"],
                    "display_name": action.get("display_name", ""),
                }
                for action in section["actions"]
            ],
        }
    return summary


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":
        if not args.chassis_type:
            raise ValueError("--format requested_tasks 必须指定 --chassis-type。")
        output = export_requested_tasks(profile, args.chassis_type)
    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)
