#!/usr/bin/env python3
"""Wire Dave's dpl3-revive renderer into our LIVE running pod.

Tails our device's VPX_FIFODUMP (VPXM records = the game's live VelociRender
wire), feeds each message into Dave's VirtualBoard, and renders every
draw_scene with his vrview software rasterizer in a real window -- using the
game's own camera (the player's RIO input drives it).

  py live_bridge.py <live.fifodump>
"""
import os, sys, struct, time, math
import numpy as np
from _backend import pick_renderer
from vrboard import VirtualBoard, Msg, A
Renderer, backend = pick_renderer()

path = sys.argv[1]
catchup = sys.argv[2] if len(sys.argv) > 2 else None
board = VirtualBoard()
board.munga = False
# BRIDGE_W/BRIDGE_H size the main out-the-window render window (default the
# 832x512 debug size; pod deploy sets the main display's native res e.g.
# 800x600). BRIDGE_BORDERLESS=1 drops the title bar for a kiosk deploy.
r = Renderer(w=int(os.environ.get('BRIDGE_W', '832')),
             h=int(os.environ.get('BRIDGE_H', '512')),
             title=f"dpl3-revive renderer (Dave) -- LIVE from our pod [{backend}]")
r.fps = int(os.environ.get('BRIDGE_FPS', '60' if backend == 'GL' else '30'))
# eye TRIM on top of the camera position (live; render window must be
# focused): UP/DOWN = height +-1, LEFT/RIGHT = seat forward/back +-0.5
# along the look direction (user: the canopy reads as "sitting too far
# back" -- tune, then pin with FP_UPOFF / FP_FWDOFF). The chain camera
# carries the true cockpit eye so the default trims are 0; the vehicle-
# root fallback adds VEH_EYE below (12 was the hand-tuned value).
UPOFF = [float(os.environ.get('FP_UPOFF', '0'))]
FWDOFF = [float(os.environ.get('FP_FWDOFF', '0'))]
VEH_EYE = 12.0
# the un-overridden munga cam-chain method (fp_cam overrides r.cam_matrix
# every frame, so grab the bound original once)
CHAIN_CAM = r.cam_matrix
# torso twist: the 0x1f batch's 2f/5f joint entries are (sin,cos) of the
# joint angle keyed by the joint's DCS handle (calibrated live vs MADCAT.SUB
# +-130deg limits). The chain DCS values EXCLUDE joint angles, so the twist
# is composed onto the chain look direction in fp_cam. JOINTS holds the
# latest (sin,cos) per handle. FP_TWIST_SIGN flips, FP_TWIST=0 disables.
JOINTS = {}
TWIST_SIGN = float(os.environ.get('FP_TWIST_SIGN', '1'))
TWIST_ON = os.environ.get('FP_TWIST', '1') != '0'

def track_joints(payload):
    """Dave's backtracking 0x1f parse (vrboard.py), keeping the joint
    (sin,cos) entries his board skips."""
    p = payload
    if len(p) < 8:
        return
    n = min(struct.unpack_from('<I', p, 0)[0], 64)
    nodes = board.nodes
    def parse(off, k):
        if k == 0:
            return [] if off == len(p) else None
        if off + 4 > len(p):
            return None
        h = struct.unpack_from('<I', p, off)[0]
        if nodes.get(h, {}).get('type') != 5:
            return None
        for nf in (12, 2, 5):
            end = off + 4 + nf * 4
            if end > len(p):
                continue
            rest = parse(end, k - 1)
            if rest is not None:
                return [(h, off + 4, nf)] + rest
        return None
    for h, o, nf in parse(4, n) or ():
        if nf in (2, 5):
            JOINTS[h] = struct.unpack_from('<2f', p, o)

def fp_cam(board, cache):
    """First-person cockpit camera, best source first:
    1. HEAD-LOOK (default): the munga cam DCS chain's translation row = the
       true cockpit eye, its +Z row = the look direction (follows torso
       twist). The chain's full rotation is singular through our wire (two
       rows collapse to +-Y), so only eye + Z row are trusted and the basis
       is rebuilt y-up. FP_CAM=vehicle forces the fallback.
    2. Fallback: the player vehicle's 0x1f root pose -- eye at hull + VEH_EYE,
       forward = -Z row (FP_FWD_SIGN flips)."""
    anim = board.anim_abs
    chain = cache.cam_chain
    h = None
    if chain and chain[-1] in anim:
        h = chain[-1]
    elif anim:
        h = max(anim, key=lambda k: float(np.abs(np.array(anim[k][9:12])).sum()))
    # h may be None before the FIRST vehicle articulation arrives (a freshly
    # dropped mech that hasn't moved sends no 0x1f yet -- both -egg and the
    # console flow). The CHAIN camera below works fine without it; bailing
    # out here left the raw default chain convention on screen, which put
    # the pre-movement view inside the mech's own geometry.
    R = t = None
    if h is not None:
        fp_cam.root = h      # player vehicle root DCS (for the pick backchannel)
        f = anim[h]
        R = np.array(f[:9]).reshape(3, 3)
        t = np.array(f[9:12])
    worldup = np.array([0.0, 1.0, 0.0])
    eye = fwd = None
    if os.environ.get('FP_CAM', 'chain') != 'vehicle':
        try:
            Mc = np.asarray(CHAIN_CAM(board), float)
            ec, fc = Mc[3, :3], Mc[2, :3]      # eye row; look = +Z row here
            n = np.linalg.norm(fc)
            if (np.isfinite(ec).all() and n > 1e-6 and
                    (t is None or np.linalg.norm(ec - t) < 100.0)):
                eye = ec + worldup * UPOFF[0]
                fwd = fc / n
        except Exception:
            pass
    if eye is None and R is None:
        return None
    if eye is None:
        # vehicle-root fallback: -Z row rendered forward (user-verified);
        # FP_FWD_SIGN flips it back if needed.
        fwd = float(os.environ.get('FP_FWD_SIGN', '-1')) * R[2]
        n = np.linalg.norm(fwd)
        if n < 1e-6:
            return None
        fwd = fwd / n
        eye = t + worldup * (VEH_EYE + UPOFF[0])
    # NOTE: a bridge-level "head-glance axis fix" (swap the hat glance's yaw<->
    # pitch) was tried 2026-07-07 and REVERTED: the hat glance, torso twist and
    # the stick-Y torso pitch all compose into this one look-vector and can't be
    # separated here (we only have the vehicle ROOT pose, not the torso joints),
    # so the swap also flipped the stick-Y vertical aim into yaw. The hat
    # left/right->up/down permutation must be fixed at the source (device
    # head-DCS decode -- the chain rotation is degenerate: X/Y rows collapse to
    # +-Y) or in the vRIO input mapping, without touching the look-vector.
    # torso twist: yaw the look direction by any chain-member joint angle
    # (jointtorso lives IN the cam chain; the shadow joint does not)
    if TWIST_ON:
        for jh in (chain or ()):
            sc = JOINTS.get(jh)
            if sc is None:
                continue
            th = TWIST_SIGN * math.atan2(sc[0], sc[1])
            cs, sn = math.cos(th), math.sin(th)
            fwd = np.array([cs * fwd[0] + sn * fwd[2], fwd[1],
                            -sn * fwd[0] + cs * fwd[2]])
    eye = eye + fwd * FWDOFF[0]      # seat forward/back trim
    back = -fwd
    right = np.cross(worldup, back)
    rn = np.linalg.norm(right)
    if rn < 1e-6:
        return None
    right /= rn
    up = np.cross(back, right)
    # FP_RIGHT_SIGN=-1 mirrors the image X (verified: exact fliplr). Tried as a
    # yaw fix 2026-07-06 but the inverted-yaw report was about the NATIVE C++
    # render, not this bridge -- the mirror just flipped the arena layout and
    # made the bridge yaw wrong too. Keep +1 (the validated look).
    right *= float(os.environ.get('FP_RIGHT_SIGN', '1'))
    M = np.eye(4)
    M[0, :3], M[1, :3], M[2, :3], M[3, :3] = right, up, back, eye
    return M

def send_cam(M):
    # Backchannel on the fifosock: hand the device OUR camera (the
    # user-validated cockpit view, twist + glance included) so its reticle
    # raycast aims where the player actually looks. The device's own view
    # decode sits at the static view-node pose, which aimed every pick --
    # and thus every missile volley and laser beam -- at one fixed wrong
    # world point (the "missiles fly off to a fixed spot" bug).
    if sock is None:
        return
    eye, fwd = M[3, :3], -M[2, :3]
    # 7th field: player vehicle root DCS -- the device skips instances on
    # this articulation subtree (own arms/torso/muzzle-flash), which the ray
    # grazes intermittently and which retargeted mid-volley missiles.
    root = getattr(fp_cam, 'root', 0) or 0
    try:
        sock.send(f"CAM {eye[0]:.3f} {eye[1]:.3f} {eye[2]:.3f} "
                  f"{fwd[0]:.6f} {fwd[1]:.6f} {fwd[2]:.6f} {root:x}\n".encode())
    except OSError:
        pass

def render(board):
    try:
        pg = r.pygame
        for ev in pg.event.get():            # drain before r.draw; tune eye height
            if ev.type == pg.QUIT:
                raise KeyboardInterrupt
            if ev.type == pg.KEYDOWN:
                hstep = {pg.K_UP: 1, pg.K_DOWN: -1}.get(ev.key)
                fstep = {pg.K_RIGHT: 0.1, pg.K_LEFT: -0.1}.get(ev.key)
                sstep = {pg.K_EQUALS: 0.05, pg.K_MINUS: -0.05,
                         pg.K_KP_PLUS: 0.05, pg.K_KP_MINUS: -0.05}.get(ev.key)
                if hstep:
                    UPOFF[0] += hstep
                if fstep:
                    FWDOFF[0] += fstep
                if sstep:
                    try:
                        import vrview_gl
                        vrview_gl.HUDSCALE[0] = max(
                            0.2, vrview_gl.HUDSCALE[0] + sstep)
                        print(f"HUD scale = {vrview_gl.HUDSCALE[0]:.2f}",
                              flush=True)
                    except Exception:
                        pass
                if hstep or fstep:
                    print(f"eye trim: height {UPOFF[0]:+.1f}  "
                          f"forward {FWDOFF[0]:+.1f}", flush=True)
        r.cache.maybe_rebuild(board)
        M = fp_cam(board, r.cache)
        if M is not None:
            r.cam_matrix = lambda _b, _M=M: _M
            send_cam(M)
        r.draw(board)
    except KeyboardInterrupt:
        raise
    except Exception as e:
        global _render_errs
        _render_errs = globals().get('_render_errs', 0) + 1
        if _render_errs <= 3:
            import traceback
            print(f"render error #{_render_errs}: {e}", flush=True)
            traceback.print_exc()
            sys.stdout.flush()

# wire source: a fifodump file to tail, or "tcp:<port>" = the device's
# VPX_FIFOSOCK live tee (same VPXM records, no file-poll quantum; recv blocks
# until data arrives so wire-to-render latency is the socket itself).
tcp_port = int(path[4:]) if path.startswith('tcp:') else None
sock = None

def read_chunk():
    global sock
    if tcp_port is None:
        return f.read(1 << 20)
    if sock is None:
        import socket as sk
        while True:
            s = sk.socket()
            try:
                s.connect(('127.0.0.1', tcp_port))
                s.settimeout(0.02)          # idle cap: keeps the event pump alive
                s.setsockopt(sk.IPPROTO_TCP, sk.TCP_NODELAY, 1)
                print("fifosock connected", flush=True)
                sock = s
                break
            except OSError:
                time.sleep(0.3)
    try:
        c = sock.recv(1 << 20)
    except TimeoutError:
        return b''
    except OSError:
        sock = None
        return b''
    if c == b'':
        print("fifosock closed; reconnecting", flush=True)
        sock = None
    return c

if tcp_port is None:
    print(f"waiting for {path} ...")
    while not os.path.exists(path):
        time.sleep(0.2)
    f = open(path, 'rb')
elif catchup and os.path.exists(catchup):
    # a socket client joining mid-mission missed the scene-create records;
    # replay the archival fifodump first (no rendering), then ride the tee.
    # Records between our EOF and the socket accept are lost -- poses are
    # absolute so the state self-heals within a frame.
    data = open(catchup, 'rb').read()
    o = fed = 0
    while o + 8 <= len(data):
        if data[o:o + 4] != b'VPXM':
            o += 1; continue
        ln = struct.unpack_from('<I', data, o + 4)[0]
        if o + 8 + ln > len(data):
            break
        body = data[o + 8:o + 8 + ln]; o += 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
                if a == 0x1f:
                    try: track_joints(body[4:])
                    except Exception: pass
                fed += 1
    print(f"catchup: {fed} records from {catchup}", flush=True)
print("tailing live wire -> Dave's renderer; drive the pod")

pending = b''
frames = 0
skipped = 0
last_report = time.time()
while True:
    chunk = read_chunk()
    if chunk:
        pending += chunk
    # Slice ALL complete records out of the buffer first, so we know which
    # draw_scene is the newest. Rendering every draw_scene in arrival order
    # means any render slowdown plays the mission slower than real time and
    # the view drifts seconds behind the game (backpressure hides in the
    # socket, not in len(pending)). State records all still apply, in order;
    # only superseded frame PRESENTS are skipped -- the view latches to the
    # freshest frame no matter how slow GL is.
    records = []
    off = 0
    n = len(pending)
    while n - off >= 8:
        if pending[off:off + 4] != b'VPXM':
            off += 1
            continue
        ln = struct.unpack_from('<I', pending, off + 4)[0]
        if n - off < 8 + ln:
            break                       # incomplete record: wait for more
        records.append(pending[off + 8:off + 8 + ln])
        off += 8 + ln
    pending = pending[off:]
    last_draw = -1
    for i, body in enumerate(records):
        if len(body) >= 4 and struct.unpack_from('<I', body, 0)[0] == 9:
            last_draw = i
    for i, body in enumerate(records):
        if len(body) < 4:
            continue
        action = struct.unpack_from('<I', body, 0)[0]
        if action < 0x100:
            try:
                board.handle(Msg(False, 0xff, action, body[4:]))
            except Exception:
                pass
            if action == 0x1f:           # torso/limb joint angles ride here
                try: track_joints(body[4:])
                except Exception: pass
            if action == 9:              # draw_scene -> present a frame
                if i == last_draw:
                    render(board)
                    frames += 1
                else:
                    skipped += 1
    if not chunk:
        try: r.pump()
        except KeyboardInterrupt: break
        except Exception: pass
        if tcp_port is None:
            time.sleep(0.02)    # socket mode already waited in recv
    if time.time() - last_report > 4:
        last_report = time.time()
        print(f"frames={frames} skipped={skipped} nodes={len(board.nodes)} "
              f"uploads={len(board.uploads)} tex={len(board.tex)} "
              f"munga={board.munga} anim_abs={len(board.anim_abs)} "
              f"backlog={len(pending)}B", flush=True)