import numpy as np
import pyzed.sl as sl
from typing import Any, cast, TYPE_CHECKING
from aruco.svo_sync import SVOReader

if TYPE_CHECKING:
    from _pytest.monkeypatch import MonkeyPatch


class FakeMat:
    def __init__(self) -> None:
        self.data: np.ndarray = np.array([])

    def get_data(self) -> np.ndarray:
        return self.data


class FakeInitParameters:
    coordinate_units: sl.UNIT

    def __init__(self, units: sl.UNIT) -> None:
        self.coordinate_units = units


class FakeCamera:
    init_params: FakeInitParameters

    def __init__(self, units: sl.UNIT) -> None:
        self.init_params = FakeInitParameters(units)

    def get_init_parameters(self) -> FakeInitParameters:
        return self.init_params

    def retrieve_measure(self, _mat: Any, _measure: sl.MEASURE) -> sl.ERROR_CODE:
        return sl.ERROR_CODE.SUCCESS


def test_retrieve_depth_unit_guard(monkeypatch: "MonkeyPatch") -> None:
    # Setup SVOReader with depth enabled
    reader = SVOReader([], depth_mode=sl.DEPTH_MODE.ULTRA)

    # Mock depth data (e.g., 2.0 meters)
    depth_data = np.full((100, 100), 2.0, dtype=np.float32)

    def fake_mat_factory() -> FakeMat:
        m = FakeMat()
        m.data = depth_data
        return m

    monkeypatch.setattr("aruco.svo_sync.sl.Mat", fake_mat_factory)

    # Case 1: Units are METER -> Should NOT divide by 1000
    fake_cam_meter = FakeCamera(sl.UNIT.METER)
    cam_meter = cast(sl.Camera, cast(object, fake_cam_meter))

    depth_meter = reader._retrieve_depth(cam_meter)  # pyright: ignore [reportPrivateUsage]
    assert depth_meter is not None
    assert np.allclose(depth_meter, 2.0)

    # Case 2: Units are MILLIMETER -> Should divide by 1000
    fake_cam_mm = FakeCamera(sl.UNIT.MILLIMETER)
    cam_mm = cast(sl.Camera, cast(object, fake_cam_mm))

    depth_mm = reader._retrieve_depth(cam_mm)  # pyright: ignore [reportPrivateUsage]
    assert depth_mm is not None
    assert np.allclose(depth_mm, 0.002)


if __name__ == "__main__":
    import pytest

    _ = pytest.main([__file__])