Presa-Mano/tests/test_pose_3d_routes.py

"""Integration test del router /api/pose_3d/*."""
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_pair():
    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_both_wrists():
    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
        g1 = MagicMock()
        g1.wrist.return_value = _fake_wrist_pair()
        g1_factory.return_value = g1
        r = client.get("/api/pose_3d/state")
        assert r.status_code == 200
        body = r.json()
        assert "wrist" in body
        assert body["wrist"]["left"]["position"] == [0.10, 0.20, 0.30]
        assert body["wrist"]["right"]["position"] == [0.30, -0.20, 0.30]
        assert "poses" in body
        assert "live_max_step_m" in body
        assert body["robot_active"] in ("g1", "h1")


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


def test_apply_dry_run_calls_g1_with_dry_run_true():
    captured = {}
    def _abs(**kw):
        captured.update(kw)
        return {"ok": True, "reached_target": True}
    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
        g1 = MagicMock()
        g1.wrist_absolute.side_effect = _abs
        g1_factory.return_value = g1
        r = client.post("/api/pose_3d/apply", json={
            "side": "right",
            "position": [0.30, -0.20, 0.30],
            "quaternion": [0, 0, 0, 1],
            "dry_run": True,
        })
        assert r.status_code == 200
        body = r.json()
        assert body["applied"] is False
        # Verifica che il client G1 sia stato creato con dry_run=True
        # (passato a _deps.g1(dry_run=...))
        assert g1_factory.call_args.kwargs.get("dry_run") is True
        assert captured["side"] == "right"
        assert captured["position"] == [0.30, -0.20, 0.30]


def test_apply_real_call():
    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
        g1 = MagicMock()
        g1.wrist_absolute.return_value = {"ok": True, "reached_target": True}
        g1_factory.return_value = g1
        r = client.post("/api/pose_3d/apply", json={
            "side": "left", "position": [0.1, 0.2, 0.3],
            "quaternion": [0, 0, 0, 1], "dry_run": False,
        })
        assert r.status_code == 200
        body = r.json()
        assert body["applied"] is True
        assert g1_factory.call_args.kwargs.get("dry_run") is False


def test_apply_422_bad_side():
    r = client.post("/api/pose_3d/apply", json={
        "side": "middle", "position": [0,0,0], "quaternion": [0,0,0,1],
    })
    assert r.status_code == 422


def test_apply_422_degenerate_quaternion():
    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
        g1_factory.return_value = MagicMock()
        r = client.post("/api/pose_3d/apply", json={
            "side": "right", "position": [0,0,0], "quaternion": [0,0,0,0],
        })
        assert r.status_code == 422
        assert "degenerate" in r.json()["detail"]


def test_apply_normalizes_quaternion_before_forwarding():
    captured = {}
    def _abs(**kw):
        captured.update(kw)
        return {"ok": True}
    with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory:
        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs
        g1_factory.return_value = g1
        # quat con scala 2.0
        r = client.post("/api/pose_3d/apply", json={
            "side": "right", "position": [0,0,0], "quaternion": [0, 0, 0, 2.0],
        })
        assert r.status_code == 200
        # Il g1 deve ricevere il quat normalizzato (norma 1)
        q = captured["quaternion"]
        norm = sum(x*x for x in q) ** 0.5
        assert abs(norm - 1.0) < 1e-6


def test_put_pose_persists_in_config():
    payload = {
        "side": "right",
        "position": [0.30, -0.20, 0.30],
        "quaternion": [0, 0, 0, 1],
        "note": "test pose",
    }
    r = client.put("/api/pose_3d/poses/test1", json=payload)
    assert r.status_code == 200
    body = r.json()
    assert body["pose"]["side"] == "right"
    assert body["pose"]["note"] == "test pose"
    # Verifica che sia anche in GET /poses
    r2 = client.get("/api/pose_3d/poses")
    assert "test1" in r2.json()


def test_put_pose_422_bad_name():
    r = client.put("/api/pose_3d/poses/with spaces!", json={
        "side": "right", "position": [0,0,0], "quaternion": [0,0,0,1],
    })
    assert r.status_code == 422
    assert "name" in r.json()["detail"].lower()


def test_delete_pose_removes_it():
    # prima salva
    client.put("/api/pose_3d/poses/todelete", json={
        "side": "left", "position": [0,0,0], "quaternion": [0,0,0,1],
    })
    # poi cancella
    r = client.delete("/api/pose_3d/poses/todelete")
    assert r.status_code == 200
    # verifica non presente
    r2 = client.get("/api/pose_3d/poses")
    assert "todelete" not in r2.json()


def test_delete_pose_404_when_missing():
    r = client.delete("/api/pose_3d/poses/nonexistent")
    assert r.status_code == 404