"""R1-3 — metric scale certification on an exact synthetic scene (CI-safe:
no torch, analytic depth).

Scene: pinhole camera (f = image width), height 1.5 m, pitch 25 deg, looking
at an infinite ground plane. Depth is computed analytically, the homography is
built exactly from four ground points with a known plane-unit scale, so the
true metersPerUnit is known in closed form.

Checks (real implementation extracted from app.py):
  1. recovery: estimate_meters_per_unit returns the true scale within 2%
  2. rejection: a sheared homography (the synthetic-VP failure mode) returns
     None instead of a confidently-wrong scale
  3. relative depth (normalised [0,1]) returns None (metric-only feature)
"""

import cv2
import numpy as np

# --- real implementation from app.py ----------------------------------------
src = open("app.py").read()
ns = {"np": np, "cv2": cv2, "depth_model_is_metric": lambda name=None: True}
start = src.index("def estimate_meters_per_unit")
end = src.index("\n# ---", start)
exec(compile(src[start:end], "app.py", "exec"), ns)
estimate_meters_per_unit = ns["estimate_meters_per_unit"]

W, H = 800, 600
F = float(W)
CAM_H = 1.5
PITCH = np.deg2rad(25.0)
UNITS_PER_M = 200.0          # plane-unit scale baked into the homography
TRUE_MPU = 1.0 / UNITS_PER_M


def scene():
    cx, cy = W / 2.0, H / 2.0
    u, v = np.meshgrid(np.arange(W, dtype=np.float64), np.arange(H, dtype=np.float64))
    # ground plane: 1/Z = (sin(t) + cos(t) * (v - cy)/f) / h  (v grows downward)
    inv_z = (np.sin(PITCH) + np.cos(PITCH) * (v - cy) / F) / CAM_H
    mask = inv_z > 1.0 / 30.0          # floor visible, within 30 m
    z = np.where(mask, 1.0 / np.maximum(inv_z, 1e-9), 0.0)

    # camera-frame 3D, then world ground coordinates. Camera pitched DOWN by
    # PITCH, world y up, image v down: world_y = -cos*y_c - sin*z (must be
    # exactly -CAM_H on the ground — asserted), forward = cos*z - sin*y_c.
    x_c = z * (u - cx) / F
    y_c = z * (v - cy) / F
    world_y = -np.cos(PITCH) * y_c - np.sin(PITCH) * z
    assert np.allclose(world_y[mask], -CAM_H, atol=1e-9), "sim geometry inconsistent"
    x_w = x_c
    fwd_w = np.cos(PITCH) * z - np.sin(PITCH) * y_c
    return mask.astype(np.uint8), z.astype(np.float32), x_w, fwd_w


def exact_homography(mask, x_w, fwd_w):
    ys, xs = np.nonzero(mask)
    # four well-spread ground points
    picks = []
    for fy, fx in [(0.95, 0.2), (0.95, 0.8), (0.55, 0.3), (0.55, 0.7)]:
        yy = int(np.percentile(ys, fy * 100))
        row = xs[ys == yy]
        xx = int(np.percentile(row, fx * 100))
        picks.append((xx, yy))
    src_pts = np.float32(picks)
    dst_pts = np.float32(
        [[x_w[y, x] * UNITS_PER_M, fwd_w[y, x] * UNITS_PER_M] for x, y in picks]
    )
    return cv2.getPerspectiveTransform(src_pts, dst_pts)


def main():
    ok = True
    mask, z, x_w, fwd_w = scene()
    Hm = exact_homography(mask, x_w, fwd_w)

    mpu = estimate_meters_per_unit(z, mask, Hm.flatten().tolist(), W, H)
    if mpu is None:
        print("  [FAIL] recovery: returned None on exact scene")
        ok = False
    else:
        err = abs(mpu - TRUE_MPU) / TRUE_MPU
        good = err <= 0.02
        print(f"  [{'PASS' if good else 'FAIL'}] recovery: mpu={mpu:.6f} "
              f"(true {TRUE_MPU:.6f}, err {err * 100:.2f}%)")
        ok &= good

    # synthetic-VP failure mode: progressive horizontal shear of plane coords
    S = np.array([[1.0, 0.35, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
    H_bad = S @ Hm
    mpu_bad = estimate_meters_per_unit(z, mask, H_bad.flatten().tolist(), W, H)
    print(f"  [{'PASS' if mpu_bad is None else 'FAIL'}] rejection: sheared homography -> {mpu_bad}")
    ok &= mpu_bad is None

    rel = (z - z[mask > 0].min()) / (z[mask > 0].max() - z[mask > 0].min())
    ns["depth_model_is_metric"] = lambda name=None: False
    mpu_rel = estimate_meters_per_unit(rel.astype(np.float32), mask, Hm.flatten().tolist(), W, H)
    print(f"  [{'PASS' if mpu_rel is None else 'FAIL'}] relative depth -> {mpu_rel}")
    ok &= mpu_rel is None

    print("\n" + ("ALL R1-3 SIM CHECKS PASSED" if ok else "R1-3 SIM CHECKS FAILED"))
    return 0 if ok else 1


if __name__ == "__main__":
    raise SystemExit(main())