"""gesture_classifier.py — Geometric gesture classification from MediaPipe landmarks.

Pure Python, no ROS2 / MediaPipe / OpenCV dependencies.
Consumes normalised landmark arrays produced by MediaPipe Hands and Pose,
classifies them into named gesture types, and tracks temporal gestures
(wave, come_here) via per-stream history buffers.

Usage
-----
>>> from saltybot_social.gesture_classifier import (
...     Landmark, classify_hand, classify_pose, WaveDetector
... )
>>> lm = [Landmark(x, y, z) for x, y, z in raw_coords]
>>> result = classify_hand(lm, is_right_hand=True, wave_det=None)
>>> result.gesture_type, result.confidence
('stop_palm', 0.92)

Coordinate convention
---------------------
MediaPipe Hands / Pose use image-normalised coordinates:
  x: 0.0 = left edge,  1.0 = right edge of frame
  y: 0.0 = top edge,   1.0 = bottom edge  (↓ positive)
  z: depth (approx), 0 at wrist/hip plane; negative = towards camera
"""

from __future__ import annotations

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

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


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


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


@dataclass
class ClassifiedGesture:
    gesture_type: str       # e.g. "stop_palm", "wave", "arms_up"
    confidence: float       # 0.0–1.0
    hand_x: float = 0.5     # normalised image x of gesture anchor
    hand_y: float = 0.5     # normalised image y of gesture anchor
    is_right_hand: bool = True
    direction: str = ""     # for "point": "left"/"right"/"up"/"forward"
    source: str = "hand"    # "hand" or "body_pose"


# ── MediaPipe Hands landmark indices ─────────────────────────────────────────
# Reference: https://developers.google.com/mediapipe/solutions/vision/hand_landmarker

_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

# ── MediaPipe Pose landmark indices ──────────────────────────────────────────

_L_SHOULDER = 11; _R_SHOULDER = 12
_L_ELBOW = 13;    _R_ELBOW = 14
_L_WRIST = 15;    _R_WRIST = 16
_L_HIP = 23;      _R_HIP = 24
_L_KNEE = 25;     _R_KNEE = 26
_L_ANKLE = 27;    _R_ANKLE = 28


# ── Finger-extension helpers ──────────────────────────────────────────────────


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


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


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


def _thumb_down(lm: List[Landmark]) -> bool:
    """True when the thumb is pointing downward (tip below the wrist)."""
    return lm[_THUMB_TIP].y > lm[_WRIST].y + 0.04


def _four_fingers_curled(lm: List[Landmark]) -> bool:
    """True when index, middle, ring, and pinky are all NOT extended."""
    fingers = [
        (_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 fingers)


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 _palm_center(lm: List[Landmark]) -> Tuple[float, float]:
    """Return (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


# ── Point direction ───────────────────────────────────────────────────────────


def _point_direction(lm: List[Landmark]) -> str:
    """Determine the 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   # positive = downward in image
    angle_deg = math.degrees(math.atan2(-dy, dx))   # flip y so up=+90
    # Quantise into 8 compass directions
    if -22.5 <= angle_deg < 22.5:
        return "right"
    elif 22.5 <= angle_deg < 67.5:
        return "upper_right"
    elif 67.5 <= angle_deg < 112.5:
        return "up"
    elif 112.5 <= angle_deg < 157.5:
        return "upper_left"
    elif angle_deg >= 157.5 or angle_deg < -157.5:
        return "left"
    elif -157.5 <= angle_deg < -112.5:
        return "lower_left"
    elif -112.5 <= angle_deg < -67.5:
        return "down"
    else:
        return "lower_right"


# ── Hand gesture classifiers ──────────────────────────────────────────────────


def _classify_stop_palm(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
    """All five fingers extended — 'stop' hand signal."""
    n_up = _count_fingers_up(lm)
    if n_up < 4:
        return None
    # Avg extension score for quality measure
    ext_scores = [
        _finger_up_score(lm, _INDEX_TIP, _INDEX_PIP, _INDEX_MCP),
        _finger_up_score(lm, _MIDDLE_TIP, _MIDDLE_PIP, _MIDDLE_MCP),
        _finger_up_score(lm, _RING_TIP, _RING_PIP, _RING_MCP),
        _finger_up_score(lm, _PINKY_TIP, _PINKY_PIP, _PINKY_MCP),
    ]
    avg_ext = sum(ext_scores) / len(ext_scores)
    conf = 0.6 + 0.35 * avg_ext + 0.05 * (n_up == 4)
    cx, cy = _palm_center(lm)
    return ClassifiedGesture(
        gesture_type="stop_palm", confidence=round(conf, 3),
        hand_x=cx, hand_y=cy
    )


def _classify_thumbs_up(lm: List[Landmark], is_right: bool) -> Optional[ClassifiedGesture]:
    """Thumb extended upward, other fingers curled."""
    if not _thumb_up(lm, is_right):
        return None
    if not _four_fingers_curled(lm):
        return None
    gap = lm[_THUMB_CMC].y - lm[_THUMB_TIP].y   # larger = more vertical
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    conf = min(0.95, 0.6 + 0.35 * min(1.0, gap / palm_h))
    cx, cy = _palm_center(lm)
    return ClassifiedGesture(
        gesture_type="thumbs_up", confidence=round(conf, 3),
        hand_x=cx, hand_y=cy
    )


def _classify_thumbs_down(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
    """Thumb pointing downward, other fingers curled."""
    if not _thumb_down(lm):
        return None
    if not _four_fingers_curled(lm):
        return None
    drop = lm[_THUMB_TIP].y - lm[_WRIST].y
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    conf = min(0.95, 0.6 + 0.35 * min(1.0, drop / palm_h))
    cx, cy = _palm_center(lm)
    return ClassifiedGesture(
        gesture_type="thumbs_down", confidence=round(conf, 3),
        hand_x=cx, hand_y=cy
    )


def _classify_point(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
    """Index finger extended, 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
    # Strongly horizontal pointing defers to "follow" gesture
    if abs(lm[_INDEX_TIP].x - lm[_INDEX_MCP].x) > 0.20:
        return None
    ext = _finger_up_score(lm, _INDEX_TIP, _INDEX_PIP, _INDEX_MCP)
    conf = 0.65 + 0.30 * ext - 0.10 * others_up
    direction = _point_direction(lm)
    return ClassifiedGesture(
        gesture_type="point", confidence=round(conf, 3),
        hand_x=lm[_INDEX_TIP].x, hand_y=lm[_INDEX_TIP].y,
        direction=direction,
    )


def _classify_come_here(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
    """Beckoning pose: index MCP raised but index TIP curled toward palm."""
    # MCP above wrist (hand raised), TIP clearly below MCP (finger curled down)
    mcp_raised = lm[_INDEX_MCP].y < lm[_WRIST].y - 0.02
    tip_curled = lm[_INDEX_TIP].y > lm[_INDEX_MCP].y + 0.03
    if not (mcp_raised and tip_curled):
        return None
    # Middle, ring, pinky should be curled too (not fully open)
    n_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 n_others_up >= 2:
        return None
    curl_depth = lm[_INDEX_TIP].y - lm[_INDEX_PIP].y
    palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
    conf = min(0.90, 0.60 + 0.30 * min(1.0, curl_depth / (palm_h * 0.5)))
    cx, cy = _palm_center(lm)
    return ClassifiedGesture(
        gesture_type="come_here", confidence=round(conf, 3),
        hand_x=cx, hand_y=cy
    )


def _classify_follow(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
    """Follow gesture: index extended horizontally, others 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 >= 2:
        return None
    # Horizontal: |dx| > |dy| between index MCP and TIP
    dx = abs(lm[_INDEX_TIP].x - lm[_INDEX_MCP].x)
    dy = abs(lm[_INDEX_TIP].y - lm[_INDEX_MCP].y)
    if dx <= dy:
        return None
    conf = 0.65 + 0.25 * min(1.0, dx / max(dy, 0.001) / 3.0)
    direction = "right" if lm[_INDEX_TIP].x > lm[_INDEX_MCP].x else "left"
    return ClassifiedGesture(
        gesture_type="follow", confidence=round(conf, 3),
        hand_x=lm[_INDEX_TIP].x, hand_y=lm[_INDEX_TIP].y,
        direction=direction,
    )


# ── Temporal wave detector ────────────────────────────────────────────────────


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

    Tracks wrist X-coordinate history and detects lateral oscillation
    (minimum 2 direction reversals above amplitude threshold).

    Args:
        history_len:    Number of frames to keep (default 24 ≈ 0.8 s at 30 fps)
        min_reversals:  Direction-reversal count required to trigger (default 2)
        min_amplitude:  Minimum peak-to-peak x excursion in normalised coords
    """

    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 new wrist-X sample.

        Returns:
            (is_waving, confidence) — confidence is 0.0 when not waving.
        """
        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

        # Count sign changes (direction reversals)
        reversals = 0
        for i in range(1, len(centered)):
            if centered[i - 1] * centered[i] < 0:
                reversals += 1

        if reversals < self._min_reversals:
            return False, 0.0

        # Confidence: blend amplitude and reversal quality
        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 hand-gesture API ───────────────────────────────────────────────────


# Priority order for static hand-gesture classifiers
_HAND_CLASSIFIERS = [
    ("stop_palm",   lambda lm, rh: _classify_stop_palm(lm)),
    ("thumbs_up",   lambda lm, rh: _classify_thumbs_up(lm, rh)),
    ("thumbs_down", lambda lm, rh: _classify_thumbs_down(lm)),
    ("point",       lambda lm, rh: _classify_point(lm)),
    ("come_here",   lambda lm, rh: _classify_come_here(lm)),
    ("follow",      lambda lm, rh: _classify_follow(lm)),
]


def classify_hand(
    landmarks: List[Landmark],
    is_right_hand: bool = True,
    wave_det: Optional[WaveDetector] = None,
    enabled: Optional[set] = None,
) -> Optional[ClassifiedGesture]:
    """Classify a single hand's 21-landmark list into a named gesture.

    Args:
        landmarks:    21 Landmark objects from MediaPipe Hands.
        is_right_hand: Handedness flag (affects thumb direction logic).
        wave_det:     Optional WaveDetector instance for wave tracking.
                      If provided, wave detection is attempted first.
        enabled:      Set of gesture type strings to allow; None = all.

    Returns:
        ClassifiedGesture or None if no gesture recognised.
    """
    if len(landmarks) < 21:
        return None

    def _allowed(name: str) -> bool:
        return enabled is None or name in enabled

    # Wave check (temporal — must come before static classifiers)
    if wave_det is not None and _allowed("wave"):
        is_waving, wconf = wave_det.push(landmarks[_WRIST].x)
        if is_waving:
            cx, cy = _palm_center(landmarks)
            return ClassifiedGesture(
                gesture_type="wave", confidence=wconf,
                hand_x=cx, hand_y=cy,
                is_right_hand=is_right_hand,
            )

    # Static classifiers — return the first (highest-priority) match
    for name, classifier in _HAND_CLASSIFIERS:
        if not _allowed(name):
            continue
        result = classifier(landmarks, is_right_hand)
        if result is not None:
            result.is_right_hand = is_right_hand
            return result

    return None


# ── Body-pose gesture classifiers ────────────────────────────────────────────


def classify_pose(
    landmarks: List[Landmark],
    enabled: Optional[set] = None,
) -> Optional[ClassifiedGesture]:
    """Classify full-body pose landmarks (33 points) into a body gesture.

    Args:
        landmarks:  33 Landmark objects from MediaPipe Pose.
        enabled:    Set of allowed gesture types; None = all.

    Returns:
        ClassifiedGesture or None.
    """
    if len(landmarks) < 29:  # need up to ankle (28)
        return None

    def _ok(name: str) -> bool:
        return enabled is None or name in enabled

    # ── arms_up: both wrists raised above their shoulders ─────────────────
    if _ok("arms_up"):
        l_above = landmarks[_L_WRIST].y < landmarks[_L_SHOULDER].y - 0.05
        r_above = landmarks[_R_WRIST].y < landmarks[_R_SHOULDER].y - 0.05
        if l_above and r_above:
            l_margin = landmarks[_L_SHOULDER].y - landmarks[_L_WRIST].y
            r_margin = landmarks[_R_SHOULDER].y - landmarks[_R_WRIST].y
            shoulder_span = abs(landmarks[_L_SHOULDER].y - landmarks[_R_HIP].y) or 0.1
            conf = min(0.95, 0.60 + 0.35 * min(1.0, (l_margin + r_margin) / (2 * shoulder_span)))
            mid_x = (landmarks[_L_WRIST].x + landmarks[_R_WRIST].x) / 2
            mid_y = (landmarks[_L_WRIST].y + landmarks[_R_WRIST].y) / 2
            return ClassifiedGesture(
                gesture_type="arms_up", confidence=round(conf, 3),
                hand_x=mid_x, hand_y=mid_y, source="body_pose",
            )

    # ── arms_spread: both arms extended laterally ──────────────────────────
    if _ok("arms_spread"):
        l_span = abs(landmarks[_L_WRIST].x - landmarks[_L_SHOULDER].x)
        r_span = abs(landmarks[_R_WRIST].x - landmarks[_R_SHOULDER].x)
        # Wrists should be at roughly shoulder height
        l_level = abs(landmarks[_L_WRIST].y - landmarks[_L_SHOULDER].y) < 0.12
        r_level = abs(landmarks[_R_WRIST].y - landmarks[_R_SHOULDER].y) < 0.12
        shoulder_w = abs(landmarks[_L_SHOULDER].x - landmarks[_R_SHOULDER].x) or 0.1
        if l_level and r_level and l_span > shoulder_w * 0.6 and r_span > shoulder_w * 0.6:
            conf = min(0.90, 0.55 + 0.35 * min(1.0, (l_span + r_span) / (shoulder_w * 2)))
            mid_x = (landmarks[_L_WRIST].x + landmarks[_R_WRIST].x) / 2
            mid_y = (landmarks[_L_SHOULDER].y + landmarks[_R_SHOULDER].y) / 2
            return ClassifiedGesture(
                gesture_type="arms_spread", confidence=round(conf, 3),
                hand_x=mid_x, hand_y=mid_y, source="body_pose",
            )

    # ── crouch: hips dropped below normal standing threshold ──────────────
    if _ok("crouch"):
        l_hip_y = landmarks[_L_HIP].y
        r_hip_y = landmarks[_R_HIP].y
        avg_hip_y = (l_hip_y + r_hip_y) / 2
        # In a crouching person the hips appear lower in frame (larger y).
        # We use 0.65 as threshold (hips in the lower 65% of frame height).
        if avg_hip_y > 0.65:
            # Additional check: knees should be significantly bent
            l_knee_y = landmarks[_L_KNEE].y if len(landmarks) > _L_KNEE else avg_hip_y
            r_knee_y = landmarks[_R_KNEE].y if len(landmarks) > _R_KNEE else avg_hip_y
            knee_drop = ((l_knee_y - l_hip_y) + (r_knee_y - r_hip_y)) / 2
            conf = min(0.90, 0.55 + 0.35 * min(1.0, (avg_hip_y - 0.65) / 0.20))
            mid_x = (landmarks[_L_HIP].x + landmarks[_R_HIP].x) / 2
            return ClassifiedGesture(
                gesture_type="crouch", confidence=round(conf, 3),
                hand_x=mid_x, hand_y=avg_hip_y, source="body_pose",
            )

    return None
