Presa-Mano/tests/test_right_arm_routes.py

"""Integration test del router /api/right_arm/*."""
from __future__ import annotations

from unittest.mock import patch, MagicMock

from fastapi.testclient import TestClient

from presa_mano.main import app

client = TestClient(app)


def _fake_wrist_response():
    return {
        "left_wrist": {"position": [0.10, 0.20, 0.30], "quaternion": [0, 0, 0, 1]},
        "right_wrist": {"position": [0.30, -0.20, 0.30], "quaternion": [0, 0, 0, 1]},
    }


def test_state_returns_wrist_pose_and_config():
    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_factory:
        g1 = MagicMock()
        g1.wrist.return_value = _fake_wrist_response()
        g1_factory.return_value = g1

        r = client.get("/api/right_arm/state")
        assert r.status_code == 200
        body = r.json()
        assert body["wrist"]["position"] == [0.30, -0.20, 0.30]
        assert "rpy_deg" in body["wrist"]
        assert body["side"] == "right" or body["side"] in ("left", "right")
        assert "home_pose" in body
        assert "custom_pose" in body
        assert "offset_camera_mm" in body
        assert "axis_signs" in body
        assert "max_step_m" in body
        assert "z_floor_m" in body
        assert "robot_active" in body
        assert "timings_ms" in body


def test_state_502_when_g1_unavailable():
    from presa_mano.clients import G1Unavailable
    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_factory:
        g1 = MagicMock()
        g1.wrist.side_effect = G1Unavailable("connection refused")
        g1_factory.return_value = g1
        r = client.get("/api/right_arm/state")
        assert r.status_code == 502


def test_snapshot_front_returns_jpeg():
    fake_jpeg = b"\xff\xd8\xff" + b"\x00" * 100  # magic JPEG bytes
    with patch("presa_mano.api.right_arm_routes._deps.front_camera") as factory:
        cam = MagicMock()
        cam.rgb_jpeg.return_value = fake_jpeg
        factory.return_value = cam
        r = client.get("/api/right_arm/snapshot/front")
        assert r.status_code == 200
        assert r.headers["content-type"].startswith("image/jpeg")
        assert r.content == fake_jpeg


def test_snapshot_wrist_returns_jpeg():
    fake_jpeg = b"\xff\xd8\xff" + b"\x00" * 50
    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as factory:
        cam = MagicMock()
        cam.rgb_jpeg.return_value = fake_jpeg
        factory.return_value = cam
        r = client.get("/api/right_arm/snapshot/wrist")
        assert r.status_code == 200
        assert r.headers["content-type"].startswith("image/jpeg")
        assert r.content == fake_jpeg


def test_snapshot_front_502_on_failure():
    with patch("presa_mano.api.right_arm_routes._deps.front_camera") as factory:
        cam = MagicMock()
        cam.rgb_jpeg.side_effect = RuntimeError("connection refused")
        factory.return_value = cam
        r = client.get("/api/right_arm/snapshot/front")
        assert r.status_code == 502


def _fake_yolo_top(class_name="pestello", conf=0.87):
    return {"detections": [
        {"class": class_name, "confidence": conf,
         "bbox": {"x1": 100.0, "y1": 80.0, "x2": 220.0, "y2": 180.0}},
        {"class": "altra", "confidence": 0.5,
         "bbox": {"x1": 10.0, "y1": 10.0, "x2": 50.0, "y2": 50.0}},
    ]}


def test_detect_front_filters_class_and_returns_top():
    with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \
         patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f:
        yolo = MagicMock()
        yolo.detect_url.return_value = _fake_yolo_top()
        ying.return_value = yolo
        cam = MagicMock()
        cam.rgb_url.return_value = "http://192.168.123.99:8878/api/rgb"
        cam.image_size.return_value = (640, 480)
        cam_f.return_value = cam

        r = client.post("/api/right_arm/detect/front",
                        json={"class_name": "pestello", "conf": 0.4})
        assert r.status_code == 200
        body = r.json()
        assert body["detections"][0]["class"] == "pestello"
        assert body["image_size"] == [640, 480]


def test_detect_front_normalizes_real_yolo_envelope_with_list_bbox():
    """Regressione: il YOLO server reale ritorna bbox come lista [x1,y1,x2,y2]
    dentro {success, data, error}. Il router deve normalizzare in {x1,y1,x2,y2}."""
    real_payload = {
        "success": True,
        "data": {
            "detections": [
                {"bbox": [376.7, 17.1, 500.7, 193.7],
                 "confidence": 0.949, "class": "Pestello"},
                {"bbox": [95.6, 134.8, 207.7, 231.3],
                 "confidence": 0.917, "class": "Mortaio"},
            ],
        },
        "error": None,
    }
    with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \
         patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f:
        yolo = MagicMock()
        yolo.detect_url.return_value = real_payload
        ying.return_value = yolo
        cam = MagicMock()
        cam.rgb_url.return_value = "http://192.168.123.99:8878/api/rgb"
        cam.image_size.return_value = (640, 480)
        cam_f.return_value = cam

        r = client.post("/api/right_arm/detect/front",
                        json={"class_name": "Pestello"})
        assert r.status_code == 200
        body = r.json()
        top = body["detections"][0]
        assert top["class"] == "Pestello"
        assert top["bbox"] == {"x1": 376.7, "y1": 17.1, "x2": 500.7, "y2": 193.7}


def test_detect_front_404_when_no_match():
    with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \
         patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f:
        yolo = MagicMock()
        yolo.detect_url.return_value = {"detections": []}
        ying.return_value = yolo
        cam = MagicMock()
        cam.rgb_url.return_value = "http://x/api/rgb"
        cam.image_size.return_value = (640, 480)
        cam_f.return_value = cam
        r = client.post("/api/right_arm/detect/front",
                        json={"class_name": "pestello"})
        assert r.status_code == 404


def test_detect_front_falls_back_to_proxy_url_on_yolo_failure():
    """Se YOLO non riesce sull'URL diretto, ripeti con `proxy_base_url`."""
    attempts = []

    def _detect_url(image_url, conf=None):
        attempts.append(image_url)
        if "192.168" in image_url:
            raise RuntimeError("dns timeout")
        return _fake_yolo_top()

    with patch("presa_mano.api.right_arm_routes._deps.yolo") as ying, \
         patch("presa_mano.api.right_arm_routes._deps.front_camera") as cam_f:
        yolo = MagicMock()
        yolo.detect_url.side_effect = _detect_url
        ying.return_value = yolo
        cam = MagicMock()
        cam.rgb_url.return_value = "http://192.168.123.99:8878/api/rgb"
        cam.image_size.return_value = (640, 480)
        cam_f.return_value = cam

        r = client.post("/api/right_arm/detect/front",
                        json={"class_name": "pestello",
                              "proxy_base_url": "http://localhost:8890"})
        assert r.status_code == 200
        assert len(attempts) == 2
        assert "192.168" in attempts[0]
        assert attempts[1].endswith("/api/right_arm/snapshot/front")


def test_detect_wrist_returns_centroid_and_distance():
    # YOLO → bbox → SAM21 → centroide
    import numpy as np
    fake_points = np.array([
        [0.01, -0.02, 0.20],
        [0.02, -0.01, 0.21],
        [0.00, -0.03, 0.19],
    ] * 5, dtype=np.float64)  # 15 punti, > 10 minimo

    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f:
        nuwa = MagicMock()
        nuwa.rgb_url.return_value = "http://localhost:8877/api/rgb"
        nuwa_f.return_value = nuwa
        yolo = MagicMock()
        yolo.detect_url.return_value = _fake_yolo_top()
        yolo_f.return_value = yolo
        sam_f.return_value = MagicMock()
        seg = MagicMock()
        seg.points_cv_m = fake_points
        seg_f.return_value = seg

        r = client.post("/api/right_arm/detect/wrist",
                        json={"class_name": "pestello"})
        assert r.status_code == 200
        body = r.json()
        assert body["detection"]["class"] == "pestello"
        assert "bbox" in body
        c = body["centroid_camera_m"]
        assert abs(c[2] - 0.20) < 0.05
        assert body["distance_m"] > 0
        assert body["n_sam21_points"] == 15
        assert body["n_centroid_points"] == 15  # cluster compatto: tutti tenuti


def test_detect_wrist_filters_far_background_from_centroid():
    """Regressione: nuvola bimodale (oggetto vicino + background lontano)
    deve dare centroide dell'oggetto, non della media (che sarebbe lontana)."""
    import numpy as np
    near = np.column_stack([np.zeros(100), np.zeros(100),
                            np.full(100, 0.25)])
    far = np.column_stack([np.zeros(50), np.zeros(50),
                           np.full(50, 4.0)])
    fake_points = np.vstack([near, far])  # media ingenua: ~1.5m; cluster: ~0.25m

    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f:
        nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa
        yolo = MagicMock(); yolo.detect_url.return_value = _fake_yolo_top(); yolo_f.return_value = yolo
        sam_f.return_value = MagicMock()
        seg = MagicMock(); seg.points_cv_m = fake_points; seg_f.return_value = seg

        r = client.post("/api/right_arm/detect/wrist",
                        json={"class_name": "pestello"})
        assert r.status_code == 200
        body = r.json()
        assert body["n_sam21_points"] == 150
        assert body["n_centroid_points"] == 100  # solo i vicini
        assert body["centroid_camera_m"][2] == 0.25  # non 1.5m della media


def test_detect_wrist_422_too_few_points():
    import numpy as np
    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f:
        nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa
        yolo = MagicMock(); yolo.detect_url.return_value = _fake_yolo_top(); yolo_f.return_value = yolo
        sam_f.return_value = MagicMock()
        seg = MagicMock(); seg.points_cv_m = np.zeros((3, 3))
        seg_f.return_value = seg
        r = client.post("/api/right_arm/detect/wrist",
                        json={"class_name": "pestello"})
        assert r.status_code == 422


def test_detect_wrist_404_when_yolo_no_match():
    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f:
        nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa
        yolo = MagicMock(); yolo.detect_url.return_value = {"detections": []}; yolo_f.return_value = yolo
        r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"})
        assert r.status_code == 404


def _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20):
    """Helper: prepara un cloud SAM21 sintetico con centroide a `centroid_m`."""
    import numpy as np
    seg_pts = np.tile(np.array(centroid_m, dtype=np.float64), (n_points, 1))
    return seg_pts


def _stub_tab_cfg(**overrides):
    """Config fissa per i test del move (isolata da modifiche utente al config.yaml)."""
    base = {
        "move_offset_camera_mm": {"x": 0.0, "y": 0.0, "z": 100.0},
        "axis_sign_cam_to_wrist": {"x": 1, "y": -1, "z": -1},
        "camera_offset_wrist_mm": {"x": 0.0, "y": 0.0, "z": 0.0},
        "cluster_depth_window_m": 0.20,
        "max_step_m": 0.15,
        "z_floor_m": None,
        "home_pose": {"position": [0.30, -0.20, 0.30], "rpy_deg": [0, 0, 0]},
        "custom_pose": {"position": [0.30, -0.20, 0.30], "rpy_deg": [0, 0, 0]},
    }
    base.update(overrides)
    return base


def test_move_toward_dry_run_returns_delta_no_call_to_g1():
    import numpy as np
    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20)
    g1_called = []

    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
         patch("presa_mano.api.right_arm_routes._deps.tab_right_arm_cfg",
               return_value=_stub_tab_cfg()), \
         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
        nuwa_f.return_value.rgb_url.return_value = "http://x"
        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
        g1 = MagicMock()
        g1.wrist.return_value = _fake_wrist_response()

        def _track_relative(**kw):
            g1_called.append(kw)
            return {"ok": True}
        g1.wrist_relative.side_effect = _track_relative
        g1_f.return_value = g1

        r = client.post("/api/right_arm/move/toward",
                        json={"class_name": "pestello", "dry_run": True})
        assert r.status_code == 200
        body = r.json()
        # offset default (0,0,100mm) → centroid - offset = (0, 0, 0.10) →
        # con sign (1,-1,-1) → (0, 0, -0.10) m
        assert abs(body["delta_wrist_m"][2] - (-0.10)) < 1e-6
        assert body["applied"] is False
        # g1.wrist_relative viene comunque invocato (G1Client gestisce dry-run
        # internamente; l'endpoint non lo salta).
        assert len(g1_called) == 1
        assert g1_called[0]["side"] == "right"


def test_move_toward_real_call_returns_wrist_after():
    import numpy as np
    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.20), n_points=20)
    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
         patch("presa_mano.api.right_arm_routes._deps.tab_right_arm_cfg",
               return_value=_stub_tab_cfg()), \
         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
        nuwa_f.return_value.rgb_url.return_value = "http://x"
        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
        g1 = MagicMock()
        g1.wrist.return_value = _fake_wrist_response()
        g1.wrist_relative.return_value = {"ok": True}
        g1_f.return_value = g1
        r = client.post("/api/right_arm/move/toward",
                        json={"class_name": "pestello", "dry_run": False})
        assert r.status_code == 200
        body = r.json()
        assert body["applied"] is True
        assert body["wrist_after"] is not None


def test_move_toward_422_oversize_delta_without_force():
    import numpy as np
    # centroid=(0,0,0.50), default offset=(0,0,0.10) → delta_z=-0.40 → > max_step 0.15
    seg_pts = _patch_full_detect_chain(centroid_m=(0.0, 0.0, 0.50), n_points=20)
    with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \
         patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \
         patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \
         patch("presa_mano.api.right_arm_routes._deps.sam21"), \
         patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
         patch("presa_mano.api.right_arm_routes._deps.tab_right_arm_cfg",
               return_value=_stub_tab_cfg()), \
         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
        nuwa_f.return_value.rgb_url.return_value = "http://x"
        yolo_f.return_value.detect_url.return_value = _fake_yolo_top()
        seg_f.return_value = MagicMock(points_cv_m=seg_pts)
        g1 = MagicMock(); g1.wrist.return_value = _fake_wrist_response(); g1_f.return_value = g1
        r = client.post("/api/right_arm/move/toward",
                        json={"class_name": "pestello", "dry_run": True, "force": False})
        assert r.status_code == 422
        assert "delta" in r.json()["detail"].lower()


def test_home_calls_wrist_absolute_with_home_pose():
    calls = []
    def _abs(**kw):
        calls.append(kw)
        return {"ok": True}
    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1
        r = client.post("/api/right_arm/home", json={"dry_run": True})
        assert r.status_code == 200
        assert len(calls) == 1
        assert calls[0]["side"] == "right"
        assert len(calls[0]["position"]) == 3
        assert len(calls[0]["quaternion"]) == 4


def test_custom_pose_executes_pose_with_dry_run_default_false():
    calls = []
    def _abs(**kw):
        calls.append(kw); return {"ok": True}
    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1
        r = client.post("/api/right_arm/custom_pose", json={})
        assert r.status_code == 200
        assert len(calls) == 1


def test_put_custom_pose_updates_config():
    new_pos = [0.40, -0.15, 0.25]
    new_rpy = [10.0, -5.0, 90.0]
    r = client.put("/api/right_arm/custom_pose",
                   json={"position": new_pos, "rpy_deg": new_rpy})
    assert r.status_code == 200
    r2 = client.get("/api/config")
    cp = r2.json()["tab_right_arm"]["custom_pose"]
    assert cp["position"] == new_pos
    assert cp["rpy_deg"] == new_rpy


def test_put_offset_updates_config():
    r = client.put("/api/right_arm/offset",
                   json={"move_offset_camera_mm": {"x": 5, "y": -3, "z": 75},
                         "axis_sign_cam_to_wrist": {"x": -1, "y": 1, "z": -1}})
    assert r.status_code == 200
    r2 = client.get("/api/config")
    body = r2.json()["tab_right_arm"]
    assert body["move_offset_camera_mm"] == {"x": 5, "y": -3, "z": 75}
    assert body["axis_sign_cam_to_wrist"] == {"x": -1, "y": 1, "z": -1}