"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。

Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达；
R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目
中心，§6.6，逐点精确、无近似）。
"""

import math
from dataclasses import dataclass
from typing import Optional, Tuple

from .config import StereoConfig
from .types import Detection

DELTA_D_PX = 1.0  # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)）


@dataclass(frozen=True)
class RangeSolution:
    """测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。"""

    z_m: float
    x_m: float
    y_m: float
    r_m: float          # 斜距（参考值，不进控制回路）


@dataclass(frozen=True)
class ValidityDecision:
    """有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。"""

    in_domain: bool  # ① 域内：D ∈ [D_min, D_max]
    budget_ok: bool  # ② 误差预算：δZ(δD = 1px) ≤ δZ_max
    valid: bool      # ① 且 ②


def forward_distance_m(disparity_px: float, config: StereoConfig) -> float:
    """Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。"""
    if disparity_px <= 0.0:
        raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}")
    return config.focal_baseline_px_m / disparity_px


def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution:
    """由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。"""
    z_m = forward_distance_m(disparity_px, config)
    x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px
    y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px
    r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m)
    return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m)


def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]:
    """虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。

    与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。
    """
    return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0)


def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
    """绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。"""
    return z_m * z_m * delta_d_px / config.focal_baseline_px_m


def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
    """相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。"""
    return z_m * delta_d_px / config.focal_baseline_px_m


def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision:
    """有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。"""
    in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px
    budget_ok = (
        z_m is not None
        and math.isfinite(z_m)
        and range_error_abs_m(z_m, config) <= config.max_delta_z_m
    )
    return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok)


def confidence_from_cost(cost: float) -> float:
    """匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。"""
    return min(1.0, max(0.0, 1.0 - cost))