"""Tests for Sequential Visual Odometry (F07)."""

import numpy as np
import pytest

from gps_denied.core.models import ModelManager
from gps_denied.core.vo import SequentialVisualOdometry
from gps_denied.schemas.flight import CameraParameters
from gps_denied.schemas.vo import Features, Matches


@pytest.fixture
def vo():
    manager = ModelManager()
    return SequentialVisualOdometry(manager)


@pytest.fixture
def cam_params():
    return CameraParameters(
        focal_length=5.0,
        sensor_width=6.4,
        sensor_height=4.8,
        resolution_width=640,
        resolution_height=480,
        principal_point=(320.0, 240.0)
    )


def test_extract_features(vo):
    img = np.zeros((480, 640, 3), dtype=np.uint8)
    features = vo.extract_features(img)
    
    assert isinstance(features, Features)
    assert features.keypoints.shape == (500, 2)
    assert features.descriptors.shape == (500, 256)


def test_match_features(vo):
    f1 = Features(
        keypoints=np.random.rand(100, 2),
        descriptors=np.random.rand(100, 256),
        scores=np.random.rand(100)
    )
    f2 = Features(
        keypoints=np.random.rand(100, 2),
        descriptors=np.random.rand(100, 256),
        scores=np.random.rand(100)
    )
    
    matches = vo.match_features(f1, f2)
    assert isinstance(matches, Matches)
    assert matches.matches.shape == (100, 2)


def test_estimate_motion_insufficient_matches(vo, cam_params):
    matches = Matches(
        matches=np.zeros((5, 2)),
        scores=np.zeros(5),
        keypoints1=np.zeros((5, 2)),
        keypoints2=np.zeros((5, 2))
    )
    
    # Less than 8 points should return None
    motion = vo.estimate_motion(matches, cam_params)
    assert motion is None


def test_estimate_motion_synthetic(vo, cam_params):
    # To reliably test compute_relative_pose, we create points strictly satisfying epipolar constraint
    # Simple straight motion: Add a small shift on X axis
    n_pts = 100
    pts1 = np.random.rand(n_pts, 2) * 400 + 100
    pts2 = pts1 + np.array([10.0, 0.0]) # moving 10 pixels right
    
    matches = Matches(
        matches=np.column_stack([np.arange(n_pts), np.arange(n_pts)]),
        scores=np.ones(n_pts),
        keypoints1=pts1,
        keypoints2=pts2
    )
    
    motion = vo.estimate_motion(matches, cam_params)
    assert motion is not None
    assert motion.inlier_count > 20
    assert motion.translation.shape == (3,)
    assert motion.rotation.shape == (3, 3)


def test_compute_relative_pose(vo, cam_params):
    img1 = np.zeros((480, 640, 3), dtype=np.uint8)
    img2 = np.zeros((480, 640, 3), dtype=np.uint8)
    
    # Given the random nature of our mock, OpenCV's findEssentialMat will likely find 0 inliers
    # or fail. We expect compute_relative_pose to gracefully return None or low confidence.
    pose = vo.compute_relative_pose(img1, img2, cam_params)
    
    if pose is not None:
        assert pose.translation.shape == (3,)
        assert pose.rotation.shape == (3, 3)
        # Because we randomize points in the mock manager, inliers will be extremely low
        assert pose.tracking_good is False