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

import numpy as np
from PIL import Image
import cv2
from autopilot import AutoPilot, RandomPilot
from simulator import Simulator
from visualization import VisualizationManager, SimMode

def test_unified_visualization():
    """Тестирует единую систему визуализации"""
    print("Тест единой системы визуализации...")
    
    # Создаем менеджер визуализации
    viz_manager = VisualizationManager("Test - Unified Visualization")
    
    # Создаем автопилот с менеджером визуализации
    autonome_pilot = AutoPilot(initial_x=0.0, initial_y=0.0, viz_manager=viz_manager)
    
    # Создаем симулятор
    simulator = Simulator(RandomPilot(), autonome_pilot)
    
    # Создаем тестовые изображения
    width, height = 640, 480
    test_image1 = np.zeros((height, width, 3), dtype=np.uint8)
    test_image2 = np.zeros((height, width, 3), dtype=np.uint8)
    
    # Добавляем объекты для отслеживания
    cv2.circle(test_image1, (width//2, height//2), 50, (255, 255, 255), -1)
    cv2.circle(test_image1, (100, 100), 20, (255, 0, 0), -1)
    
    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)
    
    # Конвертируем в PIL Image
    pil_image1 = Image.fromarray(test_image1)
    pil_image2 = Image.fromarray(test_image2)
    
    print("Обрабатываем первое изображение...")
    autonome_pilot.handle(pil_image1)
    
    print("Обрабатываем второе изображение...")
    autonome_pilot.handle(pil_image2)
    
    # Обновляем визуализацию
    viz_manager.update_display()
    
    # Получаем состояние дрона
    drone_state = autonome_pilot.get_drone_state()
    print(f"Позиция дрона: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
    print(f"Угол дрона: {drone_state['angle_degrees']:.1f}°")
    
    print("Тест завершен! Окно визуализации должно показывать:")
    print("- Глобальную карту с траекторией")
    print("- Детекцию ключевых точек")
    print("- Сопоставление точек между кадрами")
    
    # Показываем финальное состояние
    viz_manager.show_final()

if __name__ == "__main__":
    test_unified_visualization()