"""Tests for CoordinateTransformer (F13) — Real coordinate chain (ESKF-06)."""

import numpy as np
import pytest

from gps_denied.core.coordinates import (
    CoordinateTransformer,
    OriginNotSetError,
    _build_intrinsic_matrix,
    _cam_to_body_rotation,
    _quat_to_rotation_matrix,
)
from gps_denied.schemas import CameraParameters, GPSPoint


@pytest.fixture
def transformer():
    return CoordinateTransformer()


def test_enu_origin_management(transformer):
    fid = "flight_123"
    origin = GPSPoint(lat=48.0, lon=37.0)

    # Before setting
    with pytest.raises(OriginNotSetError):
        transformer.get_enu_origin(fid)

    # After setting
    transformer.set_enu_origin(fid, origin)
    assert transformer.get_enu_origin(fid).lat == 48.0


def test_gps_to_enu(transformer):
    fid = "flight_123"
    origin = GPSPoint(lat=48.0, lon=37.0)
    transformer.set_enu_origin(fid, origin)

    # Same point -> 0, 0, 0
    enu = transformer.gps_to_enu(fid, origin)
    assert enu == (0.0, 0.0, 0.0)

    # Point north
    target = GPSPoint(lat=48.01, lon=37.0)
    enu_n = transformer.gps_to_enu(fid, target)
    assert enu_n[0] == 0.0
    assert enu_n[1] > 1000.0  # 0.01 deg lat is > 1km
    assert enu_n[2] == 0.0


def test_enu_roundtrip(transformer):
    fid = "flight_123"
    origin = GPSPoint(lat=48.0, lon=37.0)
    transformer.set_enu_origin(fid, origin)

    test_gps = GPSPoint(lat=48.056, lon=37.123)
    enu = transformer.gps_to_enu(fid, test_gps)

    recovered = transformer.enu_to_gps(fid, enu)
    assert pytest.approx(recovered.lat, abs=1e-6) == test_gps.lat
    assert pytest.approx(recovered.lon, abs=1e-6) == test_gps.lon


def test_pixel_to_gps_flow(transformer):
    fid = "flight_123"
    origin = GPSPoint(lat=48.0, lon=37.0)
    transformer.set_enu_origin(fid, origin)

    cam = CameraParameters(
        focal_length=25.0,
        sensor_width=23.5,
        sensor_height=15.6,
        resolution_width=4000,
        resolution_height=3000,
    )

    # Image center should yield approximately the frame center
    cx = cam.resolution_width / 2.0
    cy = cam.resolution_height / 2.0
    pixel = (cx, cy)
    pose = {"position": [0, 0, 0]}
    q_identity = np.array([1.0, 0.0, 0.0, 0.0])

    gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0, quaternion=q_identity)
    assert pytest.approx(gps_res.lat, abs=1e-4) == origin.lat
    assert pytest.approx(gps_res.lon, abs=1e-4) == origin.lon

    # Inverse must match pixel
    pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0, quaternion=q_identity)
    assert abs(pix_res[0] - pixel[0]) < 1.0
    assert abs(pix_res[1] - pixel[1]) < 1.0

    # And image_object_to_gps should work
    obj_gps = transformer.image_object_to_gps(
        fid, 1, pixel, frame_pose=pose, camera_params=cam, altitude=100.0, quaternion=q_identity
    )
    assert pytest.approx(obj_gps.lat, abs=1e-4) == origin.lat


# ============================================================================
# ESKF-06: Real Coordinate Chain Tests
# ============================================================================


class TestIntrinsicMatrix:
    """Tests for camera intrinsic matrix construction."""

    def test_build_intrinsic_matrix_adti_20l_v1(self):
        """Test K matrix for ADTI 20L V1 camera with 16mm lens."""
        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )
        K = _build_intrinsic_matrix(cam)

        # Expected focal lengths
        fx_expected = 16.0 * 5456 / 23.2
        fy_expected = 16.0 * 3632 / 15.4

        assert K.shape == (3, 3)
        assert abs(K[0, 0] - fx_expected) < 1.0  # fx
        assert abs(K[1, 1] - fy_expected) < 1.0  # fy
        assert abs(K[0, 2] - 2728.0) < 1.0  # cx (center)
        assert abs(K[1, 2] - 1816.0) < 1.0  # cy (center)
        assert K[2, 2] == 1.0

    def test_build_intrinsic_matrix_custom_principal_point(self):
        """Test K matrix with custom principal point."""
        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
            principal_point=(2700.0, 1800.0),
        )
        K = _build_intrinsic_matrix(cam)

        assert abs(K[0, 2] - 2700.0) < 0.1  # cx
        assert abs(K[1, 2] - 1800.0) < 0.1  # cy


class TestCameraToBodyRotation:
    """Tests for camera-to-body rotation matrix."""

    def test_cam_to_body_is_180_x_rotation(self):
        """Test that camera-to-body rotation is Rx(180deg)."""
        R_cam_body = _cam_to_body_rotation()

        # Should flip Y and Z axes, keep X
        assert R_cam_body.shape == (3, 3)
        assert R_cam_body[0, 0] == 1.0
        assert R_cam_body[1, 1] == -1.0
        assert R_cam_body[2, 2] == -1.0
        assert np.allclose(np.linalg.det(R_cam_body), 1.0)  # det = 1 (proper rotation)


class TestQuaternionRotation:
    """Tests for quaternion to rotation matrix conversion."""

    def test_identity_quaternion(self):
        """Test identity quaternion [1, 0, 0, 0] produces identity matrix."""
        q = np.array([1.0, 0.0, 0.0, 0.0])
        R = _quat_to_rotation_matrix(q)

        assert np.allclose(R, np.eye(3))

    def test_90_degree_z_rotation(self):
        """Test 90-degree rotation around Z axis."""
        # q = [cos(45deg), 0, 0, sin(45deg)] for 90-degree rotation around Z
        angle = np.pi / 4  # 45 degrees
        q = np.array([np.cos(angle), 0.0, 0.0, np.sin(angle)])
        R = _quat_to_rotation_matrix(q)

        # Should rotate [1, 0, 0] to approximately [0, 1, 0]
        x_axis = np.array([1.0, 0.0, 0.0])
        rotated = R @ x_axis
        expected = np.array([0.0, 1.0, 0.0])
        assert np.allclose(rotated, expected, atol=0.01)


class TestPixelToGPSChain:
    """Tests for full pixel-to-GPS projection chain (ESKF-06)."""

    def test_image_center_nadir_projection(self):
        """Test that image center pixel projects to UAV nadir position."""
        transformer = CoordinateTransformer()
        origin = GPSPoint(lat=48.0, lon=37.0)
        transformer.set_enu_origin("f1", origin)

        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )

        # Center pixel with identity quaternion (level flight)
        pixel = (2728.0, 1816.0)
        pose = {"position": [0.0, 0.0, 0.0]}
        q_identity = np.array([1.0, 0.0, 0.0, 0.0])

        gps = transformer.pixel_to_gps(
            "f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
        )

        # Center pixel should project to UAV nadir (origin)
        assert abs(gps.lat - 48.0) < 0.001
        assert abs(gps.lon - 37.0) < 0.001

    def test_pixel_offset_produces_ground_offset(self):
        """Test that off-center pixel offsets project to corresponding ground distance."""
        transformer = CoordinateTransformer()
        origin = GPSPoint(lat=48.0, lon=37.0)
        transformer.set_enu_origin("f1", origin)

        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )

        pose = {"position": [0.0, 0.0, 0.0]}
        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
        altitude = 600.0

        # Center pixel
        center_pixel = (2728.0, 1816.0)
        center_gps = transformer.pixel_to_gps(
            "f1", center_pixel, pose, cam, altitude=altitude, quaternion=q_identity
        )

        # Offset pixel (100 pixels to the right)
        offset_pixel = (2828.0, 1816.0)
        offset_gps = transformer.pixel_to_gps(
            "f1", offset_pixel, pose, cam, altitude=altitude, quaternion=q_identity
        )

        # Longitude difference should be non-zero (right = east = longer)
        assert offset_gps.lon != center_gps.lon

    def test_altitude_scaling_is_linear(self):
        """Test that doubling altitude doubles ground distance from nadir."""
        transformer = CoordinateTransformer()
        origin = GPSPoint(lat=48.0, lon=37.0)
        transformer.set_enu_origin("f1", origin)

        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )

        pose = {"position": [0.0, 0.0, 0.0]}
        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
        pixel = (2728.0 + 100, 1816.0)  # Offset pixel

        # Project at altitude 300m
        gps_300 = transformer.pixel_to_gps(
            "f1", pixel, pose, cam, altitude=300.0, quaternion=q_identity
        )

        # Project at altitude 600m
        gps_600 = transformer.pixel_to_gps(
            "f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
        )

        # Distance from origin should scale with altitude (roughly)
        # At 600m, ground distance should be ~2x that at 300m
        dist_300 = abs(gps_300.lon - 37.0)
        dist_600 = abs(gps_600.lon - 37.0)
        ratio = dist_600 / dist_300 if dist_300 > 0 else 1.0
        assert 1.8 < ratio < 2.2  # Allow some numerical tolerance


class TestGPSToPixelInverse:
    """Tests for GPS-to-pixel projection (inverse of pixel-to-GPS)."""

    def test_gps_to_pixel_is_inverse(self):
        """Test that gps_to_pixel is the exact inverse of pixel_to_gps."""
        transformer = CoordinateTransformer()
        origin = GPSPoint(lat=48.0, lon=37.0)
        transformer.set_enu_origin("f1", origin)

        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )

        pose = {"position": [0.0, 0.0, 0.0]}
        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
        original_pixel = (2728.0 + 50, 1816.0 - 30)

        # pixel -> GPS
        gps = transformer.pixel_to_gps(
            "f1", original_pixel, pose, cam, altitude=600.0, quaternion=q_identity
        )

        # GPS -> pixel
        recovered_pixel = transformer.gps_to_pixel(
            "f1", gps, pose, cam, altitude=600.0, quaternion=q_identity
        )

        # Should recover original pixel
        assert abs(recovered_pixel[0] - original_pixel[0]) < 1.0
        assert abs(recovered_pixel[1] - original_pixel[1]) < 1.0

    def test_roundtrip_with_quaternion(self):
        """Test pixel-GPS-pixel roundtrip with non-identity quaternion."""
        transformer = CoordinateTransformer()
        origin = GPSPoint(lat=48.0, lon=37.0)
        transformer.set_enu_origin("f1", origin)

        cam = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )

        pose = {"position": [100.0, 50.0, 0.0]}
        # Small 30-degree rotation around Z
        angle = np.radians(30)
        q = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)])
        original_pixel = (2728.0, 1816.0)  # Center

        gps = transformer.pixel_to_gps(
            "f1", original_pixel, pose, cam, altitude=600.0, quaternion=q
        )

        recovered_pixel = transformer.gps_to_pixel(
            "f1", gps, pose, cam, altitude=600.0, quaternion=q
        )

        # Center pixel should still recover to center
        assert abs(recovered_pixel[0] - 2728.0) < 10.0
        assert abs(recovered_pixel[1] - 1816.0) < 10.0


class TestTransformPoints:
    """Tests for homography transform via cv2.perspectiveTransform."""

    def test_transform_points_identity(self):
        """Test homography transform with identity matrix."""
        transformer = CoordinateTransformer()

        H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]  # Identity
        points = [(10.0, 20.0), (30.0, 40.0)]

        result = transformer.transform_points(points, H)

        assert len(result) == 2
        assert abs(result[0][0] - 10.0) < 0.1
        assert abs(result[0][1] - 20.0) < 0.1
        assert abs(result[1][0] - 30.0) < 0.1
        assert abs(result[1][1] - 40.0) < 0.1

    def test_transform_points_translation(self):
        """Test homography with translation."""
        transformer = CoordinateTransformer()

        # Translate by (100, 50)
        H = [[1, 0, 100], [0, 1, 50], [0, 0, 1]]
        points = [(0.0, 0.0)]

        result = transformer.transform_points(points, H)

        assert len(result) == 1
        assert abs(result[0][0] - 100.0) < 0.1
        assert abs(result[0][1] - 50.0) < 0.1

    def test_transform_points_empty(self):
        """Test transform_points with empty point list."""
        transformer = CoordinateTransformer()
        H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
        result = transformer.transform_points([], H)
        assert result == []