"""Verify the rigs against each other: standoff, pair geometry, and a reprojection test.

The reprojection test is the end-to-end one: take the inspection point, transform it into
each camera's frame with that camera's own extrinsics, project with its intrinsics, and
check where it lands. A correctly aimed camera puts it on the principal point.
"""
import math
import omni.usd
from pxr import Gf, Usd, UsdGeom

stage = omni.usd.get_context().get_stage()
xc = UsdGeom.XformCache()
TARGET = Gf.Vec3d(-0.750, 0.0, 1.781)

def cam(name):
    for p in stage.Traverse():
        if p.IsA(UsdGeom.Camera) and p.GetName() == name:
            return p
    raise KeyError(name)

NAMES = ["RealSense_D435_Left", "RealSense_D435_Right",
         "Orbbec_Gemini305_Left", "Orbbec_Gemini305_Right",
         "Orbbec_Gemini345_Left", "Orbbec_Gemini345_Right"]
RES = {"RealSense_D435": (1280, 720), "Orbbec_Gemini305": (1280, 800),
       "Orbbec_Gemini345": (1280, 800)}

print("=== standoff (requested 600 mm) ===")
P, F = {}, {}
for n in NAMES:
    M = xc.GetLocalToWorldTransform(cam(n))
    p = Gf.Vec3d(M.ExtractTranslation())
    f = M.TransformDir(Gf.Vec3d(0, 0, -1)); f = f / (f.GetLength() or 1)
    P[n], F[n] = p, f
    print(f"  {n:>24}: {(TARGET-p).GetLength()*1000:7.1f} mm")

print("\n=== pair geometry ===")
for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"):
    l, r = P[f"{rig}_Left"], P[f"{rig}_Right"]
    fl, fr = F[f"{rig}_Left"], F[f"{rig}_Right"]
    base = (r - l).GetLength()
    ang = math.degrees(math.acos(max(-1, min(1, Gf.Dot(fl, fr)))))
    kind = ("OPPOSING - not a stereo pair" if ang > 150 else
            "rectified (parallel axes)" if ang < 0.5 else f"verged {ang:.2f} deg")
    print(f"  {rig:>18}: baseline {base*1000:8.1f} mm   axes {ang:6.2f} deg   {kind}")

print("\n=== reprojection of the inspection point (should land on the principal point) ===")
for n in NAMES:
    prim = cam(n)
    c = UsdGeom.Camera(prim)
    fl_mm = c.GetFocalLengthAttr().Get()
    ha = c.GetHorizontalApertureAttr().Get()
    va = c.GetVerticalApertureAttr().Get()
    rig = n.rsplit("_", 1)[0]
    W, H = RES[rig]
    fx = fl_mm / ha * W
    fy = fl_mm / va * H
    cx, cy = W / 2.0, H / 2.0
    M = xc.GetLocalToWorldTransform(prim)
    Pcam = M.GetInverse().Transform(TARGET)        # world -> camera frame
    z = -Pcam[2]                                    # camera looks down -Z
    if z <= 1e-6:
        print(f"  {n:>24}: BEHIND the camera"); continue
    u = cx + fx * (Pcam[0] / z)
    v = cy - fy * (Pcam[1] / z)
    print(f"  {n:>24}: depth {z*1000:6.1f} mm   pixel ({u:7.1f},{v:7.1f})   "
          f"offset from centre ({u-cx:+5.1f},{v-cy:+5.1f}) px")

print("\n=== cross-rig consistency: does every camera see the same point in front of it? ===")
depths = []
for n in NAMES:
    M = xc.GetLocalToWorldTransform(cam(n))
    depths.append(-M.GetInverse().Transform(TARGET)[2])
print(f"  depth spread across all six: {min(depths)*1000:.1f} .. {max(depths)*1000:.1f} mm"
      f"  (max-min = {(max(depths)-min(depths))*1000:.1f} mm)")