"""Your body, tracked by the webcam: MediaPipe Hands turns a bare hand in the air
into a five-dimensional control gesture (position, depth, openness, velocity).
It's musical because the hand is the oldest instrument — pitch bends, filter sweeps,
and swells all fall naturally out of where your hand is and how fast it moves.
"""

import argparse
import math
import sys
import time


def main():
    parser = argparse.ArgumentParser(description="Godstrument vision worker (webcam hand tracking).")
    parser.add_argument("--host", default="127.0.0.1")
    parser.add_argument("--port", type=int, default=9000)
    parser.add_argument("--camera", type=int, default=0, help="webcam index")
    args = parser.parse_args()

    # Lazy import of optional hardware/CV libs. MediaPipe often lacks a wheel for
    # bleeding-edge Python; fail gracefully so the rest of the rig runs.
    try:
        import cv2
        import mediapipe as mp
    except Exception:
        print("vision worker: mediapipe/opencv not available — skipping (optional)")
        sys.exit(0)

    from pythonosc.udp_client import SimpleUDPClient

    client = SimpleUDPClient(args.host, args.port)

    print("[vision] emitting /gs/hand/... every ~0.033s")

    mp_hands = mp.solutions.hands

    # MediaPipe Hands landmark indices we use.
    WRIST = 0
    INDEX_TIP = 8
    MIDDLE_MCP = 9  # middle-finger base ~ palm center reference

    def emit(addr, value):
        # Never let a transient socket hiccup kill the loop.
        try:
            client.send_message(addr, float(value))
        except Exception as exc:
            print(f"vision worker: OSC send failed ({exc}); continuing")

    # Reconnect-forever outer loop: camera unplug / driver hiccup / MediaPipe crash
    # all bounce back here with backoff instead of exiting.
    backoff = 1.0
    while True:
        cap = None
        try:
            cap = cv2.VideoCapture(args.camera)
            if not cap or not cap.isOpened():
                print(f"vision worker: cannot open camera {args.camera}; retrying in {backoff:.0f}s")
                time.sleep(backoff)
                backoff = min(backoff * 2, 30.0)
                continue

            backoff = 1.0  # camera is up
            prev_x = None
            prev_y = None

            with mp_hands.Hands(
                model_complexity=0,
                max_num_hands=1,
                min_detection_confidence=0.5,
                min_tracking_confidence=0.5,
            ) as hands:
                while True:
                    ok, frame = cap.read()
                    if not ok or frame is None:
                        print("vision worker: dropped frame / camera lost; reopening")
                        break  # bounce to outer loop to reopen camera

                    # MediaPipe wants RGB.
                    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
                    rgb.flags.writeable = False
                    result = hands.process(rgb)

                    if result.multi_hand_landmarks:
                        lm = result.multi_hand_landmarks[0].landmark

                        wrist = lm[WRIST]
                        tip = lm[INDEX_TIP]
                        palm = lm[MIDDLE_MCP]

                        # Position: normalized landmark coords are already 0..1.
                        x = min(max(tip.x, 0.0), 1.0)
                        y = min(max(tip.y, 0.0), 1.0)

                        # Depth: MediaPipe z is roughly relative to the wrist,
                        # negative = closer to camera. Map to a friendly 0..1.
                        z_raw = tip.z
                        z = min(max((z_raw + 0.15) / 0.30, 0.0), 1.0)

                        # Openness: distance from index tip to wrist, normalized by
                        # hand span (wrist->palm base) so it's scale-invariant.
                        span = math.dist((wrist.x, wrist.y), (palm.x, palm.y)) or 1e-6
                        reach = math.dist((tip.x, tip.y), (wrist.x, wrist.y))
                        openness = min(max(reach / (span * 2.5), 0.0), 1.0)

                        # Velocity: frame-to-frame motion magnitude of the tip,
                        # in normalized units, clamped to 0..1.
                        if prev_x is None:
                            vel = 0.0
                        else:
                            vel = math.dist((x, y), (prev_x, prev_y))
                            vel = min(vel * 5.0, 1.0)
                        prev_x, prev_y = x, y

                        emit("/gs/hand/x", x)
                        emit("/gs/hand/y", y)
                        emit("/gs/hand/z", z)
                        emit("/gs/hand/open", openness)
                        emit("/gs/hand/vel", vel)
                    else:
                        # No hand: reset velocity reference so re-entry isn't a jump.
                        prev_x = None
                        prev_y = None

                    # ~30fps pacing.
                    time.sleep(0.033)

        except KeyboardInterrupt:
            print("vision worker: shutting down")
            break
        except Exception as exc:
            print(f"vision worker: pipeline error ({exc}); retrying in {backoff:.0f}s")
            time.sleep(backoff)
            backoff = min(backoff * 2, 30.0)
        finally:
            try:
                if cap is not None:
                    cap.release()
            except Exception:
                pass


if __name__ == "__main__":
    main()