from robodk import robolink, robomath
import time


def execute_robot_movement_and_stop(robot, stop_after_seconds):
    """ロボットの移動を開始し、指定時間後に停止するシングルスレッド実装"""
    try:
        # 初期位置に移動
        print("初期位置に移動します...")
        print("robot.MoveJ is called")
        robot.MoveJ(robot.JointsHome(), blocking=True)
        print("robot.MoveJ is done")

        # 直線移動の目標位置
        target_pose = robomath.xyzrpw_2_pose(
            [1350.000000, 0.000000, 551.822, 0.000000, -90.000000, -180.000000]
        )

        # 移動開始時刻を記録
        start_time = time.time()
        print("長距離移動を開始します...")

        # 非ブロッキングで移動を開始
        print("robot.MoveL is called")
        robot.MoveL(target_pose, blocking=False)
        print("robot.MoveL is done")

        # 指定時間が経過するまで待機
        while time.time() - start_time < stop_after_seconds:
            if not robot.Busy():
                print("ロボットの動作が既に停止しています")
                return
            time.sleep(0.1)  # CPU負荷軽減のための短い待機

        # 指定時間経過後に停止
        print(f"{stop_after_seconds}秒経過: 停止コマンドを送信...")
        print("robot.Stop is called")
        robot.setParam("Stop")
        print("robot.Stop is done")

        # 完全に停止するまで待機
        while robot.Busy():
            time.sleep(0.1)

        robot.Disconnect()
        print("ロボットが停止しました")

    except Exception as e:
        print(f"実行中にエラーが発生: {e}")
        robot.Stop()


def main():
    try:
        # RoboDK環境の初期化
        rdk = robolink.Robolink()

        # アクティブなロボットを取得
        robot = rdk.ItemUserPick("Select a robot", robolink.ITEM_TYPE_ROBOT)
        if not robot.Valid():
            print("ロボットが選択されていません")
            return

        robot.ConnectSafe("192.168.255.1")

        # 安全のため、シミュレーション速度を50%に設定
        robot.setSpeed(50)

        print("検証プログラムを開始します")
        # 3秒後に停止するようにロボット動作を実行
        execute_robot_movement_and_stop(robot, stop_after_seconds=3)
        print("検証プログラムが終了しました")

    except KeyboardInterrupt:
        print("\nプログラムが中断されました")
        robot.Stop()


if __name__ == "__main__":
    main()