"""
detection_utils.py — Pure-Python helpers with no ROS2 dependencies.
Importable in tests without a running ROS2 environment.
"""

import numpy as np


def nms(boxes: np.ndarray, scores: np.ndarray,
        iou_threshold: float = 0.45) -> list[int]:
    """
    Non-maximum suppression.

    Args:
        boxes: [N, 4] float array of (x1, y1, x2, y2) boxes
        scores: [N] float array of confidence scores
        iou_threshold: suppress boxes with IoU > this value

    Returns:
        List of kept indices (sorted by descending score).
    """
    if len(boxes) == 0:
        return []

    x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3]
    areas = (x2 - x1) * (y2 - y1)
    order = scores.argsort()[::-1]
    keep = []

    while len(order) > 0:
        i = order[0]
        keep.append(int(i))
        if len(order) == 1:
            break
        ix1 = np.maximum(x1[i], x1[order[1:]])
        iy1 = np.maximum(y1[i], y1[order[1:]])
        ix2 = np.minimum(x2[i], x2[order[1:]])
        iy2 = np.minimum(y2[i], y2[order[1:]])
        iw = np.maximum(0.0, ix2 - ix1)
        ih = np.maximum(0.0, iy2 - iy1)
        inter = iw * ih
        iou = inter / (areas[i] + areas[order[1:]] - inter + 1e-6)
        order = order[1:][iou < iou_threshold]

    return keep


def letterbox(image: np.ndarray, size: int = 640, pad_value: int = 114):
    """
    Letterbox-resize `image` to `size`×`size`.

    Returns:
        (canvas, scale, pad_w, pad_h)
        canvas:  uint8 [size, size, 3]
        scale:   float — resize scale factor
        pad_w:   int — horizontal padding applied
        pad_h:   int — vertical padding applied
    """
    import cv2
    h, w = image.shape[:2]
    scale = min(size / w, size / h)
    new_w = int(round(w * scale))
    new_h = int(round(h * scale))
    pad_w = (size - new_w) // 2
    pad_h = (size - new_h) // 2

    resized = cv2.resize(image, (new_w, new_h), interpolation=cv2.INTER_LINEAR)
    canvas = np.full((size, size, image.shape[2] if image.ndim == 3 else 1),
                     pad_value, dtype=np.uint8)
    canvas[pad_h:pad_h + new_h, pad_w:pad_w + new_w] = resized
    return canvas, scale, pad_w, pad_h


def remap_bbox(x1, y1, x2, y2, scale, pad_w, pad_h, orig_w, orig_h):
    """Map bbox from letterboxed-image space back to original image space."""
    x1 = float(np.clip((x1 - pad_w) / scale, 0, orig_w))
    y1 = float(np.clip((y1 - pad_h) / scale, 0, orig_h))
    x2 = float(np.clip((x2 - pad_w) / scale, 0, orig_w))
    y2 = float(np.clip((y2 - pad_h) / scale, 0, orig_h))
    return x1, y1, x2, y2


def get_depth_at(depth_img: np.ndarray, u: float, v: float,
                 window: int = 7, min_d: float = 0.3, max_d: float = 6.0) -> float:
    """
    Median depth in a `window`×`window` region around pixel (u, v).

    Args:
        depth_img: float32 depth image in metres
        u, v: pixel coordinates
        window: patch side length
        min_d, max_d: valid depth range

    Returns:
        Median depth in metres, or 0.0 if no valid pixels.
    """
    h, w = depth_img.shape
    u, v = int(u), int(v)
    half = window // 2
    u1, u2 = max(0, u - half), min(w, u + half + 1)
    v1, v2 = max(0, v - half), min(h, v + half + 1)
    patch = depth_img[v1:v2, u1:u2]
    valid = patch[(patch > min_d) & (patch < max_d)]
    return float(np.median(valid)) if len(valid) > 0 else 0.0


def pixel_to_3d(u: float, v: float, depth_m: float, K) -> tuple[float, float, float]:
    """
    Back-project pixel (u, v) at depth_m to 3D point in camera frame.

    Args:
        u, v: pixel coordinates
        depth_m: depth in metres
        K: camera intrinsic matrix (row-major, 9 elements or 3×3 array)

    Returns:
        (X, Y, Z) in camera optical frame
    """
    K = np.asarray(K).ravel()
    fx, fy = K[0], K[4]
    cx, cy = K[2], K[5]
    X = (u - cx) * depth_m / fx
    Y = (v - cy) * depth_m / fy
    return X, Y, depth_m
