import socket
import struct
import time
import numpy as np

class HandUdpSender:
    MAGIC = b'HAND'
    VERSION = 2

    def __init__(self, host="127.0.0.1", port=9000):
        self.addr = (host, port)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.seq = 0

    def _encode_handedness(self, s: str) -> int:
        if s == "Left":
            return 1
        if s == "Right":
            return 2
        return 0

    def send_hands3d(self, hands3d: list[dict]):
        """
        hands3d: list of dicts:
        {
            "landmarks": (21,3) np.ndarray float,
            "handedness": "Left"/"Right"/"Unknown",
            "confidence": float (0..1)
        }
        Sends one UDP datagram containing all hands.
        """
        self.seq += 1
        ts_ms = int(time.time() * 1000)

        hand_count = min(len(hands3d), 255)

        # Header: magic(4s), version(B), seq(I), ts(Q), hand_count(B)
        packet = bytearray()
        packet += struct.pack("<4sBIQB", self.MAGIC, self.VERSION, self.seq, ts_ms, hand_count)

        for hid in range(hand_count):
            h = hands3d[hid]
            pts = np.asarray(h["landmarks"], dtype=np.float32)
            if pts.shape != (21, 3):
                continue

            point_count = 21
            handedness_code = self._encode_handedness(str(h.get("handedness", "Unknown")))
            confidence = float(h.get("confidence", 0.0))

            # per-hand header: hand_id(B), point_count(B), handedness(B), confidence(f)
            packet += struct.pack("<BBBf", hid, point_count, handedness_code, confidence)

            # points: 21*3 float32
            packet += pts.tobytes(order="C")

        self.sock.sendto(packet, self.addr)