"""Offline xArm planning compatibility and retired execution shims.

Historical versions of this module mixed useful, deterministic route-planning
helpers with raw xArm SDK commands that cleared faults, enabled motors and
moved a robot directly. Production motion now belongs exclusively to the
commissioned controller in :mod:`main.integrations.workcell`.

The offline data types and planning helpers remain available. Every function
that could capture, upload, instantiate an SDK client, actuate a gripper, or
move the robot raises the same deterministic error before any I/O. There is no
environment-variable escape hatch and no raw motion command body in this file.
"""

from __future__ import annotations

import json
from dataclasses import dataclass, field
from types import SimpleNamespace
from typing import Any, Callable, List, Optional, Sequence, Tuple, Union

import numpy as np
from scipy.interpolate import CubicSpline

# Compatibility names for callers that used to probe optional integrations.
# Importing this retired module no longer imports a camera stack, upload client,
# Omniverse, or the xArm SDK.
DigitalTwin = None
OmniverseSimulation = None
digital_twin = None
omniverse_simulation = None
XArmAPI = None

LEGACY_XARM_MOTION_DISABLED_MESSAGE = (
    "Legacy direct xArm motion is retired: use the commissioned "
    "WorkcellServices motion controller"
)


class LegacyXArmMotionDisabledError(RuntimeError):
    """Raised before a retired helper can perform I/O or command an xArm."""


def _legacy_motion_disabled(*_args, **_kwargs):
    raise LegacyXArmMotionDisabledError(LEGACY_XARM_MOTION_DISABLED_MESSAGE)


# Retain former dependency names for import/monkeypatch compatibility. They
# are failure shims, not adapters to the old camera or upload implementations.
capture_image = _legacy_motion_disabled
find_petri_dish_centers = _legacy_motion_disabled
send_to_supraworx = _legacy_motion_disabled


@dataclass
class DotTarget:
    """Representation of one dot in an offline route preview."""

    pixel_xy: Tuple[float, float]
    robot_xy: Tuple[float, float]
    radius: Optional[float] = None
    raw_detection: Optional[dict] = None


@dataclass
class PetriDishTarget:
    """Prepared Petri-dish target for an offline route preview."""

    dish_index: int
    pixel_xy: Tuple[float, float]
    robot_xy: Tuple[float, float]
    capture_z: float
    approach_z: float
    radius: Optional[float] = None
    raw_detection: Optional[dict] = None
    dot_targets: List[DotTarget] = field(default_factory=list)


def _load_xarm_ip(default: str = "192.168.1.153") -> str:
    """Return the historical default without reading deployment files."""

    return default


def _create_arm(arm_ip=None):
    """Reject SDK client creation through the retired helper module."""

    _legacy_motion_disabled(arm_ip)


def pixel_to_robot_coords(
    x,
    y,
    img_width: int = 1920,
    img_height: int = 1080,
):
    """Convert pixels for an approximate, non-executable route preview."""

    x_ratio = (x / img_width) - 0.5
    y_ratio = (y / img_height) - 0.5
    return x_ratio * 200, -y_ratio * 200


def _calibrated_workspace_point(
    mapper: Any,
    *,
    pixel_xy: Sequence[float],
    captured_at_s: Optional[float],
) -> Tuple[float, float, float]:
    """Map one preview point through an approved, synchronized mapper."""

    readiness = getattr(mapper, "is_ready", None)
    if not callable(readiness) or readiness() is not True:
        raise RuntimeError(
            "Live image-driven motion requires an approved eye-in-hand calibration"
        )
    if captured_at_s is None:
        raise RuntimeError(
            "Live image-driven motion requires the source frame timestamp"
        )
    timestamp = float(captured_at_s)
    if not np.isfinite(timestamp):
        raise RuntimeError("The source frame timestamp must be finite")
    track = SimpleNamespace(
        camera_role="tool",
        center_xy=(float(pixel_xy[0]), float(pixel_xy[1])),
        captured_at_s=timestamp,
    )
    try:
        point = tuple(float(value) for value in mapper.to_workspace(track))
    except Exception as exc:
        raise RuntimeError(
            "Calibrated image-to-workspace mapping was rejected"
        ) from exc
    if len(point) != 3 or not all(np.isfinite(value) for value in point):
        raise RuntimeError(
            "Calibrated image-to-workspace mapping returned invalid coordinates"
        )
    return point


def prepare_robot_targets(
    dish_detections: Sequence[dict],
    *,
    capture_height: float = 110.0,
    safety_offset: float = 40.0,
    min_approach_height: float = 150.0,
    image_size: Tuple[int, int] = (1920, 1080),
    workspace_mapper: Any = None,
    captured_at_s: Optional[float] = None,
) -> List[PetriDishTarget]:
    """Prepare deterministic targets for visualization or offline review."""

    targets: List[PetriDishTarget] = []
    default_width, default_height = image_size
    for index, detection in enumerate(dish_detections):
        if detection is None:
            continue
        if "dish_center" in detection:
            x_px, y_px = detection["dish_center"]
        else:
            try:
                x_px = detection["x"]
                y_px = detection["y"]
            except KeyError as exc:
                raise KeyError(
                    "Petri dish detections must include either 'dish_center' "
                    "or 'x'/'y' keys."
                ) from exc

        image_width = int(detection.get("image_width", default_width))
        image_height = int(detection.get("image_height", default_height))
        if workspace_mapper is None:
            robot_x, robot_y = pixel_to_robot_coords(
                float(x_px),
                float(y_px),
                img_width=image_width,
                img_height=image_height,
            )
            capture_z = float(detection.get("z", capture_height))
        else:
            robot_x, robot_y, mapped_z = _calibrated_workspace_point(
                workspace_mapper,
                pixel_xy=(float(x_px), float(y_px)),
                captured_at_s=captured_at_s,
            )
            capture_z = mapped_z + float(detection.get("z_offset", 0.0))

        approach_default = capture_z + safety_offset
        approach_z = max(
            float(detection.get("approach_z", approach_default)),
            min_approach_height,
        )
        radius = detection.get("radius")
        dot_targets: List[DotTarget] = []
        for dot in detection.get("dot_centers", []) or []:
            try:
                dot_x = float(dot["x"])
                dot_y = float(dot["y"])
            except (KeyError, TypeError, ValueError):
                continue
            if workspace_mapper is None:
                dot_robot_x, dot_robot_y = pixel_to_robot_coords(
                    dot_x,
                    dot_y,
                    img_width=image_width,
                    img_height=image_height,
                )
            else:
                dot_robot_x, dot_robot_y, _ = _calibrated_workspace_point(
                    workspace_mapper,
                    pixel_xy=(dot_x, dot_y),
                    captured_at_s=captured_at_s,
                )
            dot_radius = dot.get("radius")
            dot_targets.append(
                DotTarget(
                    pixel_xy=(dot_x, dot_y),
                    robot_xy=(float(dot_robot_x), float(dot_robot_y)),
                    radius=(float(dot_radius) if dot_radius is not None else None),
                    raw_detection=dict(dot),
                )
            )

        targets.append(
            PetriDishTarget(
                dish_index=index,
                pixel_xy=(float(x_px), float(y_px)),
                robot_xy=(float(robot_x), float(robot_y)),
                capture_z=capture_z,
                approach_z=approach_z,
                radius=float(radius) if radius is not None else None,
                raw_detection=detection,
                dot_targets=dot_targets,
            )
        )
    return targets


def _nearest_neighbor_order(
    coords: np.ndarray,
    start_idx: int,
) -> List[int]:
    remaining = set(range(len(coords)))
    order: List[int] = []
    current = start_idx
    while remaining:
        order.append(current)
        remaining.remove(current)
        if not remaining:
            break
        current = min(
            remaining,
            key=lambda idx: float(np.linalg.norm(coords[idx] - coords[current])),
        )
    return order


def _total_path_length(
    order: Sequence[int],
    coords: np.ndarray,
    start: np.ndarray,
) -> float:
    if not order:
        return 0.0
    total = float(np.linalg.norm(coords[order[0]] - start))
    for first, second in zip(order, order[1:]):
        total += float(np.linalg.norm(coords[second] - coords[first]))
    return total


def _two_opt_order(
    order: List[int],
    coords: np.ndarray,
    start: np.ndarray,
) -> List[int]:
    if len(order) < 3:
        return order
    best = order[:]
    best_distance = _total_path_length(best, coords, start)
    improved = True
    while improved:
        improved = False
        for first in range(1, len(best) - 2):
            for second in range(first + 1, len(best)):
                if second - first == 1:
                    continue
                candidate = best[:first] + best[first:second][::-1] + best[second:]
                distance = _total_path_length(candidate, coords, start)
                if distance + 1e-9 < best_distance:
                    best = candidate
                    best_distance = distance
                    improved = True
                    break
            if improved:
                break
    return best


def optimize_target_order(
    targets: Sequence[PetriDishTarget],
    *,
    start_xy: Tuple[float, float] = (0.0, 0.0),
) -> List[PetriDishTarget]:
    """Order preview targets for minimal travel distance."""

    if not targets:
        return []
    coords = np.array([target.robot_xy for target in targets], dtype=float)
    start = np.array(start_xy, dtype=float)
    start_index = int(np.argmin([np.linalg.norm(coord - start) for coord in coords]))
    order = _nearest_neighbor_order(coords, start_index)
    return [targets[index] for index in _two_opt_order(order, coords, start)]


def _optimize_dot_targets(
    dot_targets: Sequence[DotTarget],
    *,
    start_xy: Tuple[float, float],
) -> List[DotTarget]:
    """Order preview dot targets for minimal travel distance."""

    if not dot_targets:
        return []
    coords = np.array(
        [target.robot_xy for target in dot_targets],
        dtype=float,
    )
    start = np.array(start_xy, dtype=float)
    start_index = int(np.argmin([np.linalg.norm(coord - start) for coord in coords]))
    order = _nearest_neighbor_order(coords, start_index)
    return [dot_targets[index] for index in _two_opt_order(order, coords, start)]


MAX_JOINT_LIMITS = np.radians([180, 135, 135, 180, 135, 180])
MIN_JOINT_LIMITS = np.radians([-180, -135, -135, -180, -135, -180])
MAX_VELOCITY = 100
MAX_ACCELERATION = 500


def plan_smooth_path(
    start_pos,
    end_pos,
    duration,
    time_step: float = 0.01,
):
    """Create a deterministic offline joint-trajectory preview."""

    time_points = np.arange(0, duration + time_step, time_step)
    spline = CubicSpline(
        [0, duration],
        [start_pos, end_pos],
        bc_type="clamped",
    )
    return spline(time_points)


def check_joint_limits(position) -> None:
    """Validate a preview position against conservative Lite 6 limits."""

    for index, angle in enumerate(position):
        if angle < MIN_JOINT_LIMITS[index] or angle > MAX_JOINT_LIMITS[index]:
            raise ValueError(
                "Joint {} exceeds limits: {:.2f} degrees".format(
                    index + 1,
                    np.degrees(angle),
                )
            )


def _ensure_centers_list(
    dish_centers: Union[Sequence[dict], str],
) -> List[dict]:
    if isinstance(dish_centers, str):
        try:
            dish_centers = json.loads(dish_centers)
        except json.JSONDecodeError as exc:
            raise ValueError("Invalid JSON dish data") from exc
    return [dict(center) for center in dish_centers]


def _optimize_point_order(
    points: Sequence[Sequence[float]],
    *,
    start_xy: Sequence[float] = (0.0, 0.0),
) -> List[int]:
    if not points:
        return []
    coords = np.array(points, dtype=float)
    start = np.array(start_xy, dtype=float)
    start_index = int(np.argmin([np.linalg.norm(coord - start) for coord in coords]))
    order = _nearest_neighbor_order(coords, start_index)
    return _two_opt_order(order, coords, start)


def plan_optimized_route(
    dish_centers: Union[Sequence[dict], str],
    *,
    start_xy: Sequence[float] = (0.0, 0.0),
    default_capture_height: float = 80.0,
    workspace_mapper: Any = None,
    captured_at_s: Optional[float] = None,
) -> List[dict]:
    """Build a non-executable optimized route preview."""

    dishes = _ensure_centers_list(dish_centers)
    if not dishes:
        return []
    points: List[Tuple[float, float]] = []
    for dish in dishes:
        try:
            x_px = dish["x"]
            y_px = dish["y"]
        except KeyError as exc:
            raise ValueError("Dish centres must contain 'x' and 'y'") from exc
        if workspace_mapper is None:
            x_mm, y_mm = pixel_to_robot_coords(x_px, y_px)
            capture_height = float(dish.get("z", default_capture_height))
        else:
            x_mm, y_mm, mapped_z = _calibrated_workspace_point(
                workspace_mapper,
                pixel_xy=(x_px, y_px),
                captured_at_s=captured_at_s,
            )
            capture_height = mapped_z + float(dish.get("z_offset", 0.0))
        dish["robot_position"] = (x_mm, y_mm, capture_height)
        points.append((x_mm, y_mm))
    order = _optimize_point_order(points, start_xy=start_xy)
    return [dishes[index] for index in order]


def execute_optimized_petri_capture(
    dish_detections: Sequence[dict],
    *,
    arm_ip: Optional[str] = None,
    speed: float = 120.0,
    capture_speed: Optional[float] = None,
    acceleration: float = 2000.0,
    capture_height: float = 110.0,
    safety_offset: float = 40.0,
    min_approach_height: float = 150.0,
    dwell_time: float = 0.3,
    orientation: Tuple[float, float, float] = (180.0, 0.0, 0.0),
    image_size: Tuple[int, int] = (1920, 1080),
    capture_callback: Optional[Callable[[], Optional[str]]] = None,
    upload_callback: Optional[Callable[[str, dict], None]] = None,
    extra_metadata: Optional[dict] = None,
    nfc_reader: Optional[Callable[[], Optional[Union[dict, str]]]] = None,
    workspace_mapper: Any = None,
    captured_at_s: Optional[float] = None,
) -> List[dict]:
    _legacy_motion_disabled()


def xarm_init_and_safe_trajectory_planning(sector, arm_ip=None):
    _legacy_motion_disabled()


def move_to_dish_center(
    dish_center,
    z_height=150,
    speed=100,
    acceleration=2000,
    arm_ip=None,
):
    _legacy_motion_disabled()


def deblock_and_move_init(
    arm_ip=None,
    init_position=(107.2, -301.0, 178.8, -177.1, -6.2, 131.4),
    speed=50,
    acceleration=500,
):
    _legacy_motion_disabled()


def move_to_center_position(
    arm_ip="192.168.1.153",
    center_position=(150.0, 0.0, 200.0, 180.0, 0.0, 0.0),
    speed=50,
    acceleration=500,
):
    _legacy_motion_disabled()


def open_gripper(arm_ip="192.168.1.153", width=850, speed=500):
    _legacy_motion_disabled()


def close_gripper(arm_ip="192.168.1.153", speed=500):
    _legacy_motion_disabled()


def activate_gripper(enable=True, arm_ip=None):
    _legacy_motion_disabled()


def send_trajectory_to_xarm(
    trajectory,
    arm_ip=None,
    velocity=MAX_VELOCITY,
    acceleration=MAX_ACCELERATION,
):
    _legacy_motion_disabled()


def execute_optimized_route(
    dish_centers: Union[Sequence[dict], str],
    *,
    arm_ip: Optional[str] = None,
    safe_height: float = 200.0,
    capture_height_offset: float = 0.0,
    start_pose: Optional[Sequence[float]] = (
        150.0,
        0.0,
        250.0,
        180.0,
        0.0,
        0.0,
    ),
    home_pose: Optional[Sequence[float]] = None,
    orientation: Sequence[float] = (180.0, 0.0, 0.0),
    speed: int = 80,
    acceleration: int = 1500,
    send_results: bool = True,
    workspace_mapper: Any = None,
    captured_at_s: Optional[float] = None,
) -> List[dict]:
    _legacy_motion_disabled()


def capture_and_execute_optimized_route(
    *,
    arm_ip: Optional[str] = None,
    **kwargs,
):
    _legacy_motion_disabled()
