"""
source_arbiter.py — Source confidence scoring and selection for tracking fusion.

Two sensor sources are supported:
  UWB    — geometry_msgs/PoseStamped from /uwb/target   (triangulated, ~10 Hz)
  Camera — geometry_msgs/PoseStamped from /person/target (depth+YOLO, ~30 Hz)

Confidence model
----------------
Each source's confidence is its raw measurement quality multiplied by a
linear staleness factor that drops to zero at its respective timeout:

    conf = quality * max(0, 1 - age / timeout)

UWB quality is always 1.0 (the ranging hardware confidence is not exposed
by the driver in origin/main; the UWB node already applies Kalman filtering).

Camera quality defaults to 1.0; callers may pass a lower value when the
detection confidence is available.

Source selection
----------------
  Both fresh   → "fused"     (confidence-weighted position blend)
  UWB only     → "uwb"
  Camera only  → "camera"
  Neither fresh → "predicted" (Kalman coasts)

Pure module — no ROS2 dependency; fully unit-testable.
"""

import math


def _staleness_factor(age_s: float, timeout_s: float) -> float:
    """Linear decay: 1.0 at age=0, 0.0 at age=timeout, clamped."""
    if timeout_s <= 0.0:
        return 0.0
    return max(0.0, 1.0 - age_s / timeout_s)


def uwb_confidence(age_s: float, timeout_s: float, quality: float = 1.0) -> float:
    """
    UWB source confidence.

    age_s    : seconds since last UWB measurement (≥0; use large value if never)
    timeout_s: staleness threshold (s); confidence reaches 0 at this age
    quality  : inherent measurement quality [0, 1] (default 1.0)
    """
    return quality * _staleness_factor(age_s, timeout_s)


def camera_confidence(
    age_s: float, timeout_s: float, quality: float = 1.0
) -> float:
    """
    Camera source confidence.

    age_s    : seconds since last camera detection (≥0; use large value if never)
    timeout_s: staleness threshold (s)
    quality  : YOLO detection confidence or other quality score [0, 1]
    """
    return quality * _staleness_factor(age_s, timeout_s)


def select_source(
    uwb_conf: float,
    cam_conf: float,
    threshold: float = 0.15,
) -> str:
    """
    Choose the active tracking source label.

    Returns one of: "fused", "uwb", "camera", "predicted".

    threshold: minimum confidence for a source to be considered live.
               Sources below threshold are ignored.
    """
    uwb_ok = uwb_conf >= threshold
    cam_ok = cam_conf >= threshold

    if uwb_ok and cam_ok:
        return "fused"
    if uwb_ok:
        return "uwb"
    if cam_ok:
        return "camera"
    return "predicted"


def fuse_positions(
    uwb_x: float,
    uwb_y: float,
    uwb_conf: float,
    cam_x: float,
    cam_y: float,
    cam_conf: float,
) -> tuple:
    """
    Confidence-weighted position fusion.

    Returns (fused_x, fused_y).

    When total confidence is zero (shouldn't happen in "fused" state, but
    guarded), returns the UWB position as fallback.
    """
    total = uwb_conf + cam_conf
    if total <= 0.0:
        return uwb_x, uwb_y
    w = uwb_conf / total
    return (
        w * uwb_x + (1.0 - w) * cam_x,
        w * uwb_y + (1.0 - w) * cam_y,
    )


def composite_confidence(
    uwb_conf: float,
    cam_conf: float,
    source: str,
    kf_uncertainty_m: float,
    max_kf_uncertainty_m: float = 3.0,
) -> float:
    """
    Compute a single composite confidence value [0, 1] for the fused output.

    source             : current source label (from select_source)
    kf_uncertainty_m   : current KF positional RMS uncertainty
    max_kf_uncertainty_m: uncertainty at which confidence collapses to 0
    """
    if source == "predicted":
        # Decay with growing KF uncertainty; no sensor feeds are live
        raw = max(0.0, 1.0 - kf_uncertainty_m / max_kf_uncertainty_m)
        return min(0.4, raw)   # cap at 0.4 — caller should know this is dead-reckoning

    if source == "fused":
        raw = max(uwb_conf, cam_conf)
    elif source == "uwb":
        raw = uwb_conf
    else:   # "camera"
        raw = cam_conf

    # Scale by KF health (full confidence only if KF is tight)
    kf_health = max(0.0, 1.0 - kf_uncertainty_m / max_kf_uncertainty_m)
    return raw * (0.5 + 0.5 * kf_health)


def bearing_and_range(x: float, y: float) -> tuple:
    """
    Compute bearing (rad, +ve = left) and range (m) to position (x, y).

    Consistent with person_follower_node conventions:
        bearing = atan2(y, x)   (base_link frame: +X forward, +Y left)
        range   = sqrt(x² + y²)
    """
    return math.atan2(y, x), math.sqrt(x * x + y * y)
