"""AZ-385 ``SourceLabelStateMachine`` — source-label + spoof-promotion gate.

Per the C5 contract (Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8) the
``EstimatorOutput.source_label`` MUST reflect three real states:

* ``SATELLITE_ANCHORED`` — the spoof-promotion gate is open AND a
  recent satellite anchor exists.
* ``VISUAL_PROPAGATED`` — anchor is stale OR the spoof-promotion gate
  is closed.
* ``DEAD_RECKONED`` — no satellite anchor has ever been observed.

The gate latches CLOSED on the first FC-reported ``SPOOFED`` GPS
health; it re-opens only when BOTH of these are true:

1. FC GPS health has been ``STABLE_NON_SPOOFED`` for at least
   ``spoof_promotion_min_stable_s`` (default 10 s).
2. The next satellite anchor agrees with the FC GPS within
   ``spoof_promotion_visual_consistency_tol_m`` metres (default 30 m).

Every attempted promotion that fails either condition emits ONE
``c5.state.spoof_rejected`` FDR record + fans out a STATUSTEXT
callback to subscribers (composition root wires the C8 GCS adapter
here). R07 / AC-6 — the FDR and subscriber paths bypass the standard
logger, so silencing logs cannot suppress the reject trail.

Threading: the state machine takes an explicit lock around every
state mutation so the composition root's eventual C8-inbound-thread
producer (``notify_gps_health``) does not race the C5 ingest-thread
producer (``notify_satellite_anchor`` + reads from
``current_estimate``). Single-writer thread per Invariant 1 is the
deployed contract, but the lock is cheap and makes the unit-test
matrix simpler.
"""

from __future__ import annotations

import threading
from collections.abc import Callable
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Final, Protocol, runtime_checkable

from gps_denied_onboard._types.fc import GpsStatus, Severity
from gps_denied_onboard._types.state import PoseSourceLabel
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger

if TYPE_CHECKING:
    from gps_denied_onboard._types.fc import GpsHealth
    from gps_denied_onboard._types.geo import LatLonAlt
    from gps_denied_onboard.fdr_client.client import FdrClient

__all__ = [
    "BOUNDED_DELTA_REJECT",
    "BOUNDED_DELTA_SOFT",
    "RejectionCallback",
    "RejectionSubscription",
    "SourceLabelStateMachine",
]

# AZ-490 / Principle #11 third clause: tri-state outcome of the
# bounded-delta gate. Kept as module-level string constants rather
# than additions to ``PoseSourceLabel`` because the public C4/C5
# label enum is stable and a soft-admission outcome doesn't change
# the externally observable pose-provenance contract.
BOUNDED_DELTA_SOFT: Final[str] = "BOUNDED_DELTA_SOFT"
BOUNDED_DELTA_REJECT: Final[str] = "BOUNDED_DELTA_REJECT"


# Subscriber signature — composition root receives
# (reason, severity, statustext) on every reject. ``severity`` is
# always ``WARNING`` per AC-12. ``statustext`` is pre-formatted and
# capped at 50 chars (MAVLink ``STATUSTEXT.text`` max).
RejectionCallback = Callable[[str, Severity, str], None]


# AC-12 MAVLink STATUSTEXT max payload length (50 chars; longer texts
# are silently truncated on the wire).
_STATUSTEXT_MAX_LEN: Final[int] = 50

# AC-12 STATUSTEXT severity — WARNING for spoof rejections.
_REJECT_SEVERITY: Final[Severity] = Severity.WARNING

# Anchor-staleness threshold for AC-3 ``VISUAL_PROPAGATED`` (ms).
# Anchors older than this fall back to the visual-propagated label
# even when the gate is open. The contract spec lists the threshold
# as configurable in principle, but the only consumer (this state
# machine) treats it as an implementation constant tied to the
# D-C5-3 keyframe cadence (~3 Hz → 1 s between anchors at worst).
_ANCHOR_STALENESS_THRESHOLD_MS: Final[int] = 1000

# Reject reason codes — short tokens that fit into the 50-char
# STATUSTEXT budget AFTER the ``"GPS spoof rejected: "`` prefix.
_REASON_GPS_STILL_SPOOFED: Final[str] = "gps_spoofed"
_REASON_DWELL_INSUFFICIENT: Final[str] = "dwell_short"
_REASON_CONSISTENCY_VIOLATION: Final[str] = "consistency"
_REASON_NO_GPS_OBSERVATION: Final[str] = "no_gps_obs"


@runtime_checkable
class RejectionSubscription(Protocol):
    """Handle returned by :meth:`SourceLabelStateMachine.subscribe_rejection`.

    Calling :meth:`cancel` removes the callback from the next
    rejection dispatch. Subsequent cancels are no-ops.
    """

    def cancel(self) -> None: ...


class _Subscription:
    def __init__(
        self,
        registry: dict[int, RejectionCallback],
        sub_id: int,
        lock: threading.Lock,
    ) -> None:
        self._registry = registry
        self._sub_id = sub_id
        self._lock = lock

    def cancel(self) -> None:
        with self._lock:
            self._registry.pop(self._sub_id, None)


class SourceLabelStateMachine:
    """Source-label + spoof-promotion state machine. One per estimator.

    Public API:

    * :meth:`notify_gps_health` — feed FC-reported GPS health; updates
      the spoof-block latch + the stable-dwell timer.
    * :meth:`notify_satellite_anchor` — feed a new satellite anchor;
      may lift the spoof block (when BOTH AC-NEW-2 conditions hold)
      or emit a reject record (otherwise, while blocked).
    * :meth:`current_label` — pure read; returns the current label.
    * :meth:`is_spoof_promotion_blocked` — pure read; returns the
      gate state.
    * :meth:`subscribe_rejection` — register a callback for the
      STATUSTEXT mirror; returns a cancel handle.

    The label is recomputed lazily on every read (``current_label``),
    using the cached anchor timestamp + block state + clock. The
    machine never mutates state from a read; only the two
    ``notify_*`` methods change anything observable.
    """

    def __init__(
        self,
        *,
        spoof_promotion_min_stable_s: float,
        spoof_promotion_visual_consistency_tol_m: float,
        spoof_promotion_bounded_delta_m: float,
        fdr_client: FdrClient | None,
        clock_ns: Callable[[], int],
        producer_id: str = "c5_state",
    ) -> None:
        if spoof_promotion_min_stable_s <= 0.0:
            raise ValueError(
                "SourceLabelStateMachine.spoof_promotion_min_stable_s must be > 0; "
                f"got {spoof_promotion_min_stable_s}"
            )
        if spoof_promotion_visual_consistency_tol_m <= 0.0:
            raise ValueError(
                "SourceLabelStateMachine.spoof_promotion_visual_consistency_tol_m "
                f"must be > 0; got {spoof_promotion_visual_consistency_tol_m}"
            )
        if spoof_promotion_bounded_delta_m <= 0.0:
            raise ValueError(
                "SourceLabelStateMachine.spoof_promotion_bounded_delta_m must be > 0; "
                f"got {spoof_promotion_bounded_delta_m}"
            )
        self._min_stable_ns: int = int(spoof_promotion_min_stable_s * 1_000_000_000)
        self._consistency_tol_m: float = spoof_promotion_visual_consistency_tol_m
        self._bounded_delta_m: float = spoof_promotion_bounded_delta_m
        self._fdr_client: FdrClient | None = fdr_client
        self._producer_id: str = producer_id
        self._clock_ns: Callable[[], int] = clock_ns
        self._log = get_logger("c5_state.source_label_sm")

        self._lock = threading.Lock()
        # Cached state — all writes go through the two ``notify_*``
        # methods. Reads from ``current_label`` / ``is_spoof_promotion_blocked``
        # snapshot under the same lock for consistency.
        self._last_anchored_frame_ns: int | None = None
        self._gps_status: GpsStatus | None = None
        # Set when the FC reports STABLE_NON_SPOOFED; cleared on any
        # other status. The dwell test is ``now - stable_since_ns >=
        # min_stable_ns``.
        self._gps_health_stable_since_ns: int | None = None
        self._promotion_blocked: bool = False
        self._last_label: PoseSourceLabel = PoseSourceLabel.DEAD_RECKONED

        # Subscriber registry — composition root attaches C8's
        # ``QgcTelemetryAdapter.send_statustext`` here so STATUSTEXT
        # mirrors fire on the C8 outbound thread.
        self._rejection_subs: dict[int, RejectionCallback] = {}
        self._next_sub_id: int = 1

    # ------------------------------------------------------------------
    # Read API — pure; never mutates.

    def current_label(self) -> PoseSourceLabel:
        """Recompute + return the current source label.

        Recomputation rules (per AC-1/2/3 + Invariant 5):

        * No anchor ever observed → ``DEAD_RECKONED``.
        * Spoof gate latched closed → ``VISUAL_PROPAGATED``.
        * Last anchor older than ``_ANCHOR_STALENESS_THRESHOLD_MS``
          → ``VISUAL_PROPAGATED``.
        * Else → ``SATELLITE_ANCHORED``.
        """
        with self._lock:
            label = self._recompute_label_locked()
            self._last_label = label
            return label

    def is_spoof_promotion_blocked(self) -> bool:
        """Return whether the spoof-promotion gate is currently closed."""
        with self._lock:
            return self._promotion_blocked

    # ------------------------------------------------------------------
    # Write API — both methods may transition state.

    def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
        """Feed a new ``GpsHealth`` observation.

        On a SPOOFED → True transition the gate latches closed and one
        ``c5.state.spoof_gate_engaged`` INFO log fires. On a transition
        to ``STABLE_NON_SPOOFED`` the dwell timer starts; on any other
        status the dwell timer clears.
        """
        ts = now_ns if now_ns is not None else self._clock_ns()
        transition_info: tuple[bool, str, str] | None = None  # (block_engaged, from, to)
        with self._lock:
            prev_status = self._gps_status
            self._gps_status = gps_health.status
            if gps_health.status == GpsStatus.STABLE_NON_SPOOFED:
                if prev_status != GpsStatus.STABLE_NON_SPOOFED:
                    self._gps_health_stable_since_ns = ts
            else:
                self._gps_health_stable_since_ns = None
            if gps_health.status == GpsStatus.SPOOFED and not self._promotion_blocked:
                self._promotion_blocked = True
                transition_info = (
                    True,
                    (prev_status.value if prev_status is not None else "init"),
                    gps_health.status.value,
                )

        if transition_info is not None:
            _engaged, prev, curr = transition_info
            self._log.warning(
                "c5.state.spoof_gate_engaged",
                extra={
                    "kind": "c5.state.spoof_gate_engaged",
                    "kv": {"from": prev, "to": curr},
                },
            )

    def notify_satellite_anchor(
        self,
        now_ns: int,
        gps_consistency_delta_m: float | None,
    ) -> None:
        """Feed a new satellite anchor.

        While the gate is open this is a pure bookkeeping call —
        updates ``_last_anchored_frame_ns`` + may transition the
        label from ``DEAD_RECKONED`` → ``SATELLITE_ANCHORED``.

        While the gate is closed this is also a *promotion attempt*:
        if both AC-NEW-2 conditions hold (≥10 s STABLE_NON_SPOOFED
        AND consistency within tol_m), the gate re-opens and one
        ``c5.state.spoof_gate_lifted`` INFO log fires; otherwise one
        ``c5.state.spoof_rejected`` FDR record + STATUSTEXT mirror
        fires (AC-6 — unsilenceable; bypasses the standard logger).
        """
        emit_reject: tuple[str, str, float, float | None] | None = None
        emit_gate_lift: bool = False
        emit_label_change: tuple[PoseSourceLabel, PoseSourceLabel] | None = None
        with self._lock:
            prev_label = self._last_label
            self._last_anchored_frame_ns = now_ns
            if self._promotion_blocked:
                stable_ok = self._stable_long_enough_locked(now_ns)
                consistency_ok = (
                    gps_consistency_delta_m is not None
                    and gps_consistency_delta_m <= self._consistency_tol_m
                )
                if stable_ok and consistency_ok:
                    self._promotion_blocked = False
                    self._gps_health_stable_since_ns = None
                    emit_gate_lift = True
                else:
                    reason = self._classify_reject_reason_locked(
                        stable_ok=stable_ok,
                        consistency_ok=consistency_ok,
                        gps_consistency_delta_m=gps_consistency_delta_m,
                    )
                    time_since_stable_s = self._time_since_stable_s_locked(now_ns)
                    gps_status_str = (
                        self._gps_status.value if self._gps_status is not None else "unknown"
                    )
                    emit_reject = (
                        reason,
                        gps_status_str,
                        time_since_stable_s,
                        gps_consistency_delta_m,
                    )
            new_label = self._recompute_label_locked()
            if new_label != prev_label:
                self._last_label = new_label
                emit_label_change = (prev_label, new_label)

        if emit_reject is not None:
            self._emit_reject(*emit_reject)
        if emit_gate_lift:
            self._log.info(
                "c5.state.spoof_gate_lifted",
                extra={
                    "kind": "c5.state.spoof_gate_lifted",
                    "kv": {"consistency_tol_m": self._consistency_tol_m},
                },
            )
        if emit_label_change is not None:
            prev, curr = emit_label_change
            self._log.info(
                "c5.state.source_label_changed",
                extra={
                    "kind": "c5.state.source_label_changed",
                    "kv": {
                        "from": prev.value,
                        "to": curr.value,
                        "reason": "anchor_event",
                    },
                },
            )

    # ------------------------------------------------------------------
    # AZ-490: third clause — bounded-delta GPS gate.

    def process_gps_sample(
        self,
        sample: LatLonAlt,
        *,
        smoother_estimate: LatLonAlt | None,
        now_ns: int | None = None,
    ) -> str | None:
        """Bounded-delta admission test for an inbound FC GPS sample.

        Returns:
        * :data:`BOUNDED_DELTA_SOFT` — sample is within the
          ``spoof_promotion_bounded_delta_m`` ring; the composition
          root may attach it as a soft pose-anchor factor.
        * :data:`BOUNDED_DELTA_REJECT` — sample is outside the ring;
          the composition root MUST drop it; the dwell-time clause
          (clause 1 of the spoof-promotion gate) is reset so a
          subsequent reanchor must wait the full
          ``spoof_promotion_min_stable_s`` window.
        * ``None`` — no smoother estimate is available yet (cold
          start / fallback engaged); the gate is skipped and the
          caller decides whether to admit or drop.

        AC-15: distance is computed via :meth:`WgsConverter.horizontal_distance_m`,
        which routes through ``pyproj``'s ECEF chain (matches Vincenty
        within sub-mm at the bounded-delta operating range), so the
        haversine-on-equirectangular shortcut is excluded.
        """
        if smoother_estimate is None:
            return None
        ts = now_ns if now_ns is not None else self._clock_ns()
        try:
            distance_m = WgsConverter.horizontal_distance_m(smoother_estimate, sample)
        except Exception as exc:
            self._log.error(
                "c5.gps_bounded_delta.distance_failed",
                extra={
                    "kind": "c5.gps_bounded_delta.distance_failed",
                    "kv": {"error": str(exc)},
                },
            )
            return None
        threshold_m = self._bounded_delta_m
        admitted = distance_m <= threshold_m
        kind = (
            "c5.gps_bounded_delta.accept" if admitted else "c5.gps_bounded_delta.reject"
        )
        record = FdrRecord(
            schema_version=1,
            ts=datetime.now(tz=timezone.utc).isoformat(),
            producer_id=self._producer_id,
            kind=kind,
            payload={
                "sample_lat": sample.lat_deg,
                "sample_lon": sample.lon_deg,
                "smoother_lat": smoother_estimate.lat_deg,
                "smoother_lon": smoother_estimate.lon_deg,
                "distance_m": distance_m,
                "threshold_m": threshold_m,
            },
        )
        if self._fdr_client is not None:
            try:
                self._fdr_client.enqueue(record)
            except Exception as exc:
                self._log.warning(
                    "c5.gps_bounded_delta.fdr_enqueue_failed",
                    extra={
                        "kind": "c5.gps_bounded_delta.fdr_enqueue_failed",
                        "kv": {"reject": not admitted, "error": repr(exc)},
                    },
                )
        if admitted:
            self._log.info(
                "c5.gps_bounded_delta.accept",
                extra={
                    "kind": "c5.gps_bounded_delta.accept",
                    "kv": {
                        "distance_m": distance_m,
                        "threshold_m": threshold_m,
                    },
                },
            )
            return BOUNDED_DELTA_SOFT
        # Reject path: bump the dwell-time clause so the AZ-385 gate
        # holds steady — a wildly-off GPS sample is a strong signal
        # the FC stream is not (yet) reliable, even when health is
        # nominally STABLE_NON_SPOOFED.
        with self._lock:
            self._gps_health_stable_since_ns = None
        self._log.warning(
            "c5.gps_bounded_delta.reject",
            extra={
                "kind": "c5.gps_bounded_delta.reject",
                "kv": {
                    "distance_m": distance_m,
                    "threshold_m": threshold_m,
                    "sample_lat": sample.lat_deg,
                    "sample_lon": sample.lon_deg,
                    "smoother_lat": smoother_estimate.lat_deg,
                    "smoother_lon": smoother_estimate.lon_deg,
                    "ts_ns": ts,
                },
            },
        )
        return BOUNDED_DELTA_REJECT

    # ------------------------------------------------------------------
    # Subscription API.

    def subscribe_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
        """Register a callback invoked once per spoof-rejection event."""
        with self._lock:
            sub_id = self._next_sub_id
            self._next_sub_id += 1
            self._rejection_subs[sub_id] = callback
        return _Subscription(self._rejection_subs, sub_id, self._lock)

    # ------------------------------------------------------------------
    # Internal helpers — all run under the lock unless suffixed
    # ``_unlocked``.

    def _recompute_label_locked(self) -> PoseSourceLabel:
        if self._last_anchored_frame_ns is None:
            return PoseSourceLabel.DEAD_RECKONED
        if self._promotion_blocked:
            return PoseSourceLabel.VISUAL_PROPAGATED
        now_ns = self._clock_ns()
        age_ms = (now_ns - self._last_anchored_frame_ns) / 1_000_000
        if age_ms > _ANCHOR_STALENESS_THRESHOLD_MS:
            return PoseSourceLabel.VISUAL_PROPAGATED
        return PoseSourceLabel.SATELLITE_ANCHORED

    def _stable_long_enough_locked(self, now_ns: int) -> bool:
        if self._gps_health_stable_since_ns is None:
            return False
        return (now_ns - self._gps_health_stable_since_ns) >= self._min_stable_ns

    def _time_since_stable_s_locked(self, now_ns: int) -> float:
        if self._gps_health_stable_since_ns is None:
            return 0.0
        return (now_ns - self._gps_health_stable_since_ns) / 1_000_000_000

    def _classify_reject_reason_locked(
        self,
        *,
        stable_ok: bool,
        consistency_ok: bool,
        gps_consistency_delta_m: float | None,
    ) -> str:
        if self._gps_status == GpsStatus.SPOOFED:
            return _REASON_GPS_STILL_SPOOFED
        if gps_consistency_delta_m is None:
            return _REASON_NO_GPS_OBSERVATION
        if not stable_ok:
            return _REASON_DWELL_INSUFFICIENT
        if not consistency_ok:
            return _REASON_CONSISTENCY_VIOLATION
        return _REASON_DWELL_INSUFFICIENT

    def _emit_reject(
        self,
        reason: str,
        gps_status: str,
        time_since_stable_s: float,
        gps_consistency_delta_m: float | None,
    ) -> None:
        # AC-6: FDR + subscriber paths bypass the standard logger so
        # silencing logs cannot suppress the spoof-rejection trail.
        record = FdrRecord(
            schema_version=1,
            ts=datetime.now(tz=timezone.utc).isoformat(),
            producer_id=self._producer_id,
            kind="c5.state.spoof_rejected",
            payload={
                "reason": reason,
                "gps_health": gps_status,
                "time_since_stable_s": time_since_stable_s,
                "visual_consistency_delta_m": gps_consistency_delta_m,
            },
        )
        if self._fdr_client is not None:
            try:
                self._fdr_client.enqueue(record)
            except Exception as exc:
                self._log.debug(
                    "c5.state.spoof_rejected_fdr_enqueue_failed",
                    extra={
                        "kind": "c5.state.spoof_rejected_fdr_enqueue_failed",
                        "kv": {"error": repr(exc)},
                    },
                )
        text = _format_statustext(reason)
        with self._lock:
            subs = list(self._rejection_subs.values())
        for cb in subs:
            try:
                cb(reason, _REJECT_SEVERITY, text)
            except Exception as exc:
                self._log.debug(
                    "c5.state.spoof_rejected_callback_failed",
                    extra={
                        "kind": "c5.state.spoof_rejected_callback_failed",
                        "kv": {"error": repr(exc)},
                    },
                )
        self._log.warning(
            "c5.state.spoof_rejected",
            extra={
                "kind": "c5.state.spoof_rejected",
                "kv": {
                    "reason": reason,
                    "gps_health": gps_status,
                    "time_since_stable_s": time_since_stable_s,
                    "visual_consistency_delta_m": gps_consistency_delta_m,
                },
            },
        )


def _format_statustext(reason: str) -> str:
    """Build the 50-char MAVLink STATUSTEXT payload for a reject reason."""
    text = f"GPS spoof rejected: {reason}"
    if len(text) > _STATUSTEXT_MAX_LEN:
        text = text[:_STATUSTEXT_MAX_LEN]
    return text
