"""像素偏移 → 视线角（针孔模型）。

对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp
（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。

全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似
双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制
环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。
"""

import math


class PixelToAngle:
    def __init__(self, size_px: float, fov_rad: float) -> None:
        # 构造期校验：非法参数直接抛异常（初始化抛异常规范）
        if not (size_px > 0.0):
            raise ValueError("PixelToAngle: size_px 必须为正")
        if not (0.0 < fov_rad < math.pi):
            raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内")
        self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px

    def convert(self, offset_px: float) -> float:
        """偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。"""
        return math.atan(offset_px * self._pinhole_scale)