"""Query FFG identity/angles only. No force commands, ROS publication or CAN access."""
import sys
import time
from pathlib import Path
import json
import numpy as np
import serial

sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linkerhand_retarget/linkerhand_retarget'))
from linkerhand.linkerforce import FrameParser, FrameHandler
from linkerhand.constants import HandType

handler = FrameHandler(HandType.left)
parser = FrameParser()
samples = []
identity = None
print('准备开始：请戴好手套，接下来按提示缓慢张手、握拳，不要强掰关节。', file=sys.stderr, flush=True)
for remaining in (3, 2, 1):
    print(f'倒计时 {remaining}', file=sys.stderr, flush=True)
    time.sleep(1)
with serial.Serial('/dev/ttyUSB0', 2000000, timeout=0.1, exclusive=True) as port:
    port.write(handler.pack_version_query())
    start = time.monotonic()
    end = start + 20
    next_query = 0
    next_identity_query = start + 1
    last_phase = -1
    while time.monotonic() < end:
        if identity is None and time.monotonic() >= next_identity_query:
            port.write(handler.pack_version_query())
            next_identity_query = time.monotonic() + 1
        phase = min(int((time.monotonic() - start) // 5), 3)
        if phase != last_phase:
            gesture = ('张开手并保持', '缓慢握拳并保持', '再次张开手并保持', '再次缓慢握拳并保持')[phase]
            print(f'{gesture}（5秒）；已收到 {len(samples)} 帧', file=sys.stderr, flush=True)
            last_phase = phase
        if time.monotonic() >= next_query:
            port.write(handler.pack_position_query())
            next_query = time.monotonic() + 0.1
        for byte in port.read(port.in_waiting or 1):
            if parser.process_byte(byte):
                result = handler.handle_frame(parser.frame_buf)
                parser.reset()
                if result and 'version' in result:
                    identity = result
                if result and 'poslist' in result:
                    samples.append(result['poslist'])
report = {'identity': identity, 'frames': len(samples), 'baudrate': 2000000,
          'writes': ['version query 0x01', 'position query 0x03']}
if samples:
    a = np.asarray(samples)
    report.update(dimension=int(a.shape[1]), finite=bool(np.isfinite(a).all()),
                  last_degrees=np.rad2deg(a[-1]).round(2).tolist(),
                  range_degrees=np.rad2deg(np.ptp(a, axis=0)).round(2).tolist())
print(json.dumps(report, ensure_ascii=False, indent=2))
if not identity or identity.get('raw_handtype') != 'Left' or not samples:
    sys.exit(1)