import cv2 as cv 
import mediapipe as mp
import serial
import time

# --- SETUP ---
PORT = "com6"  # Ensure this matches your Device Manager
BAUD = 115200

try:
    arduino = serial.Serial(PORT, BAUD, timeout=0.1)
    time.sleep(2) # Vital: Wait for Arduino to reboot after connection
    print("Connection Successful")
except:
    print("Check COM Port / Close Serial Monitor")
    arduino = None

mp_hands = mp.solutions.hands
mp_drawing = mp.solutions.drawing_utils

# Track states to prevent spamming the Arduino
# O = Open, C = Closed
last_state = {'T':'O', 'I':'O', 'M':'O', 'R':'O', 'P':'O'}

cap = cv.VideoCapture(0)

with mp_hands.Hands(min_detection_confidence=0.7, min_tracking_confidence=0.7) as hands:
    while cap.isOpened():
        success, frame = cap.read()
        if not success: break
        
        frame = cv.flip(frame, 1)
        rgb_frame = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
        results = hands.process(rgb_frame)

        if results.multi_hand_landmarks:
            for hand_landmarks in results.multi_hand_landmarks:
                lm = hand_landmarks.landmark
                h, w, _ = frame.shape
                
                # Logic converted to list for easier processing
                # Tip ID vs PIP ID
                fingers = {
                    'I': lm[8].y < lm[6].y,   # Index
                    'M': lm[12].y < lm[10].y, # Middle
                    'R': lm[16].y < lm[14].y, # Ring
                    'P': lm[20].y < lm[18].y  # Pinky
                }
                
                # Thumb logic (Horizontal distance)
                thumb_open = abs(lm[4].x - lm[2].x) > 0.05
                fingers['T'] = thumb_open

                # Send commands ONLY if state changed
                if arduino:
                    for f, is_open in fingers.items():
                        current = 'O' if is_open else 'C'
                        if current != last_state[f]:
                            cmd = f"{f}_{current}\r"
                            arduino.write(cmd.encode())
                            last_state[f] = current
                            print(f"Sent: {cmd.strip()}")

                mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)

        cv.imshow("Bionic Control", frame)
        if cv.waitKey(1) & 0xFF == ord('q'):
            break

cap.release()
cv.destroyAllWindows()
if arduino: arduino.close()