#!/usr/bin/env python3
"""O6 中指 step 控制指令（真机 / 仿真共用，时序完全一致）。

时序（默认）：
  pre_wait=3s  保持张开
  hold=3s      中指握紧 (middle u8=0)
  hold=3s      中指张开 (middle u8=255)

用法（单独发令，配合录制脚本）：
  export ROS_DOMAIN_ID=31
  python3 tools/middle_step_cmd_pub.py
"""

from __future__ import annotations

import argparse
import time

# 与 run_middle_step_experiment.py 共用，改这里两边一起变
DEFAULT_PRE_WAIT = 3.0
DEFAULT_HOLD = 3.0
DEFAULT_RATE_HZ = 10.0
FINGER_ROS_IDX = {"index": 2, "middle": 3, "ring": 4, "pinky": 5}


def hold_finger_pose(pub, hand: str, finger: str, u8: float, duration: float, rate_hz: float):
    from sensor_msgs.msg import JointState

    idx = FINGER_ROS_IDX[finger]
    end = time.perf_counter() + duration
    dt = 1.0 / max(1.0, rate_hz)
    while time.perf_counter() < end:
        msg = JointState()
        msg.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
        msg.position[idx] = float(u8)
        pub.publish(msg)
        time.sleep(dt)


def run_step_sequence(
    node,
    *,
    hand: str = "left",
    finger: str = "middle",
    pre_wait: float = DEFAULT_PRE_WAIT,
    hold: float = DEFAULT_HOLD,
    rate_hz: float = DEFAULT_RATE_HZ,
    cmd_topic: str | None = None,
):
    from sensor_msgs.msg import JointState

    topic = cmd_topic or f"/cb_{hand}_hand_control_cmd"
    pub = node.create_publisher(JointState, topic, 10)
    time.sleep(0.3)

    t0 = time.perf_counter()
    node.get_logger().info(
        f"step cmd | finger={finger} | pre={pre_wait}s hold={hold}s | topic={topic}"
    )

    # 阶段0：保持张开
    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] baseline open (255)")
    hold_finger_pose(pub, hand, finger, 255.0, pre_wait, rate_hz)

    # 阶段1：握紧
    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] CLOSE middle u8=0")
    hold_finger_pose(pub, hand, finger, 0.0, hold, rate_hz)

    # 阶段2：张开
    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] OPEN middle u8=255")
    hold_finger_pose(pub, hand, finger, 255.0, hold, rate_hz)

    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] sequence done")
    return t0


def total_duration(pre_wait: float, hold: float, record_after: float = 0.0) -> float:
    return pre_wait + hold + hold + record_after + 1.0


def main():
    parser = argparse.ArgumentParser(description="O6 middle finger step command publisher")
    parser.add_argument("--hand", default="left", choices=["left", "right"])
    parser.add_argument("--finger", default="middle", choices=list(FINGER_ROS_IDX))
    parser.add_argument("--pre-wait", type=float, default=DEFAULT_PRE_WAIT)
    parser.add_argument("--hold", type=float, default=DEFAULT_HOLD)
    parser.add_argument("--rate", type=float, default=DEFAULT_RATE_HZ)
    parser.add_argument("--cmd-topic", default="")
    args = parser.parse_args()

    import rclpy
    from rclpy.node import Node

    rclpy.init()
    node = Node("middle_step_cmd_pub")
    try:
        run_step_sequence(
            node,
            hand=args.hand,
            finger=args.finger,
            pre_wait=args.pre_wait,
            hold=args.hold,
            rate_hz=args.rate,
            cmd_topic=args.cmd_topic or None,
        )
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()