#!/bin/bash
# daemon_fast_read.sh — Pi の mecharm_daemon.py の角度の読み直しを速くして起動し直す (2026-11-05)
#
#   tools/daemon_fast_read.sh
#
# なぜ: 動いている間は get_angles が空で返ることが多く, daemon は 0.2 秒待って 12 回まで読み直す (1 回最長約 2.6 秒).
#       送る命令のたびに記録用にも読むので, 送る命令の返事も遅れ, ゲームパッドが最長 15 秒受け付けなかった.
# 変えること: 読み直しの待ち 0.2 → 0.02 秒. 送ったあとの記録用の読み取りは 1 回だけ (読めなくても待たない).
#       ほかは 1 文字も変えない. AI の実機推論も同じ daemon を通る (送る命令の返りが速くなるだけ).
# 流れ:
#   1. Pi で今動いている /home/ubuntu/mecharm_daemon.py を Mac に持ち帰り, KIOXIA の archive/ に写しを残す
#   2. すでに書き換えてあれば何もしない. なければ書き換え, 構文を確かめる
#   3. Pi へ一時名で送ってから入れ替え, systemd で起動し直し, ping と get_angles が返事をするか確かめる
#      うまくいかなければ, 元に戻すコマンドを出して止まる
# 流す前に: ゲームパッドなど daemon を使うものを止め, アームを HOME など安全な姿勢にしておく
#           (daemon を起動し直すと MyCobot の初期化が走る)
# 書き足した daemon は tools/pi/mecharm_daemon.py にも置く (これからの手元の正. ルートの mecharm_daemon.py は古い版なので触らない)
set -u
PI=ubuntu@192.168.2.1
KEY="$HOME/.ssh/id_rsa_pi"
SSH=(ssh -n -i "$KEY" -o ConnectTimeout=5 "$PI")
UNIT=mecharm-daemon.service
HERE="$(cd "$(dirname "$0")" && pwd)"
ROOT="$(dirname "$HERE")"
DATA_ROOT=/Volumes/KIOXIA/r2r2r-data
[ -d "$DATA_ROOT/archive" ] || { echo "ABORT: $DATA_ROOT/archive がない (KIOXIA がマウントされていない?)"; exit 1; }
STAMP=$(date +%Y%m%d_%H%M%S)
BK="$DATA_ROOT/archive/mecharm_daemon_premerge_fastread_$STAMP"
mkdir "$BK" || exit 1
WORK=$(mktemp -d)

scp -i "$KEY" -q -o ConnectTimeout=5 "$PI:/home/ubuntu/mecharm_daemon.py" "$WORK/before.py" || { echo "ABORT: Pi から持ち帰れない"; exit 1; }
cp "$WORK/before.py" "$BK/mecharm_daemon.py" || { echo "ABORT: 写しを残せない (Pi の daemon は変えていません)"; exit 1; }
echo "今の daemon の写し: $BK/mecharm_daemon.py"

python3 - "$WORK/before.py" "$WORK/after.py" <<'PY'
import re, sys
src = open(sys.argv[1], encoding="utf-8").read()
if "read_angles_once_for_log" in src:
    print("すでに書き換えてあります。書き換えません")
    open(sys.argv[2], "w", encoding="utf-8").write(src)
    sys.exit(0)
out = src
n = len(re.findall(r"^GET_ANGLES_INTERVAL_SEC = 0\.2$", out, flags=re.M))
assert n == 1, "GET_ANGLES_INTERVAL_SEC = 0.2 が 1 つに決まらない"
out = re.sub(r"^GET_ANGLES_INTERVAL_SEC = 0\.2$",
             "GET_ANGLES_INTERVAL_SEC = 0.02   # 2026-11-05: 0.2 → 0.02 (動いている間は空で返ることが多く, 1 回の読み取りに最長 2.6 秒かかっていた)",
             out, flags=re.M)
for label in ("send_angles_after", "send_angle_after"):
    old = f'read_angles_with_retry("{label}", log_fail=False)'
    assert out.count(old) == 1, f"{old} が 1 つに決まらない"
    out = out.replace(old, f'read_angles_once_for_log()')
anchor = "def handle_command(cmd_obj):"
assert out.count(anchor) == 1
helper = '''def read_angles_once_for_log():
    """送ったあとの記録用に 1 回だけ読む (2026-11-05). 読めなくても待たない.
    以前は読み直しつきで, 読めないと送る命令の返事が最長 2.6 秒遅れていた."""
    try:
        a = _mc.get_angles()
    except Exception:
        return None
    return a if isinstance(a, (list, tuple)) and len(a) == 6 else None


'''
out = out.replace(anchor, helper + anchor)
compile(out, "mecharm_daemon.py", "exec")
open(sys.argv[2], "w", encoding="utf-8").write(out)
print("読み取りを速くしました")
PY
[ $? = 0 ] || { echo "ABORT: 書き足せない (Pi の daemon は変えていません)"; exit 1; }
cp "$WORK/after.py" "$BK/mecharm_daemon_fast_read.py" || exit 1
cp "$WORK/after.py" "$HERE/pi/mecharm_daemon.py" || exit 1
RESTORE="scp -i $KEY $BK/mecharm_daemon.py $PI:/home/ubuntu/mecharm_daemon.py && ssh -i $KEY $PI sudo systemctl restart $UNIT"
fail() { echo "ABORT: $1"; echo "元に戻すには: $RESTORE"; exit 1; }
echo "--- 差分"
diff "$WORK/before.py" "$WORK/after.py"

scp -i "$KEY" -q -o ConnectTimeout=5 "$WORK/after.py" "$PI:/home/ubuntu/mecharm_daemon.py.new" || { echo "ABORT: Pi へ送れない (Pi の daemon は変えていません)"; exit 1; }
"${SSH[@]}" "python3 -m py_compile /home/ubuntu/mecharm_daemon.py.new && mv /home/ubuntu/mecharm_daemon.py.new /home/ubuntu/mecharm_daemon.py" \
    || { echo "ABORT: Pi で入れ替えられない (Pi の daemon は変えていません)"; exit 1; }
"${SSH[@]}" "sudo systemctl restart $UNIT" || fail "systemctl restart が失敗した"
UP=no
for i in 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15; do
    sleep 2
    "${SSH[@]}" "ss -ltn | grep -q ':5555 '" && { UP=yes; break; }
done
[ "$UP" = yes ] || fail "30 秒たっても 5555 が開かない"
python3 - <<'PY'
import json, socket
s = socket.create_connection(("192.168.2.1", 5555), timeout=5)
f = s.makefile("rwb")
for c in ({"cmd": "ping"}, {"cmd": "get_angles"}):
    f.write((json.dumps(c) + "\n").encode()); f.flush()
    r = json.loads(f.readline().decode())
    print(c["cmd"], "→", r)
    assert r.get("ok"), c["cmd"] + " が ok を返さない"
s.close()
PY
[ $? = 0 ] || fail "ping か get_angles が ok を返さない"
echo "できました。記録: $BK"