import numpy as np

from tools.analyze_mono3d_head_targets import (
    activation_lateral_half_span_m,
    best_in_box_offset_cells,
    expand_bbox_for_assigner,
    infer_cut_label,
    recommend_l1_norm,
)


def test_expand_bbox_for_assigner_enforces_min_side():
    expanded = expand_bbox_for_assigner(np.array([10.0, 10.0, 14.0, 14.0]), 16.0)
    assert np.allclose(expanded, np.array([4.0, 4.0, 20.0, 20.0]))


def test_best_in_box_offset_cells_returns_best_case_grid_offset():
    offset = best_in_box_offset_cells(
        target_uv_px=np.array([40.0, 24.0]),
        bbox_xyxy=np.array([16.0, 8.0, 64.0, 40.0]),
        img_w=96,
        img_h=64,
        stride=8,
    )
    assert np.allclose(offset, np.array([0.5, 0.5]))


def test_infer_cut_label_matches_loss_mapping_for_cut_in_and_cut_out():
    cut_in = np.zeros(42, dtype=np.float64)
    cut_in[18:24] = -1
    cut_in[25] = 0
    cut_in[26:32] = -1
    cut_in[33] = 0
    cut_in[34:40] = -1
    cut_in[41] = 0
    assert infer_cut_label(cut_in) == 1

    cut_out = np.zeros(42, dtype=np.float64)
    cut_out[10:16] = -1
    cut_out[17] = 0
    cut_out[26:32] = -1
    cut_out[33] = 0
    cut_out[34:40] = -1
    cut_out[41] = 0
    assert infer_cut_label(cut_out) == 2


def test_recommend_l1_norm_prefers_median_and_robust_scale():
    rec = recommend_l1_norm([1.0, 2.0, 3.0, 10.0])
    assert rec["offset_median"] == 2.5
    assert np.isclose(rec["offset_mean"], 4.0)
    assert np.isclose(rec["scale_std"], np.std([1.0, 2.0, 3.0, 10.0], ddof=0))
    assert rec["scale_p84_p16_half"] < rec["scale_std"]


def test_activation_lateral_half_span_m_uses_depth_and_fx():
    half_span = activation_lateral_half_span_m(
        anchor_uv_px=np.array([100.0, 50.0]),
        target_v_px=50.0,
        stride=8,
        calib={"fx": 200.0, "fy": 200.0, "cx": 100.0, "cy": 50.0, "distort_coeffs": []},
        depth_metric=25.0,
    )
    expected = (8.0 * 8.0 / 200.0) * 25.0
    assert np.isclose(half_span, expected)