#!/usr/bin/env python3
"""Test the auto-cal portion of record_testing.py without launching the
full record flow. Writes to /tmp/auto_cal_test.json (the production
calibration_results.json is never touched)."""
import json
import sys
from pathlib import Path

# Only exo_redo needs to be on sys.path (ExoConfigs + exo_utils live
# there). Adding /home/robot-lab/raiden_internal itself would shadow the
# base raiden package (both trees expose a raiden/ subdir).
for p in ("/home/robot-lab/raiden_internal/third_party/exo_redo",):
    if p not in sys.path:
        sys.path.insert(0, p)

from raiden._config import CAMERA_CONFIG
from raiden.record_testing import (
    _patch_scene_zed_hd1080, _run_auto_calibration,
    _downshift_scene_cams_to_hd720)
from raiden.recorder import load_cameras_from_config

# Determine scene serials
with open(CAMERA_CONFIG) as f:
    cfg = json.load(f)
scene_serials = {int(e["serial"]) for e in cfg.values()
                 if isinstance(e, dict) and e.get("role") == "scene"
                 and e.get("type") == "zed"}
print(f"[test] scene serials: {sorted(scene_serials)}")

# Patch + open all cams
_patch_scene_zed_hd1080(scene_serials)
print("[test] opening cameras (scene @ HD1080, wrist @ HD720)...")
cameras = load_cameras_from_config(CAMERA_CONFIG)
print(f"[test] opened {len(cameras)} cams")

# Run auto-cal against a temp path
tmp_out = Path("/tmp/auto_cal_test.json")
tmp_out.unlink(missing_ok=True)
ok = _run_auto_calibration(CAMERA_CONFIG, str(tmp_out), cameras,
                            n_frames_per_cam=15)
print(f"\n[test] auto-cal returned: {ok}")

# Inspect the output
if tmp_out.exists():
    result = json.loads(tmp_out.read_text())
    cams = result.get("cameras", {})
    print(f"[test] cameras[] keys in output: {list(cams.keys())}")
    for name, entry in cams.items():
        ext = entry.get("extrinsics", {}).get("T_cam_in_base", [])
        if ext and len(ext) == 4:
            t = [ext[0][3], ext[1][3], ext[2][3]]
            print(f"  {name!r} (arm={entry.get('arm')}): "
                  f"T_cam_in_base_t=({t[0]:+.3f},{t[1]:+.3f},{t[2]:+.3f}) m")
    if "bimanual_transform" in result:
        bt = result["bimanual_transform"]
        t = bt.get("translation_m_right_base_in_left_base", [])
        print(f"  bimanual_transform: right_base_in_left_base_t={t}")
    else:
        print("  ⚠ no bimanual_transform in output")

# Verify downshift path — after auto-cal, scene cams should reopen at HD720
print("\n[test] downshifting scene cams to HD720...")
n = _downshift_scene_cams_to_hd720(cameras, scene_serials)
print(f"[test] downshifted {n} scene cam(s)")

# Sanity: grab one frame from each cam post-downshift and print shape
print("\n[test] post-downshift frame shapes:")
for cam in cameras:
    try:
        ok = cam.grab()
        if not ok:
            print(f"  {cam._name}: grab failed")
            continue
        frame = cam.get_frame()
        if frame is None or frame.color is None:
            print(f"  {cam._name}: no frame")
            continue
        print(f"  {cam._name} (S/N {cam._serial}): {frame.color.shape}")
    except Exception as e:
        print(f"  {cam._name}: {e}")

# Close cams
for cam in cameras:
    try:
        cam.close()
    except Exception:
        pass
print("\n[test] cameras closed.")