"""stereo_ranger 测距核心单元测试。

规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、
§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框，
再反推验证测距/方位输出。
"""

import math

import pytest

from stereo_ranger.config import StereoConfig
from stereo_ranger.stereo_ranger import (
    DISP_SOURCE_CENTER,
    DISP_SOURCE_EDGE,
    StereoRanger,
    focal_from_hfov,
    range_error_abs,
    range_error_rel,
)
from stereo_ranger.types import Detection

CFG = StereoConfig()
FB = CFG.fx_px * CFG.baseline_m  # 57.6 px·m


def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
    return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2,
                     conf, cls_id, track_id)


def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
    """目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。"""
    u_l = CFG.fx_px * x_m / z_m + CFG.cx_px
    v_l = CFG.fy_px * y_m / z_m + CFG.cy_px
    u_r = u_l - FB / z_m
    return (box(u_l, v_l, w, h, cls_id, conf, track_id),
            box(u_r, v_l, w, h, cls_id, conf, track_id))


def one_report(ranger, left, right, **kw):
    reports = ranger.update(left, right, **kw)
    assert len(reports) == len(left)
    return reports


# ---- §4.2 / §5.3 几何与误差公式 ----

def test_focal_from_hfov_matches_doc_examples():
    assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0)
    assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769)


def test_range_error_matches_doc_table():
    # 文档 §5.3 示意表：B=0.06, F=960, δD=1 px
    expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444,
                30: 15.625, 50: 43.4028, 100: 173.611}
    for z, err in expected.items():
        assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3)
    assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3)


def test_on_axis_target_z_and_bearing():
    left, right = target_pair(0.0, 0.0, 10.0)
    r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0]
    assert r.matched and r.valid
    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
    assert r.disparity_px == pytest.approx(FB / 10.0)
    assert r.disparity_source == DISP_SOURCE_EDGE
    assert r.lateral_m == pytest.approx(0.0, abs=1e-9)
    assert r.vertical_m == pytest.approx(0.0, abs=1e-9)
    assert r.slant_range_m == pytest.approx(10.0, rel=1e-9)
    assert r.frame_sequence == 7 and r.timestamp_s == 1.5
    # 虚拟双目中心：两目中心平均，y 取左目（§11.3）
    assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0)
    assert r.centre_pixel_y_mid == left.cy


def test_off_axis_target_xyz_and_slant_range():
    left, right = target_pair(1.2, 0.3, 10.0)
    r = one_report(StereoRanger(), [left], [right])[0]
    assert r.valid
    assert r.lateral_m == pytest.approx(1.2, rel=1e-9)
    assert r.vertical_m == pytest.approx(0.3, rel=1e-9)
    assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9)
    assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))


def test_x_uses_left_camera_frame_not_virtual_center():
    # X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3）
    left, right = target_pair(0.0, 0.0, 10.0)
    r = one_report(StereoRanger(), [left], [right])[0]
    assert r.lateral_m == pytest.approx(0.0, abs=1e-9)  # 目标在左光轴上
    assert r.centre_pixel_x_mid < left.cx  # 但虚拟中心相对左光轴偏左 B/2


def test_width_diff_on_right_edge_only_does_not_taint_left_edge():
    # §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽
    # 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。
    left, right = target_pair(0.0, 0.0, 10.0)
    right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2)
    r = one_report(StereoRanger(), [left], [right_extended])[0]
    assert r.matched and r.valid
    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
    assert r.disparity_source == DISP_SOURCE_EDGE
    assert right_extended.width != left.width  # 确有宽度差


def test_center_fallback_when_edge_out_of_window(caplog):
    # 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏
    left, right = target_pair(0.0, 0.0, 10.0)
    left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
    ranger = StereoRanger()
    r = one_report(ranger, [left_wider], [right])[0]
    r2 = one_report(ranger, [left_wider], [right])[0]
    assert r.matched and r.valid
    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
    assert r.disparity_source == DISP_SOURCE_CENTER
    assert r2.disparity_source == DISP_SOURCE_CENTER
    assert len(caplog.records) == 1


def test_center_fallback_disabled_leaves_unmatched():
    left, right = target_pair(0.0, 0.0, 10.0)
    left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
    cfg = StereoConfig(use_center_fallback=False)
    r = one_report(StereoRanger(cfg), [left_wider], [right])[0]
    assert not r.matched and not r.valid
    assert r.forward_distance_m is None


# ---- §6 关联硬约束与贪心一一配对 ----

def test_class_mismatch_rejected():
    left, _ = target_pair(0.0, 0.0, 10.0)
    _, right = target_pair(0.0, 0.0, 10.0, cls_id=1)
    r = one_report(StereoRanger(), [left], [right])[0]
    assert not r.matched


def test_vertical_residual_beyond_tau_rejected():
    left, _ = target_pair(0.0, 0.0, 10.0)
    right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0)  # |Δy| = 3 > τy = 2
    r = one_report(StereoRanger(), [left], [right])[0]
    assert not r.matched


def test_height_ratio_beyond_tau_rejected():
    left, _ = target_pair(0.0, 0.0, 10.0)
    _, right = target_pair(0.0, 0.0, 10.0, h=40.0)  # |60-40|/60 > 0.25
    r = one_report(StereoRanger(), [left], [right])[0]
    assert not r.matched


def test_cost_equal_one_rejected():
    # 文档 §6：仅当 cost < 1 时接受（严格小于）
    # dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0
    left, _ = target_pair(0.0, 0.0, 10.0)
    x_off = FB / 10.0
    right = Detection(left.x1 - x_off, left.y1 + 4.75,
                      left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5)
    # 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125）
    assert right.cy == pytest.approx(left.cy + 1.0)
    r = one_report(StereoRanger(), [left], [right])[0]
    assert not r.matched


def test_disparity_out_of_z_window_rejected():
    # 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6）
    near_l, near_r = target_pair(0.0, 0.0, 0.5)
    far_l, far_r = target_pair(0.0, 0.0, 60.0)
    r = one_report(StereoRanger(), [near_l], [near_r])[0]
    assert not r.matched
    r = one_report(StereoRanger(), [far_l], [far_r])[0]
    assert not r.matched


def test_greedy_one_to_one_crossing_indices():
    # 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。
    # 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。
    l_a, r_a = target_pair(0.0, 0.0, 10.0)
    l_b, r_b = target_pair(0.3, 0.0, 6.0)
    reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
    assert all(r.matched for r in reports)
    assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9)
    assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9)
    assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2)


def test_track_id_continuity_preference():
    # §6：存在轨迹标识时优先匹配相同 track id
    l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7)
    l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8)
    reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
    by_track = {r.track_id: r.forward_distance_m for r in reports}
    assert by_track[7] == pytest.approx(10.0, rel=1e-9)
    assert by_track[8] == pytest.approx(8.0, rel=1e-9)


def test_unmatched_left_reports_bearing_only():
    left, right = target_pair(0.0, 0.0, 10.0)
    stray = box(500.0, 300.0, cls_id=3, conf=0.8)
    reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5)
    assert reports[0].matched and reports[0].valid
    u = reports[1]
    assert not u.matched and not u.valid
    assert u.forward_distance_m is None
    assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy
    assert u.confidence == 0.8 and u.class_id == 3
    assert u.frame_sequence == 42 and u.timestamp_s == 1.5


def test_empty_sides():
    ranger = StereoRanger()
    assert ranger.update([], []) == []
    r = one_report(ranger, [box(100.0, 100.0)], [])[0]
    assert not r.matched


def test_report_order_follows_left_detections():
    ranger = StereoRanger()
    l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0)
    l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2)
    reports = ranger.update([l2, l1], [r1, r2])  # 输出顺序与输入左目一致
    assert [r.class_id for r in reports] == [2, 0]
    assert reports[0].confidence == min(l2.conf, r2.conf)


# ---- §5.4 有效性判定 ----

def test_beyond_range_tolerance_invalid_but_distance_kept():
    left, right = target_pair(0.0, 0.0, 15.0)  # δZ ≈ 3.9 m > 2 m
    r = one_report(StereoRanger(), [left], [right])[0]
    assert r.matched
    assert not r.valid
    assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9)
    assert r.lateral_m is not None  # 方位/坐标仍发布（§5.4 域外标记无效）
    assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))


def test_within_range_tolerance_valid():
    left, right = target_pair(0.0, 0.0, 10.0)  # δZ ≈ 1.74 m <= 2 m
    r = one_report(StereoRanger(), [left], [right])[0]
    assert r.valid


def test_invalid_when_disparity_below_dmin():
    # 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效
    cfg = StereoConfig(range_tol_m=50.0)
    left, right = target_pair(0.0, 0.0, 25.0)
    r = one_report(StereoRanger(cfg), [left], [right])[0]
    assert r.matched and not r.valid


# ---- §7 时间滤波（可选） ----

def moving_pair(z_m, track_id=1, cls_id=0):
    """返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。"""
    left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id)
    right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id)
    return [left], [right]


def test_filter_smooths_and_resets_on_jump():
    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2,
                       range_tol_m=50.0)  # 放宽有效域，让 10~13 m 均有效以观察滤波
    ranger = StereoRanger(cfg)
    z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0]
    assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9)  # 首帧置位
    z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]  # 跳变 10% <= 20%：平滑
    assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9)
    z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0]  # 跳变 26% > 20%：重置
    assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9)


def test_filter_skipped_without_track_id():
    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3)
    ranger = StereoRanger(cfg)
    one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1)
    z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0]
    assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9)


def test_reset_clears_filter_state():
    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
    ranger = StereoRanger(cfg)
    one_report(ranger, *moving_pair(10.0), frame_sequence=1)  # 状态 = 10.0
    ranger.reset()
    # 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0
    z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]
    assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9)


def test_invalid_measurements_do_not_pollute_filter_state():
    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
    ranger = StereoRanger(cfg)
    one_report(ranger, *moving_pair(10.0), frame_sequence=1)
    z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0]  # D < D_min：无效
    assert not z2.valid
    z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0]
    # 无效帧未污染状态：仍按与首帧 10.0 平滑
    assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9)