#!/usr/bin/env python3
"""Pure state machines for robot-side right-hand button gestures.

This module deliberately has no ROS dependencies so the hold/release safety
rules can be exercised offline before the bridge is deployed on a robot.
"""

from __future__ import annotations

import math
from typing import Any, Mapping, Sequence


class GestureToggle:
    """Toggle one persistent gesture with a guarded long press.

    A new session always starts release-locked.  A continuous release clears
    that lock, then a continuous press toggles the gesture once.  The same
    physical press cannot toggle twice; another stable release is required.
    """

    def __init__(self, hold_seconds: float, release_seconds: float) -> None:
        if not math.isfinite(hold_seconds) or hold_seconds <= 0.0:
            raise ValueError("gesture hold time must be positive and finite")
        if not math.isfinite(release_seconds) or release_seconds <= 0.0:
            raise ValueError("gesture release time must be positive and finite")
        self.hold_seconds = float(hold_seconds)
        self.release_seconds = float(release_seconds)
        self.active = False
        self.hold_started_at: float | None = None
        self.release_started_at: float | None = None
        self.require_release = True
        self.armed = False
        self.input_healthy = False
        self.button_pressed: bool | None = None
        self._freeze_right_hand = False
        self.toggle_count = 0
        self.last_transition = "initialized"

    def new_session(self) -> None:
        """Start release-locked and never carry a gesture across sessions."""

        self.active = False
        self.hold_started_at = None
        self.release_started_at = None
        self.require_release = True
        self.armed = True
        self.input_healthy = False
        self.button_pressed = None
        self._freeze_right_hand = True
        self.last_transition = "new_session"

    def disarm(self, reason: str = "disarmed") -> None:
        """Clear the logical gesture without commanding any hand movement."""

        self.active = False
        self.hold_started_at = None
        self.release_started_at = None
        self.require_release = True
        self.armed = False
        self.input_healthy = False
        self.button_pressed = None
        self._freeze_right_hand = False
        self.last_transition = reason

    def update(
        self,
        now: float,
        *,
        armed: bool,
        input_healthy: bool,
        pressed: bool | None,
    ) -> bool:
        """Advance the state and return ``True`` only when a toggle occurs.

        ``pressed=None`` represents a missing or malformed B-button sample.  It
        cancels a pending hold and requires a fresh stable release.  A hand
        feedback gap behaves the same way, but an already active gesture is
        retained so it can resume through the bridge's measured-feedback slew.
        """

        if not math.isfinite(now):
            raise ValueError("gesture clock must be finite")
        if not armed:
            if self.armed or self.active:
                self.disarm()
            return False
        if not self.armed:
            self.new_session()

        self.input_healthy = bool(input_healthy)
        self.button_pressed = pressed
        if not input_healthy or pressed is None:
            self.hold_started_at = None
            self.release_started_at = None
            self.require_release = True
            if not self.active:
                self._freeze_right_hand = True
            return False

        if self.require_release:
            self.hold_started_at = None
            if pressed:
                self.release_started_at = None
                return False
            if self.release_started_at is None:
                self.release_started_at = now
                return False
            if now - self.release_started_at >= self.release_seconds:
                self.require_release = False
                self.release_started_at = None
                if not self.active:
                    self._freeze_right_hand = False
            return False

        self.release_started_at = None
        if not pressed:
            if self.hold_started_at is not None:
                # A short/interrupted press is not a toggle.  Debounce the
                # release before another hold can start.
                self.hold_started_at = None
                self.release_started_at = now
                self.require_release = True
                if not self.active:
                    self._freeze_right_hand = True
            return False

        if self.hold_started_at is None:
            self.hold_started_at = now
            if not self.active:
                self._freeze_right_hand = True
            return False
        if now - self.hold_started_at < self.hold_seconds:
            return False

        self.active = not self.active
        self.toggle_count += 1
        self.last_transition = "activated" if self.active else "deactivated"
        # Activation uses the point target.  Deactivation starts slewing back
        # to live hand input immediately; the release lock only prevents a
        # second toggle from the same physical press.
        self._freeze_right_hand = False
        self.hold_started_at = None
        self.release_started_at = None
        self.require_release = True
        return True

    @property
    def freeze_right_hand(self) -> bool:
        """Whether the bridge must hold its last right-hand command.

        While inactive, freeze through an incomplete B hold and its required
        release.  A completed deactivation starts returning to live input at
        once; its release lock prevents only another toggle.  This keeps a
        short B press from leaking through xTELE's processed command stream.
        """

        return self.armed and not self.active and self._freeze_right_hand

    def hold_elapsed(self, now: float) -> float:
        if self.hold_started_at is None:
            return 0.0
        return max(0.0, now - self.hold_started_at)

    def release_elapsed(self, now: float) -> float:
        if self.release_started_at is None:
            return 0.0
        return max(0.0, now - self.release_started_at)

    @property
    def state(self) -> str:
        if not self.armed:
            return "disarmed"
        if not self.input_healthy:
            return "input_unhealthy"
        if self.button_pressed is None:
            return "invalid_button"
        if self.require_release:
            return "awaiting_release"
        if self.hold_started_at is not None:
            return "holding_stop" if self.active else "holding_start"
        return "active" if self.active else "idle"


class GuardedMomentaryGesture:
    """Hold one gesture only while its button is pressed.

    A newly armed teleoperation session must first observe a stable release,
    preventing a button held before START from moving the hand.  Missing or
    malformed input fails closed and requires another stable release.
    """

    def __init__(self, release_seconds: float) -> None:
        if not math.isfinite(release_seconds) or release_seconds <= 0.0:
            raise ValueError("gesture release time must be positive and finite")
        self.release_seconds = float(release_seconds)
        self.active = False
        self.release_started_at: float | None = None
        self.require_release = True
        self.armed = False
        self.input_healthy = False
        self.button_pressed: bool | None = None
        self._freeze_right_hand = False
        self.activation_count = 0
        self.last_transition = "initialized"

    def new_session(self) -> None:
        self.active = False
        self.release_started_at = None
        self.require_release = True
        self.armed = True
        self.input_healthy = False
        self.button_pressed = None
        self._freeze_right_hand = True
        self.last_transition = "new_session"

    def disarm(self, reason: str = "disarmed") -> None:
        self.active = False
        self.release_started_at = None
        self.require_release = True
        self.armed = False
        self.input_healthy = False
        self.button_pressed = None
        self._freeze_right_hand = False
        self.last_transition = reason

    def update(
        self,
        now: float,
        *,
        armed: bool,
        input_healthy: bool,
        pressed: bool | None,
    ) -> bool:
        """Advance the gate and report only active/inactive edge changes."""

        if not math.isfinite(now):
            raise ValueError("gesture clock must be finite")
        was_active = self.active
        if not armed:
            if self.armed or self.active:
                self.disarm()
            return was_active != self.active
        if not self.armed:
            self.new_session()

        self.input_healthy = bool(input_healthy)
        self.button_pressed = pressed
        if not input_healthy or pressed is None:
            self.active = False
            self.release_started_at = None
            self.require_release = True
            self._freeze_right_hand = True
            self.last_transition = "input_unhealthy"
            return was_active != self.active

        if self.require_release:
            self.active = False
            self._freeze_right_hand = True
            if pressed:
                self.release_started_at = None
            elif self.release_started_at is None:
                self.release_started_at = now
            elif now - self.release_started_at >= self.release_seconds:
                self.require_release = False
                self.release_started_at = None
                self._freeze_right_hand = False
                self.last_transition = "release_ready"
            return was_active != self.active

        self.release_started_at = None
        self._freeze_right_hand = False
        self.active = bool(pressed)
        if self.active != was_active:
            if self.active:
                self.activation_count += 1
                self.last_transition = "activated"
            else:
                self.last_transition = "deactivated"
        return was_active != self.active

    @property
    def freeze_right_hand(self) -> bool:
        return self.armed and not self.active and self._freeze_right_hand

    def release_elapsed(self, now: float) -> float:
        if self.release_started_at is None:
            return 0.0
        return max(0.0, now - self.release_started_at)

    @property
    def state(self) -> str:
        if not self.armed:
            return "disarmed"
        if not self.input_healthy:
            return "input_unhealthy"
        if self.button_pressed is None:
            return "invalid_button"
        if self.require_release:
            return "awaiting_release"
        return "active" if self.active else "idle"


def right_a_pressed(data: Mapping[str, Any]) -> bool | None:
    """Return TS1P right-A state, or ``None`` for a malformed sample."""

    try:
        buttons = data["button"]
        if not isinstance(buttons, Mapping):
            return None
        right = buttons["right"]
        if not isinstance(right, (list, tuple)) or len(right) < 1:
            return None
        value = right[0]
        if isinstance(value, bool):
            return value
        if isinstance(value, int) and value in (0, 1):
            return bool(value)
        return None
    except (KeyError, TypeError):
        return None


def right_b_pressed(data: Mapping[str, Any]) -> bool | None:
    """Return TS1P right-B state, or ``None`` for a malformed sample."""

    try:
        buttons = data["button"]
        if not isinstance(buttons, Mapping):
            return None
        right = buttons["right"]
        if not isinstance(right, (list, tuple)) or len(right) < 2:
            return None
        value = right[1]
        if isinstance(value, bool):
            return value
        if isinstance(value, int) and value in (0, 1):
            return bool(value)
        return None
    except (KeyError, TypeError):
        return None


def normalized_pose_to_positions(
    pose: Sequence[float], minimum: int, maximum: int
) -> list[int]:
    """Convert a six-motor normalized BrainCo pose to driver positions."""

    if len(pose) != 6:
        raise ValueError("gesture pose must contain 6 normalized values")
    values = [float(value) for value in pose]
    if not all(math.isfinite(value) and 0.0 <= value <= 1.0 for value in values):
        raise ValueError("gesture pose must contain finite values in [0, 1]")
    if minimum < 0 or maximum <= minimum:
        raise ValueError("invalid BrainCo position range")
    return [
        int(round(minimum + value * (maximum - minimum))) for value in values
    ]


def select_hand_target(
    source_target: Sequence[int],
    previous_command: Sequence[int] | None,
    gesture_target: Sequence[int],
    *,
    active: bool,
    freeze: bool,
) -> list[int]:
    """Apply one hand's gesture override or fail-safe input freeze."""

    source = list(source_target)
    gesture = list(gesture_target)
    if len(source) != 6 or len(gesture) != 6:
        raise ValueError("right-hand targets must each contain 6 positions")
    if active:
        return gesture
    if freeze:
        previous = [] if previous_command is None else list(previous_command)
        if len(previous) != 6:
            raise ValueError("cannot freeze without a complete previous command")
        return previous
    return source


def select_right_hand_target(
    source_target: Sequence[int],
    previous_command: Sequence[int] | None,
    gesture_target: Sequence[int],
    *,
    active: bool,
    freeze: bool,
) -> list[int]:
    """Backward-compatible name for the original right-only implementation."""

    return select_hand_target(
        source_target,
        previous_command,
        gesture_target,
        active=active,
        freeze=freeze,
    )