"""Unit tests for camera-to-pixel projection."""
from __future__ import annotations

import numpy as np

from presa_mano.perception.projection import K_from_intrinsics, project_camera_to_pixels


def _K():
    return np.array([[600.0,   0.0, 320.0],
                     [  0.0, 600.0, 240.0],
                     [  0.0,   0.0,   1.0]])


def test_K_from_intrinsics_matches_manual():
    intr = {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0,
            "width": 640, "height": 480}
    K = K_from_intrinsics(intr)
    np.testing.assert_array_equal(K, _K())


def test_project_principal_point():
    """Punto sull'asse ottico a 1m -> cx,cy."""
    pts = np.array([[0.0, 0.0, 1000.0]])  # mm
    out = project_camera_to_pixels(pts, _K(), (640, 480))
    assert out.shape == (1, 2)
    assert out[0, 0] == 320 and out[0, 1] == 240


def test_project_behind_camera_dropped():
    """Z<=0 deve essere scartato."""
    pts = np.array([[0.0, 0.0, -1000.0], [10.0, 20.0, 0.0]])
    out = project_camera_to_pixels(pts, _K(), (640, 480))
    assert out.shape == (0, 2)


def test_project_outside_frame_dropped():
    """Punto che proietta fuori bbox immagine viene scartato."""
    pts = np.array([
        [0.0, 0.0, 1000.0],         # -> (320, 240): inside
        [10000.0, 0.0, 1000.0],     # -> u molto fuori bbox: dropped
        [0.0, -10000.0, 1000.0],    # -> v molto fuori bbox (sopra): dropped
    ])
    out = project_camera_to_pixels(pts, _K(), (640, 480))
    assert out.shape == (1, 2)
    assert out[0, 0] == 320 and out[0, 1] == 240


def test_project_known_offset():
    """Punto in (0.1, 0.0, 1.0) m -> u = cx + fx*(0.1/1.0) = 320 + 60 = 380."""
    pts = np.array([[100.0, 0.0, 1000.0]])  # mm
    out = project_camera_to_pixels(pts, _K(), (640, 480))
    assert out[0, 0] == 380 and out[0, 1] == 240


def test_project_unit_invariance():
    """Stesso punto in m vs mm proietta sullo stesso pixel (K@P/P.z elide unità)."""
    pts_mm = np.array([[100.0, 0.0, 1000.0]])
    pts_m  = pts_mm / 1000.0
    out_mm = project_camera_to_pixels(pts_mm, _K(), (640, 480))
    out_m  = project_camera_to_pixels(pts_m,  _K(), (640, 480))
    np.testing.assert_array_equal(out_mm, out_m)


def test_project_empty_input():
    out = project_camera_to_pixels(np.empty((0, 3)), _K(), (640, 480))
    assert out.shape == (0, 2)