#!/usr/bin/env python3
"""test_recorder_rpc.py — 記録の角度の抜け (依頼 46 B) を偽の daemon で確かめる (アームなし. 2026-10-31).

  python3 pi05/test_recorder_rpc.py          # 全部
  python3 pi05/test_recorder_rpc.py -v

偽の daemon は本物 (mecharm_daemon.py) と同じく 1 本のつながりを 1 件ずつ処理する.
send_angles を受けると, 腕を 速度 20 の速さ (いちばん大きく動く関節 30°/秒) で目標へ動かし始め,
送ったあとの読み直しの分 (READBACK_SEC) 待ってから返事をする. get_angles は 35ms で今の角度を返す.
本体の側は run_policy_client.py の中継点と同じ順 (read_angles → _send_and_wait_arrival → read_angles) で送る.

確かめること:
  - 記録をつけても daemon に届く指令 (cmd・目標・速度・wait) の並びは変わらない
  - send_angles 行 (始めた時刻・目標・やりとりの長さ) と, 本体が読んだ角度の actual 行 (note=main) が残る
  - 動いている間の実測角の間隔 (読み直し 1.6 秒の daemon / 読み直しのない daemon の比較を表で出す)
KIOXIA にもアームにも触らない (一時フォルダと 127.0.0.1 だけ).
"""
import csv
import io
import json
import socket
import sys
import tempfile
import threading
import time
import unittest
from pathlib import Path
from unittest import mock

import numpy as np

_HERE = Path(__file__).resolve().parent
sys.path.insert(0, str(_HERE))

import run_policy_client as rpc  # noqa: E402
import policy_recorder as pr  # noqa: E402
import visual_servo_pick as vsp  # noqa: E402

SPEED_DPS = 30.0
GET_SEC = 0.035
START = [0.0, 10.0, 10.0, 0.0, -10.0, -30.0]
VIAS = [[2.0, 16.0, 14.0, -0.5, -18.0, -29.5],
        [4.0, 22.0, 18.0, -1.0, -26.0, -29.0],
        [6.0, 28.0, 22.0, -1.5, -34.0, -28.5]]


class FakeDaemon:
    """1 本のつながりを 1 件ずつ処理する偽の daemon (腕の動きは時刻で決める)."""

    def __init__(self, readback_sec):
        self.readback_sec = readback_sec
        self.cmds = []
        self.pose0 = np.array(START, dtype=float)
        self.goal = self.pose0.copy()
        self.t_move = 0.0
        self.srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.srv.bind(("127.0.0.1", 0))
        self.srv.listen(1)
        self.port = self.srv.getsockname()[1]
        self.stop = False
        self.th = threading.Thread(target=self._serve, daemon=True)
        self.th.start()

    def pose(self, now=None):
        now = time.time() if now is None else now
        d = float(np.max(np.abs(self.goal - self.pose0)))
        if d <= 0:
            return self.goal.copy()
        f = min(1.0, (now - self.t_move) * SPEED_DPS / d)
        return self.pose0 + f * (self.goal - self.pose0)

    def _handle(self, c):
        cmd = c.get("cmd")
        if cmd == "ping":
            return {"ok": True}
        self.cmds.append({k: c[k] for k in sorted(c)})
        if cmd == "get_angles":
            time.sleep(GET_SEC)
            return {"ok": True, "angles": [round(float(v), 2) for v in self.pose()]}
        if cmd == "send_angles":
            self.pose0 = self.pose()
            self.goal = np.array(c["angles"], dtype=float)
            self.t_move = time.time()
            time.sleep(float(c.get("wait", 0)) + self.readback_sec)   # 送ったあとの読み直しの分
            return {"ok": True, "result": 1, "elapsed_ms": 10.0}
        return {"ok": False, "error": f"unknown cmd: {cmd}"}

    def _serve(self):
        while not self.stop:
            try:
                cs, _ = self.srv.accept()
            except OSError:
                return
            buf = b""
            with cs:
                while not self.stop:
                    data = cs.recv(4096)
                    if not data:
                        break
                    buf += data
                    while b"\n" in buf:
                        line, buf = buf.split(b"\n", 1)
                        resp = self._handle(json.loads(line))
                        cs.sendall((json.dumps(resp) + "\n").encode())

    def close(self):
        self.stop = True
        try:
            self.srv.close()
        except Exception:
            pass


class FakeCap:
    def read(self):
        time.sleep(1 / 30)
        return True, np.full((72, 128, 3), 100, np.uint8)


def run_vias(readback_sec, record):
    """偽の daemon に中継点を 3 つ送る. 戻り値: (daemon に届いた指令, states.csv の行, 動いた窓)."""
    d = FakeDaemon(readback_sec)
    vsp.close_daemon_socket()
    pr.install_daemon_lock()
    windows = []
    with tempfile.TemporaryDirectory() as td, \
            mock.patch.object(vsp, "DAEMON_HOST", "127.0.0.1"), mock.patch.object(vsp, "DAEMON_PORT", d.port), \
            mock.patch("sys.stdout", new=io.StringIO()):
        rec = None
        if record:
            rec = pr.PolicyRecorder(Path(td), FakeCap(), rpc.read_angles_once, lambda f: (f, f),
                                    angles_ok=rpc.wr._is_valid_angles)
            rec.start()
        time.sleep(0.5)
        for i, v in enumerate(VIAS):
            qa = rpc.read_angles()
            t_send = time.time()
            ok, _ = rpc._send_and_wait_arrival(list(v), f"via{i}", 3.0, send_wait_sec=rpc.SEND_WAIT)
            assert ok
            dist = float(np.max(np.abs(np.asarray(v) - qa)))
            windows.append((t_send, t_send + dist / SPEED_DPS))
            rpc.read_angles()
        time.sleep(0.5)
        rows = []
        if rec is not None:
            rec.stop()
            with (Path(td) / "recording" / "states.csv").open(newline="") as f:
                rows = list(csv.DictReader(f))
    vsp.close_daemon_socket()
    d.close()
    return d.cmds, rows, windows


def motion_intervals(rows, windows, include_main):
    """動いている窓にかかる, 続けて残った実測角の間隔 (秒) の最大と, 窓の中に残った実測角の数.
    窓の始まりは send_angles 行の時刻 (daemon へ送り始めた時刻) にそろえる."""
    sends = [float(r["ts"]) for r in rows if r["event_type"] == "send_angles"]
    if len(sends) == len(windows):
        windows = [(s, s + (b - a)) for s, (a, b) in zip(sends, windows)]
    ts = sorted(float(r["ts"]) for r in rows if r["event_type"] == "actual"
                and (include_main or r["note"] != "main"))
    ts = np.array(ts)
    worst, inside = 0.0, 0
    for a, b in windows:
        inside += int(((ts > a) & (ts < b)).sum())
        k0 = max(0, int(np.searchsorted(ts, a)) - 1)
        k1 = min(len(ts) - 1, int(np.searchsorted(ts, b)))
        if k1 > k0:
            worst = max(worst, float(np.max(np.diff(ts[k0:k1 + 1]))))
    return worst, inside


def _strip_gets(cmds):
    return [c for c in cmds if c["cmd"] != "get_angles"]


class TestRecorderRpc(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        cls.res = {rb: run_vias(rb, True) for rb in (1.6, 0.0)}
        cls.plain = run_vias(1.6, False)

    def test_commands_unchanged(self):
        """記録をつけても daemon に届く送信の並び (目標・速度・wait) は記録なしと同じ."""
        with_rec = _strip_gets(self.res[1.6][0])
        without = _strip_gets(self.plain[0])
        self.assertEqual(with_rec, without)
        self.assertEqual(len(with_rec), len(VIAS))
        for c, v in zip(with_rec, VIAS):
            self.assertEqual(c["angles"], v)
            self.assertEqual(c["speed"], rpc.SPEED)
            self.assertEqual(c["wait"], rpc.SEND_WAIT)

    def test_send_rows_and_main_reads(self):
        cmds, rows, _ = self.res[1.6]
        sends = [r for r in rows if r["event_type"] == "send_angles"]
        self.assertEqual(len(sends), len(VIAS))
        for r, v in zip(sends, VIAS):
            self.assertEqual([float(r[f"j{k}"]) for k in range(6)], v)
            self.assertEqual(r["ts"], r["ts_send"])
            rpc_sec = float(r["note"].split("rpc_sec=")[1].split()[0])
            self.assertGreater(rpc_sec, 1.5)          # 読み直しの間, つながりがふさがる
        main = [r for r in rows if r["event_type"] == "actual" and r["note"] == "main"]
        self.assertGreater(len(main), 0)
        # 記録スレッドの分を二重に残さない: main 行 + 記録スレッドの行 = daemon に届いた get_angles の数
        own = [r for r in rows if r["event_type"] == "actual" and r["note"] != "main"]
        self.assertEqual(len(main) + len(own), sum(1 for c in cmds if c["cmd"] == "get_angles"))

    def test_report_intervals(self):
        """動いている間の実測角の間隔を出す (表は標準エラーへ). 読み直し 1.6 秒では抜けが残ることも確かめる."""
        out = []
        for rb in (1.6, 0.0):
            _, rows, win = self.res[rb]
            for inc in (False, True):
                w, n = motion_intervals(rows, win, inc)
                out.append((rb, inc, w, n))
        lines = ["", "daemon の送ったあとの読み直し | 本体の読みも残す | 動いている間の最大の間隔 | 動いている間に残った数"]
        for rb, inc, w, n in out:
            lines.append(f"  {rb:.1f} 秒 | {'する' if inc else 'しない (前)'} | {w:.2f} 秒 | {n}")
        print("\n".join(lines), file=sys.stderr)
        w16 = [w for rb, inc, w, n in out if rb == 1.6]
        self.assertTrue(all(w > 1.0 for w in w16))   # 読み直しの間はだれも読めない
        w0 = [w for rb, inc, w, n in out if rb == 0.0 and inc]
        self.assertLess(w0[0], 0.3)                   # 読み直しがなければ 10Hz 近くで残る


class TestObserverHook(unittest.TestCase):
    """install_daemon_lock の観察口: 返事・例外をそのまま返し, 入れ子の ping は渡さず, 観察の失敗で止まらない."""

    def _install(self, fake):
        saved = vsp._daemon_request
        self.addCleanup(setattr, vsp, "_daemon_request", saved)
        with mock.patch.object(vsp, "_daemon_request", fake):
            pr.install_daemon_lock()
            wrapped = vsp._daemon_request
        return wrapped

    def test_passthrough_nested_and_errors(self):
        seen = []

        def fake(cmd_obj, timeout_override=None):
            if cmd_obj["cmd"] == "get_angles":
                vsp._daemon_request({"cmd": "ping"})     # 再接続の中の ping (入れ子)
                return {"ok": True, "angles": [1, 2, 3, 4, 5, 6]}
            if cmd_obj["cmd"] == "boom":
                raise RuntimeError("daemon timeout")
            return {"ok": True}
        wrapped = self._install(fake)
        with mock.patch.object(vsp, "_daemon_request", wrapped):
            def obs(c, r, t0, t1, e):
                seen.append((c["cmd"], r, e))
            def bad(*a):
                raise ValueError("observer bug")
            pr.add_rpc_observer(bad)
            pr.add_rpc_observer(obs)
            self.addCleanup(pr.remove_rpc_observer, bad)
            self.addCleanup(pr.remove_rpc_observer, obs)
            r = vsp._daemon_request({"cmd": "get_angles"}, timeout_override=1.0)
            self.assertEqual(r["angles"], [1, 2, 3, 4, 5, 6])
            with self.assertRaises(RuntimeError) as cm:
                vsp._daemon_request({"cmd": "boom"})
            self.assertEqual(str(cm.exception), "daemon timeout")
        self.assertEqual([x[0] for x in seen], ["get_angles", "boom"])   # 入れ子の ping は渡さない
        self.assertIsInstance(seen[1][2], RuntimeError)


if __name__ == "__main__":
    unittest.main()