#!/usr/bin/env python3
"""
Тестовый скрипт для проверки работы с координатами относительно центра изображения
"""

import numpy as np
from PIL import Image
import cv2
from autopilot import AutoPilot

def test_center_coordinates():
    """Тестирует работу с координатами относительно центра изображения"""
    print("Тест координат относительно центра изображения...")
    
    # Создаем тестовое изображение
    width, height = 640, 480
    test_image = np.zeros((height, width, 3), dtype=np.uint8)
    
    # Добавляем некоторые объекты для отслеживания
    cv2.circle(test_image, (width//2, height//2), 50, (255, 255, 255), -1)  # Центр
    cv2.circle(test_image, (100, 100), 20, (255, 0, 0), -1)  # Левый верхний угол
    cv2.circle(test_image, (width-100, height-100), 20, (0, 255, 0), -1)  # Правый нижний угол
    
    # Создаем второе изображение с небольшим смещением
    test_image2 = np.zeros((height, width, 3), dtype=np.uint8)
    cv2.circle(test_image2, (width//2 + 10, height//2 + 5), 50, (255, 255, 255), -1)  # Смещенный центр
    cv2.circle(test_image2, (110, 105), 20, (255, 0, 0), -1)  # Смещенный левый верхний угол
    cv2.circle(test_image2, (width-90, height-95), 20, (0, 255, 0), -1)  # Смещенный правый нижний угол
    
    # Конвертируем в PIL Image
    pil_image1 = Image.fromarray(test_image)
    pil_image2 = Image.fromarray(test_image2)
    
    # Создаем автопилот
    pilot = AutoPilot(initial_x=0.0, initial_y=0.0)
    
    print(f"Размер изображения: {width}x{height}")
    print(f"Ожидаемый центр: ({width//2}, {height//2})")
    
    # Обрабатываем первое изображение
    pilot.handle(pil_image1)
    print(f"Центр изображения после первого кадра: {pilot.image_center}")
    
    # Обрабатываем второе изображение
    pilot.handle(pil_image2)
    print(f"Центр изображения после второго кадра: {pilot.image_center}")
    
    # Получаем состояние дрона
    drone_state = pilot.get_drone_state()
    print(f"Позиция дрона: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
    print(f"Угол дрона: {drone_state['angle_degrees']:.1f}°")
    
    print("\nАнализ отцентрированных координат:")
    print("Ожидаемое смещение центра: (10, 5) пикселей")
    print("Ожидаемое смещение левого верхнего угла: (10, 5) пикселей")
    print("Ожидаемое смещение правого нижнего угла: (10, 5) пикселей")
    
    print("Тест завершен!")

def test_keypoint_centering():
    """Тестирует отцентрирование ключевых точек"""
    print("\nТест отцентрирования ключевых точек...")
    
    # Создаем простое изображение с одной точкой
    width, height = 100, 100
    test_image = np.zeros((height, width, 3), dtype=np.uint8)
    cv2.circle(test_image, (25, 25), 5, (255, 255, 255), -1)  # Точка в (25, 25)
    
    # Создаем второе изображение с той же точкой
    test_image2 = np.zeros((height, width, 3), dtype=np.uint8)
    cv2.circle(test_image2, (25, 25), 5, (255, 255, 255), -1)
    
    # Конвертируем в PIL Image
    pil_image1 = Image.fromarray(test_image)
    pil_image2 = Image.fromarray(test_image2)
    
    # Создаем автопилот
    pilot = AutoPilot(initial_x=0.0, initial_y=0.0)
    
    print(f"Размер изображения: {width}x{height}")
    print(f"Центр изображения: ({width//2}, {height//2}) = (50, 50)")
    print(f"Позиция точки: (25, 25)")
    print(f"Ожидаемые отцентрированные координаты: (25-50, 25-50) = (-25, -25)")
    
    # Обрабатываем изображения
    pilot.handle(pil_image1)
    pilot.handle(pil_image2)
    
    drone_state = pilot.get_drone_state()
    print(f"Результат: позиция дрона ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
    
    print("Тест завершен!")

if __name__ == "__main__":
    test_center_coordinates()
    test_keypoint_centering()