"""demo_stereo_ranger.py —— 双目稀疏目标测距演示（规格 docs/双目测距技术路线.md）。

全部场景用**解析构造**的左右目检测框驱动：不需要相机、不需要图像、不需要
飞控，几何是闭式可验证的（规格 §11.4）。覆盖：

  ① 逼近：三条有效性门依次生效（域外 → 预算门 → 有效）+ 时间滤波平滑
  ② 远场域外：valid = false 但 c_mid（虚拟双目中心方位）照常发布
  ③ 单目退化：仅一目检出 → 仅方位记录
  ④ 非同帧拒收：左右帧标识不一致 → 整帧拒收 + 告警节流
  ⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效
  ⑥ 多目标：贪心一一配对，各自独立状态；未匹配右目退化为仅方位

用法：
  python utils/demo_stereo_ranger.py                  # 默认配置
  python utils/demo_stereo_ranger.py --config <path>  # 指定配置

⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
  本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
  （规格 §12.3 红线）。
"""

import argparse
import logging
from pathlib import Path
from typing import List, Optional, Tuple

from stereo_ranger import (
    Detection,
    DetectionFrame,
    RangeRecord,
    StereoConfig,
    StereoRanger,
    load_from_json,
)

logger = logging.getLogger("demo_stereo_ranger")

_DEMO_DIR = Path(__file__).resolve().parent
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"


# ---------------------------------------------------------------- 合成框对


def boxes_for(
    cfg: StereoConfig,
    x_center_m: float,
    y_m: float,
    z_m: float,
    *,
    w_m: float = 0.5,
    h_m: float = 0.4,
    class_id: int = 0,
    track_id: Optional[int] = None,
    confidence: float = 0.95,
) -> Tuple[Detection, Detection]:
    """模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。

    两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
    """
    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) -> 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)
    right = Detection(
        1000.0 - d_px - width / 2,
        y_center - height / 2,
        1000.0 - d_px + width / 2,
        y_center + height / 2,
    )
    return left, right


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


# ---------------------------------------------------------------- 打印


def describe(record: RangeRecord) -> str:
    z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else "   —   "
    r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else "   —   "
    d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else "   —   "
    return (
        f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
        f"Z={z_text} m  R={r_text} m  D={d_text} px  "
        f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f})  "
        f"conf={record.confidence:.2f}"
    )


def log_scene(title: str) -> None:
    logger.info("")
    logger.info("── %s", title)


def log_config_summary(cfg: StereoConfig) -> None:
    logger.info("配置：B=%.3f m  F=%.1f px  (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
    logger.info(
        "      视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
        cfg.disparity_min_px,
        cfg.disparity_max_px,
        cfg.z_min_m,
        cfg.z_max_m,
    )
    logger.info(
        "      误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
        cfg.max_delta_z_m,
        cfg.z_error_budget_max_m,
        cfg.consistency_N_frames,
        cfg.consistency_M_frames,
        cfg.consistency_jump_px,
    )
    logger.info(
        "      滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
        cfg.smooth_alpha,
        1.0 / cfg.smooth_alpha,
        cfg.jump_reset_m,
        cfg.isolation_gate_px,
    )


# ---------------------------------------------------------------- 场景


def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
    """① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
    ranger.reset()  # 场景独立：清空跨场景的滤波/一致性/隔离状态
    log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
    logger.info("   %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
    user_ms = 1000
    for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
        left, right = boxes_for(cfg, 0.0, 0.0, z_true)
        record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
        user_ms += 50
        if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
            note = "门① 视差域外 → invalid"
        elif not record.valid and record.disparity_px is not None:
            note = "门② 误差预算不满足 → invalid"
        elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
            note = f"有效（滤波后，真值 {z_true:.2f}）"
        else:
            note = "有效（滤波器初始化）"
        logger.info(
            "   %-7.2f %-9.2f %-6d %-7s %s",
            z_true,
            record.disparity_px if record.disparity_px is not None else float("nan"),
            int(record.valid),
            f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
            note,
        )
    logger.info(
        "   → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
        "有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
    )


def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
    """② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
    ranger.reset()
    log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
    x_center_m = 0.6
    left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
    record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
    logger.info("   %s", describe(record))
    logger.info(
        "   c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
        record.center_mid_px[0],
        cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
    )
    logger.info("   → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")


def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
    """③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
    ranger.reset()
    log_scene("③ 单目退化：仅左目检出")
    left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
    records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
    logger.info("   %s", describe(records[0]))
    logger.info("   → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")


def scene_frame_mismatch(ranger: StereoRanger) -> None:
    """④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
    ranger.reset()
    log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
    left_pair = pair_with_disparity(9.6)
    for _ in range(2):
        records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
    logger.info("   两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
    logger.info("   → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")


def scene_consistency(ranger: StereoRanger) -> None:
    """⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
    ranger.reset()
    log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
    for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
        left, right = pair_with_disparity(d_px)
        record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
        logger.info("   %s", describe(record))
    logger.info("   → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")


def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
    """⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
    ranger.reset()
    log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
    left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
    left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
    extra_right = Detection(360.0, 480.0, 420.0, 540.0)
    records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
    for record in records:
        logger.info("   %s", describe(record))
    logger.info("   → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")


# ---------------------------------------------------------------- 入口


def run_demo(config_path: str) -> None:
    cfg = load_from_json(config_path)
    logger.info("配置来源：%s", config_path)
    log_config_summary(cfg)
    logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")

    ranger = StereoRanger(cfg)  # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
    scene_approach(ranger, cfg)
    scene_far_field_bearing(ranger, cfg)
    scene_monocular_degradation(ranger, cfg)
    scene_frame_mismatch(ranger)
    scene_consistency(ranger)
    scene_multi_target(ranger, cfg)
    logger.info("")
    logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")


def main() -> None:
    parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
    parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
    args = parser.parse_args()
    logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
    run_demo(args.config)


if __name__ == "__main__":
    main()