"""Valve detection, tracking, calibration, and robot-target safety gates."""

from __future__ import annotations

import math
from dataclasses import dataclass
from typing import (
    Any,
    Callable,
    Dict,
    Iterable,
    List,
    Mapping,
    Optional,
    Protocol,
    Sequence,
    Tuple,
)

from .cameras import (
    CameraFrame,
    CameraRole,
    OptionalCameraDependencyError,
)

Point2D = Tuple[float, float]
Point3D = Tuple[float, float, float]


@dataclass(frozen=True)
class BoundingBox:
    x_min: float
    y_min: float
    x_max: float
    y_max: float

    def __post_init__(self) -> None:
        if self.x_max <= self.x_min or self.y_max <= self.y_min:
            raise ValueError("bounding box must have positive area")

    @property
    def center(self) -> Point2D:
        return (
            (self.x_min + self.x_max) / 2.0,
            (self.y_min + self.y_max) / 2.0,
        )


@dataclass(frozen=True)
class ValveDetection:
    """One valve candidate in image coordinates."""

    camera_role: CameraRole
    center_xy: Point2D
    bbox: BoundingBox
    confidence: float
    label: str = "valve"
    orientation_deg: Optional[float] = None

    def __post_init__(self) -> None:
        if not 0.0 <= self.confidence <= 1.0:
            raise ValueError("confidence must be between 0 and 1")
        if not all(math.isfinite(value) for value in self.center_xy):
            raise ValueError("center coordinates must be finite")


class ValveDetector(Protocol):
    def detect(self, frame: CameraFrame) -> Sequence[ValveDetection]:
        """Detect valves in a camera frame."""


class ModelValveDetector:
    """Adapter for a model callable that returns mapping-like detections.

    The callable may wrap YOLO, TensorFlow Lite, ONNX Runtime, or a remote
    inference service.  Keeping model loading outside this adapter allows tests
    and Raspberry Pi deployments to choose an appropriate runtime.
    """

    def __init__(
        self,
        predict: Callable[[Any], Iterable[Mapping[str, Any]]],
        *,
        confidence_threshold: float = 0.5,
        allowed_labels: Sequence[str] = ("valve",),
    ) -> None:
        if not 0.0 <= confidence_threshold <= 1.0:
            raise ValueError("confidence_threshold must be between 0 and 1")
        self.predict = predict
        self.confidence_threshold = confidence_threshold
        self.allowed_labels = frozenset(label.lower() for label in allowed_labels)

    def detect(self, frame: CameraFrame) -> Sequence[ValveDetection]:
        detections: List[ValveDetection] = []
        for raw in self.predict(frame.image):
            try:
                label = str(raw.get("label", "valve"))
                confidence = float(raw["confidence"])
                if (
                    label.lower() not in self.allowed_labels
                    or confidence < self.confidence_threshold
                ):
                    continue
                bbox = _parse_bbox(raw)
                center = raw.get("center_xy")
                center_xy = (
                    (float(center[0]), float(center[1]))
                    if center is not None
                    else bbox.center
                )
                orientation = raw.get("orientation_deg")
                detections.append(
                    ValveDetection(
                        camera_role=frame.role,
                        center_xy=center_xy,
                        bbox=bbox,
                        confidence=confidence,
                        label=label,
                        orientation_deg=(
                            None if orientation is None else float(orientation)
                        ),
                    )
                )
            except (KeyError, TypeError, ValueError, IndexError):
                # A malformed model row must not take down the control loop.
                continue
        return detections


class RemoteValveDetector:
    """Adapt correlated API-Gateway/ShortcutJob inference to valve detections."""

    def __init__(
        self,
        infer: Callable[[CameraFrame], Iterable[Mapping[str, Any]]],
        *,
        confidence_threshold: float = 0.5,
        allowed_labels: Sequence[str] = ("valve",),
    ) -> None:
        self.infer = infer
        self.confidence_threshold = float(confidence_threshold)
        self.allowed_labels = tuple(allowed_labels)

    def detect(self, frame: CameraFrame) -> Sequence[ValveDetection]:
        rows = tuple(self.infer(frame))
        return ModelValveDetector(
            lambda _image: rows,
            confidence_threshold=self.confidence_threshold,
            allowed_labels=self.allowed_labels,
        ).detect(frame)


def _parse_bbox(raw: Mapping[str, Any]) -> BoundingBox:
    if "bbox" in raw:
        values = raw["bbox"]
        return BoundingBox(*(float(values[index]) for index in range(4)))
    return BoundingBox(
        float(raw["x_min"]),
        float(raw["y_min"]),
        float(raw["x_max"]),
        float(raw["y_max"]),
    )


class CircleValveDetector:
    """Configurable circular-valve fallback using OpenCV Hough detection.

    A trained detector is preferable where handles are non-circular or
    frequently occluded.  This implementation is useful for commissioning and
    as a model-independent fallback.
    """

    def __init__(
        self,
        *,
        min_radius_px: int = 12,
        max_radius_px: int = 180,
        min_distance_px: int = 40,
        hough_threshold: float = 30.0,
        confidence: float = 0.55,
    ) -> None:
        if min_radius_px <= 0 or max_radius_px < min_radius_px:
            raise ValueError("invalid valve radius limits")
        if not 0.0 <= confidence <= 1.0:
            raise ValueError("confidence must be between 0 and 1")
        self.min_radius_px = min_radius_px
        self.max_radius_px = max_radius_px
        self.min_distance_px = min_distance_px
        self.hough_threshold = hough_threshold
        self.confidence = confidence

    def detect(self, frame: CameraFrame) -> Sequence[ValveDetection]:
        try:
            import cv2
        except ImportError as exc:  # pragma: no cover - deployment only
            raise OptionalCameraDependencyError(
                "OpenCV is required for CircleValveDetector"
            ) from exc

        gray = cv2.cvtColor(frame.image, cv2.COLOR_BGR2GRAY)
        gray = cv2.medianBlur(gray, 5)
        circles = cv2.HoughCircles(
            gray,
            cv2.HOUGH_GRADIENT,
            dp=1.2,
            minDist=self.min_distance_px,
            param1=100,
            param2=self.hough_threshold,
            minRadius=self.min_radius_px,
            maxRadius=self.max_radius_px,
        )
        if circles is None:
            return []

        height, width = frame.image.shape[:2]
        output: List[ValveDetection] = []
        for x, y, radius in circles[0]:
            radius = float(radius)
            bbox = BoundingBox(
                max(0.0, float(x) - radius),
                max(0.0, float(y) - radius),
                min(float(width), float(x) + radius),
                min(float(height), float(y) + radius),
            )
            output.append(
                ValveDetection(
                    camera_role=frame.role,
                    center_xy=(float(x), float(y)),
                    bbox=bbox,
                    confidence=self.confidence,
                    label="valve-circle",
                )
            )
        return output


@dataclass(frozen=True)
class TrackedValve:
    track_id: str
    camera_role: CameraRole
    center_xy: Point2D
    bbox: BoundingBox
    confidence: float
    velocity_px_s: Point2D
    orientation_deg: Optional[float]
    hits: int
    missed_frames: int
    captured_at_s: float
    confirmed: bool

    @property
    def speed_px_s(self) -> float:
        return math.hypot(*self.velocity_px_s)


@dataclass
class _MutableTrack:
    track_id: str
    detection: ValveDetection
    velocity_px_s: Point2D
    hits: int
    missed_frames: int
    captured_at_s: float


class ValveTracker:
    """Small multi-object tracker with prediction and deterministic matching."""

    def __init__(
        self,
        camera_role: CameraRole,
        *,
        max_match_distance_px: float = 80.0,
        max_missed_frames: int = 4,
        min_confirmed_hits: int = 3,
        velocity_smoothing: float = 0.4,
    ) -> None:
        if max_match_distance_px <= 0:
            raise ValueError("max_match_distance_px must be positive")
        if max_missed_frames < 0:
            raise ValueError("max_missed_frames must be non-negative")
        if min_confirmed_hits < 1:
            raise ValueError("min_confirmed_hits must be positive")
        if not 0.0 <= velocity_smoothing <= 1.0:
            raise ValueError("velocity_smoothing must be between 0 and 1")
        self.camera_role = CameraRole(camera_role)
        self.max_match_distance_px = max_match_distance_px
        self.max_missed_frames = max_missed_frames
        self.min_confirmed_hits = min_confirmed_hits
        self.velocity_smoothing = velocity_smoothing
        self._tracks: Dict[str, _MutableTrack] = {}
        self._next_id = 1

    def update(
        self,
        detections: Sequence[ValveDetection],
        *,
        captured_at_s: float,
    ) -> Sequence[TrackedValve]:
        """Update tracks and return current matched and temporarily lost tracks."""

        if not math.isfinite(captured_at_s):
            raise ValueError("captured_at_s must be finite")
        valid = [
            detection
            for detection in detections
            if detection.camera_role == self.camera_role
        ]

        candidates: List[Tuple[float, str, int]] = []
        for track_id, track in self._tracks.items():
            dt = max(0.0, captured_at_s - track.captured_at_s)
            predicted = (
                track.detection.center_xy[0] + track.velocity_px_s[0] * dt,
                track.detection.center_xy[1] + track.velocity_px_s[1] * dt,
            )
            for index, detection in enumerate(valid):
                distance = math.hypot(
                    predicted[0] - detection.center_xy[0],
                    predicted[1] - detection.center_xy[1],
                )
                if distance <= self.max_match_distance_px:
                    candidates.append((distance, track_id, index))

        matched_tracks = set()
        matched_detections = set()
        for _distance, track_id, index in sorted(candidates):
            if track_id in matched_tracks or index in matched_detections:
                continue
            track = self._tracks[track_id]
            detection = valid[index]
            dt = captured_at_s - track.captured_at_s
            if dt > 1e-6:
                measured_velocity = (
                    (detection.center_xy[0] - track.detection.center_xy[0]) / dt,
                    (detection.center_xy[1] - track.detection.center_xy[1]) / dt,
                )
                alpha = self.velocity_smoothing
                track.velocity_px_s = (
                    alpha * measured_velocity[0]
                    + (1.0 - alpha) * track.velocity_px_s[0],
                    alpha * measured_velocity[1]
                    + (1.0 - alpha) * track.velocity_px_s[1],
                )
            track.detection = detection
            track.hits += 1
            track.missed_frames = 0
            track.captured_at_s = captured_at_s
            matched_tracks.add(track_id)
            matched_detections.add(index)

        for track_id, track in list(self._tracks.items()):
            if track_id not in matched_tracks:
                track.missed_frames += 1
                if track.missed_frames > self.max_missed_frames:
                    del self._tracks[track_id]

        for index, detection in enumerate(valid):
            if index in matched_detections:
                continue
            track_id = "{}-valve-{}".format(
                self.camera_role.value, self._next_id
            )
            self._next_id += 1
            self._tracks[track_id] = _MutableTrack(
                track_id=track_id,
                detection=detection,
                velocity_px_s=(0.0, 0.0),
                hits=1,
                missed_frames=0,
                captured_at_s=captured_at_s,
            )

        return self.tracks()

    def tracks(self) -> Sequence[TrackedValve]:
        return [
            TrackedValve(
                track_id=track.track_id,
                camera_role=self.camera_role,
                center_xy=track.detection.center_xy,
                bbox=track.detection.bbox,
                confidence=track.detection.confidence,
                velocity_px_s=track.velocity_px_s,
                orientation_deg=track.detection.orientation_deg,
                hits=track.hits,
                missed_frames=track.missed_frames,
                captured_at_s=track.captured_at_s,
                confirmed=track.hits >= self.min_confirmed_hits,
            )
            for track in sorted(self._tracks.values(), key=lambda item: item.track_id)
        ]


class ValveTrackingService:
    """Run role-specific detection and tracking without coupling to a robot."""

    def __init__(
        self,
        detectors: Mapping[CameraRole, ValveDetector],
        trackers: Optional[Mapping[CameraRole, ValveTracker]] = None,
    ) -> None:
        self._detectors = {
            CameraRole(role): detector for role, detector in detectors.items()
        }
        self._trackers = {
            role: (
                trackers[role]
                if trackers is not None and role in trackers
                else ValveTracker(role)
            )
            for role in self._detectors
        }

    def process(self, frame: CameraFrame) -> Sequence[TrackedValve]:
        try:
            detector = self._detectors[frame.role]
            tracker = self._trackers[frame.role]
        except KeyError as exc:
            raise ValueError(
                "no detector configured for {} camera".format(frame.role.value)
            ) from exc
        detections = detector.detect(frame)
        return tracker.update(detections, captured_at_s=frame.captured_at_s)

    def active_tracks(self) -> Sequence[TrackedValve]:
        tracks: List[TrackedValve] = []
        for tracker in self._trackers.values():
            tracks.extend(tracker.tracks())
        return tracks


class WorkspaceMapper(Protocol):
    """Calibration boundary between pixels and robot workspace coordinates."""

    def to_workspace(self, track: TrackedValve) -> Point3D:
        """Map a track to the robot's configured workspace frame."""


class PlanarHomographyMapper:
    """Map static-camera pixels onto a planar robot workspace."""

    def __init__(
        self,
        homographies: Mapping[CameraRole, Sequence[Sequence[float]]],
        *,
        workspace_z_mm: float,
    ) -> None:
        self._homographies: Dict[
            CameraRole, Tuple[Tuple[float, float, float], ...]
        ] = {}
        for role, matrix in homographies.items():
            if len(matrix) != 3 or any(len(row) != 3 for row in matrix):
                raise ValueError("homography must be a 3x3 matrix")
            converted = tuple(
                tuple(float(value) for value in row) for row in matrix
            )
            if not all(
                math.isfinite(value) for row in converted for value in row
            ):
                raise ValueError("homography values must be finite")
            self._homographies[CameraRole(role)] = converted
        self.workspace_z_mm = float(workspace_z_mm)

    def to_workspace(self, track: TrackedValve) -> Point3D:
        try:
            matrix = self._homographies[track.camera_role]
        except KeyError as exc:
            raise ValueError(
                "camera {} has no workspace calibration".format(
                    track.camera_role.value
                )
            ) from exc
        x, y = track.center_xy
        scale = matrix[2][0] * x + matrix[2][1] * y + matrix[2][2]
        if abs(scale) < 1e-9:
            raise ValueError("homography maps point to infinity")
        workspace_x = (
            matrix[0][0] * x + matrix[0][1] * y + matrix[0][2]
        ) / scale
        workspace_y = (
            matrix[1][0] * x + matrix[1][1] * y + matrix[1][2]
        ) / scale
        return (workspace_x, workspace_y, self.workspace_z_mm)


@dataclass(frozen=True)
class ValveTargetPolicy:
    """Conditions a visual track must satisfy before motion planning sees it."""

    min_confidence: float = 0.70
    min_confirmed_hits: int = 3
    max_age_s: float = 0.30
    max_missed_frames: int = 0
    max_speed_px_s: float = 120.0
    allowed_roles: Tuple[CameraRole, ...] = (CameraRole.WORKSPACE,)
    x_limits_mm: Tuple[float, float] = (-500.0, 500.0)
    y_limits_mm: Tuple[float, float] = (-500.0, 500.0)
    z_limits_mm: Tuple[float, float] = (0.0, 500.0)

    def __post_init__(self) -> None:
        if not 0.0 <= self.min_confidence <= 1.0:
            raise ValueError("min_confidence must be between 0 and 1")
        if self.min_confirmed_hits < 1:
            raise ValueError("min_confirmed_hits must be positive")
        if self.max_age_s < 0 or self.max_speed_px_s < 0:
            raise ValueError("age and speed limits must be non-negative")
        for lower, upper in (
            self.x_limits_mm,
            self.y_limits_mm,
            self.z_limits_mm,
        ):
            if lower > upper:
                raise ValueError("workspace limit lower bound exceeds upper bound")


@dataclass(frozen=True)
class SafeValveTarget:
    """Calibrated target that has passed the visual safety policy."""

    track_id: str
    camera_role: CameraRole
    workspace_xyz_mm: Point3D
    image_xy: Point2D
    confidence: float
    orientation_deg: Optional[float]
    observed_at_s: float


@dataclass(frozen=True)
class TargetDecision:
    accepted: bool
    target: Optional[SafeValveTarget]
    reasons: Tuple[str, ...]


class ValveTargetGate:
    """Fail-closed conversion from a visual track to a motion-planning input."""

    def __init__(
        self,
        mapper: WorkspaceMapper,
        policy: ValveTargetPolicy = ValveTargetPolicy(),
    ) -> None:
        self.mapper = mapper
        self.policy = policy

    def evaluate(self, track: TrackedValve, *, now_s: float) -> TargetDecision:
        reasons: List[str] = []
        if track.camera_role not in self.policy.allowed_roles:
            reasons.append("camera role is not approved for targeting")
        if track.camera_role == CameraRole.TOOL:
            readiness_check = getattr(self.mapper, "is_ready", None)
            if not callable(readiness_check):
                reasons.append(
                    "tool-camera mapper has no eye-in-hand calibration readiness"
                )
            else:
                try:
                    if not bool(readiness_check()):
                        reasons.append("eye-in-hand calibration is not ready")
                except Exception:
                    reasons.append(
                        "eye-in-hand calibration readiness could not be verified"
                    )
        if not track.confirmed or track.hits < self.policy.min_confirmed_hits:
            reasons.append("track is not confirmed")
        if track.confidence < self.policy.min_confidence:
            reasons.append("confidence is below threshold")
        age = now_s - track.captured_at_s
        if age < 0:
            reasons.append("track timestamp is in the future")
        elif age > self.policy.max_age_s:
            reasons.append("track is stale")
        if track.missed_frames > self.policy.max_missed_frames:
            reasons.append("track was not visible in the latest frame")
        if track.speed_px_s > self.policy.max_speed_px_s:
            reasons.append("track image velocity exceeds limit")

        workspace_xyz: Optional[Point3D] = None
        if not reasons:
            try:
                workspace_xyz = self.mapper.to_workspace(track)
                if not all(math.isfinite(value) for value in workspace_xyz):
                    reasons.append("calibration produced non-finite coordinates")
                elif not _inside_workspace(workspace_xyz, self.policy):
                    reasons.append("target is outside configured workspace")
            except Exception:
                reasons.append("camera calibration could not map target")

        if reasons or workspace_xyz is None:
            return TargetDecision(False, None, tuple(reasons))
        return TargetDecision(
            True,
            SafeValveTarget(
                track_id=track.track_id,
                camera_role=track.camera_role,
                workspace_xyz_mm=workspace_xyz,
                image_xy=track.center_xy,
                confidence=track.confidence,
                orientation_deg=track.orientation_deg,
                observed_at_s=track.captured_at_s,
            ),
            (),
        )

    def choose(
        self, tracks: Iterable[TrackedValve], *, now_s: float
    ) -> TargetDecision:
        """Choose the strongest safe target, or return all rejection reasons."""

        accepted: List[SafeValveTarget] = []
        rejected: List[str] = []
        for track in tracks:
            decision = self.evaluate(track, now_s=now_s)
            if decision.target is not None:
                accepted.append(decision.target)
            else:
                rejected.extend(
                    "{}: {}".format(track.track_id, reason)
                    for reason in decision.reasons
                )
        if not accepted:
            return TargetDecision(
                False,
                None,
                tuple(rejected) or ("no valve tracks available",),
            )
        accepted.sort(
            key=lambda target: (
                -target.confidence,
                -target.observed_at_s,
                target.track_id,
            )
        )
        return TargetDecision(True, accepted[0], ())


def _inside_workspace(
    point: Point3D, policy: ValveTargetPolicy
) -> bool:
    return (
        policy.x_limits_mm[0] <= point[0] <= policy.x_limits_mm[1]
        and policy.y_limits_mm[0] <= point[1] <= policy.y_limits_mm[1]
        and policy.z_limits_mm[0] <= point[2] <= policy.z_limits_mm[1]
    )
