"""
_hand_gestures.py — Robot-command gesture classification from MediaPipe
hand landmarks.  No ROS2 / MediaPipe / OpenCV dependencies.

Gesture vocabulary
------------------
  "stop"      — open palm (4+ fingers extended)        → pause/stop robot
  "point"     — index extended, others curled           → direction command
  "disarm"    — fist (all fingers + thumb curled)       → disarm/emergency-off
  "confirm"   — thumbs-up                               → confirm action
  "follow_me" — victory/peace sign (index+middle up)   → follow mode
  "greeting"  — lateral wrist oscillation (wave)        → greeting response
  "none"      — no recognised gesture

Coordinate convention
---------------------
MediaPipe Hands landmark coordinates are image-normalised:
  x: 0.0 = left edge, 1.0 = right edge
  y: 0.0 = top edge,  1.0 = bottom edge  (y increases downward)
  z: depth relative to wrist; negative = toward camera

Landmark indices (MediaPipe Hands topology)
-------------------------------------------
  0  WRIST
  1  THUMB_CMC   2  THUMB_MCP   3  THUMB_IP    4  THUMB_TIP
  5  INDEX_MCP   6  INDEX_PIP   7  INDEX_DIP   8  INDEX_TIP
  9  MIDDLE_MCP 10  MIDDLE_PIP 11  MIDDLE_DIP  12 MIDDLE_TIP
 13  RING_MCP   14  RING_PIP   15  RING_DIP    16 RING_TIP
 17  PINKY_MCP  18  PINKY_PIP  19  PINKY_DIP   20 PINKY_TIP

Public API
----------
  Landmark          dataclass(x, y, z)
  HandGestureResult NamedTuple
  WaveDetector      sliding-window wrist-oscillation detector
  classify_hand(landmarks, is_right, wave_det) → HandGestureResult
"""

from __future__ import annotations

import math
from collections import deque
from dataclasses import dataclass
from typing import Deque, List, NamedTuple, Optional, Tuple


# ── Landmark type ──────────────────────────────────────────────────────────────

@dataclass(frozen=True)
class Landmark:
    x: float
    y: float
    z: float = 0.0


# ── Result type ────────────────────────────────────────────────────────────────

class HandGestureResult(NamedTuple):
    gesture:    str    # "stop"|"point"|"disarm"|"confirm"|"follow_me"|"greeting"|"none"
    confidence: float  # 0.0–1.0
    direction:  str    # non-empty only when gesture == "point"
    wrist_x:    float  # normalised wrist position (image coords)
    wrist_y:    float


# ── Landmark index constants ───────────────────────────────────────────────────

_WRIST = 0
_THUMB_CMC = 1; _THUMB_MCP = 2; _THUMB_IP = 3;  _THUMB_TIP = 4
_INDEX_MCP = 5; _INDEX_PIP = 6; _INDEX_DIP = 7;  _INDEX_TIP = 8
_MIDDLE_MCP = 9; _MIDDLE_PIP = 10; _MIDDLE_DIP = 11; _MIDDLE_TIP = 12
_RING_MCP = 13; _RING_PIP = 14; _RING_DIP = 15;  _RING_TIP = 16
_PINKY_MCP = 17; _PINKY_PIP = 18; _PINKY_DIP = 19; _PINKY_TIP = 20

_NONE = HandGestureResult("none", 0.0, "", 0.0, 0.0)


# ── Low-level geometry helpers ─────────────────────────────────────────────────

def _finger_up(lm: List[Landmark], tip: int, pip: int) -> bool:
    """True if the finger tip is above (smaller y) its PIP joint."""
    return lm[tip].y < lm[pip].y


def _finger_ext_score(lm: List[Landmark], tip: int, pip: int, mcp: int) -> float:
    """Extension score in [0, 1]: how far the tip is above the MCP knuckle."""
    spread = lm[mcp].y - lm[tip].y          # positive = tip above mcp
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    return max(0.0, min(1.0, spread / palm_h))


def _count_fingers_up(lm: List[Landmark]) -> int:
    """Count how many of index/middle/ring/pinky are extended."""
    pairs = [
        (_INDEX_TIP, _INDEX_PIP), (_MIDDLE_TIP, _MIDDLE_PIP),
        (_RING_TIP, _RING_PIP),   (_PINKY_TIP, _PINKY_PIP),
    ]
    return sum(_finger_up(lm, t, p) for t, p in pairs)


def _four_fingers_curled(lm: List[Landmark]) -> bool:
    """True when index, middle, ring, and pinky are all curled (not extended)."""
    pairs = [
        (_INDEX_TIP, _INDEX_PIP), (_MIDDLE_TIP, _MIDDLE_PIP),
        (_RING_TIP, _RING_PIP),   (_PINKY_TIP, _PINKY_PIP),
    ]
    return not any(_finger_up(lm, t, p) for t, p in pairs)


def _thumb_curled(lm: List[Landmark]) -> bool:
    """True when the thumb tip is below (same or lower y than) the thumb MCP."""
    return lm[_THUMB_TIP].y >= lm[_THUMB_MCP].y


def _thumb_extended_up(lm: List[Landmark]) -> bool:
    """True when the thumb tip is clearly above the thumb CMC base."""
    return lm[_THUMB_TIP].y < lm[_THUMB_CMC].y - 0.02


def _palm_center(lm: List[Landmark]) -> Tuple[float, float]:
    """(x, y) centroid of the four MCP knuckles."""
    xs = [lm[i].x for i in (_INDEX_MCP, _MIDDLE_MCP, _RING_MCP, _PINKY_MCP)]
    ys = [lm[i].y for i in (_INDEX_MCP, _MIDDLE_MCP, _RING_MCP, _PINKY_MCP)]
    return sum(xs) / 4, sum(ys) / 4


# ── Pointing direction ─────────────────────────────────────────────────────────

def _point_direction(lm: List[Landmark]) -> str:
    """8-compass pointing direction from index MCP → TIP vector."""
    dx = lm[_INDEX_TIP].x - lm[_INDEX_MCP].x
    dy = lm[_INDEX_TIP].y - lm[_INDEX_MCP].y   # +y = downward in image
    angle = math.degrees(math.atan2(-dy, dx))   # flip y so up = +90°
    if   -22.5  <= angle <  22.5:  return "right"
    elif  22.5  <= angle <  67.5:  return "upper_right"
    elif  67.5  <= angle < 112.5:  return "up"
    elif 112.5  <= angle < 157.5:  return "upper_left"
    elif  angle >= 157.5 or angle < -157.5:  return "left"
    elif -157.5 <= angle < -112.5: return "lower_left"
    elif -112.5 <= angle < -67.5:  return "down"
    else:                          return "lower_right"


# ── Static gesture classifiers ─────────────────────────────────────────────────

def _classify_stop(lm: List[Landmark]) -> Optional[HandGestureResult]:
    """Open palm: 4 or more fingers extended upward."""
    n = _count_fingers_up(lm)
    if n < 4:
        return None
    scores = [
        _finger_ext_score(lm, _INDEX_TIP,  _INDEX_PIP,  _INDEX_MCP),
        _finger_ext_score(lm, _MIDDLE_TIP, _MIDDLE_PIP, _MIDDLE_MCP),
        _finger_ext_score(lm, _RING_TIP,   _RING_PIP,   _RING_MCP),
        _finger_ext_score(lm, _PINKY_TIP,  _PINKY_PIP,  _PINKY_MCP),
    ]
    conf = round(0.60 + 0.35 * (sum(scores) / len(scores)), 3)
    return HandGestureResult("stop", conf, "", lm[_WRIST].x, lm[_WRIST].y)


def _classify_point(lm: List[Landmark]) -> Optional[HandGestureResult]:
    """Index extended upward; middle/ring/pinky curled."""
    if not _finger_up(lm, _INDEX_TIP, _INDEX_PIP):
        return None
    others_up = sum([
        _finger_up(lm, _MIDDLE_TIP, _MIDDLE_PIP),
        _finger_up(lm, _RING_TIP,   _RING_PIP),
        _finger_up(lm, _PINKY_TIP,  _PINKY_PIP),
    ])
    if others_up >= 1:
        return None
    ext = _finger_ext_score(lm, _INDEX_TIP, _INDEX_PIP, _INDEX_MCP)
    conf = round(0.65 + 0.30 * ext, 3)
    direction = _point_direction(lm)
    return HandGestureResult("point", conf, direction, lm[_WRIST].x, lm[_WRIST].y)


def _classify_disarm(lm: List[Landmark]) -> Optional[HandGestureResult]:
    """Fist: all four fingers curled AND thumb tucked (tip at or below MCP)."""
    if not _four_fingers_curled(lm):
        return None
    if not _thumb_curled(lm):
        return None
    # Extra confidence: fingertips close to palm = deep fist
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    curl_depth = sum(
        max(0.0, lm[t].y - lm[p].y)
        for t, p in (
            (_INDEX_TIP, _INDEX_MCP), (_MIDDLE_TIP, _MIDDLE_MCP),
            (_RING_TIP, _RING_MCP),   (_PINKY_TIP, _PINKY_MCP),
        )
    ) / 4
    conf = round(min(0.95, 0.60 + 0.35 * min(1.0, curl_depth / (palm_h * 0.5))), 3)
    return HandGestureResult("disarm", conf, "", lm[_WRIST].x, lm[_WRIST].y)


def _classify_confirm(lm: List[Landmark]) -> Optional[HandGestureResult]:
    """Thumbs-up: thumb extended upward, four fingers curled."""
    if not _thumb_extended_up(lm):
        return None
    if not _four_fingers_curled(lm):
        return None
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    gap = lm[_THUMB_CMC].y - lm[_THUMB_TIP].y
    conf = round(min(0.95, 0.60 + 0.35 * min(1.0, gap / palm_h)), 3)
    return HandGestureResult("confirm", conf, "", lm[_WRIST].x, lm[_WRIST].y)


def _classify_follow_me(lm: List[Landmark]) -> Optional[HandGestureResult]:
    """Peace/victory sign: index and middle extended; ring and pinky curled."""
    if not _finger_up(lm, _INDEX_TIP,  _INDEX_PIP):
        return None
    if not _finger_up(lm, _MIDDLE_TIP, _MIDDLE_PIP):
        return None
    if _finger_up(lm, _RING_TIP,  _RING_PIP):
        return None
    if _finger_up(lm, _PINKY_TIP, _PINKY_PIP):
        return None
    idx_ext = _finger_ext_score(lm, _INDEX_TIP,  _INDEX_PIP,  _INDEX_MCP)
    mid_ext = _finger_ext_score(lm, _MIDDLE_TIP, _MIDDLE_PIP, _MIDDLE_MCP)
    conf = round(0.60 + 0.35 * ((idx_ext + mid_ext) / 2), 3)
    return HandGestureResult("follow_me", conf, "", lm[_WRIST].x, lm[_WRIST].y)


# ── Priority order (first match wins) ─────────────────────────────────────────

_CLASSIFIERS = [
    _classify_stop,
    _classify_confirm,    # before point — thumbs-up would also pass point partially
    _classify_follow_me,  # before point — index+middle would partially match
    _classify_point,
    _classify_disarm,
]


# ── Wave (temporal) detector ───────────────────────────────────────────────────

class WaveDetector:
    """Sliding-window wave gesture detector.

    Tracks the wrist X-coordinate over time and fires when there are at least
    `min_reversals` direction reversals with peak-to-peak amplitude ≥
    `min_amplitude` (normalised image coords).

    Args:
        history_len   : number of frames to keep (default 24 ≈ 0.8 s at 30 fps)
        min_reversals : direction reversals required to trigger (default 2)
        min_amplitude : peak-to-peak x excursion threshold (default 0.08)
    """

    def __init__(
        self,
        history_len:   int   = 24,
        min_reversals: int   = 2,
        min_amplitude: float = 0.08,
    ) -> None:
        self._history: Deque[float] = deque(maxlen=history_len)
        self._min_reversals  = min_reversals
        self._min_amplitude  = min_amplitude

    def push(self, wrist_x: float) -> Tuple[bool, float]:
        """Add a wrist-X sample.  Returns (is_waving, confidence)."""
        self._history.append(wrist_x)
        if len(self._history) < 6:
            return False, 0.0
        return self._detect()

    def reset(self) -> None:
        self._history.clear()

    def _detect(self) -> Tuple[bool, float]:
        samples  = list(self._history)
        mean_x   = sum(samples) / len(samples)
        centered = [x - mean_x for x in samples]
        amplitude = max(centered) - min(centered)
        if amplitude < self._min_amplitude:
            return False, 0.0

        reversals = sum(
            1 for i in range(1, len(centered))
            if centered[i - 1] * centered[i] < 0
        )
        if reversals < self._min_reversals:
            return False, 0.0

        amp_score = min(1.0, amplitude / 0.30)
        rev_score = min(1.0, reversals / 6.0)
        conf = round(0.5 * amp_score + 0.5 * rev_score, 3)
        return True, conf


# ── Public API ─────────────────────────────────────────────────────────────────

def classify_hand(
    landmarks: List[Landmark],
    is_right:  bool                   = True,
    wave_det:  Optional[WaveDetector] = None,
) -> HandGestureResult:
    """Classify one hand's 21 MediaPipe landmarks into a robot-command gesture.

    Parameters
    ----------
    landmarks : 21 Landmark objects (MediaPipe normalised coords).
    is_right  : True for right hand (affects thumb direction checks).
    wave_det  : Optional WaveDetector for temporal wave tracking.
                Wave is evaluated before static classifiers.

    Returns
    -------
    HandGestureResult — gesture, confidence, direction, wrist_x, wrist_y.
    """
    if len(landmarks) < 21:
        return _NONE

    # Wave (temporal) — highest priority
    if wave_det is not None:
        is_waving, wconf = wave_det.push(landmarks[_WRIST].x)
        if is_waving:
            return HandGestureResult(
                "greeting", wconf, "",
                landmarks[_WRIST].x, landmarks[_WRIST].y,
            )

    # Static classifiers in priority order
    for clf in _CLASSIFIERS:
        result = clf(landmarks)
        if result is not None:
            return result

    return HandGestureResult(
        "none", 0.0, "", landmarks[_WRIST].x, landmarks[_WRIST].y
    )
