#!/usr/bin/env python3
"""Test visual odometry functionality."""

import pytest
import numpy as np
import cv2


def test_feature_detector_orb():
    detector = cv2.ORB_create(nfeatures=500)
    img = np.random.randint(0, 255, (480, 640), dtype=np.uint8)
    keypoints, descriptors = detector.detectAndCompute(img, None)
    assert keypoints is not None


def test_feature_detector_sift():
    detector = cv2.SIFT_create(nfeatures=500)
    img = np.random.randint(0, 255, (480, 640), dtype=np.uint8)
    keypoints, descriptors = detector.detectAndCompute(img, None)
    assert keypoints is not None


def test_feature_matching():
    detector = cv2.ORB_create(nfeatures=100)
    matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
    
    img1 = np.random.randint(0, 255, (240, 320), dtype=np.uint8)
    img2 = np.random.randint(0, 255, (240, 320), dtype=np.uint8)
    
    kp1, desc1 = detector.detectAndCompute(img1, None)
    kp2, desc2 = detector.detectAndCompute(img2, None)
    
    if desc1 is not None and desc2 is not None:
        matches = matcher.knnMatch(desc1, desc2, k=2)
        assert matches is not None


def test_essential_matrix():
    K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]], dtype=np.float64)
    
    pts1 = np.random.rand(10, 2) * 100 + 100
    pts2 = pts1 + np.random.rand(10, 2) * 5
    
    pts1 = pts1.astype(np.float32)
    pts2 = pts2.astype(np.float32)
    
    E, mask = cv2.findEssentialMat(pts1, pts2, K, method=cv2.RANSAC)
    assert E is not None or mask is not None


def test_rotation_from_scipy():
    from scipy.spatial.transform import Rotation
    
    R_mat = np.eye(3)
    r = Rotation.from_matrix(R_mat)
    quat = r.as_quat()
    
    assert len(quat) == 4
    assert np.isclose(quat[3], 1.0)


if __name__ == '__main__':
    pytest.main([__file__, '-v'])