#!/bin/bash
# jog_probe.sh — Mac から, Pi の mechArm で根元 j0 を 4 つの動かし方で動かし, なめらかさと限界を測る (2026-11-03)
#
#   tools/jog_probe.sh     A 1 回の send_angles / B 送り直し (今の長押し) / C 送り直し + fresh_mode / D jog (speed 10・30)
#                          テストごとに Enter で進む。q と Enter で止める
#
# 流れ (tools/servo_check.sh と同じ):
#   0. KIOXIA のマウントを確かめ, diag/jog_probe_{日時}/ を作る
#   1. tools/pi/jog_probe_pi.py を Pi へ送る
#   2. デーモンを止める (シリアルを直接使うため). 止まったこと・5555 とシリアルが空いたことを確かめる
#   3. Pi で jog_probe_pi.py を流す (PYTHONPATH に 2.9.6 の置き場所を入れる)
#   4. 終わったら (途中で止めても) 測るプロセスが残っていれば止め, 結果の JSON を持ち帰り, デーモンを元の起動のしかたで戻す
# 画面の出力は全部 console.txt に残す。

set -u
PI=ubuntu@192.168.2.1
KEY="$HOME/.ssh/id_rsa_pi"
SSH_OPTS=(-i "$KEY" -o ConnectTimeout=5 -o ServerAliveInterval=5 -o ServerAliveCountMax=3)   # 切れたら約 15 秒で返る
SSH=(ssh "${SSH_OPTS[@]}" "$PI")           # 標準入力を使うもの (heredoc)
SSHN=(ssh -n "${SSH_OPTS[@]}" "$PI")       # 端末の入力を読まないもの
UNIT=mecharm-daemon.service
DATA_ROOT="${SERVO_CHECK_DATA_ROOT:-/Volumes/KIOXIA/r2r2r-data}"
PI_SCRIPT=/home/ubuntu/jog_probe_pi.py
PI_PYTHONPATH=/usr/local/lib/python2.7/dist-packages   # 対話シェルの ~/.bashrc と同じ (pymycobot 2.9.6)
DAEMON_UP_WAIT=${SERVO_CHECK_UP_WAIT:-60}   # 戻したデーモンが 5555 を開くまで待つ上限 (秒)
HERE="$(cd "$(dirname "$0")" && pwd)"

case "${1:-}" in
    "") ;;
    *) echo "使い方: $0" >&2; exit 2 ;;
esac

if [ ! -d "$DATA_ROOT/diag" ]; then
    echo "ABORT: $DATA_ROOT/diag がない (KIOXIA がマウントされていない?)" >&2
    exit 1
fi
STAMP=$(date +%Y%m%d_%H%M%S)
OUT="$DATA_ROOT/diag/jog_probe_$STAMP"
if [ -e "$OUT" ]; then
    echo "ABORT: $OUT がすでにある" >&2
    exit 1
fi
mkdir "$OUT" || exit 1
PI_OUT=/tmp/jog_probe_$STAMP.json

# Pi のデーモンの状態を key=value で出す
remote_state() {
    "${SSH[@]}" 'bash -s' <<'REMOTE'
pid=$(pgrep -f '[m]echarm_daemon\.py' | head -n 1)
if [ "$(systemctl is-active mecharm-daemon.service 2>/dev/null)" = active ]; then mode=systemd
elif [ -n "$pid" ]; then mode=manual
else mode=none; fi
echo "mode=$mode"
echo "pid=${pid:-なし}"
if ss -ltn | grep -q ':5555 '; then echo "listen=yes"; else echo "listen=no"; fi
if [ -n "$pid" ]; then
    pp=$(tr '\0' '\n' < /proc/$pid/environ 2>/dev/null | sed -n 's/^PYTHONPATH=//p')
    echo "daemon_pythonpath=${pp:-(なし)}"
fi
# /dev/ttyAMA0 を開いているプロセス (root のものも見るため sudo。読めなければ「不明」)
if sudo -n true 2>/dev/null; then
    u=$(sudo -n find /proc/[0-9]*/fd -lname /dev/ttyAMA0 2>/dev/null | cut -d/ -f3 | sort -u | tr '\n' ' ')
    u=${u% }
    echo "serial_users=${u:-(なし)}"
else
    echo "serial_users=不明"
fi
if pgrep -f '[j]og_probe_pi\.py' > /dev/null; then echo "checker=yes"; else echo "checker=no"; fi
REMOTE
}

state_val() { sed -n "s/^$1=//p" <<<"$2" | head -n 1; }

MODE=""
RESTORED=0
restore() {                 # デーモンを元に戻す (MODE が決まる前に終わったときは何もしない)
    [ "$RESTORED" = 1 ] && return
    RESTORED=1
    [ -n "$MODE" ] || return
    # 点検のプロセスが残っていれば止めてから戻す (シリアルを二重に開かない)
    "${SSHN[@]}" "pkill -INT -f '[j]og_probe_pi\.py'; for i in 1 2 3 4 5; do pgrep -f '[j]og_probe_pi\.py' > /dev/null || exit 0; sleep 1; done; pkill -KILL -f '[j]og_probe_pi\.py'; sleep 1"
    scp -i "$KEY" -q -o ConnectTimeout=5 "$PI:$PI_OUT" "$OUT/result.json" 2>/dev/null &&
        echo "結果の JSON: $OUT/result.json" || echo "結果の JSON は持ち帰れなかった"
    case "$MODE" in
        systemd)
            echo "--- デーモンを戻す (systemctl start)"
            "${SSHN[@]}" "sudo systemctl start $UNIT" ;;
        manual)
            echo "--- デーモンを戻す (対話シェルで手で起動)"
            "${SSHN[@]}" 'bash -ic "nohup python3 /home/ubuntu/mecharm_daemon.py >> /home/ubuntu/daemon.log 2>&1 &"' ;;
        *) return ;;
    esac
    local i st
    for ((i = 0; i < DAEMON_UP_WAIT; i += 5)); do
        st=$(remote_state)
        [ "$(state_val listen "$st")" = yes ] && break
        sleep 5
    done
    echo "$st"
    if [ "$(state_val listen "$st")" = yes ]; then
        echo "デーモンは戻った (5555 LISTEN)"
        case "$(state_val daemon_pythonpath "$st")" in
            *python2.7*) ;;
            *) echo "注意: デーモンの PYTHONPATH に python2.7 がない (pymycobot 4.0.4 を読んでいるかもしれない)" ;;
        esac
    else
        echo "注意: デーモンが 5555 を開いていない。tools/daemon_ctl.sh status で確かめる"
    fi
}

main() {
    echo "jog_probe $STAMP (結果: $OUT)"
    trap 'restore' EXIT
    trap 'exit 130' INT TERM

    scp -i "$KEY" -q -o ConnectTimeout=5 "$HERE/pi/jog_probe_pi.py" "$PI:$PI_SCRIPT" || { echo "ABORT: Pi へ送れなかった"; exit 1; }

    echo "--- デーモンの状態 (止める前)"
    local st
    st=$(remote_state) || { echo "ABORT: Pi につながらない"; exit 1; }
    echo "$st"
    MODE=$(state_val mode "$st")
    case "$MODE" in
        systemd) "${SSHN[@]}" "sudo systemctl stop $UNIT" ;;
        manual) "${SSHN[@]}" "pkill -TERM -f '[m]echarm_daemon\.py'" ;;
        none) echo "デーモンは動いていない (終わったあとも起動しない)" ;;
        *) echo "ABORT: デーモンの状態が読めない"; MODE=""; exit 1 ;;
    esac
    local i
    for ((i = 0; i < 10; i++)); do
        st=$(remote_state)
        [ "$(state_val pid "$st")" = なし ] && [ "$(state_val listen "$st")" = no ] && break
        sleep 1
    done
    echo "--- デーモンの状態 (止めたあと)"
    echo "$st"
    if [ "$(state_val pid "$st")" != なし ]; then
        echo "ABORT: デーモンが止まらない"
        exit 1
    fi
    if [ "$(state_val listen "$st")" != no ]; then
        echo "ABORT: 5555 がまだ開いている"
        exit 1
    fi
    if [ "$(state_val serial_users "$st")" != "(なし)" ]; then
        echo "ABORT: /dev/ttyAMA0 を開いているプロセスがある・確かめられない ($(state_val serial_users "$st"))"
        exit 1
    fi

    ssh -tt "${SSH_OPTS[@]}" "$PI" "PYTHONPATH=$PI_PYTHONPATH python3 -u $PI_SCRIPT --out $PI_OUT"
    local rc=$?
    echo "--- jog_probe_pi.py の終了コード: $rc"
    exit "$rc"
}

main 2>&1 | tee -i "$OUT/console.txt"
exit "${PIPESTATUS[0]}"