"""SUPERSEDED - kept for history only.

SUPERSEDED by cam_configs.apply_config(stage, cam_configs.DEFAULT).
Hard-codes the old 600 mm standoff.
"""

"""Reposition the camera rigs:
  * RealSense D435: Left and Right on OPPOSITE sides of the belt, facing each other.
  * All three rigs: working distance exactly 600 mm to the inspection point.
Gemini 305/345 keep their azimuth and stay rectified (shared orientation, right eye
offset along the camera's local +X by the baseline).
"""
import json, math
import omni.usd
from pxr import Gf, Usd, UsdGeom

stage = omni.usd.get_context().get_stage()
TARGET = Gf.Vec3d(-0.750, 0.0, 1.781)      # inspection point on the belt
STANDOFF = 0.60                             # requested working distance, metres
SIDE_ELEV_DEG = 20.0                        # D435 sits low, at the side
UP = Gf.Vec3d(0, 0, 1)

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

def look_at_matrix(pos, target):
    """USD camera convention: looks down local -Z, local +Y is up."""
    fwd = (target - pos)
    fwd = fwd / (fwd.GetLength() or 1.0)
    zax = -fwd
    up = UP
    if abs(Gf.Dot(up, zax)) > 0.999:
        up = Gf.Vec3d(0, 1, 0)
    xax = Gf.Cross(up, zax); xax = xax / (xax.GetLength() or 1.0)
    yax = Gf.Cross(zax, xax)
    M = Gf.Matrix4d(1.0)
    M.SetRow3(0, xax); M.SetRow3(1, yax); M.SetRow3(2, zax)
    M.SetTranslateOnly(pos)
    return M, xax, fwd

def set_pose(prim, M):
    xf = UsdGeom.Xformable(prim)
    xf.ClearXformOpOrder()
    xf.AddTransformOp().Set(M)

def current_dir_from_target(prim):
    p = UsdGeom.XformCache().GetLocalToWorldTransform(prim).ExtractTranslation()
    d = Gf.Vec3d(p) - TARGET
    return d / (d.GetLength() or 1.0)

report = {}

# ---- D435: opposing pair, one each side, both aimed at the belt --------------------
th = math.radians(SIDE_ELEV_DEG)
for side_name, sgn in (("Left", +1.0), ("Right", -1.0)):
    prim = find(f"RealSense_D435_{side_name}")
    pos = TARGET + Gf.Vec3d(0.0, sgn * STANDOFF * math.cos(th), STANDOFF * math.sin(th))
    M, _, fwd = look_at_matrix(pos, TARGET)
    set_pose(prim, M)
    report[f"RealSense_D435_{side_name}"] = dict(
        pos=[round(v, 4) for v in pos], fwd=[round(v, 4) for v in fwd],
        dist_mm=round((TARGET - pos).GetLength() * 1000, 1))

# ---- Gemini pairs: same azimuth, distance forced to 600 mm, stay RECTIFIED ---------
for rig, baseline_m in (("Orbbec_Gemini305", 0.0265), ("Orbbec_Gemini345", 0.1294)):
    left, right = find(f"{rig}_Left"), find(f"{rig}_Right")
    # keep the direction the pair currently views from, measured at its midpoint
    xc = UsdGeom.XformCache()
    pl = Gf.Vec3d(xc.GetLocalToWorldTransform(left).ExtractTranslation())
    pr = Gf.Vec3d(xc.GetLocalToWorldTransform(right).ExtractTranslation())
    mid = (pl + pr) * 0.5
    d = mid - TARGET
    d = d / (d.GetLength() or 1.0)
    centre = TARGET + d * STANDOFF
    M, xax, fwd = look_at_matrix(centre, TARGET)
    # both eyes share ONE orientation - that is what keeps the pair rectified; the right
    # eye is displaced along the camera's own +X by the baseline
    for prim, off in ((left, -baseline_m / 2.0), (right, +baseline_m / 2.0)):
        Mi = Gf.Matrix4d(M)
        Mi.SetTranslateOnly(centre + xax * off)
        set_pose(prim, Mi)
        report[prim.GetName()] = dict(
            pos=[round(v, 4) for v in (centre + xax * off)],
            fwd=[round(v, 4) for v in fwd],
            dist_mm=round((TARGET - (centre + xax * off)).GetLength() * 1000, 1))

print(f"{'camera':>26} {'position':>26} {'dist to belt':>13}")
for k, v in report.items():
    print(f"{k:>26} ({v['pos'][0]:+6.3f},{v['pos'][1]:+6.3f},{v['pos'][2]:+6.3f}) {v['dist_mm']:>10.1f} mm")

out = "/home/dasha/robozon-sorter/control_test/calib_rs_side.json"
json.dump(dict(target=[round(v, 4) for v in TARGET], standoff_m=STANDOFF,
               side_elevation_deg=SIDE_ELEV_DEG, cameras=report),
          open(out, "w"), indent=2)
print(f"\nextrinsics -> {out}")