import pytest
import numpy as np
from unittest.mock import Mock
import math

from f14_coordinate_transformer import CoordinateTransformer, GPSPoint, Sim3Transform, OriginNotSetError

@pytest.fixture
def mock_camera_model():
    model = Mock()
    # By default, mock the camera's un-projection mapping a pixel directly forward (+Z)
    model.pixel_to_ray.return_value = np.array([0.0, 0.0, 1.0])
    return model

@pytest.fixture
def transformer(mock_camera_model):
    return CoordinateTransformer(camera_model=mock_camera_model)

class TestCoordinateTransformer:
    
    def test_enu_to_gps_without_origin_raises_error(self, transformer):
        """Verifies that an exception is raised if global conversion is attempted without an ENU origin."""
        with pytest.raises(OriginNotSetError, match="Origin not set for flight"):
            transformer.enu_to_gps("invalid_flight", (10.0, 10.0, 5.0))
            
    def test_gps_to_enu_at_origin_returns_zero(self, transformer):
        origin = GPSPoint(lat=48.0, lon=37.0, altitude_m=100.0)
        flight_id = "test_flight"
        transformer.set_enu_origin(flight_id, origin)
        
        enu = transformer.gps_to_enu(flight_id, origin)
        assert np.isclose(enu[0], 0.0)
        assert np.isclose(enu[1], 0.0)
        assert np.isclose(enu[2], 0.0)

    def test_enu_to_gps_conversion(self, transformer):
        """Tests local Metric (ENU) to Global Geodetic (GPS) coordinate transformations."""
        origin = GPSPoint(lat=48.0, lon=37.0, altitude_m=100.0)
        flight_id = "test_flight"
        transformer.set_enu_origin(flight_id, origin)

        # Move 1000m East, 2000m North, 50m Up
        x, y, z = 1000.0, 2000.0, 50.0
        
        result = transformer.enu_to_gps(flight_id, (x, y, z))
        
        assert result.altitude_m == 150.0
        assert result.lat > 48.0
        assert result.lon > 37.0
        
        # Validate spherical approximation logic
        earth_radius = 6378137.0
        expected_lat = 48.0 + math.degrees(y / earth_radius)
        expected_lon = 37.0 + math.degrees(x / (earth_radius * math.cos(math.radians(48.0))))
        
        np.testing.assert_allclose(result.lat, expected_lat)
        np.testing.assert_allclose(result.lon, expected_lon)

    def test_ray_cloud_intersection_hit(self, transformer):
        """Tests that the vector math correctly identifies the closest 3D point in front of the ray."""
        ray_origin = np.array([0.0, 0.0, 0.0])
        ray_dir = np.array([0.0, 0.0, 1.0]) # Pointing straight forward (+Z)
        
        point_cloud = np.array([
            [0.0, 0.0, -10.0],  # Rejected: Behind the camera
            [5.0, 5.0, 10.0],   # Rejected: Too far laterally (dist = sqrt(50) > 2.0)
            [0.5, 0.5, 10.0]    # Selected: Hits within the 2.0m max_dist radius!
        ])
        
        result = transformer._ray_cloud_intersection(ray_origin, ray_dir, point_cloud, max_dist=2.0)
        
        assert result is not None
        np.testing.assert_array_equal(result, np.array([0.5, 0.5, 10.0]))

    def test_ray_cloud_intersection_miss(self, transformer):
        """Tests that intersection gracefully returns None when no map features align with the ray."""
        ray_origin = np.array([0.0, 0.0, 0.0])
        ray_dir = np.array([0.0, 0.0, 1.0])
        
        # All points too far from the central ray
        point_cloud = np.array([
            [10.0, 0.0, 5.0],
            [0.0, 10.0, 5.0]
        ])
        
        result = transformer._ray_cloud_intersection(ray_origin, ray_dir, point_cloud, max_dist=2.0)
        assert result is None
        
    def test_pixel_to_gps_success(self, transformer, mock_camera_model):
        """End-to-End test of the Object-Click to absolute GPS calculation passing through Sim(3) graphs."""
        flight_id = "test_flight"
        transformer.set_enu_origin(flight_id, GPSPoint(lat=48.0, lon=37.0, altitude_m=0.0))
        
        u, v = 320.0, 240.0
        local_pose = np.eye(4) # Local V-SLAM camera at origin, looking down +Z
        local_point_cloud = np.array([[0.0, 0.0, 20.0]]) # 3D Feature point directly on the target
        
        # Sim(3) transformation from global optimization (AC-2 correction)
        sim3 = Sim3Transform(
            translation=np.array([10.0, 20.0, 30.0]), 
            rotation=np.eye(3),
            scale=2.0
        )
        
        response = transformer.pixel_to_gps(flight_id, u, v, local_pose, local_point_cloud, sim3)
        
        assert response is not None
        # Accuracy is geometrically coupled with the Sim3 scale factor (5.0m base * 2.0 scale)
        assert response.accuracy_meters == 10.0 
        
        # p_local = [0, 0, 20]
        # p_metric = scale * (R * p_local) + T
        # p_metric = 2.0 * [0, 0, 20] + [10, 20, 30] 
        # p_metric = [10, 20, 70]
        
        assert response.gps.altitude_m == 70.0