#!/usr/bin/env python3
"""Type-2 G20-protocol matrix queries only. No driver/ROS/serial/motor writes."""
import argparse
import json
import subprocess
import time
from pathlib import Path
import can

NAMES = ['thumb', 'index', 'middle', 'ring', 'little']
ROW_IDS = {i * 16: i for i in range(12)}


def query(bus, command, payload=(), timeout=0.15):
    # Drain old traffic so incomplete responses never reuse cached matrices.
    deadline = time.monotonic() + 0.05
    while bus.recv(0) is not None:
        if time.monotonic() > deadline:
            raise RuntimeError('Unexpected continuous CAN traffic; stop other CAN clients')
    bus.send(can.Message(arbitration_id=0x28, is_extended_id=False,
                         data=[command, *payload]), timeout=0.2)
    frames = []
    end = time.monotonic() + timeout
    while time.monotonic() < end:
        msg = bus.recv(max(0, end-time.monotonic()))
        if msg is None:
            break
        if msg.is_error_frame:
            raise RuntimeError('CAN error frame')
        if (msg.is_rx and not msg.is_extended_id and msg.arbitration_id == 0x28
                and msg.data and msg.data[0] == command):
            frames.append(list(msg.data))
    return frames


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--duration', type=float, default=25)
    args = parser.parse_args()
    if not 1 <= args.duration <= 120:
        parser.error('duration must be 1..120 seconds')
    processes = subprocess.check_output(['ps', '-eo', 'args='], text=True)
    if any('linker_hand_sdk' in line and 'python' in line for line in processes.splitlines()):
        raise SystemExit('Stop linker_hand_sdk first. This test requires no motion controller.')
    records = []
    print('READ ONLY: type-2 touch raw intensity, NOT calibrated N or grams.', flush=True)
    with can.Bus(channel='can0', interface='socketcan', receive_own_messages=False) as bus:
        frames = query(bus, 0xB0)
        if not any(len(f) >= 2 and f[1] == 2 for f in frames):
            raise RuntimeError(f'Expected touch type 2, got {frames}')
        start = time.monotonic()
        try:
            while time.monotonic()-start < args.duration:
                sample = {'elapsed': round(time.monotonic()-start, 2), 'fingers': {}}
                for i, name in enumerate(NAMES):
                    frames = query(bus, 0xB1+i, [0xC6], timeout=0.08)
                    rows = {ROW_IDS[f[1]]: f[2:] for f in frames
                            if len(f) == 8 and f[1] in ROW_IDS}
                    complete = len(rows) == 12
                    values = [v for row in rows.values() for v in row]
                    sample['fingers'][name] = {
                        'complete': complete, 'rows': len(rows),
                        'max': max(values) if complete else None,
                        'sum_raw': sum(values) if complete else None,
                        'matrix': [rows.get(r) for r in range(12)],
                    }
                records.append(sample)
                print(' '.join(f"{name}:max={v['max']} rows={v['rows']}/12"
                               for name,v in sample['fingers'].items()), flush=True)
        finally:
            destination = Path(__file__).resolve().parents[1]/'diagnostics'/f'touch-readonly-{time.time_ns()}.json'
            destination.parent.mkdir(exist_ok=True)
            destination.write_text(json.dumps(records, indent=2))
            print(f'Saved: {destination}', flush=True)


if __name__ == '__main__':
    main()