def _mode_colour(px, prior=None):
    """Dominant colour of a pixel set: 16-level 3-D histogram, summed over 2x2x2 neighbouring bins (so a cluster
    that straddles a bin edge is not split), optionally restricted to cubes within 48 levels of a prior colour.
    Returns (median colour of the winning cube, robust spread, share of pixels in the cube)."""
    px = np.asarray(px, dtype=np.float64).reshape(-1, 3)
    q = np.clip(px, 0, 255).astype(np.int64) // 16
    h = np.bincount(q[:, 0] * 256 + q[:, 1] * 16 + q[:, 2], minlength=4096).reshape(16, 16, 16).astype(np.float64)
    c = (h[:-1, :-1, :-1] + h[1:, :-1, :-1] + h[:-1, 1:, :-1] + h[:-1, :-1, 1:]
         + h[1:, 1:, :-1] + h[1:, :-1, 1:] + h[:-1, 1:, 1:] + h[1:, 1:, 1:])
    if prior is not None:
        g = (np.arange(15) + 1) * 16.0
        ok = ((np.abs(g[:, None, None] - prior[0]) <= 48) & (np.abs(g[None, :, None] - prior[1]) <= 48)
              & (np.abs(g[None, None, :] - prior[2]) <= 48))
        if float((c * ok).max()) > 0:
            c = c * ok
    k = int(np.argmax(c))
    centre = np.array([k // 225 + 1, (k // 15) % 15 + 1, k % 15 + 1], dtype=np.float64) * 16.0
    near = np.max(np.abs(px - centre), axis=1) <= 16.0
    if int(near.sum()) < 20:
        return np.median(px, axis=0), 6.0, 0.0
    med = np.median(px[near], axis=0)
    dev = np.max(np.abs(px[near] - med), axis=1)
    return med, 1.4826 * float(np.median(dev)), float(near.mean())


def locate(image, cam, pts_world, ref_world, search_m, min_score=0.45, min_inside=0.50, diag=None):
    """Estimate the horizontal offset (dx, dy) of an object from its nominal pose.

    pts_world : (N,3) mesh points of the object at its NOMINAL pose (world frame)
    ref_world : (3,) reference point of the object (used for the pixel<->table-plane Jacobian)
    search_m  : half-width of the feasible placement square (metres)
    Returns (dx, dy, info) with dx, dy clipped to +-search_m, or None if the match is not trustworthy.

    Method: project the points -> silhouette template T. Foreground = pixels whose colour differs from the
    table colour. For every integer image shift, score = (foreground fill inside T) - (foreground fill in a
    ring around T). The ring is split into left/right and top/bottom sectors and only the cleaner sector of
    each pair counts, so a neighbouring / occluding object on one side per image axis does not spoil the
    match while both axes stay constrained.
    """
    try:
        img = np.asarray(image)
        if img.shape != (256, 256, 3):
            return None
        u, v, z = cam.project(pts_world)
        if not (np.all(np.isfinite(u)) and np.all(np.isfinite(v))) or np.any(z < 0.05):
            return None
        # pixel <-> world Jacobian on the horizontal plane through the reference point
        e = 0.01
        ref = np.asarray(ref_world, dtype=np.float64)
        pr = np.array([ref, ref + [e, 0, 0], ref + [0, e, 0]])
        ur, vr, _ = cam.project(pr)
        J = np.array([[(ur[1] - ur[0]) / e, (ur[2] - ur[0]) / e],
                      [(vr[1] - vr[0]) / e, (vr[2] - vr[0]) / e]])    # d(col,row)/d(x,y)
        if abs(np.linalg.det(J)) < 1e-6:
            return None
        Jinv = np.linalg.inv(J)
        reach = search_m + 0.006
        S = int(math.ceil(max(abs(J[0, 0]) + abs(J[0, 1]), abs(J[1, 0]) + abs(J[1, 1])) * reach)) + 1
        RING = 4
        M = S + RING + 3
        c0 = int(math.floor(u.min())) - M
        c1 = int(math.ceil(u.max())) + M + 1
        r0 = int(math.floor(v.min())) - M
        r1 = int(math.ceil(v.max())) + M + 1
        if c0 < 0 or r0 < 0 or c1 > 256 or r1 > 256:
            return None
        Hh, Ww = r1 - r0, c1 - c0
        # template: splat points, close holes
        T = np.zeros((Hh, Ww), dtype=bool)
        ci = np.clip(np.rint(u).astype(np.int64) - c0, 0, Ww - 1)
        ri = np.clip(np.rint(v).astype(np.int64) - r0, 0, Hh - 1)
        T[ri, ci] = True
        T = _erode(_dilate(T, 2), 2)
        nT = int(T.sum())
        if nT < 40:
            return None
        ring = _dilate(T, RING) & ~_dilate(T, 1)
        rr, cc = np.nonzero(T)
        cy, cx = float(rr.mean()), float(cc.mean())
        gy, gx = np.mgrid[0:Hh, 0:Ww]
        dy_, dx_ = gy - cy, gx - cx
        horiz = np.abs(dx_) * (rr.max() - rr.min() + 1) >= np.abs(dy_) * (cc.max() - cc.min() + 1)
        sectors = [ring & horiz & (dx_ < 0), ring & horiz & (dx_ >= 0), ring & ~horiz & (dy_ < 0), ring & ~horiz & (dy_ >= 0)]
        sect_f = [s_.astype(np.float64) for s_ in sectors]
        sect_n = [max(float(s_.sum()), 1.0) for s_ in sectors]
        # background model
        win = img[r0:r1, c0:c1, :].astype(np.float64)
        prior, _, _ = _mode_colour(img[96:256, :, :])          # table = dominant colour of the lower image
        med, sigma, share = _mode_colour(win, prior)
        if share < 0.08:
            return None
        dev = np.max(np.abs(win - med), axis=2)
        tau = min(40.0, max(22.0, 4.0 * sigma))
        fg = dev > tau
        frac = float(fg.mean())
        if frac > 0.85 or frac < 0.02:
            return None
        fgf = fg.astype(np.float64)
        Tf = T.astype(np.float64)
        n = 2 * S + 1
        score = np.zeros((n, n))
        inside = np.zeros((n, n))
        for iy in range(n):
            dy = iy - S
            fgy = np.roll(fgf, -dy, axis=0)
            for ix in range(n):
                dx = ix - S
                sh = np.roll(fgy, -dx, axis=1)                 # fg sampled at (r+dy, c+dx)
                a = float((sh * Tf).sum()) / nT
                rs = [float((sh * sf).sum()) / sn for sf, sn in zip(sect_f, sect_n)]
                inside[iy, ix] = a
                score[iy, ix] = a - 0.5 * (min(rs[0], rs[1]) + min(rs[2], rs[3]))
        k = int(np.argmax(score))
        iy, ix = k // n, k % n
        best = float(score[iy, ix])
        info = dict(score=best, inside=float(inside[iy, ix]), tau=tau, frac=frac, S=S, share=share,
                    shift=(iy - S, ix - S), med=[float(x) for x in med])
        if diag is not None:
            diag.update(info)
        if best < min_score or inside[iy, ix] < min_inside:
            return None
        if iy == 0 or ix == 0 or iy == n - 1 or ix == n - 1:
            return None
        # sub-pixel: weighted centroid of the 3x3 neighbourhood
        sub = score[iy - 1:iy + 2, ix - 1:ix + 2]
        w = np.maximum(sub - (best - 0.10), 0.0)
        sw = float(w.sum())
        if sw <= 0:
            return None
        wy, wx = np.mgrid[-1:2, -1:2]
        d_row = (iy - S) + float((w * wy).sum()) / sw
        d_col = (ix - S) + float((w * wx).sum()) / sw
        dxy = Jinv @ np.array([d_col, d_row])
        if not np.all(np.isfinite(dxy)):
            return None
        dxy = np.clip(dxy, -search_m, search_m)
        info["dxy"] = (float(dxy[0]), float(dxy[1]))
        return float(dxy[0]), float(dxy[1]), info
    except Exception:
        return None


