NODEDC_MISSION_CORE/src/k1link/perception/motion.py

"""Class-independent bounded-history motion estimation for Mission Core."""

from __future__ import annotations

import math
from dataclasses import dataclass, replace

from .contracts import MotionState, TemporalObstacle, TemporalState
from .providers import SourcePacket
from .temporal import MOTION_PROVIDER_ID, MotionEstimatorProfile, TemporalMotionProfile


class MotionEstimatorError(RuntimeError):
    """Temporal history cannot support a deterministic motion decision."""


@dataclass(frozen=True, slots=True)
class MotionEstimatorSnapshot:
    input_frames: int
    input_obstacles: int
    moving: int
    stationary: int
    unknown: int
    insufficient_history: int
    stale_support: int
    map_frame_discontinuity: int
    confidence_below_threshold: int
    threshold_deadband: int
    implausible_speed: int


class ClassIndependentMotionEstimator:
    """Estimate map-frame motion without labels, detector IDs or tracklets."""

    provider_id: str = MOTION_PROVIDER_ID

    def __init__(self, *, profile: TemporalMotionProfile) -> None:
        self.profile = profile
        self.config = profile.motion
        self._input_frames = 0
        self._input_obstacles = 0
        self._moving = 0
        self._stationary = 0
        self._unknown = 0
        self._reasons: dict[str, int] = {
            "insufficient-history": 0,
            "stale-support": 0,
            "map-frame-discontinuity": 0,
            "confidence-below-threshold": 0,
            "threshold-deadband": 0,
            "implausible-speed": 0,
        }

    def estimate(
        self,
        packet: SourcePacket,
        obstacles: tuple[TemporalObstacle, ...],
    ) -> tuple[TemporalObstacle, ...]:
        if (
            packet.envelope.source_id != self.profile.source_id
            or packet.envelope.session_id != self.profile.session_id
        ):
            raise MotionEstimatorError("packet escaped the motion source profile")
        self._input_frames += 1
        self._input_obstacles += len(obstacles)
        result = tuple(self._estimate_one(packet, obstacle) for obstacle in obstacles)
        for obstacle in result:
            if obstacle.motion is MotionState.MOVING:
                self._moving += 1
            elif obstacle.motion is MotionState.STATIONARY:
                self._stationary += 1
            else:
                self._unknown += 1
                if obstacle.motion_reason in self._reasons:
                    self._reasons[obstacle.motion_reason] += 1
        return result

    def _estimate_one(
        self,
        packet: SourcePacket,
        obstacle: TemporalObstacle,
    ) -> TemporalObstacle:
        now_ns = packet.envelope.timestamps.source_ns
        if obstacle.last_hit_ns > now_ns or obstacle.age_ns != now_ns - obstacle.last_hit_ns:
            raise MotionEstimatorError("temporal obstacle time escaped its source packet")
        if obstacle.state is not TemporalState.CURRENT:
            return _unknown(obstacle, "stale-support")
        if obstacle.association_basis == "map-frame-discontinuity":
            return _unknown(obstacle, "map-frame-discontinuity")
        history = obstacle.history
        times = tuple(sample.evidence_time_ns for sample in history)
        if any(right <= left for left, right in zip(times, times[1:], strict=False)):
            raise MotionEstimatorError("motion history is not strictly monotonic")
        if history[-1].evidence_time_ns != obstacle.last_hit_ns:
            raise MotionEstimatorError("motion history does not end at the current hit")
        if len(history) < self.config.minimum_observations:
            return _unknown(obstacle, "insufficient-history")
        span_seconds = (times[-1] - times[0]) / 1_000_000_000
        if span_seconds < self.config.minimum_span_seconds:
            return _unknown(obstacle, "insufficient-history")
        displacement_m = math.dist(
            history[0].centroid_xyz_m,
            history[-1].centroid_xyz_m,
        )
        speed_mps = displacement_m / span_seconds
        if speed_mps > self.config.maximum_speed_mps:
            return _unknown(obstacle, "implausible-speed")
        confidence = _evidence_confidence(
            self.config,
            observation_count=len(history),
            span_seconds=span_seconds,
        )
        if confidence < self.config.minimum_confidence:
            return _unknown(obstacle, "confidence-below-threshold")
        if (
            displacement_m >= self.config.moving_minimum_displacement_m
            and speed_mps >= self.config.moving_minimum_speed_mps
        ):
            return replace(
                obstacle,
                motion=MotionState.MOVING,
                motion_confidence=confidence,
                motion_reason="bounded-map-history-moving",
            )
        if (
            displacement_m <= self.config.stationary_maximum_displacement_m
            and speed_mps <= self.config.stationary_maximum_speed_mps
        ):
            return replace(
                obstacle,
                motion=MotionState.STATIONARY,
                motion_confidence=confidence,
                motion_reason="bounded-map-history-stationary",
            )
        return _unknown(obstacle, "threshold-deadband")

    def snapshot(self) -> MotionEstimatorSnapshot:
        return MotionEstimatorSnapshot(
            input_frames=self._input_frames,
            input_obstacles=self._input_obstacles,
            moving=self._moving,
            stationary=self._stationary,
            unknown=self._unknown,
            insufficient_history=self._reasons["insufficient-history"],
            stale_support=self._reasons["stale-support"],
            map_frame_discontinuity=self._reasons["map-frame-discontinuity"],
            confidence_below_threshold=self._reasons["confidence-below-threshold"],
            threshold_deadband=self._reasons["threshold-deadband"],
            implausible_speed=self._reasons["implausible-speed"],
        )


def _evidence_confidence(
    config: MotionEstimatorProfile,
    *,
    observation_count: int,
    span_seconds: float,
) -> float:
    """Return bounded evidence sufficiency, not a statistical class probability."""

    return round(
        min(
            1.0,
            observation_count / config.full_confidence_observations,
            span_seconds / config.full_confidence_span_seconds,
        ),
        12,
    )


def _unknown(obstacle: TemporalObstacle, reason: str) -> TemporalObstacle:
    return replace(
        obstacle,
        motion=MotionState.UNKNOWN,
        motion_confidence=0.0,
        motion_reason=reason,
    )


__all__ = [
    "ClassIndependentMotionEstimator",
    "MotionEstimatorError",
    "MotionEstimatorSnapshot",
]