"""全流程测试（规格 §5/§11.4：单目标、多目标、单目退化、非同帧拒收、越界、跳变）。

合成框对驱动，无需真实图像：几何是解析可断言的（§11.4）。
"""

import logging
import math
from typing import List, Optional, Tuple

import pytest

from stereo_ranger import (
    Degradation,
    Detection,
    DetectionFrame,
    Eye,
    MatchPair,
    StereoConfig,
    StereoRanger,
)

CFG = StereoConfig()
LOGGER_NAME = "stereo_ranger"


def _boxes(
    x_center_m: float = 0.0,
    y_m: float = 0.0,
    z_m: float = 6.0,
    *,
    w_m: float = 0.5,
    h_m: float = 0.3,
    class_id: int = 0,
    track_id: Optional[int] = None,
    confidence: float = 0.95,
) -> Tuple[Detection, Detection]:
    """模组中心系 (X_C, Y, Z) 目标 → 左右目检测框（两目同宽，边缘视差 = 中心视差）。"""
    x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
    x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
    y = CFG.cy_px + CFG.fy_px * y_m / z_m
    w_px = CFG.fx_px * w_m / z_m
    h_px = CFG.fy_px * h_m / z_m

    def _box(cx_px: float) -> Detection:
        return Detection(
            cx_px - w_px / 2.0,
            y - h_px / 2.0,
            cx_px + w_px / 2.0,
            y + h_px / 2.0,
            confidence=confidence,
            class_id=class_id,
            track_id=track_id,
        )

    return _box(x_l), _box(x_r)


def _pair_with_disparity(d_px: float, *, y_center: float = 500.0, class_id: int = 0) -> Tuple[Detection, Detection]:
    """构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
    width, height = 60.0, 40.0
    left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2, class_id=class_id)
    right = Detection(
        1000.0 - d_px - width / 2,
        y_center - height / 2,
        1000.0 - d_px + width / 2,
        y_center + height / 2,
        class_id=class_id,
    )
    return left, right


def _frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
    return DetectionFrame(frame_id=frame_id, detections=detections)


def test_single_target_complete_valid_record():
    ranger = StereoRanger()
    left, right = _boxes(x_center_m=0.0, z_m=6.0)
    records = ranger.update(_frame(1, [left]), _frame(1, [right]), now_ms=1000)

    assert len(records) == 1
    record = records[0]
    assert record.degradation is Degradation.COMPLETE
    assert record.valid is True
    assert record.forward_distance_m == pytest.approx(6.0)
    assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 6.0))  # R 含左目系 +B/2 横向偏移
    assert record.disparity_px == pytest.approx(CFG.focal_baseline_px_m / 6.0)
    assert record.center_mid_px == pytest.approx((CFG.cx_px, CFG.cy_px))
    assert record.reference_box == left.bbox
    assert record.reference_eye is Eye.LEFT
    assert record.confidence == pytest.approx(1.0)
    assert record.frame_id == 1 and record.timestamp_ms == 1000


def test_filter_smooths_across_valid_frames():
    ranger = StereoRanger()
    left_1, right_1 = _boxes(z_m=6.0)
    left_2, right_2 = _boxes(z_m=6.5)
    ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
    records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
    assert records[0].valid is True
    assert records[0].forward_distance_m == pytest.approx(6.0 + 0.3 * (6.5 - 6.0))
    # 参考斜距与已发布 Z 同基准：按 z_out/z_raw 从本帧原始斜距精确缩放（R ≥ Z）
    raw_slant = math.hypot(CFG.baseline_m / 2.0, 6.5)
    assert records[0].slant_range_m == pytest.approx(raw_slant * 6.15 / 6.5)


def test_jump_reset_skips_smoothing():
    ranger = StereoRanger()
    left_1, right_1 = _boxes(z_m=4.0)
    left_2, right_2 = _boxes(z_m=10.0)  # 跳变 6 m > jump_reset_m = 5 m
    ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
    records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
    assert records[0].forward_distance_m == pytest.approx(10.0)


def test_far_field_out_of_domain_still_matched():
    # 越界 = 有效性门（决策）：配对仍成立，c_mid（无偏方位）与 D 照常发布，无 Z
    ranger = StereoRanger()
    left, right = _boxes(x_center_m=0.6, z_m=25.0)  # D ≈ 2.3 px < 3 px
    records = ranger.update(_frame(1, [left]), _frame(1, [right]))

    record = records[0]
    assert record.degradation is Degradation.COMPLETE
    assert record.valid is False
    assert record.forward_distance_m is None
    assert record.disparity_px == pytest.approx(2.304)
    assert record.center_mid_px == pytest.approx((CFG.cx_px + CFG.fx_px * 0.6 / 25.0, CFG.cy_px))
    assert record.slant_range_m == pytest.approx(math.hypot(0.6 + CFG.baseline_m / 2.0, 25.0))


def test_error_budget_gate_blocks_far_but_in_domain():
    ranger = StereoRanger()
    left, right = _boxes(z_m=12.0)  # D = 4.8 px 域内，δZ = 2.5 m > 2 m
    record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
    assert record.disparity_px == pytest.approx(4.8)
    assert record.valid is False
    assert record.forward_distance_m is None
    assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 12.0))


def test_error_budget_gate_passes_just_inside():
    ranger = StereoRanger()
    left, right = _pair_with_disparity(5.8)  # Z ≈ 9.93 m，δZ ≈ 1.71 m ≤ 2 m
    record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
    assert record.valid is True
    assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 5.8)


def test_negative_disparity_violation(caplog):
    ranger = StereoRanger()
    left = Detection(1000.0, 500.0, 1060.0, 540.0)
    right = Detection(1010.0, 500.0, 1070.0, 540.0)
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
    assert record.degradation is Degradation.COMPLETE
    assert record.valid is False
    assert record.forward_distance_m is None
    assert record.slant_range_m is None
    assert record.disparity_px == pytest.approx(-10.0)
    assert any("disparity_violation" in message for message in caplog.messages)


def test_center_fallback_keeps_target_valid(caplog):
    ranger = StereoRanger()
    left = Detection(1000.0, 500.0, 1060.0, 540.0)  # 宽 60, center 1030
    right = Detection(940.0, 500.0, 1040.0, 540.0)  # 宽 100, center 990
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
    assert record.disparity_px == pytest.approx(40.0)  # 边缘 60 px 越窗 → 中心 40 px
    assert record.valid is True
    assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 40.0)
    assert any("center_fallback" in message for message in caplog.messages)


def test_single_eye_degradation_left_and_right():
    ranger = StereoRanger()
    left, right = _boxes(x_center_m=0.3, z_m=4.0)

    left_only = ranger.update(_frame(1, [left]), _frame(1, []))
    assert len(left_only) == 1
    record = left_only[0]
    assert record.degradation is Degradation.BEARING_ONLY
    assert record.valid is False
    assert record.center_mid_px == pytest.approx((left.cx, left.cy))
    assert record.forward_distance_m is None and record.disparity_px is None
    assert record.reference_eye is Eye.LEFT
    assert record.confidence == pytest.approx(left.confidence)

    right_only = ranger.update(_frame(2, []), _frame(2, [right]))
    assert len(right_only) == 1
    assert right_only[0].reference_eye is Eye.RIGHT
    assert right_only[0].center_mid_px == pytest.approx((right.cx, right.cy))


def test_records_order_left_first_then_unmatched_right():
    ranger = StereoRanger()
    left, right_matched = _boxes(z_m=6.0, class_id=0)
    _, right_extra = _boxes(z_m=6.0, class_id=0, y_m=0.0)
    right_extra = Detection(400.0, 500.0, 460.0, 540.0)  # 远处无对应左框
    records = ranger.update(_frame(1, [left]), _frame(1, [right_matched, right_extra]))
    assert len(records) == 2
    assert records[0].degradation is Degradation.COMPLETE
    assert records[0].valid is True
    assert records[1].degradation is Degradation.BEARING_ONLY
    assert records[1].reference_eye is Eye.RIGHT


def test_frame_id_mismatch_rejects_frame_and_throttles_warning(caplog):
    ranger = StereoRanger()
    left, right = _boxes()
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        assert ranger.update(_frame(1, [left]), _frame(2, [right])) == []
        assert ranger.update(_frame(3, [left]), _frame(4, [right])) == []
    warnings = [message for message in caplog.messages if "frame_mismatch" in message]
    assert len(warnings) == 1  # 第 1 次打印，第 2 次被节流
    assert "累计 1 次" in warnings[0]


def test_empty_input_returns_empty_without_warning(caplog):
    ranger = StereoRanger()
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        assert ranger.update(_frame(1, []), _frame(1, [])) == []
    assert caplog.messages == []


def test_non_finite_box_dropped_with_warning(caplog):
    ranger = StereoRanger()
    bad = Detection(float("nan"), 500.0, 1060.0, 540.0)
    good_left, good_right = _boxes(z_m=6.0)
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        records = ranger.update(_frame(1, [bad, good_left]), _frame(1, [good_right]))
    assert len(records) == 1
    assert records[0].valid is True
    assert any("invalid_box" in message for message in caplog.messages)


def test_consistency_degrades_then_invalidates(caplog):
    ranger = StereoRanger()
    disparities = [9.5, 13.5, 17.5, 21.5, 25.5, 29.5]  # 每帧跳变 4 px > 3 px
    records = []
    with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
        for index, d_px in enumerate(disparities, start=1):
            left, right = _pair_with_disparity(d_px)
            records.append(ranger.update(_frame(index, [left]), _frame(index, [right]))[0])

    assert [record.valid for record in records] == [True, True, True, True, True, False]
    assert records[3].confidence == pytest.approx(0.5)  # 第 3 帧起置信度降级（N = 3）
    assert records[5].forward_distance_m is None
    assert records[5].confidence == pytest.approx(0.0)
    assert any("consistency_invalid" in message for message in caplog.messages)


def test_track_id_keeps_state_across_large_pixel_move():
    ranger = StereoRanger()
    left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0, track_id=7)
    left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5, track_id=7)  # c_mid 移动 144 px > 门限
    ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
    records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
    assert records[0].track_id == 7
    assert records[0].forward_distance_m == pytest.approx(4.0 + 0.3 * (4.5 - 4.0))


def test_without_track_id_large_move_starts_new_state():
    ranger = StereoRanger()
    left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0)
    left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5)  # 最近邻越门限 → 新目标状态
    ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
    records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
    assert records[0].forward_distance_m == pytest.approx(4.5)  # 未平滑


def test_custom_matcher_injection():
    class _NoMatchMatcher:
        def match(self, left, right, config) -> List[MatchPair]:
            return []

    ranger = StereoRanger(matcher=_NoMatchMatcher())
    left, right = _boxes(z_m=6.0)
    records = ranger.update(_frame(1, [left]), _frame(1, [right]))
    assert len(records) == 2
    assert all(record.degradation is Degradation.BEARING_ONLY for record in records)


def test_reset_clears_state():
    ranger = StereoRanger()
    left_1, right_1 = _boxes(z_m=6.0)
    left_2, right_2 = _boxes(z_m=6.5)
    ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
    ranger.reset()
    records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
    assert records[0].forward_distance_m == pytest.approx(6.5)  # 未平滑（状态已清空）