import pytest
import numpy as np

from h01_camera_model import CameraModel
from f02_1_flight_lifecycle_manager import CameraParameters

@pytest.fixture
def camera_model():
    return CameraModel()

@pytest.fixture
def camera_params():
    return CameraParameters(
        focal_length_mm=25.0,
        sensor_width_mm=36.0,
        resolution={"width": 640, "height": 480}
    )

class TestCameraModel:
    
    def test_intrinsic_matrix_structure(self, camera_model, camera_params):
        K = camera_model._get_intrinsics(camera_params)
        # fx = 25 * 640 / 36 = 444.444
        assert np.isclose(K[0, 0], 444.444, atol=1e-3)
        assert np.isclose(K[0, 2], 320.0)
        
    def test_pixel_to_ray_principal_point(self, camera_model, camera_params):
        ray = camera_model.unproject((320.0, 240.0), 1.0, camera_params)
        np.testing.assert_allclose(ray, np.array([0.0, 0.0, 1.0]))
        
    def test_project_to_pixel(self, camera_model, camera_params):
        point_3d = np.array([0.0, 0.0, 10.0])
        u, v = camera_model.project(point_3d, camera_params)
        
        assert u == 320.0
        assert v == 240.0
        
    def test_projection_unprojection_cycle(self, camera_model, camera_params):
        original_u, original_v = 150.0, 400.0
        ray = camera_model.unproject((original_u, original_v), 50.0, camera_params)
        projected_u, projected_v = camera_model.project(ray, camera_params)
        assert np.isclose(original_u, projected_u)
        assert np.isclose(original_v, projected_v)