#!/usr/bin/env python3
"""
mecharm_daemon.py
Pi 側で常駐する MyCobot 制御デーモン。
TCP socket で Mac からの JSON コマンドを受け取り、永続化された MyCobot 経由で実機に転送する。
"""

import json
import socket
import sys
import time
import signal
import traceback
from pymycobot.mycobot import MyCobot

DAEMON_HOST = "0.0.0.0"
DAEMON_PORT = 5555
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000
GET_ANGLES_RETRIES = 12
GET_ANGLES_INTERVAL_SEC = 0.02   # 2026-11-05: 0.2 → 0.02 (動いている間は空で返ることが多く, 1 回の読み取りに最長 2.6 秒かかっていた)

_mc = None
_server_sock = None
_running = True
_fresh_mode_state = 0


def log(msg):
    print(f"[DAEMON {time.strftime('%H:%M:%S')}] {msg}", flush=True)


def init_mycobot():
    global _mc
    log(f"MyCobot 初期化中 ({SERIAL_PORT}, baud={BAUDRATE})...")
    _mc = MyCobot(SERIAL_PORT, BAUDRATE)
    log("✓ MyCobot 初期化完了")
    warmup_angles = read_angles_with_retry("startup_warmup", log_fail=False)
    log(f"startup_warmup angles={warmup_angles}")


def read_angles_with_retry(label="get_angles", log_fail=True):
    """pymycobot の空配列対策つき角度取得。"""
    angles = None
    for attempt in range(GET_ANGLES_RETRIES):
        angles = _mc.get_angles()
        if isinstance(angles, (list, tuple)) and len(angles) == 6:
            if attempt > 0:
                log(f"{label}: get_angles success retry={attempt} angles={angles}")
            return angles
        time.sleep(GET_ANGLES_INTERVAL_SEC)
    if log_fail:
        log(f"{label}: get_angles empty/invalid after {GET_ANGLES_RETRIES} retries: {angles}")
    return None


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


def handle_command(cmd_obj):
    """コマンドを処理してレスポンス dict を返す。"""
    global _fresh_mode_state
    cmd = cmd_obj.get("cmd")

    if cmd == "ping":
        return {"ok": True, "daemon": "mecharm_daemon", "fresh_mode": _fresh_mode_state}

    elif cmd == "send_angles":
        try:
            angles = cmd_obj["angles"]
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))
            if len(angles) != 6:
                return {"ok": False, "error": f"angles must be length 6, got {len(angles)}"}

            log(f"send_angles 呼び出し前: angles={angles}, speed={speed}, wait={wait}")
            t0 = time.perf_counter()
            result = _mc.send_angles(angles, speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            log(f"send_angles 呼び出し後: result={result}, elapsed_ms={elapsed_ms:.1f}")

            if wait > 0:
                time.sleep(wait)
            current_angles = read_angles_once_for_log()
            log(f"send_angles 後の現在角度: {current_angles}")

            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angles 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "send_angle":
        try:
            joint_id = int(cmd_obj["joint_id"])
            angle = float(cmd_obj["angle"])
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))
            if not (1 <= joint_id <= 6):
                return {"ok": False, "error": f"joint_id must be 1..6, got {joint_id}"}

            log(f"send_angle 呼び出し前: joint_id={joint_id}, angle={angle}, speed={speed}, wait={wait}")
            t0 = time.perf_counter()
            result = _mc.send_angle(joint_id, angle, speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            log(f"send_angle 呼び出し後: result={result}, elapsed_ms={elapsed_ms:.1f}")

            if wait > 0:
                time.sleep(wait)
            current_angles = read_angles_once_for_log()
            log(f"send_angle 後の現在角度: {current_angles}")

            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angle 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "get_angles":
        # DAY190: mc.get_angles() は起動直後や通信直後に [] / None を返すことがある。
        a = read_angles_with_retry("get_angles")
        if a is None:
            return {
                "ok": False,
                "error": f"get_angles returned empty/invalid after {GET_ANGLES_RETRIES} retries",
            }
        return {"ok": True, "angles": a}

    elif cmd == "set_fresh_mode":
        value = int(cmd_obj["value"])
        if value not in (0, 1):
            return {"ok": False, "error": f"value must be 0 or 1, got {value}"}
        try:
            _mc.set_fresh_mode(value)
        except AttributeError:
            return {"ok": False, "error": "set_fresh_mode is not available in this pymycobot environment"}
        _fresh_mode_state = value
        log(f"set_fresh_mode({value})")
        return {"ok": True, "fresh_mode": _fresh_mode_state}

    elif cmd == "set_gripper":
        state = int(cmd_obj["state"])
        speed = int(cmd_obj.get("speed", 100))
        wait = float(cmd_obj.get("wait", 0))
        # DAY175 対策: 連続送信時の動作不具合回避のため事前 sleep
        time.sleep(2)
        result = _mc.set_gripper_state(state, speed)
        if wait > 0:
            time.sleep(wait)
        return {"ok": True, "result": result}

    # DAY261: 値ベース把持 cmd (純追記).
    # state ベースの set_gripper とは独立。閉じ切らない目標値で握って押し込み力を
    # 残す検証用。値域ガード必須 (value 0-100 / speed 1-100)。範囲外は
    # ハードに送らず error を返す。応答フォーマットは set_gripper と同形 {ok, result}。
    elif cmd == "set_gripper_value":
        try:
            value = int(cmd_obj["value"])
            speed = int(cmd_obj.get("speed", 100))
        except (KeyError, TypeError, ValueError) as e:
            return {"ok": False, "error": f"bad args: {type(e).__name__}: {e}"}
        if not (0 <= value <= 100):
            return {"ok": False, "error": f"value must be 0..100, got {value}"}
        if not (1 <= speed <= 100):
            return {"ok": False, "error": f"speed must be 1..100, got {speed}"}
        try:
            result = _mc.set_gripper_value(value, speed)
        except AttributeError:
            return {"ok": False,
                    "error": "set_gripper_value is not available in this pymycobot environment"}
        except Exception as e:
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}
        log(f"set_gripper_value(value={value}, speed={speed}) result={result}")
        return {"ok": True, "result": result}

    # DAY259 グリッパー保持診断 (A-2)。読み取り専用の getter のみ追加。
    # pymycobot の純 getter (シリアル read のみ・モーター駆動しない) を想定。
    # 例外はクライアントへ転送するだけで、daemon は落とさない。
    elif cmd == "get_gripper_value":
        try:
            val = _mc.get_gripper_value()
            return {"ok": True, "value": int(val) if val is not None else None}
        except Exception as e:
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "is_gripper_moving":
        try:
            flag = _mc.is_gripper_moving()
            return {"ok": True, "moving": bool(flag) if flag is not None else None}
        except Exception as e:
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    # ---- LOG352 (2026-10-19): ストリーミング再生用の 2 コマンド (純追記) --------
    # 既存 send_angles / get_angles は 1 文字も変えない (本線 run_pick_place.py が依存)。
    #
    # 背景: 既存 send_angles は送信後に read_angles_with_retry() を呼ぶ。
    # 動作中の _mc.get_angles() は空リストを返しやすく、12 回 x (読み ~0.11s +
    # sleep 0.2s) = 約 3.7 秒ブロックする。0.1 秒周期で送り続ける補間ストリーミング
    # 再生 (record_teach_demo.py) では、474 送信中 27 回がこの 3.7 秒を踏み、
    # tick 周期が平均 0.46-0.55s まで伸びた (公称 0.1s / 実測 2026-10-19)。
    # _mc.send_angles() 自体は平均 10.4ms なので、遅いのは読み直しだけ。
    # Pi 直の waypoint_streaming_replay.py は送信後に角度を読まない。それと同じ
    # 送り方を daemon 経由でも使えるようにする。

    elif cmd == "send_angles_fast":
        # 送信後に角度を読み直さない send_angles。
        # 応答は _mc.send_angles() の実行時間だけ (実測 ~10ms)。
        # 到達確認は呼び出し側が区間の終わりで既存 get_angles を使って行う。
        try:
            angles = cmd_obj["angles"]
            speed = int(cmd_obj.get("speed", 20))
            if not isinstance(angles, (list, tuple)) or len(angles) != 6:
                return {"ok": False,
                        "error": f"angles must be length 6, got {len(angles) if isinstance(angles, (list, tuple)) else type(angles).__name__}"}
            t0 = time.perf_counter()
            result = _mc.send_angles(list(angles), speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            # 1 送信 1 行だけログに残す (今回の原因特定は daemon ログで出来た)。
            log(f"send_angles_fast: angles={angles}, speed={speed}, "
                f"result={result}, elapsed_ms={elapsed_ms:.1f}")
            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angles_fast 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "get_angles_once":
        # 1 回だけ読んで、読めなければ angles=None を返す (リトライしない)。
        # 記録用の観測に使う。読めないことは異常ではないので ok=True で返す
        # (ok=False にすると呼び出し側の _daemon_request が例外にしてしまう)。
        # 既存 get_angles (12 回リトライ) は到達確認用にそのまま残す。
        try:
            a = _mc.get_angles()
        except Exception as e:
            return {"ok": True, "angles": None,
                    "error": f"{type(e).__name__}: {e}"}
        if isinstance(a, (list, tuple)) and len(a) == 6:
            return {"ok": True, "angles": list(a)}
        return {"ok": True, "angles": None, "raw": repr(a)[:80]}

    # 2026-11-05: 動いている途中で止める (ゲームパッドの長押しを離したとき・L2+R2).
    # jog_probe (diag/jog_probe_20261105_*) で jog_stop と stop はどちらも効かない回があったので両方呼ぶ.
    elif cmd == "stop":
        res = {}
        for name in ("jog_stop", "stop"):
            try:
                res[name] = getattr(_mc, name)()
            except Exception as e:
                res[name] = f"{type(e).__name__}: {e}"
        log(f"stop: {res}")
        if all(isinstance(v, str) for v in res.values()):
            return {"ok": False, "error": f"stop failed: {res}"}
        return {"ok": True, "result": res}

    else:
        return {"ok": False, "error": f"unknown cmd: {cmd}"}


def serve_client(client_sock, addr):
    log(f"接続: {addr}")
    buf = b""
    try:
        while _running:
            data = client_sock.recv(4096)
            if not data:
                break
            buf += data
            while b"\n" in buf:
                line, buf = buf.split(b"\n", 1)
                line = line.strip()
                if not line:
                    continue
                try:
                    cmd_obj = json.loads(line.decode("utf-8"))
                    log(f"cmd received: {cmd_obj.get('cmd')}")
                    response = handle_command(cmd_obj)
                except Exception as e:
                    response = {"ok": False, "error": f"{type(e).__name__}: {e}"}
                    log(f"コマンド処理エラー: {e}")
                resp_line = (json.dumps(response) + "\n").encode("utf-8")
                client_sock.sendall(resp_line)
    except ConnectionResetError:
        log(f"接続リセット: {addr}")
    except Exception as e:
        log(f"クライアントエラー: {e}")
    finally:
        client_sock.close()
        log(f"切断: {addr}")


def shutdown_handler(signum, frame):
    global _running
    log(f"シグナル受信 ({signum}), シャットダウン開始")
    _running = False
    if _mc is not None:
        try:
            _mc.set_fresh_mode(0)
            log("set_fresh_mode(0)")
        except Exception:
            pass
    if _server_sock:
        try:
            _server_sock.close()
        except Exception:
            pass
    sys.exit(0)


def main():
    global _server_sock

    signal.signal(signal.SIGTERM, shutdown_handler)
    signal.signal(signal.SIGINT, shutdown_handler)

    init_mycobot()

    _server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    _server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    _server_sock.bind((DAEMON_HOST, DAEMON_PORT))
    _server_sock.listen(1)
    log(f"ポート {DAEMON_PORT} で待ち受け中...")

    try:
        while _running:
            try:
                client_sock, addr = _server_sock.accept()
            except OSError:
                break
            serve_client(client_sock, addr)
    finally:
        if _mc is not None:
            try:
                _mc.set_fresh_mode(0)
                log("set_fresh_mode(0)")
            except Exception:
                pass
        _server_sock.close()
        log("シャットダウン完了")


if __name__ == "__main__":
    main()