import time
import keyboard

from demo_auto_grasp import TactileSensorDAQ, RobotDriver, SafetyGuard

# ====== 可按实际机构方向微调的参数 ======
SERVO_HOME_ANGLE = 90
SERVO_CW_TO_OPPOSE_ANGLE = 120
SERVO_HOLD_SEC = 1.0
FORCE_STOP_THRESHOLD = 0.3
PRESSURE_DISPLAY_KPA = 80
ROBOT_COM_PORT = 'COM9'


def wait_with_pause(safety, duration_s, step_s=0.02):
    start = time.time()
    while (time.time() - start) < duration_s:
        safety.check_pause()
        time.sleep(step_s)


def run_servo_single_cycle(robot, safety, servo_id):
    print(f"[Mode {servo_id}] 舵机{servo_id}开始: 手指1顺时针到对握位 -> 回位")

    safety.check_pause()
    # 仅发送指定舵机命令，其他舵机不动作
    robot.set_servo(servo_id, SERVO_CW_TO_OPPOSE_ANGLE)

    wait_with_pause(safety, SERVO_HOLD_SEC)

    safety.check_pause()
    robot.set_servo(servo_id, SERVO_HOME_ANGLE)
    wait_with_pause(safety, 0.5)

    # 明确停止直线电机，确保其不动作
    robot.motor_stop()
    print(f"[Mode {servo_id}] 完成。")


def run_linear_mode(robot, safety):
    print("[Mode 3] 直线电机控制模式 (无传感器)")
    print("按键: w=张开, s=闭合, q=退出模式")
    print("规则: 无传感器，按住s持续闭合")
    print("全局: 空格急停/恢复")

    motion_state = 'idle'  # idle/open/close/force_stop
    prev_q = False
    linear_start_time = None

    def print_runtime(final=False):
        if linear_start_time is None:
            return
        elapsed = time.time() - linear_start_time
        if final:
            print(f"[Mode 3] s执行总时长: {elapsed:.2f}s")

    while True:
        safety.check_pause()

        w_now = keyboard.is_pressed('w')
        s_now = keyboard.is_pressed('s')
        q_now = keyboard.is_pressed('q')

        q_edge = q_now and not prev_q

        if q_edge:
            robot.motor_stop()
            print_runtime(final=True)
            linear_start_time = None
            print("[Mode 3] 退出直线电机控制模式")
            break

        # 仅允许单键控制: w 和 s 同时按下时停止，防止方向冲突
        if w_now and not s_now:
            if motion_state != 'open':
                robot.motor_open()
                motion_state = 'open'
                print("[Mode 3] 张开中 (按住 w)")
        elif s_now and not w_now:
            if linear_start_time is None:
                linear_start_time = time.time()
            if motion_state != 'close':
                robot.motor_close()
                motion_state = 'close'
                print("[Mode 3] 闭合中 (按住 s)")
        else:
            if motion_state in ('open', 'close', 'force_stop'):
                robot.motor_stop()
                print_runtime(final=True)
                motion_state = 'idle'
                print("[Mode 3] 松开按键, 电机停止")
                linear_start_time = None

        prev_q = q_now

        time.sleep(0.02)


def main():
    # sensor = TactileSensorDAQ()
    robot = RobotDriver(port=ROBOT_COM_PORT)
    safety = SafetyGuard(robot)

    try:
        # sensor.start()
        # print("等待传感器稳定...")
        # time.sleep(2)
        # sensor.tare()

        while True:
            print("\n========================")
            print("自由度控制选择:")
            print("  1: 舵机1控制 (手指1顺时针到对握后回位)")
            print("  2: 舵机2控制 (手指1顺时针到对握后回位)")
            print(f"  3: 直线电机控制 (w张开/s闭合, 压强显示固定为 {PRESSURE_DISPLAY_KPA}KPa)")
            print("  q: 退出程序")
            print("全局: 空格急停/恢复")

            choice = input("请输入 1/2/3/q: ").strip().lower()

            if choice == '1':
                run_servo_single_cycle(robot, safety, servo_id=1)
            elif choice == '2':
                run_servo_single_cycle(robot, safety, servo_id=2)
            elif choice == '3':
                run_linear_mode(robot, safety)
            elif choice == 'q':
                break
            else:
                print("输入无效，请重新输入。")

    except KeyboardInterrupt:
        pass
    finally:
        robot.motor_stop()
        robot.close()
        # sensor.stop()
        try:
            keyboard.unhook_all()
        except Exception:
            pass
        print("System All Shutdown.")


if __name__ == '__main__':
    main()