import cv2
import mediapipe as mp
from pymodbus.client.sync import ModbusTcpClient

# 初始化 Modbus TCP 客戶端
client = ModbusTcpClient('169.254.11.8', port=502)
client.unit_id = 255  # 設定 Modbus Slave ID（通常是 1 或 255）

# 嘗試連線 PLC
if not client.connect():
    print("❌ 無法連接 PLC，請確認 IP 與 Slave 模式是否開啟")
    exit()
print("✅ 已連接 PLC")

# 初始化 MediaPipe 手部辨識
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(max_num_hands=1)
mp_draw = mp.solutions.drawing_utils

# 開啟攝影機
cap = cv2.VideoCapture(0)

def count_fingers(hand_landmarks):
    fingers = []

    # 拇指（以右手為例）
    if hand_landmarks.landmark[4].x < hand_landmarks.landmark[3].x:
        fingers.append(1)
    else:
        fingers.append(0)

    # 其他手指
    for tip_id in [8, 12, 16, 20]:
        if hand_landmarks.landmark[tip_id].y < hand_landmarks.landmark[tip_id - 2].y:
            fingers.append(1)
        else:
            fingers.append(0)

    return fingers

# 各手勢定義
def is_thumb_up(fingers): return fingers == [1, 0, 0, 0, 0]
def is_fist(fingers): return fingers == [1, 1, 1, 1, 1]
def is_speed3_custom(fingers): return fingers == [0, 1, 1, 1, 0]  # 食+中+無名指

while True:
    success, frame = cap.read()
    if not success:
        break

    frame = cv2.flip(frame, 1)  # 左右反轉
    rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    result = hands.process(rgb_frame)

    if result.multi_hand_landmarks:
        for hand_landmarks in result.multi_hand_landmarks:
            mp_draw.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)

            fingers = count_fingers(hand_landmarks)

            if is_thumb_up(fingers):
                print("👍 啟動")
                client.write_register(address=1632, value=1, slave=255)  # 啟動信號 (motion_go)

            elif is_fist(fingers):
                print("✊ 停止")
                client.write_register(address=1632, value=0, slave=255)  # 停止速度 (py_Value)

            elif is_speed3_custom(fingers):
                print("🤟 速度 = 3")
                client.write_register(address=1631, value=3, slave=255)

            else:
                speed = sum(fingers)
                if 1 <= speed <= 4:
                    print(f"🤚 速度 = {speed}")
                    client.write_register(address=1631, value=speed, slave=255)

    cv2.imshow("Hand Detection", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()
client.close()