"""Typed, non-retrying AGV command sink for cmvr-es."""

from __future__ import annotations

import logging
import math
from collections.abc import Mapping
from importlib import import_module
from time import time_ns
from typing import Any

from cmvr_edge_ai.contracts import ApprovedRobotCommand
from cmvr_edge_ai.core.component import ComponentContext, Sink
from cmvr_edge_ai.core.envelope import Envelope
from cmvr_edge_ai.transports.grpc import GrpcChannelPool

from .._context import endpoint_value, grpc_channel_settings, resolve_endpoint
from .errors import CmvrApiPackageError, CmvrServiceError


class CmvrAgvCommandSink(Sink):
    """Map an allowlisted ``RobotCommand`` to explicit AgvService methods.

    The sink deliberately has no generic method-name dispatch and performs no
    automatic retry.  Motion-command retry requires an idempotency contract
    that the current cmvr-es API does not provide.
    """

    _DIRECT_ACTIONS = {
        "emergency_stop": "emergencyStop",
        "clear_fault": "clearFault",
        "pause_navigation": "pauseNavigation",
        "resume_navigation": "resumeNavigation",
        "cancel_navigation": "cancelNavigation",
        "stop_velocity_control": "stopVelocityControl",
        "stop_mapping": "stopMapping",
    }

    def __init__(self, node_id: str, params: Mapping[str, Any]) -> None:
        self._node_id = node_id
        self._endpoint_id = str(params.get("endpoint", "cmvr_es"))
        configured_device = params.get("device_id")
        if not isinstance(configured_device, str) or not configured_device.strip():
            raise ValueError("AGV command sink requires a fixed, non-empty device_id")
        self._device_id = configured_device.strip()
        self._timeout_s: float | None = None
        self._unsafe_allow_unleased_velocity = _strict_bool(
            params,
            "unsafe_allow_unleased_velocity",
            default=False,
        )
        self._velocity_limits = _parse_velocity_limits(params.get("velocity_limits"))
        if self._unsafe_allow_unleased_velocity and self._velocity_limits is None:
            raise ValueError(
                "unsafe_allow_unleased_velocity requires explicit velocity_limits"
            )
        self._velocity_active = False
        self._logger = logging.getLogger(__name__)
        self._stub: Any = None
        self._common_pb2: Any = None
        self._agv_command_pb2: Any = None
        self._agv_pb2: Any = None

    async def setup(self, context: ComponentContext) -> None:
        endpoint = resolve_endpoint(context, self._endpoint_id)
        pool = context.metadata.get("grpc_channel_pool")
        if not isinstance(pool, GrpcChannelPool):
            raise TypeError("component context is missing GrpcChannelPool")
        channel = await pool.get(self._endpoint_id, grpc_channel_settings(endpoint))
        self._timeout_s = float(endpoint_value(endpoint, "timeout_s", 5.0))
        try:
            self._common_pb2 = import_module("cmvr.api.common_pb2")
            self._agv_command_pb2 = import_module("cmvr.api.agv_command_pb2")
            self._agv_pb2 = import_module("cmvr.msgs.agv_pb2")
            service = import_module("cmvr.api.agv_service_pb2_grpc")
        except ImportError as exc:
            raise CmvrApiPackageError(
                "cmvr Python API bindings are missing; run "
                "scripts/generate_cmvr_stubs.py or install the cmvr-api wheel"
            ) from exc
        self._stub = service.AgvServiceStub(channel)

    async def consume(
        self, envelope: Envelope[Any], input_port: str = "commands"
    ) -> None:
        del input_port
        approved = envelope.payload
        if not isinstance(approved, ApprovedRobotCommand):
            raise TypeError(
                f"{self._node_id} expected ApprovedRobotCommand, "
                f"got {type(approved).__name__}"
            )
        command = approved.command
        if envelope.is_expired() or command.valid_until_ns <= time_ns():
            return
        if command.device_id != self._device_id:
            raise PermissionError(
                f"command targets {command.device_id!r}, sink is bound to "
                f"{self._device_id!r}"
            )
        if self._stub is None:
            raise RuntimeError("AGV sink has not been set up")

        remaining_s = (command.valid_until_ns - time_ns()) / 1_000_000_000
        if remaining_s <= 0:
            return
        rpc_timeout = min(self._timeout_s or remaining_s, remaining_s)

        if command.action in self._DIRECT_ACTIONS:
            request = self._command_header()
            rpc = getattr(self._stub, self._DIRECT_ACTIONS[command.action])
        elif command.action == "set_velocity":
            if not self._unsafe_allow_unleased_velocity:
                raise PermissionError(
                    "set_velocity is disabled until cmvr-es enforces a server-side "
                    "lease; unsafe override must be explicit"
                )
            assert self._velocity_limits is not None
            vx = _bounded_number(command.arguments, "vx", self._velocity_limits)
            vy = _bounded_number(command.arguments, "vy", self._velocity_limits)
            wz = _bounded_number(command.arguments, "wz", self._velocity_limits)
            request = self._agv_command_pb2.AgvSetVelocityCommand.Request(
                header=self._command_header(),
                velocity=self._agv_pb2.AgvVelocity(vx=vx, vy=vy, wz=wz),
            )
            rpc = self._stub.setVelocity
        else:
            raise PermissionError(f"unsupported AGV action: {command.action}")

        response = await rpc(request, timeout=rpc_timeout)
        feedback = getattr(response, "header", response)
        if not feedback.success:
            raise CmvrServiceError(
                feedback.error_message
                or f"cmvr-es rejected AGV action {command.action!r}"
            )
        if command.action == "set_velocity":
            self._velocity_active = True
        elif command.action in {"stop_velocity_control", "emergency_stop"}:
            self._velocity_active = False

    async def stop(self) -> None:
        if not self._velocity_active or self._stub is None:
            return
        try:
            request = self._command_header()
            response = await self._stub.stopVelocityControl(
                request, timeout=self._timeout_s
            )
            feedback = getattr(response, "header", response)
            if not feedback.success:
                self._logger.error(
                    "cmvr-es rejected stopVelocityControl during sink shutdown: %s",
                    feedback.error_message,
                )
        except Exception:
            self._logger.exception(
                "failed to stop AGV velocity during sink shutdown; "
                "server-side lease is required for crash safety"
            )
        finally:
            self._velocity_active = False

    def _command_header(self) -> Any:
        header = self._common_pb2.CommandHeader.Request(device_id=self._device_id)
        # The current cmvr-es API has no valid-until/lease field.  Sending the
        # request time is useful for diagnostics but is not a server-side lease.
        header.timestamp.GetCurrentTime()
        return header


def _bounded_number(
    arguments: Mapping[str, Any],
    name: str,
    limits: Mapping[str, tuple[float, float]],
) -> float:
    if name not in arguments:
        raise ValueError(f"set_velocity requires argument {name!r}")
    value = float(arguments[name])
    if not math.isfinite(value):
        raise ValueError(f"set_velocity argument {name!r} must be finite")
    minimum, maximum = limits[name]
    if not minimum <= value <= maximum:
        raise PermissionError(
            f"set_velocity argument {name!r}={value!r} is outside "
            f"[{minimum}, {maximum}]"
        )
    return value


def _parse_velocity_limits(
    value: Any,
) -> dict[str, tuple[float, float]] | None:
    if value is None:
        return None
    if not isinstance(value, Mapping):
        raise ValueError("velocity_limits must be a mapping")
    missing = {"vx", "vy", "wz"} - set(value)
    if missing:
        raise ValueError(
            "velocity_limits must define vx, vy and wz; missing: "
            + ", ".join(sorted(missing))
        )
    parsed: dict[str, tuple[float, float]] = {}
    for name in ("vx", "vy", "wz"):
        bounds = value[name]
        if (
            not isinstance(bounds, Mapping)
            or "min" not in bounds
            or "max" not in bounds
        ):
            raise ValueError(f"velocity_limits.{name} requires min and max")
        minimum = float(bounds["min"])
        maximum = float(bounds["max"])
        if not math.isfinite(minimum) or not math.isfinite(maximum):
            raise ValueError("velocity limits must be finite")
        if minimum > maximum:
            raise ValueError(f"velocity_limits.{name} min exceeds max")
        parsed[name] = (minimum, maximum)
    return parsed


def _strict_bool(params: Mapping[str, Any], name: str, *, default: bool) -> bool:
    value = params.get(name, default)
    if type(value) is not bool:
        raise ValueError(f"{name} must be a boolean, not {value!r}")
    return value


__all__ = ["CmvrAgvCommandSink"]
