"""stereo_ranger demo：合成多目标场景演示单帧 update() 全流程。

本 demo 不使用任何检测器：按已知 (X, Y, Z) 目标解析投影生成左右目
检测框（模拟"外部检测器输出，框已在校正后原生坐标系"），并叠加 ±1 px
亚像素抖动，随后用 StereoRanger 测距并对照真值。

运行（pytorch 环境）：
    python utils/demo_stereo_ranger.py [--config path/to/stereo.json]

依赖仅为标准库 + stereo_ranger 包（将 utils/ 加入 PYTHONPATH 或于 utils/ 下运行）。
"""

import argparse
import random
import sys
from pathlib import Path

sys.path.insert(0, str(Path(__file__).resolve().parent))

from stereo_ranger import Detection, StereoConfig, StereoRanger, focal_from_hfov, load_from_json, range_error_abs

# 模拟相机：1920x1080，HFOV 60°（文档 §5.3 示例之一）
IMAGE_W = 1920
IMAGE_H = 1080
HFOV_DEG = 60.0
BASELINE_M = 0.06


def project_to_boxes(x_m, y_m, z_m, fx, fy, cx, cy, b, rng, w_m=0.5, h_m=0.3):
    """目标物理宽高 -> 左右目框；叠加亚像素抖动模拟检测噪声（种子固定）。"""
    w_px = fx * w_m / z_m
    h_px = fy * h_m / z_m
    u_l = fx * x_m / z_m + cx
    v_l = fy * y_m / z_m + cy
    u_r = u_l - fx * b / z_m
    jitter = lambda: rng.uniform(-1.0, 1.0)  # ±1 px 框边缘抖动
    left = Detection(u_l - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
                     u_l + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
                     conf=0.95, cls_id=0, track_id=1)
    right = Detection(u_r - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
                      u_r + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
                      conf=0.9, cls_id=0, track_id=1)
    return left, right


def main() -> None:
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--config", default=None, help="StereoConfig JSON 文件（可选）")
    args = parser.parse_args()

    cfg = load_from_json(args.config) if args.config else StereoConfig(
        fx_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
        fy_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
        cx_px=IMAGE_W / 2.0, cy_px=IMAGE_H / 2.0,
        baseline_m=BASELINE_M,
        range_tol_m=2.0,
    )
    ranger = StereoRanger(cfg)
    rng = random.Random(20260909)

    targets = [(0.0, 0.0, 5.0), (2.0, 0.5, 8.0), (-1.5, -0.4, 12.0)]
    left_dets, right_dets = [], []
    for x_m, y_m, z_m in targets:
        l, r = project_to_boxes(x_m, y_m, z_m, cfg.fx_px, cfg.fy_px, cfg.cx_px, cfg.cy_px,
                                cfg.baseline_m, rng)
        left_dets.append(l)
        right_dets.append(r)
    # 一个只在左目检出的干扰框 -> 应输出 matched=False 的方位记录
    left_dets.append(Detection(100.0, 900.0, 160.0, 950.0, conf=0.6, cls_id=9))

    reports = ranger.update(left_dets, right_dets, frame_sequence=1, timestamp_s=0.0)

    print(f"内参: F={cfg.fx_px:.1f}px  cx={cfg.cx_px:.0f}  B={cfg.baseline_m:.3f}m  "
          f"Z 窗口 [{cfg.z_min_m}, {cfg.z_max_m}]m")
    print(f"{'真值 Z':>8} {'估计 Z':>8} {'真值 X':>7} {'估计 X':>7} {'视差':>6} {'δZ估计':>7} "
          f"{'有效':>5} {'x_mid':>7}  备注")
    for i, r in enumerate(reports):
        if not r.matched:
            print(f"{'-':>8} {'-':>8} {'-':>7} {'-':>7} {'-':>6} {'-':>7} {'-':>5} "
                  f"{r.centre_pixel_x_mid:7.1f}  左目未配对（方位直发）")
            continue
        truth = targets[i]
        print(f"{truth[2]:8.2f} {r.forward_distance_m:8.2f} {truth[0]:7.2f} {r.lateral_m:7.2f} "
              f"{r.disparity_px:6.1f} {r.error_est_m:7.2f} {str(r.valid):>5} "
              f"{r.centre_pixel_x_mid:7.1f}  src={r.disparity_source}")
    print(f"\n共 {len(reports)} 条记录（{sum(1 for r in reports if r.valid)} 条有效）")

    print("\n误差模型示意（规格 §5.3，δD = 1 px）：")
    print(f"{'Z (m)':>7} {'δZ (m)':>8}")
    for z in [5, 10, 15, 20, 30]:
        err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, 1.0)
        print(f"{z:7d} {err:8.2f}")


if __name__ == "__main__":
    main()