"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。

对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp
（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。

启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整
字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。
"""

import json
from dataclasses import dataclass, field
from enum import Enum
from typing import Any

from .types import ControlMode  # noqa: F401  (re-export 便利性)


class HeadingMode(Enum):
    """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。"""

    RATE = "rate"          # β → 偏航角速度，输出 yaw_rate_dps
    POSITION = "position"  # β → 修正量，与当前航向合成绝对航向输出 yaw_deg


class NoDistanceAction(Enum):
    """目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）。"""

    HOLD = "hold"                  # 保持：前向速度指令清零
    SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
    EXIT = "exit"                  # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
                                   # 才降级保持；grace 期间按 SLOW_APPROACH 行为


@dataclass
class ControlGroupConfig:
    frequency_hz: float = 20.0
    visual_stale_ms: int = 200
    distance_stale_ms: int = 500
    attitude_stale_ms: int = 300


@dataclass
class CameraGroupConfig:
    image_width: int = 1920
    image_height: int = 1080
    fov_h_deg: float = 60.0
    fov_v_deg: float = 35.0


@dataclass
class HeadingGroupConfig:
    mode: HeadingMode = HeadingMode.RATE
    gain: float = 1.0
    yaw_rate_limit_dps: float = 30.0
    yaw_slew_limit_dps2: float = 60.0


@dataclass
class VerticalGroupConfig:
    vz_gain_mps_per_deg: float = 0.1
    vz_limit_mps: float = 2.0


@dataclass
class DistanceGroupConfig:
    d_exp_m: float = 10.0
    kp: float = 0.0
    ki: float = 0.0
    kd: float = 0.0
    integral_limit: float = 0.0
    derivative_filter_coef: float = 0.0
    approach_velocity_limit_mps: float = 3.0
    retreat_velocity_limit_mps: float = 1.0
    # 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
    # 持续无距离（传感器级故障）场景
    no_distance_action: NoDistanceAction = NoDistanceAction.SLOW_APPROACH
    no_distance_approach_limit_mps: float = 0.5
    no_distance_exit_grace_ms: int = 3000  # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）


@dataclass
class AccelLimitConfig:
    ax_mps2: float = 2.0
    ay_mps2: float = 2.0
    az_mps2: float = 1.5


@dataclass
class TrackingConfig:
    control: ControlGroupConfig = field(default_factory=ControlGroupConfig)
    camera: CameraGroupConfig = field(default_factory=CameraGroupConfig)
    heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig)
    vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig)
    distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig)
    accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig)


# ---------------------------------------------------------------------------
# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致）
# ---------------------------------------------------------------------------

def _require_section(root: dict, section: str) -> dict:
    value = root.get(section)
    if not isinstance(value, dict):
        raise ValueError(f"配置缺失分组或类型错误: {section}")
    return value


def _is_number(value: Any) -> bool:
    return isinstance(value, (int, float)) and not isinstance(value, bool)


def _is_integer(value: Any) -> bool:
    return isinstance(value, int) and not isinstance(value, bool)


def _require_number(section: dict, section_name: str, key: str) -> float:
    value = section.get(key)
    if not _is_number(value):
        raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}")
    return float(value)


def _require_integer(section: dict, section_name: str, key: str) -> int:
    value = section.get(key)
    if not _is_integer(value):
        raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}")
    return int(value)


def _require_string(section: dict, section_name: str, key: str) -> str:
    value = section.get(key)
    if not isinstance(value, str):
        raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}")
    return value


def _require_positive(value: float, field_name: str) -> float:
    if not (value > 0.0):
        raise ValueError(f"配置非法(必须为正): {field_name}")
    return value


def _require_non_negative(value: float, field_name: str) -> float:
    if value < 0.0:
        raise ValueError(f"配置非法(不得为负): {field_name}")
    return value


def _require_range(value: float, lo: float, hi: float, field_name: str) -> float:
    if not (lo < value < hi):
        raise ValueError(f"配置非法(超出范围): {field_name}")
    return value


def _load_control(root: dict) -> ControlGroupConfig:
    s = _require_section(root, "control")
    c = ControlGroupConfig()
    c.frequency_hz = _require_positive(
        _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
    c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
    c.distance_stale_ms = _require_integer(s, "control", "distance_stale_ms")
    c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
    if c.visual_stale_ms <= 0 or c.distance_stale_ms <= 0 or c.attitude_stale_ms <= 0:
        raise ValueError("配置非法: control.*_stale_ms 必须为正")
    return c


def _load_camera(root: dict) -> CameraGroupConfig:
    s = _require_section(root, "camera")
    c = CameraGroupConfig()
    c.image_width = int(_require_positive(
        float(_require_integer(s, "camera", "image_width")), "camera.image_width"))
    c.image_height = int(_require_positive(
        float(_require_integer(s, "camera", "image_height")), "camera.image_height"))
    c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"),
                                 0.0, 180.0, "camera.fov_h_deg")
    c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"),
                                 0.0, 180.0, "camera.fov_v_deg")
    return c


def _load_heading(root: dict) -> HeadingGroupConfig:
    s = _require_section(root, "heading")
    h = HeadingGroupConfig()
    mode = _require_string(s, "heading", "mode")
    if mode == "position":
        h.mode = HeadingMode.POSITION
    elif mode == "rate":
        h.mode = HeadingMode.RATE
    else:
        raise ValueError("配置非法: heading.mode 仅支持 position / rate")
    h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain")
    h.yaw_rate_limit_dps = _require_positive(
        _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps")
    h.yaw_slew_limit_dps2 = _require_positive(
        _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2")
    return h


def _load_vertical(root: dict) -> VerticalGroupConfig:
    s = _require_section(root, "vertical")
    v = VerticalGroupConfig()
    v.vz_gain_mps_per_deg = _require_non_negative(
        _require_number(s, "vertical", "vz_gain_mps_per_deg"),
        "vertical.vz_gain_mps_per_deg")
    v.vz_limit_mps = _require_positive(
        _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps")
    return v


def _load_distance(root: dict) -> DistanceGroupConfig:
    s = _require_section(root, "distance")
    d = DistanceGroupConfig()
    d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"),
                                  "distance.d_exp_m")
    d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp")
    d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki")
    d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd")
    d.integral_limit = _require_non_negative(
        _require_number(s, "distance", "integral_limit"), "distance.integral_limit")
    # 容许 -0.0：范围下界略低于 0，读入后归一化为 0
    d.derivative_filter_coef = _require_range(
        _require_number(s, "distance", "derivative_filter_coef"),
        -0.000001, 1.0, "distance.derivative_filter_coef")
    if d.derivative_filter_coef < 0.0:
        d.derivative_filter_coef = 0.0
    d.approach_velocity_limit_mps = _require_positive(
        _require_number(s, "distance", "approach_velocity_limit_mps"),
        "distance.approach_velocity_limit_mps")
    d.retreat_velocity_limit_mps = _require_positive(
        _require_number(s, "distance", "retreat_velocity_limit_mps"),
        "distance.retreat_velocity_limit_mps")
    action = _require_string(s, "distance", "no_distance_action")
    if action == "hold":
        d.no_distance_action = NoDistanceAction.HOLD
    elif action == "slow_approach":
        d.no_distance_action = NoDistanceAction.SLOW_APPROACH
    elif action == "exit":
        d.no_distance_action = NoDistanceAction.EXIT
    else:
        raise ValueError(
            "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit")
    d.no_distance_approach_limit_mps = _require_non_negative(
        _require_number(s, "distance", "no_distance_approach_limit_mps"),
        "distance.no_distance_approach_limit_mps")
    # 0 容许：等效旧版"首个无效拍即退出"行为
    d.no_distance_exit_grace_ms = _require_non_negative(
        _require_integer(s, "distance", "no_distance_exit_grace_ms"),
        "distance.no_distance_exit_grace_ms")
    return d


def _load_accel_limit(root: dict) -> AccelLimitConfig:
    s = _require_section(root, "accel_limit")
    a = AccelLimitConfig()
    a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"),
                                  "accel_limit.ax_mps2")
    a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"),
                                  "accel_limit.ay_mps2")
    a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"),
                                  "accel_limit.az_mps2")
    return a


def load_from_json(path: str) -> TrackingConfig:
    """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。"""
    try:
        with open(path, "r", encoding="utf-8") as fp:
            root = json.load(fp)
    except OSError:
        raise ValueError(f"配置文件打开失败: {path}") from None
    except json.JSONDecodeError as exc:
        raise ValueError(f"配置文件解析失败: {path} ({exc})") from None

    if not isinstance(root, dict):
        raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")

    cfg = TrackingConfig()
    cfg.control = _load_control(root)
    cfg.camera = _load_camera(root)
    cfg.heading = _load_heading(root)
    cfg.vertical = _load_vertical(root)
    cfg.distance = _load_distance(root)
    cfg.accel_limit = _load_accel_limit(root)
    return cfg