"""
_person_tracker.py — Multi-person tracker for follow-me mode (no ROS2 deps).

Pipeline (called once per colour frame)
-----------------------------------------
1. Person detections (bounding boxes + detector confidence) arrive from an
   external detector (YOLOv8n, MobileNetSSD, etc.) — not this module's concern.
2. Active tracks are predicted one step forward via a constant-velocity Kalman.
3. Detections are matched to predicted tracks with greedy IoU ≥ iou_threshold.
4. Unmatched detections that survive re-ID histogram matching against LOST
   tracks are reattached to those tracks (brief-occlusion recovery).
5. Remaining unmatched detections start new TENTATIVE tracks.
6. Tracks not updated for max_lost_frames are removed permanently.
7. Bearing and range to a designated follow target are derived from camera
   intrinsics + an aligned depth image.

Re-identification
-----------------
Each track stores an HSV colour histogram of the person's torso region
(middle 50 % of bbox height, centre 80 % width).  After occlusion, new
detections whose histogram Bhattacharyya similarity exceeds reid_threshold
*and* whose predicted position is within reid_max_dist pixels are candidates
for re-identification.  Closest histogram match wins.

Kalman state (8-D, one per track)
----------------------------------
  x = [cx, cy, w, h,  vcx, vcy, vw, vh]
  Measurement z = [cx, cy, w, h]
  dt = 1 frame (constant velocity model)

Public API
----------
  BBox                  NamedTuple  (x, y, w, h) — pixel coordinates
  Detection             NamedTuple  (bbox, confidence, frame_bgr)
  TrackState            Enum        TENTATIVE / ACTIVE / LOST
  PersonTrack           dataclass   per-track state snapshot
  PersonTracker         class       .update() → list[PersonTrack]

  iou(a, b)                              → float
  bearing_from_pixel(u, cx_px, fx)       → float  (degrees)
  depth_at_bbox(depth_u16, bbox, …)      → (depth_m, quality)
  extract_torso_hist(bgr, bbox, …)       → ndarray | None
  hist_similarity(h1, h2)               → float  (0 = different, 1 = same)
  KalmanBoxFilter                        class
"""

from __future__ import annotations

import math
from dataclasses import dataclass, field
from enum import IntEnum
from typing import List, NamedTuple, Optional, Tuple

import numpy as np


# ── Simple types ──────────────────────────────────────────────────────────────

class BBox(NamedTuple):
    """Axis-aligned bounding box in pixel coordinates."""
    x: int    # left edge
    y: int    # top edge
    w: int    # width  (≥ 1)
    h: int    # height (≥ 1)


class Detection(NamedTuple):
    """One person detection from an external detector."""
    bbox:       BBox
    confidence: float                       # 0–1 detector score
    frame_bgr:  Optional[np.ndarray] = None # colour frame (for histogram)


class TrackState(IntEnum):
    TENTATIVE = 0   # seen < min_hits frames; not yet published
    ACTIVE    = 1   # confirmed; published to follow-me controller
    LOST      = 2   # missing; still kept for re-ID up to max_lost_frames


# ── Depth quality levels ──────────────────────────────────────────────────────
DEPTH_INVALID      = 0
DEPTH_EXTRAPOLATED = 1
DEPTH_MARGINAL     = 2
DEPTH_GOOD         = 3


# ── IoU ───────────────────────────────────────────────────────────────────────

def iou(a: BBox, b: BBox) -> float:
    """Intersection-over-union of two bounding boxes."""
    ax2, ay2 = a.x + a.w, a.y + a.h
    bx2, by2 = b.x + b.w, b.y + b.h
    ix1 = max(a.x, b.x);  iy1 = max(a.y, b.y)
    ix2 = min(ax2, bx2);  iy2 = min(ay2, by2)
    inter = max(0, ix2 - ix1) * max(0, iy2 - iy1)
    union = a.w * a.h + b.w * b.h - inter
    return float(inter) / max(float(union), 1e-6)


# ── Kalman box filter ─────────────────────────────────────────────────────────

class KalmanBoxFilter:
    """
    Constant-velocity 8-state Kalman filter for bounding-box tracking.

    State  x = [cx, cy, w, h,  vcx, vcy, vw, vh]
    Meas   z = [cx, cy, w, h]

    Process noise  Q: position uncertainty σ=0.1 px/frame²,
                      velocity uncertainty σ=10 px/frame
    Measurement noise R: ±2 px std-dev on bbox edges
    """

    def __init__(self, initial_bbox: BBox) -> None:
        # Transition matrix (dt = 1 frame)
        self._F = np.eye(8, dtype=np.float64)
        self._F[:4, 4:] = np.eye(4)

        # Measurement matrix
        self._H = np.zeros((4, 8), dtype=np.float64)
        self._H[:4, :4] = np.eye(4)

        # Process noise
        self._Q = np.diag([1., 1., 1., 1., 100., 100., 100., 100.]).astype(np.float64)

        # Measurement noise (~2 px std-dev → var = 4)
        self._R = np.eye(4, dtype=np.float64) * 4.0

        # Initial state
        cx = initial_bbox.x + initial_bbox.w * 0.5
        cy = initial_bbox.y + initial_bbox.h * 0.5
        self._x = np.array(
            [cx, cy, float(initial_bbox.w), float(initial_bbox.h), 0., 0., 0., 0.],
            dtype=np.float64,
        )

        # Initial covariance — small pos uncertainty, large velocity uncertainty
        self._P = np.diag([10., 10., 10., 10., 1000., 1000., 1000., 1000.]).astype(np.float64)

    # -- predict ──────────────────────────────────────────────────────────────

    def predict(self) -> BBox:
        """Advance state one step forward; return predicted BBox."""
        self._x = self._F @ self._x
        self._P = self._F @ self._P @ self._F.T + self._Q
        return self._to_bbox()

    # -- update ───────────────────────────────────────────────────────────────

    def update(self, bbox: BBox) -> BBox:
        """Correct state with observation; return corrected BBox."""
        z = np.array(
            [bbox.x + bbox.w * 0.5, bbox.y + bbox.h * 0.5,
             float(bbox.w), float(bbox.h)],
            dtype=np.float64,
        )
        y = z - self._H @ self._x
        S = self._H @ self._P @ self._H.T + self._R
        K = self._P @ self._H.T @ np.linalg.inv(S)
        self._x = self._x + K @ y
        self._P = (np.eye(8) - K @ self._H) @ self._P
        return self._to_bbox()

    # -- accessors ────────────────────────────────────────────────────────────

    @property
    def velocity_px(self) -> Tuple[float, float]:
        """(vcx, vcy) in pixels / frame from Kalman state."""
        return float(self._x[4]), float(self._x[5])

    @property
    def bbox(self) -> BBox:
        return self._to_bbox()

    def _to_bbox(self) -> BBox:
        cx, cy, w, h = self._x[:4]
        w = max(1.0, w);  h = max(1.0, h)
        return BBox(
            int(round(cx - w * 0.5)),
            int(round(cy - h * 0.5)),
            int(round(w)),
            int(round(h)),
        )


# ── Colour histogram (HSV torso) ──────────────────────────────────────────────

_HIST_H_BINS = 16
_HIST_S_BINS = 8
_HIST_SIZE   = _HIST_H_BINS * _HIST_S_BINS   # 128


def extract_torso_hist(
    bgr:     np.ndarray,
    bbox:    BBox,
    h_bins:  int = _HIST_H_BINS,
    s_bins:  int = _HIST_S_BINS,
) -> Optional[np.ndarray]:
    """
    Extract a normalised HSV colour histogram from the torso region of a
    person bounding box.

    Torso region: middle 50 % of bbox height, centre 80 % of bbox width.

    Parameters
    ----------
    bgr   : (H, W, 3) uint8 colour image
    bbox  : person bounding box
    h_bins, s_bins : histogram bins for H and S channels

    Returns
    -------
    Normalised 1-D histogram of length h_bins * s_bins, or None if the
    crop is too small or bgr is None.
    """
    if bgr is None:
        return None

    ih, iw = bgr.shape[:2]

    # Torso crop
    y0 = bbox.y + bbox.h // 4          # skip top 25 % (head)
    y1 = bbox.y + bbox.h * 3 // 4      # skip bottom 25 % (legs)
    x0 = bbox.x + bbox.w // 10
    x1 = bbox.x + bbox.w * 9 // 10

    y0 = max(0, y0);  y1 = min(ih, y1)
    x0 = max(0, x0);  x1 = min(iw, x1)

    if y1 - y0 < 4 or x1 - x0 < 4:
        return None

    crop = bgr[y0:y1, x0:x1]

    # BGR → HSV (manual, no OpenCV dependency for tests)
    crop_f = crop.astype(np.float32) / 255.0
    r, g, b = crop_f[..., 2], crop_f[..., 1], crop_f[..., 0]

    cmax = np.maximum(np.maximum(r, g), b)
    cmin = np.minimum(np.minimum(r, g), b)
    delta_h = cmax - cmin + 1e-7  # epsilon only for hue angle computation

    # Hue in [0, 360)
    h = np.where(
        cmax == r, 60.0 * ((g - b) / delta_h % 6),
        np.where(cmax == g, 60.0 * ((b - r) / delta_h + 2),
                 60.0 * ((r - g) / delta_h + 4)),
    )
    h = np.clip(h % 360.0, 0.0, 359.9999)
    # Saturation in [0, 1] — no epsilon so pure-colour pixels stay ≤ 1.0
    s = np.clip(np.where(cmax > 1e-6, (cmax - cmin) / cmax, 0.0), 0.0, 1.0)

    h_flat = h.ravel()
    s_flat = s.ravel()

    hist, _, _ = np.histogram2d(
        h_flat, s_flat,
        bins=[h_bins, s_bins],
        range=[[0, 360], [0, 1]],
    )
    hist = hist.ravel().astype(np.float32)
    total = hist.sum()
    if total > 0:
        hist /= total
    return hist


def hist_similarity(h1: np.ndarray, h2: np.ndarray) -> float:
    """
    Bhattacharyya similarity between two normalised histograms.

    Returns a value in [0, 1]: 1 = identical, 0 = completely different.
    """
    bc = float(np.sum(np.sqrt(h1 * h2)))
    return float(np.clip(bc, 0.0, 1.0))


# ── Camera geometry ───────────────────────────────────────────────────────────

def bearing_from_pixel(
    u:     float,
    cx_px: float,
    fx:    float,
) -> float:
    """
    Convert a horizontal pixel coordinate to a bearing angle.

    Parameters
    ----------
    u     : pixel column (horizontal image coordinate)
    cx_px : principal point x (from CameraInfo.K[2])
    fx    : horizontal focal length in pixels (from CameraInfo.K[0])

    Returns
    -------
    bearing_deg : signed degrees; positive = right of camera centre.
    """
    return math.degrees(math.atan2(float(u - cx_px), float(fx)))


def depth_at_bbox(
    depth_u16:   np.ndarray,
    bbox:        BBox,
    depth_scale: float = 0.001,
    window_frac: float = 0.3,
) -> Tuple[float, int]:
    """
    Sample median depth from the central torso region of a bounding box in a
    uint16 depth image (D435i mm units by default).

    Parameters
    ----------
    depth_u16   : (H, W) uint16 depth image
    bbox        : person bounding box (colour image coordinates, assumed aligned)
    depth_scale : multiply raw value to get metres (D435i: 0.001)
    window_frac : fraction of bbox dimensions to use as central sample window

    Returns
    -------
    (depth_m, quality)
      depth_m  : median depth in metres (0.0 when no valid pixels)
      quality  : DEPTH_GOOD / DEPTH_MARGINAL / DEPTH_EXTRAPOLATED / DEPTH_INVALID
    """
    ih, iw = depth_u16.shape

    # Central window
    wf = max(0.1, min(1.0, window_frac))
    cx = bbox.x + bbox.w * 0.5
    cy = bbox.y + bbox.h * 0.5
    hw = bbox.w * wf * 0.5
    hh = bbox.h * wf * 0.5

    r0 = int(max(0,  cy - hh))
    r1 = int(min(ih, cy + hh + 1))
    c0 = int(max(0,  cx - hw))
    c1 = int(min(iw, cx + hw + 1))

    if r1 <= r0 or c1 <= c0:
        return 0.0, DEPTH_INVALID

    patch = depth_u16[r0:r1, c0:c1]
    valid = patch[patch > 0]
    n_total = patch.size

    if len(valid) == 0:
        return 0.0, DEPTH_INVALID

    fill_ratio = len(valid) / max(n_total, 1)
    depth_m    = float(np.median(valid)) * depth_scale

    if fill_ratio > 0.6:
        quality = DEPTH_GOOD
    elif fill_ratio > 0.25:
        quality = DEPTH_MARGINAL
    else:
        quality = DEPTH_EXTRAPOLATED

    return depth_m, quality


# ── Per-track state ───────────────────────────────────────────────────────────

@dataclass
class PersonTrack:
    """Current state of one tracked person."""
    track_id:   int
    state:      TrackState
    bbox:       BBox                        # smoothed Kalman bbox (colour img px)
    bearing:    float = 0.0                 # degrees; 0 until cam params set
    distance:   float = 0.0                 # metres; 0 until depth available
    depth_qual: int   = DEPTH_INVALID
    confidence: float = 0.0                 # 0–1 combined score
    vel_u:      float = 0.0                 # Kalman horizontal velocity (px/frame)
    vel_v:      float = 0.0                 # Kalman vertical velocity   (px/frame)

    hits:       int   = 0                   # consecutive matched frames
    age:        int   = 0                   # total frames since creation
    lost_age:   int   = 0                   # consecutive unmatched frames

    color_hist: Optional[np.ndarray] = None # HSV torso histogram

    # Internal — not serialised
    _kalman:    Optional[KalmanBoxFilter] = field(default=None, repr=False)


# ── Camera parameters (minimal) ──────────────────────────────────────────────

@dataclass
class CamParams:
    """Minimal camera intrinsics needed for bearing calculation."""
    fx:    float = 615.0    # D435i 640×480 depth defaults
    fy:    float = 615.0
    cx:    float = 320.0
    cy:    float = 240.0
    fps:   float = 30.0     # frame rate (for velocity conversion px→m/s)


# ── PersonTracker ─────────────────────────────────────────────────────────────

class PersonTracker:
    """
    Multi-person tracker combining Kalman prediction, IoU data association,
    and HSV colour histogram re-identification.

    Parameters
    ----------
    iou_threshold   : minimum IoU to consider a detection-track match
    min_hits        : frames before a track transitions TENTATIVE → ACTIVE
    max_lost_frames : frames a track survives without a detection before removal
    reid_threshold  : minimum histogram Bhattacharyya similarity for re-ID
    reid_max_dist   : max predicted-to-detection centre distance for re-ID (px)
    """

    def __init__(
        self,
        iou_threshold:   float = 0.30,
        min_hits:        int   = 3,
        max_lost_frames: int   = 30,
        reid_threshold:  float = 0.55,
        reid_max_dist:   float = 150.0,
    ) -> None:
        self.iou_threshold   = iou_threshold
        self.min_hits        = min_hits
        self.max_lost_frames = max_lost_frames
        self.reid_threshold  = reid_threshold
        self.reid_max_dist   = reid_max_dist

        self._tracks:  List[PersonTrack] = []
        self._next_id: int = 0

    # ── Public API ────────────────────────────────────────────────────────────

    @property
    def tracks(self) -> List[PersonTrack]:
        return list(self._tracks)

    def update(
        self,
        detections:  List[Detection],
        cam:         Optional[CamParams]   = None,
        depth_u16:   Optional[np.ndarray]  = None,
        depth_scale: float                 = 0.001,
    ) -> List[PersonTrack]:
        """
        Process one frame of detections.

        Parameters
        ----------
        detections  : list of Detection from an external detector
        cam         : camera intrinsics (for bearing computation); None = skip
        depth_u16   : aligned uint16 depth image; None = depth unavailable
        depth_scale : mm-to-metres scale factor (D435i default 0.001)

        Returns
        -------
        list of PersonTrack (ACTIVE state only)
        """
        # ── Step 1: Predict all tracks ────────────────────────────────────────
        for trk in self._tracks:
            if trk._kalman is not None:
                trk.bbox = trk._kalman.predict()
            trk.age += 1

        # ── Step 2: IoU matching ──────────────────────────────────────────────
        active_tracks = [t for t in self._tracks if t.state != TrackState.LOST]
        matched_t, matched_d, unmatched_t, unmatched_d = \
            self._match_iou(active_tracks, detections)

        # ── Step 3: Update matched tracks ────────────────────────────────────
        for t_idx, d_idx in zip(matched_t, matched_d):
            trk = active_tracks[t_idx]
            det = detections[d_idx]
            if trk._kalman is not None:
                trk.bbox = trk._kalman.update(det.bbox)
                trk.vel_u, trk.vel_v = trk._kalman.velocity_px
            trk.hits     += 1
            trk.lost_age  = 0
            trk.confidence = float(det.confidence)
            if trk.state == TrackState.TENTATIVE and trk.hits >= self.min_hits:
                trk.state = TrackState.ACTIVE
            self._update_hist(trk, det)

        # ── Step 4: Re-ID for unmatched detections vs LOST tracks ────────────
        lost_tracks    = [t for t in self._tracks if t.state == TrackState.LOST]
        still_unmatched_d = list(unmatched_d)

        for d_idx in list(still_unmatched_d):
            det = detections[d_idx]
            best_trk, best_sim = self._reid_match(det, lost_tracks)
            if best_trk is not None:
                if best_trk._kalman is not None:
                    best_trk.bbox = best_trk._kalman.update(det.bbox)
                else:
                    best_trk.bbox = det.bbox
                best_trk.state    = TrackState.ACTIVE
                best_trk.hits    += 1
                best_trk.lost_age = 0
                best_trk.confidence = float(det.confidence)
                self._update_hist(best_trk, det)
                still_unmatched_d.remove(d_idx)

        # ── Step 5: Create new tracks for still-unmatched detections ─────────
        for d_idx in still_unmatched_d:
            det = detections[d_idx]
            trk = PersonTrack(
                track_id  = self._next_id,
                state     = TrackState.TENTATIVE,
                bbox      = det.bbox,
                hits      = 1,
                confidence= float(det.confidence),
                _kalman   = KalmanBoxFilter(det.bbox),
            )
            self._update_hist(trk, det)
            self._tracks.append(trk)
            self._next_id += 1

        # ── Step 6: Age lost tracks, remove stale ────────────────────────────
        # Mark newly-unmatched tracks as LOST (reset lost_age to 0)
        for t_idx in unmatched_t:
            trk = active_tracks[t_idx]
            if trk.state != TrackState.LOST:
                trk.lost_age = 0
            trk.state = TrackState.LOST

        # Increment lost_age for every LOST track (including previously LOST ones)
        for trk in self._tracks:
            if trk.state == TrackState.LOST:
                trk.lost_age += 1

        self._tracks = [
            t for t in self._tracks
            if t.lost_age < self.max_lost_frames
        ]

        # ── Step 7: Update bearing / depth for all active tracks ─────────────
        for trk in self._tracks:
            if trk.state != TrackState.ACTIVE:
                continue
            u_centre = trk.bbox.x + trk.bbox.w * 0.5
            if cam is not None:
                trk.bearing = bearing_from_pixel(u_centre, cam.cx, cam.fx)
            if depth_u16 is not None:
                trk.distance, trk.depth_qual = depth_at_bbox(
                    depth_u16, trk.bbox, depth_scale=depth_scale
                )

        return [t for t in self._tracks if t.state == TrackState.ACTIVE]

    def reset(self) -> None:
        """Clear all tracks."""
        self._tracks.clear()
        self._next_id = 0

    # ── Internal helpers ──────────────────────────────────────────────────────

    def _match_iou(
        self,
        tracks:     List[PersonTrack],
        detections: List[Detection],
    ) -> Tuple[List[int], List[int], List[int], List[int]]:
        """
        Greedy IoU matching between tracks and detections.

        Returns (matched_t_idx, matched_d_idx, unmatched_t_idx, unmatched_d_idx).
        """
        if not tracks or not detections:
            return [], [], list(range(len(tracks))), list(range(len(detections)))

        iou_mat = np.zeros((len(tracks), len(detections)), dtype=np.float32)
        for ti, trk in enumerate(tracks):
            for di, det in enumerate(detections):
                iou_mat[ti, di] = iou(trk.bbox, det.bbox)

        matched_t: List[int] = []
        matched_d: List[int] = []
        used_t = set()
        used_d = set()

        # Greedy: highest IoU first
        flat_order = np.argsort(iou_mat.ravel())[::-1]
        for flat_idx in flat_order:
            ti, di = divmod(int(flat_idx), len(detections))
            if iou_mat[ti, di] < self.iou_threshold:
                break
            if ti not in used_t and di not in used_d:
                matched_t.append(ti)
                matched_d.append(di)
                used_t.add(ti)
                used_d.add(di)

        unmatched_t = [i for i in range(len(tracks))     if i not in used_t]
        unmatched_d = [i for i in range(len(detections)) if i not in used_d]
        return matched_t, matched_d, unmatched_t, unmatched_d

    def _reid_match(
        self,
        det:    Detection,
        lost:   List[PersonTrack],
    ) -> Tuple[Optional[PersonTrack], float]:
        """
        Find the best re-identification match for a detection among lost tracks.

        Returns (best_track, similarity) or (None, 0.0) if no match found.
        """
        if not lost:
            return None, 0.0

        det_hist = None
        if det.frame_bgr is not None:
            det_hist = extract_torso_hist(det.frame_bgr, det.bbox)

        best_trk: Optional[PersonTrack] = None
        best_sim: float = 0.0

        det_cx = det.bbox.x + det.bbox.w * 0.5
        det_cy = det.bbox.y + det.bbox.h * 0.5

        for trk in lost:
            # Spatial gate: predicted centre must be close enough
            trk_cx = trk.bbox.x + trk.bbox.w * 0.5
            trk_cy = trk.bbox.y + trk.bbox.h * 0.5
            dist = math.sqrt((det_cx - trk_cx) ** 2 + (det_cy - trk_cy) ** 2)
            if dist > self.reid_max_dist:
                continue

            # Histogram similarity
            if det_hist is not None and trk.color_hist is not None:
                sim = hist_similarity(det_hist, trk.color_hist)
                if sim > self.reid_threshold and sim > best_sim:
                    best_sim = sim
                    best_trk = trk

        return best_trk, best_sim

    @staticmethod
    def _update_hist(trk: PersonTrack, det: Detection) -> None:
        """Update track's colour histogram with exponential decay."""
        if det.frame_bgr is None:
            return
        new_hist = extract_torso_hist(det.frame_bgr, det.bbox)
        if new_hist is None:
            return
        if trk.color_hist is None:
            trk.color_hist = new_hist
        else:
            # Running average (α = 0.3 — new frame contributes 30 %)
            trk.color_hist = 0.7 * trk.color_hist + 0.3 * new_hist


# ── Follow-target selector ────────────────────────────────────────────────────

class FollowTargetSelector:
    """
    Selects and locks onto a single PersonTrack as the follow target.

    Strategy
    --------
    • On start() or when no target is locked: choose the nearest active track
      (by depth distance, or by image-centre proximity when depth unavailable).
    • Re-lock onto the same track_id each frame (continuous tracking).
    • If the locked track disappears: hold the last known state for
      `hold_frames` frames, then go inactive.
    """

    def __init__(self, hold_frames: int = 15) -> None:
        self.hold_frames   = hold_frames
        self._target_id:   Optional[int]         = None
        self._last_target: Optional[PersonTrack] = None
        self._held_frames: int                   = 0
        self._active:      bool                  = False

    # -- control ──────────────────────────────────────────────────────────────

    def start(self) -> None:
        """(Re-)enable follow mode; re-select target on next update."""
        self._active     = True
        self._target_id  = None
        self._held_frames = 0

    def stop(self) -> None:
        """Disable follow mode."""
        self._active     = False
        self._target_id  = None
        self._last_target = None

    # -- update ───────────────────────────────────────────────────────────────

    def update(
        self,
        active_tracks: List[PersonTrack],
        img_cx:        float = 320.0,   # image centre x (px)
    ) -> Optional[PersonTrack]:
        """
        Select the follow target from a list of active tracks.

        Returns the locked PersonTrack, or None if follow mode is inactive or
        no candidate found.
        """
        if not self._active:
            return None

        if not active_tracks:
            if self._last_target is not None and self._held_frames < self.hold_frames:
                self._held_frames += 1
                return self._last_target
            self._last_target = None
            return None

        self._held_frames = 0

        # Re-find locked track
        if self._target_id is not None:
            for t in active_tracks:
                if t.track_id == self._target_id:
                    self._last_target = t
                    return t
            # Locked track lost — re-select
            self._target_id = None

        # Select: prefer by smallest distance, then by image-centre proximity
        def _score(t: PersonTrack) -> float:
            if t.distance > 0:
                return t.distance
            return abs((t.bbox.x + t.bbox.w * 0.5) - img_cx)

        chosen = min(active_tracks, key=_score)
        self._target_id  = chosen.track_id
        self._last_target = chosen
        return chosen
