import warnings
warnings.filterwarnings("ignore", category=UserWarning)

import cv2
import numpy as np
import joblib
import time
import socket
import json

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
BRIDGE_ADDR = ("127.0.0.1", 5005)
from collections import deque
from picamera2 import Picamera2
import mediapipe.python.solutions.hands as mp_hands

model = joblib.load("gesture_model.joblib")

hands = mp_hands.Hands(max_num_hands=1, model_complexity=0,
    min_detection_confidence=0.6, min_tracking_confidence=0.5)

def normalize(lm):
    pts = np.array([[p.x, p.y, p.z] for p in lm])
    pts = pts - pts[0]
    scale = np.linalg.norm(pts[9])
    pts = pts / (scale + 1e-6)
    return pts.flatten()

COMMANDS = {
    "FIST": "SIT",
    "PEACE": "STAND",
    "OPEN_PALM": "TURN_90_LEFT",
    "POINTING": "FORWARD_5M",
    "THREE_FINGERS": "STRAFE_LEFT",
    "FOUR_FINGERS": "STRAFE_RIGHT",
}

CONF_THRESHOLD = 0.6
HOLD_FRAMES = 15
prob_window = deque(maxlen=7)
last_gesture = None
stable_count = 0
fired = None
last_status = 0.0

picam2 = Picamera2()
picam2.configure(picam2.create_preview_configuration(
    main={"format": "RGB888", "size": (640, 480)}))
picam2.start()

print("Headless gesture recognition running. Ctrl+C to quit.")

try:
    while True:
        frame = picam2.capture_array()
        sock.sendto(json.dumps({"cmd": "HEARTBEAT", "t": time.time()}).encode(), BRIDGE_ADDR)
        results = hands.process(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))

        gesture = "no hand"
        conf = 0.0
        if results.multi_hand_landmarks:
            h = results.multi_hand_landmarks[0]
            feat = normalize(h.landmark).reshape(1, -1)
            probs = model.predict_proba(feat)[0]
            prob_window.append(probs)
            avg = np.mean(prob_window, axis=0)
            i = int(np.argmax(avg))
            conf = float(avg[i])
            gesture = model.classes_[i] if conf >= CONF_THRESHOLD else "UNSURE"
        else:
            prob_window.clear()

        if gesture == last_gesture:
            stable_count += 1
        else:
            stable_count = 0
            last_gesture = gesture
            fired = None

        if stable_count == HOLD_FRAMES and gesture in COMMANDS and fired != gesture:
            cmd = COMMANDS[gesture]
            msg = json.dumps({"cmd": cmd, "conf": round(conf, 3), "t": time.time()})
            sock.sendto(msg.encode(), BRIDGE_ADDR)
            print(f"SENT: {cmd} (conf={conf:.2f})")
            fired = gesture

        # status line every 2 s so you can see it thinking
        if time.time() - last_status > 2.0:
            print(f"  [{time.strftime('%H:%M:%S')}] seeing: {gesture} ({conf:.2f})")
            last_status = time.time()

except KeyboardInterrupt:
    pass

picam2.stop()
print("Stopped.")