#!/usr/bin/env python3
"""pose_probe -- offline chase render of the articulated mech in a fifodump.

    py pose_probe.py <capture.fifodump> <out.png> [updir]

Frames the camera on the LAST-articulated root (board.anim_abs), a chase
offset away, and renders one frame.  The proof harness for any "is the mech
posed/assembled right?" question -- offline, against a copy of the live dump,
zero interference with a running pod.

UP SENSE (updir, default +1): the wire's model/scene data is Y-UP -- proven
three ways on 2026-07-29: MAD.SKL offsets are anatomically coherent only with
+Y up (hip +5.29 above ground, knees/ankles descending), the BGF vertex
extents agree (foot meshes extend -1.0 below their ankle joint, torso +3.8
above shakey), and the SHIPPED pod's wire carries the same signs (hip +6.2,
knees negative, yaw-only vehicle root -- no flip anywhere on the wire).  So a
scene-space render wants screen-up = +Y, which is updir=+1 here.

The LIVE bridge world is the other way around -- "-y is up here" -- because
the calibrated live path (fp_cam / the mirrored det -1 root-basis family,
FP_RIGHT_SIGN, COCKPIT-CAGE-NOTES.md) maps the wire into Dave's GL
presentation.  The first version of this harness copied the aerial camera's
up hint from that world and rendered the mech hanging from its own shadow;
the operator caught it.  updir=-1 reproduces that view if you ever need it.
"""
import sys, os, struct
sys.path.insert(0, r'C:\VWE\TeslaRel410\dpl3-revive\patha')
os.environ.setdefault('SDL_VIDEODRIVER', 'dummy')
import numpy as np
from vrboard import VirtualBoard, Msg
from vrview import Renderer

cap = sys.argv[1]
out = sys.argv[2] if len(sys.argv) > 2 else "pose_probe.png"
updir = float(sys.argv[3]) if len(sys.argv) > 3 else 1.0

data = open(cap, "rb").read()
board = VirtualBoard()
off = 0
while off + 8 <= len(data):
    if data[off:off+4] != b'VPXM':
        off += 1; continue
    ln = struct.unpack_from('<I', data, off+4)[0]
    body = data[off+8:off+8+ln]
    if len(body) >= 4:
        a = struct.unpack_from('<I', body, 0)[0]
        if a < 0x100:
            try: board.handle(Msg(False, 0xff, a, body[4:]))
            except Exception: pass
    off += 8 + ln

if not board.anim_abs:
    print("no articulated roots in this capture"); sys.exit(1)

r = Renderer(w=832, h=512); c = r.cache; c.maybe_rebuild(board)
root = max(board.anim_abs.values(),
           key=lambda f: float(np.abs(np.array(f[9:12])).sum()))
target = np.array(root[9:12]); target[1] += updir * 6.0     # mid-body
eye = target + np.array([30.0, updir * 14.0, 40.0])         # above + beside
fwd = target - eye; fwd /= np.linalg.norm(fwd); back = -fwd
right = np.cross([0.0, updir, 0.0], back); right /= np.linalg.norm(right)
up = np.cross(back, right)
M = np.eye(4); M[0,:3], M[1,:3], M[2,:3], M[3,:3] = right, up, back, eye
r.cam_matrix = lambda _b, _M=M: _M
if c.view is not None and c.view in board.nodes:
    vb = bytearray(board.nodes[c.view].get('body') or b'')
    if len(vb) >= 56:
        struct.pack_into('<f', vb, 52, 1.0e6)
        board.nodes[c.view]['body'] = bytes(vb)
r.draw(board)
r.pygame.image.save(r.screen, out)
print("wrote %s  root at %s  updir=%+g" % (out, target.round(1), updir))