#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
interactive_servo_test.py — 基于 Python 的 ESP32 舵机控制交互脚本
依赖：pip install pyserial
用法：
  python3 interactive_servo_test.py /dev/ttyUSB0 [baudrate]
功能：
  1. 控制单个舵机
  2. 控制多个舵机
  3. 退出
  4. 自动增减模式：从 0 到 180° 每步 +6° → -6° 循环，50Hz 频率发送
  5. 设置自动模式舵机数量（通道 1~N）
  6. 眨眼模式：让五个舵机执行预设角度循环，模拟眨眼，8号舵机保持120度
  7. 打哈欠模式：模拟打哈欠动作
  8. 说话模式：模拟嘴巴开合动作
"""
import sys
import time
import serial
import random

FRAME_HEADER = 0xAA  # 帧头

def wakeup_esp32(port, baud=115200):
    """通过 RTS/DTR 线复位并唤醒 ESP32"""
    try:
        ser = serial.Serial(port, baud, timeout=1)
        ser.setDTR(False)
        ser.setRTS(False)
        time.sleep(0.01)
        ser.setDTR(True)
        ser.setRTS(True)
        ser.write(b"\r\n")
        time.sleep(1)
    except Exception as e:
        print(f"ESP32 唤醒失败: {e}")
    finally:
        try:
            ser.close()
        except:
            pass


def open_serial(port, baud):
    """打开并配置串口"""
    try:
        ser = serial.Serial(port, baud, timeout=1)
        print(f"已打开串口: {port} @ {baud}bps")
        return ser
    except Exception as e:
        print(f"打开串口失败: {e}")
        sys.exit(1)


def calc_checksum(data: bytearray) -> int:
    """计算 XOR 校验和"""
    cs = 0
    for b in data:
        cs ^= b
    return cs


def pack_commands(cmds):
    """
    cmds: list of (addr, channel, angle, duration_ms)
    返回：完整帧字节数组
    格式： [FRAME_HEADER][count] [addr, ch, ang, durL, durH]... [checksum]
    """
    pkt = bytearray([FRAME_HEADER, len(cmds)])
    for addr, ch, ang, dur in cmds:
        pkt.extend([addr, ch, ang, dur & 0xFF, (dur >> 8) & 0xFF])
    pkt.append(calc_checksum(pkt))
    return pkt


def print_packet(pkt):
    """打印十六进制数据包"""
    print("发送数据包:", ' '.join(f"0x{b:02X}" for b in pkt))


def input_hex(prompt):
    """输入 0x 开头或十进制整型"""
    val = input(prompt).strip()
    try:
        return int(val, 0)
    except ValueError:
        print(f"无效数字: {val}")
        return input_hex(prompt)


def input_cmd_single():
    """交互输入单条命令"""
    addr = input_hex("输入 addr (hex or dec): ")
    ch   = input_hex("输入 channel (hex or dec): ")
    ang  = input_hex("输入 angle (0-180): ")
    dur  = input_hex("输入 duration(ms): ")
    return [(addr, ch, ang, dur)]


def input_cmd_multiple():
    """交互输入多条命令"""
    cnt = input_hex("输入命令数量: ")
    cmds = []
    for i in range(cnt):
        print(f"第 {i+1} 条:")
        cmds.extend(input_cmd_single())
    return cmds


def auto_mode(ser, channels):
    """自动增减模式：0→180→0，步长6，50Hz，多舵机"""
    addr = input_hex("自动模式: 输入 addr (hex或dec): ")
    interval = 1.0 / 50.0
    dur_ms   = int(interval * 1000)
    angle    = 0
    direction = 1
    step      = 6

    print(f"启动自动模式 @50Hz, addr=0x{addr:02X}, channels={channels}, step={step}°")
    try:
        while True:
            cmds = [(addr, ch, angle, dur_ms) for ch in channels]
            pkt  = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)
            # 更新角度
            angle += direction * step
            if angle >= 180:
                angle = 180
                direction = -1
            elif angle <= 0:
                angle = 0
                direction = 1
            time.sleep(interval)
    except KeyboardInterrupt:
        print("\n自动模式已停止，返回菜单")


def set_channels():
    """自定义舵机数量，返回通道列表 1~N"""
    cnt = input_hex("输入自动模式舵机数量 N: ")
    if cnt < 1:
        print("数量必须 >= 1")
        return None
    channels = list(range(1, cnt+1))
    print(f"已设置通道列表: {channels}")
    return channels


def blink_mode(ser):
    """眨眼模式：根据输入的角度进行指定舵机的眨眼操作"""
    addr = input_hex("眨眼模式: 输入 addr (hex或dec): ")
    blink_count = 10  # 循环10次
    interval = 0.1 # 每秒眨眼一次

    # 定义五个舵机的通道
    servo_channels = [4, 5, 6, 7, 8]  # 控制五个舵机
    blink_angles = [90, 90, 90, 90, 90]   # 眨眼时的角度
    open_angles =  [50, 130, 130, 60, 90]    # 睁开时的角度（4-7号舵机回到90度，8号保持120度）

    print(f"启动眨眼模式，10次眨眼，每次间隔 0.1 秒")

    try:
        for i in range(blink_count):
            print(f"第 {i+1} 次眨眼...")
            
            # 眨眼动作：同步发送所有舵机到眨眼角度
            cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, blink_angles)]
            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)
            time.sleep(interval)
            
            # 睁开动作：同步发送4-7号舵机回到90度，8号保持120度
            print("睁开动作：舵机4-7回到90度")
            cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, open_angles)]
            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)
            time.sleep(interval)

        print("眨眼模式结束，返回菜单")
    except KeyboardInterrupt:
        print("\n眨眼模式已停止，返回菜单")


def yawn_mode(ser):
    """打哈欠模式：控制多个舵机模拟打哈欠动作"""
    addr = input_hex("打哈欠模式: 输入 addr (hex或dec): ")
    yawn_count = 5  # 循环5次
    interval = 1 # 每次打哈欠动作间隔1秒

    # 定义控制的舵机通道
    # 打哈欠时使用舵机1, 3, 4, 5, 6, 7, 8（通道1~8）
    # 闭嘴时使用舵机8、9（65号舵机）
    yawning_channels = [1, 3, 4, 5, 6, 7, 8]  # 控制通道1~8
    yawning_angles = [70, 110, 60, 130, 120, 80, 90]  # 打哈欠时的角度（根据需要调整）
    closing_angles = [90, 90, 90, 90, 90, 90, 90]  # 闭嘴时的角度（闭嘴动作）

    # 65号舵机（8、9号通道）设置不同的角度，模拟控制头部动作
    yawning_65_angles = [120, 45, 60, 120]  # 65号舵机（8、9号通道）在打哈欠时的角度
    closing_65_angles = [120, 45, 90, 90]   # 闭嘴时65号舵机（8、9号通道）的角度

    print(f"启动打哈欠模式，循环 {yawn_count} 次，每次间隔 {interval} 秒")

    try:
        for i in range(yawn_count):
            print(f"第 {i+1} 次打哈欠...")

            # 打哈欠动作：同步发送所有舵机到打哈欠角度
            cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, yawning_angles)]
            cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], yawning_65_angles)]  # 控制65号舵机（8、9号通道）
            cmds.extend(cmds_65)  # 合并65号舵机的指令

            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)  # 一次性发送所有指令
            time.sleep(interval)

            # 闭嘴动作：同步发送舵机回到闭嘴角度
            print("闭嘴动作：所有舵机回到闭嘴角度")
            cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, closing_angles)]
            cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], closing_65_angles)]  # 65号舵机（8、9号通道）
            cmds.extend(cmds_65)  # 合并65号舵机的指令

            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)  # 一次性发送所有指令
            time.sleep(interval)

        print("打哈欠模式结束，返回菜单")
    except KeyboardInterrupt:
        print("\n打哈欠模式已停止，返回菜单")


def talk_mode(ser):
    """说话模式：模拟嘴巴开合动作"""
    addr = input_hex("说话模式: 输入 addr (hex或dec): ")
    talk_count = 50  # 循环5次

    # 定义控制的舵机通道
    mouth_channels = [0, 1, 8, 9]  # 控制3个舵机（口型）
    mouth_open_angles = [120, 45, 80, 110]  # 嘴巴打开时的角度
    mouth_close_angles = [120, 45, 90, 90]  # 嘴巴关闭时的角度（正常闭合）

    print(f"启动说话模式，循环 {talk_count} 次，每次间隔随机生成在0~1秒之间")

    try:
        for i in range(talk_count):
            print(f"第 {i + 1} 次说话...")

            # 开口动作：同步发送所有舵机到嘴巴打开的角度
            cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_open_angles)]
            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)  # 一次性发送所有指令
            
            # 生成一个 0 到 1 之间的随机间隔时间
            interval = random.uniform(0, 0.3)
            print(f"随机间隔时间：{interval:.2f}秒")
            time.sleep(interval)

            # 闭口动作：同步发送所有舵机回到嘴巴关闭的角度
            print("闭口动作：舵机回到闭口角度")
            cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_close_angles)]
            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)  # 一次性发送所有指令

            # 生成一个 0 到 1 之间的随机间隔时间
            interval = random.uniform(0, 0.3)
            print(f"随机间隔时间：{interval:.2f}秒")
            time.sleep(interval)

        print("说话模式结束，返回菜单")
    except KeyboardInterrupt:
        print("\n说话模式已停止，返回菜单")


def main():
    if len(sys.argv) < 2:
        print(f"用法: {sys.argv[0]} /dev/ttyUSB0 [baudrate]")
        sys.exit(1)
    port = sys.argv[1]
    baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200

    print("唤醒 ESP32...")
    wakeup_esp32(port, baud)
    ser = open_serial(port, baud)
    channels = list(range(1, 10))  # 默认通道 1-9

    try:
        while True:
            print("\n=== 操作菜单 ===")
            print("1. 控制单个舵机")
            print("2. 控制多个舵机")
            print("3. 退出")
            print("4. 自动增减模式(50Hz)")
            print("5. 设置自动模式舵机数量")
            print("6. 眨眼模式")
            print("7. 打哈欠模式")
            print("8. 说话模式")  # 新增选项
            opt = input("选择: ").strip()
            if opt == '1':
                cmds = input_cmd_single()
            elif opt == '2':
                cmds = input_cmd_multiple()
            elif opt == '4':
                auto_mode(ser, channels)
                continue
            elif opt == '5':
                new_ch = set_channels()
                if new_ch:
                    channels = new_ch
                continue
            elif opt == '6':
                blink_mode(ser)
                continue
            elif opt == '7':  # 调用打哈欠模式
                yawn_mode(ser)
                continue
            elif opt == '8':  # 调用说话模式
                talk_mode(ser)
                continue
            elif opt == '3':
                break
            else:
                print("无效选项，重试")
                continue

            pkt = pack_commands(cmds)
            print_packet(pkt)
            ser.write(pkt)
            print("✅ 指令已发送")
    except KeyboardInterrupt:
        print("\n用户中断，退出")
    finally:
        ser.close()
        print("串口已关闭")


if __name__ == '__main__':
    main()