gps-denied-onboard/src/gps_denied_onboard/runtime_root/pose_factory.py

"""Composition-root factory for C4 pose estimator (AZ-355 / E-C4).

Mirrors :mod:`gps_denied_onboard.runtime_root.state_factory` — per
ADR-001 the composition root is the single registration site for
all strategy factories. C4 has exactly ONE concrete strategy
(``opencv_gtsam`` → AZ-358); the Protocol exists for ADR-009
(interface-first DI) so callers don't import the concrete class.

The runtime root constructs the dependencies (RansacFilter,
WgsConverter, SE3Utils, ISam2GraphHandle) ONCE and passes references
through this factory. The factory does NOT instantiate them.

Per the C4 contract Invariant 1 + AC-9: the C4 estimator is bound
to the SAME ingest thread as C5 (ADR-003 shared GTSAM substrate is
not thread-safe). The thread-binding helper is shared with the C5
state factory (``bind_state_ingest_thread``); a second binding
from a different thread raises
:class:`StateIngestThreadAlreadyBoundError`.
"""

from __future__ import annotations

import importlib
from collections.abc import Callable
from typing import TYPE_CHECKING, Any, Final

from gps_denied_onboard.components.c4_pose._isam2_handle import ISam2GraphHandle
from gps_denied_onboard.components.c4_pose.config import (
    KNOWN_POSE_STRATEGIES,
    C4PoseConfig,
)
from gps_denied_onboard.components.c4_pose.errors import PoseEstimatorConfigError
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
from gps_denied_onboard.logging import get_logger

if TYPE_CHECKING:
    from gps_denied_onboard.config import Config

__all__ = [
    "PoseEstimatorFactory",
    "build_pose_estimator",
    "clear_pose_registry",
    "list_registered_pose_strategies",
    "register_pose_estimator",
]


PoseEstimatorFactory = Callable[..., PoseEstimator]

_POSE_REGISTRY: dict[str, PoseEstimatorFactory] = {}

# Single concrete strategy (ADR-001); module path for lazy-import
# fallback when the test/binary did not pre-register.
_STRATEGY_MODULE_PATHS: Final[dict[str, str]] = {
    "opencv_gtsam": "gps_denied_onboard.components.c4_pose.opencv_gtsam_estimator",
}


def register_pose_estimator(strategy: str, factory: PoseEstimatorFactory) -> None:
    """Register a concrete ``PoseEstimator`` strategy.

    Duplicate registration with a different factory raises
    :class:`PoseEstimatorConfigError`.
    """
    existing = _POSE_REGISTRY.get(strategy)
    if existing is not None and existing is not factory:
        raise PoseEstimatorConfigError(
            f"duplicate PoseEstimator registration for strategy {strategy!r}"
        )
    _POSE_REGISTRY[strategy] = factory


def clear_pose_registry() -> None:
    """Reset the pose registry; unit-test isolation only."""
    _POSE_REGISTRY.clear()


def list_registered_pose_strategies() -> list[str]:
    return sorted(_POSE_REGISTRY)


def build_pose_estimator(
    config: Config,
    *,
    ransac_filter: Any,
    wgs_converter: Any,
    se3_utils: Any,
    isam2_graph_handle: ISam2GraphHandle,
    fdr_client: Any | None = None,
    clock: Any | None = None,
) -> PoseEstimator:
    """Resolve + build the configured C4 pose estimator.

    Validation order: config block lookup → strategy known? →
    isam2_graph_handle conforms? → factory lookup (with lazy-import
    fallback) → INFO log on success.

    ``fdr_client`` and ``clock`` are AZ-358-introduced optional
    dependencies. When omitted (e.g. AZ-355 protocol-only tests that
    register a fake factory) the runtime root passes ``None`` and
    the concrete strategy decides how to handle it (the
    ``opencv_gtsam`` impl no-ops FDR enqueues + falls back to
    :class:`MonotonicClock` for rate-limiting).

    Raises:
        PoseEstimatorConfigError: invalid config, unknown strategy,
            non-conforming graph handle, or registry miss after
            lazy-import fallback.
    """
    block = _read_pose_block(config)
    strategy = block.strategy
    log = get_logger("runtime_root.pose_factory")
    if strategy not in KNOWN_POSE_STRATEGIES:
        log.error(
            "c4.pose.unknown_strategy",
            extra={
                "kind": "c4.pose.unknown_strategy",
                "kv": {
                    "strategy": strategy,
                    "known": sorted(KNOWN_POSE_STRATEGIES),
                },
            },
        )
        raise PoseEstimatorConfigError(
            f"C4PoseConfig.strategy={strategy!r} not in {sorted(KNOWN_POSE_STRATEGIES)}"
        )
    if not isinstance(isam2_graph_handle, ISam2GraphHandle):
        raise PoseEstimatorConfigError(
            "build_pose_estimator: isam2_graph_handle does not satisfy "
            "the C4 ISam2GraphHandle Protocol (missing get_pose_key / "
            "update / compute_marginals / last_anchor_age_ms?)"
        )

    factory = _resolve_factory(strategy)
    estimator = factory(
        config=config,
        ransac_filter=ransac_filter,
        wgs_converter=wgs_converter,
        se3_utils=se3_utils,
        isam2_graph_handle=isam2_graph_handle,
        fdr_client=fdr_client,
        clock=clock,
    )
    log.info(
        f"c4.pose.strategy_loaded: strategy={strategy} "
        f"ransac_iterations={block.ransac_iterations} "
        f"ransac_reprojection_threshold_px={block.ransac_reprojection_threshold_px}",
        extra={
            "kind": "c4.pose.strategy_loaded",
            "kv": {
                "strategy": strategy,
                "ransac_iterations": block.ransac_iterations,
                "ransac_reprojection_threshold_px": block.ransac_reprojection_threshold_px,
                "thermal_throttle_threshold_celsius": (block.thermal_throttle_threshold_celsius),
            },
        },
    )
    return estimator


def _read_pose_block(config: Config) -> C4PoseConfig:
    components = getattr(config, "components", None) or {}
    block = components.get("c4_pose") if isinstance(components, dict) else None
    if block is None:
        return C4PoseConfig()
    if isinstance(block, C4PoseConfig):
        return block
    raise PoseEstimatorConfigError(
        f"config.components['c4_pose'] must be a C4PoseConfig; got {type(block).__name__}"
    )


def _resolve_factory(strategy: str) -> PoseEstimatorFactory:
    factory = _POSE_REGISTRY.get(strategy)
    if factory is not None:
        return factory
    module_path = _STRATEGY_MODULE_PATHS.get(strategy)
    if module_path is None:
        raise PoseEstimatorConfigError(
            f"pose strategy {strategy!r} has no module-path mapping for lazy import"
        )
    try:
        module = importlib.import_module(module_path)
    except ImportError as exc:
        raise PoseEstimatorConfigError(
            f"pose strategy {strategy!r} module {module_path!r} not importable: {exc}"
        ) from exc
    factory_obj = getattr(module, "create", None)
    if factory_obj is None or not callable(factory_obj):
        raise PoseEstimatorConfigError(
            f"pose strategy {strategy!r} module {module_path!r} has no create(...) factory"
        )
    return factory_obj