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

SCRIPT_DIR = Path(__file__).resolve().parent
if str(SCRIPT_DIR) not in sys.path:
    sys.path.insert(0, str(SCRIPT_DIR))

from stage_control_parameter_commit import (
    CHASSIS_TYPES,
    CONTROL_AXES,
    CONTROLLER_ALGORITHMS,
    CONTROL_ROLES,
    validate_control_role,
)


TASK_TYPES = {
    "trajectory_tracking",
    "velocity_step",
    "acceleration_deceleration",
    "accel_decel",
    "stop_accuracy",
}

TASK_TYPE_TO_METADATA = {
    "trajectory_tracking": "trajectory_tracking",
    "velocity_step": "velocity_step",
    "acceleration_deceleration": "accel_decel",
    "accel_decel": "accel_decel",
    "stop_accuracy": "stop_accuracy",
}

TASK_PAYLOAD_KEYS = {
    "trajectory_tracking": "trajectory_tracking",
    "velocity_step": "velocity_step",
    "acceleration_deceleration": "accel_decel",
    "accel_decel": "accel_decel",
    "stop_accuracy": "stop_accuracy",
}

TRAJECTORY_POINT_KEYS = ["x_m", "y_m", "yaw_rad", "target_speed_ms"]


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="校验并导出运控评估现场 profile。")
    parser.add_argument(
        "--profile",
        default="src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml",
        help="运控评估 profile YAML 路径。",
    )
    parser.add_argument(
        "--chassis-type",
        choices=sorted(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 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 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_safety(profile: dict[str, Any]) -> dict[str, float]:
    safety = require_map(profile.get("safety", {}), "safety")
    max_linear_speed_ms = as_float(safety.get("max_linear_speed_ms", 0.0), "safety.max_linear_speed_ms")
    max_accel_ms2 = as_float(safety.get("max_accel_ms2", 0.0), "safety.max_accel_ms2")
    if max_linear_speed_ms < 0.0:
        raise ValueError("safety.max_linear_speed_ms 不能小于 0。")
    if max_accel_ms2 < 0.0:
        raise ValueError("safety.max_accel_ms2 不能小于 0。")
    return {
        "max_linear_speed_ms": max_linear_speed_ms,
        "max_accel_ms2": max_accel_ms2,
    }


def validate_non_negative(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 validate_positive(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 validate_speed(value: Any, field_name: str, safety: dict[str, float]) -> float:
    speed = validate_non_negative(value, field_name)
    max_linear_speed_ms = safety["max_linear_speed_ms"]
    if max_linear_speed_ms > 0.0 and speed > max_linear_speed_ms:
        raise ValueError(f"{field_name}={speed} 超过安全上限 {max_linear_speed_ms}。")
    return speed


def validate_accel(value: Any, field_name: str, safety: dict[str, float]) -> float:
    accel = as_float(value, field_name)
    max_accel_ms2 = safety["max_accel_ms2"]
    if max_accel_ms2 > 0.0 and abs(accel) > max_accel_ms2:
        raise ValueError(f"{field_name}={accel} 超过安全上限 {max_accel_ms2}。")
    if accel == 0.0:
        raise ValueError(f"{field_name} 不能为 0。")
    return accel


def validate_task_identity(
    action: dict[str, Any],
    action_name: str,
    chassis_type: str,
) -> tuple[str, str, str, str]:
    selected_task = require_string(action.get("selected_task"), f"{action_name}.selected_task")
    if selected_task not in TASK_TYPES:
        raise ValueError(f"{action_name}.selected_task 不受支持：{selected_task}")

    control_axis = require_string(action.get("control_axis"), f"{action_name}.control_axis")
    if control_axis not in CONTROL_AXES:
        raise ValueError(f"{action_name}.control_axis 必须是 {sorted(CONTROL_AXES)} 之一。")

    controller_algorithm = require_string(
        action.get("controller_algorithm"),
        f"{action_name}.controller_algorithm",
    )
    if controller_algorithm not in CONTROLLER_ALGORITHMS:
        raise ValueError(
            f"{action_name}.controller_algorithm 必须是 {sorted(CONTROLLER_ALGORITHMS)} 之一。"
        )

    control_role = require_string(action.get("control_role"), f"{action_name}.control_role")
    if control_role not in CONTROL_ROLES:
        raise ValueError(f"{action_name}.control_role 必须是 {sorted(CONTROL_ROLES)} 之一。")
    validate_control_role(controller_algorithm, control_axis, control_role, action_name, chassis_type)
    return selected_task, control_axis, controller_algorithm, control_role


def validate_trajectory_payload(
    payload: dict[str, Any],
    action_name: str,
    safety: dict[str, float],
) -> None:
    validate_positive(payload.get("timeout_sec"), f"{action_name}.trajectory_tracking.timeout_sec")
    path = require_list(payload.get("path"), f"{action_name}.trajectory_tracking.path")
    if len(path) < 2:
        raise ValueError(f"{action_name}.trajectory_tracking.path 至少需要 2 个轨迹点。")

    for index, point_value in enumerate(path):
        point = require_map(point_value, f"{action_name}.trajectory_tracking.path[{index}]")
        for key in TRAJECTORY_POINT_KEYS:
            if key not in point:
                raise ValueError(f"{action_name}.trajectory_tracking.path[{index}] 缺少 {key}。")
        as_float(point["x_m"], f"{action_name}.trajectory_tracking.path[{index}].x_m")
        as_float(point["y_m"], f"{action_name}.trajectory_tracking.path[{index}].y_m")
        as_float(point["yaw_rad"], f"{action_name}.trajectory_tracking.path[{index}].yaw_rad")
        validate_speed(
            point["target_speed_ms"],
            f"{action_name}.trajectory_tracking.path[{index}].target_speed_ms",
            safety,
        )

    if "max_external_pose_age_ms" in payload:
        validate_positive(
            payload["max_external_pose_age_ms"],
            f"{action_name}.trajectory_tracking.max_external_pose_age_ms",
        )
    if "min_external_pose_quality_score" in payload:
        quality = as_float(
            payload["min_external_pose_quality_score"],
            f"{action_name}.trajectory_tracking.min_external_pose_quality_score",
        )
        if quality < 0.0 or quality > 1.0:
            raise ValueError(f"{action_name}.trajectory_tracking.min_external_pose_quality_score 必须在 0 到 1 之间。")
    if "segment_index" in payload:
        segment_index = as_int(payload["segment_index"], f"{action_name}.trajectory_tracking.segment_index")
        if segment_index < 0:
            raise ValueError(f"{action_name}.trajectory_tracking.segment_index 不能小于 0。")
    if "total_segments" in payload:
        total_segments = as_int(payload["total_segments"], f"{action_name}.trajectory_tracking.total_segments")
        if total_segments < 0:
            raise ValueError(f"{action_name}.trajectory_tracking.total_segments 不能小于 0。")


def validate_velocity_step_payload(
    payload: dict[str, Any],
    action_name: str,
    safety: dict[str, float],
) -> None:
    validate_speed(payload.get("target_velocity_ms"), f"{action_name}.velocity_step.target_velocity_ms", safety)
    validate_positive(payload.get("hold_time_sec"), f"{action_name}.velocity_step.hold_time_sec")
    validate_non_negative(
        payload.get("settle_before_step_sec"),
        f"{action_name}.velocity_step.settle_before_step_sec",
    )


def validate_accel_decel_payload(
    payload: dict[str, Any],
    action_name: str,
    safety: dict[str, float],
) -> None:
    validate_speed(payload.get("start_velocity_ms"), f"{action_name}.accel_decel.start_velocity_ms", safety)
    validate_speed(payload.get("target_velocity_ms"), f"{action_name}.accel_decel.target_velocity_ms", safety)
    validate_accel(payload.get("target_accel_ms2"), f"{action_name}.accel_decel.target_accel_ms2", safety)
    validate_positive(payload.get("hold_time_sec"), f"{action_name}.accel_decel.hold_time_sec")


def validate_stop_accuracy_payload(payload: dict[str, Any], action_name: str) -> None:
    as_float(payload.get("target_stop_x_m"), f"{action_name}.stop_accuracy.target_stop_x_m")
    as_float(payload.get("target_stop_y_m"), f"{action_name}.stop_accuracy.target_stop_y_m")
    as_float(payload.get("target_stop_yaw_rad"), f"{action_name}.stop_accuracy.target_stop_yaw_rad")
    validate_positive(payload.get("timeout_sec"), f"{action_name}.stop_accuracy.timeout_sec")


def validate_action(
    action: dict[str, Any],
    action_index: int,
    chassis_type: str,
    safety: dict[str, float],
) -> None:
    action_name = f"chassis_profiles.{chassis_type}.actions[{action_index}]"
    require_string(action.get("task_code"), f"{action_name}.task_code")
    selected_task, _, _, _ = validate_task_identity(action, action_name, chassis_type)

    payload_key = TASK_PAYLOAD_KEYS[selected_task]
    payload = require_map(action.get(payload_key), f"{action_name}.{payload_key}")
    if selected_task == "trajectory_tracking":
        validate_trajectory_payload(payload, action_name, safety)
    elif selected_task == "velocity_step":
        validate_velocity_step_payload(payload, action_name, safety)
    elif selected_task in {"acceleration_deceleration", "accel_decel"}:
        validate_accel_decel_payload(payload, action_name, safety)
    elif selected_task == "stop_accuracy":
        validate_stop_accuracy_payload(payload, action_name)


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。")

    safety = read_safety(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(CHASSIS_TYPES - set(chassis_profiles))
    if missing_profiles:
        raise ValueError(f"缺少底盘类型配置：{', '.join(missing_profiles)}")

    for chassis_type, section_value in chassis_profiles.items():
        if chassis_type not in 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, chassis_type, safety)
    return profile


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


def add_if_present(metadata: dict[str, Any], key: str, payload: dict[str, Any], source_key: str) -> None:
    if source_key in payload and payload[source_key] not in (None, ""):
        metadata[key] = payload[source_key]


def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any]) -> None:
    metadata["control.stop_at_end"] = payload.get("stop_at_end", True)
    metadata["control.timeout_sec"] = payload["timeout_sec"]
    add_if_present(metadata, "trajectory_tracking.required_external_pose_source_id", payload, "required_external_pose_source_id")
    add_if_present(metadata, "trajectory_tracking.max_external_pose_age_ms", payload, "max_external_pose_age_ms")
    add_if_present(metadata, "trajectory_tracking.min_external_pose_quality_score", payload, "min_external_pose_quality_score")
    add_if_present(metadata, "trajectory_tracking.trajectory_id", payload, "trajectory_id")
    add_if_present(metadata, "trajectory_tracking.segment_index", payload, "segment_index")
    add_if_present(metadata, "trajectory_tracking.total_segments", payload, "total_segments")
    add_if_present(metadata, "trajectory_tracking.is_final_segment", payload, "is_final_segment")

    for index, point in enumerate(payload["path"]):
        metadata[f"traj_pt_{index}_x_m"] = point["x_m"]
        metadata[f"traj_pt_{index}_y_m"] = point["y_m"]
        metadata[f"traj_pt_{index}_yaw_rad"] = point["yaw_rad"]
        metadata[f"traj_pt_{index}_speed_ms"] = point["target_speed_ms"]


def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[str, Any]:
    selected_task = action["selected_task"]
    payload_key = TASK_PAYLOAD_KEYS[selected_task]
    payload = action[payload_key]
    metadata: dict[str, Any] = {
        "control.task_type": TASK_TYPE_TO_METADATA[selected_task],
        "control.chassis_type": chassis_type,
        "control.axis": action["control_axis"],
        "control.algorithm": action["controller_algorithm"],
        "control.role": action["control_role"],
    }
    add_if_present(metadata, "control.loop_name", action, "loop_name")

    if selected_task == "trajectory_tracking":
        flatten_trajectory_payload(metadata, payload)
    elif selected_task == "velocity_step":
        metadata["velocity_step.target_velocity_ms"] = payload["target_velocity_ms"]
        metadata["velocity_step.hold_time_sec"] = payload["hold_time_sec"]
        metadata["velocity_step.settle_before_step_sec"] = payload["settle_before_step_sec"]
    elif selected_task in {"acceleration_deceleration", "accel_decel"}:
        metadata["accel_decel.start_velocity_ms"] = payload["start_velocity_ms"]
        metadata["accel_decel.target_velocity_ms"] = payload["target_velocity_ms"]
        metadata["accel_decel.target_accel_ms2"] = payload["target_accel_ms2"]
        metadata["accel_decel.hold_time_sec"] = payload["hold_time_sec"]
    elif selected_task == "stop_accuracy":
        metadata["stop_accuracy.target_stop_x_m"] = payload["target_stop_x_m"]
        metadata["stop_accuracy.target_stop_y_m"] = payload["target_stop_y_m"]
        metadata["stop_accuracy.target_stop_yaw_rad"] = payload["target_stop_yaw_rad"]
        metadata["control.timeout_sec"] = payload["timeout_sec"]
    return metadata


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 = flatten_action_metadata(action, chassis_type)
        task_params = [make_task_param(key, metadata[key]) for key in sorted(metadata)]
        tasks.append({
            "stage_type": "CONTROL_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"],
                    "selected_task": action["selected_task"],
                    "control_axis": action["control_axis"],
                    "controller_algorithm": action["controller_algorithm"],
                    "control_role": action["control_role"],
                    "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)
