"""Unit test del filtro cluster sui punti SAM21."""
from __future__ import annotations

import numpy as np

from presa_mano.right_arm.cluster import filter_nearest_cluster


def test_empty_input_returns_empty():
    out, diag = filter_nearest_cluster(np.zeros((0, 3)), window_m=0.2)
    assert out.shape == (0, 3)
    assert diag["n_input"] == 0 and diag["n_kept"] == 0
    assert diag["z_near_m"] is None


def test_bimodal_keeps_only_near_cluster():
    # 100 punti vicini (Z ~0.25 m) + 50 background lontano (Z ~4.0 m).
    near = np.column_stack([np.zeros(100), np.zeros(100),
                            np.random.uniform(0.22, 0.28, 100)])
    far = np.column_stack([np.zeros(50), np.zeros(50),
                           np.random.uniform(3.8, 4.2, 50)])
    pts = np.vstack([near, far])
    out, diag = filter_nearest_cluster(pts, window_m=0.20)
    # Tutti i punti vicini, nessun punto lontano
    assert diag["n_kept"] == 100
    assert out[:, 2].max() < 0.30
    assert diag["z_near_m"] < 0.25  # p5 ~= 0.22


def test_single_cluster_keeps_all():
    # Unica nuvola compatta: tutto rientra nella window.
    pts = np.column_stack([np.zeros(100), np.zeros(100),
                           np.random.uniform(0.20, 0.30, 100)])
    out, diag = filter_nearest_cluster(pts, window_m=0.20)
    assert diag["n_kept"] == 100


def test_window_too_small_keeps_only_nearest():
    pts = np.array([
        [0.0, 0.0, 0.20],
        [0.0, 0.0, 0.30],
        [0.0, 0.0, 0.40],
        [0.0, 0.0, 4.00],
    ])
    out, diag = filter_nearest_cluster(pts, window_m=0.05)
    # z_near = p5 di [0.20,0.30,0.40,4.00] ≈ 0.215 → cutoff ≈ 0.265 → solo 0.20
    assert out.shape[0] == 1
    assert out[0, 2] == 0.20


def test_diag_reports_inputs_and_kept():
    pts = np.array([[0.0, 0.0, 0.25]] * 20 + [[0.0, 0.0, 4.0]] * 5)
    out, diag = filter_nearest_cluster(pts, window_m=0.10)
    assert diag["n_input"] == 25
    assert diag["n_kept"] == 20
    assert diag["z_cutoff_m"] == diag["z_near_m"] + 0.10