"""
Примеры использования системы отслеживания БПЛА
"""

import numpy as np
from PIL import Image
import cv2
from autopilot import AutoPilot
import matplotlib.pyplot as plt

def example_basic_usage():
    """Базовый пример использования автопилота"""
    print("=== Базовый пример использования ===")
    
    # Создаем автопилот с начальными параметрами
    autopilot = AutoPilot(
        initial_x=0.0,      # Начальная X координата
        initial_y=0.0,      # Начальная Y координата
        initial_altitude=15.0  # Начальная высота
    )
    
    # Создаем тестовые изображения
    img1 = create_simple_test_image()
    img2 = create_simple_test_image(offset_x=20, offset_y=10)
    
    # Обрабатываем изображения
    autopilot.handle(img1)
    autopilot.handle(img2)
    
    # Получаем состояние
    state = autopilot.get_drone_state()
    print(f"Позиция БПЛА: ({state['x']:.2f}, {state['y']:.2f})")
    print(f"Угол БПЛА: {state['angle_degrees']:.1f}°")
    print(f"Высота: {state['altitude']:.1f}м")
    print(f"Обработано кадров: {state['frame_count']}")

def example_altitude_change():
    """Пример изменения высоты и пересчета масштаба"""
    print("\n=== Пример изменения высоты ===")
    
    autopilot = AutoPilot(initial_altitude=10.0)
    
    # Создаем изображения
    img1 = create_simple_test_image()
    img2 = create_simple_test_image(offset_x=10, offset_y=5)
    
    # Обрабатываем при высоте 10м
    autopilot.handle(img1)
    autopilot.handle(img2)
    
    state1 = autopilot.get_drone_state()
    print(f"При высоте 10м: ({state1['x']:.2f}, {state1['y']:.2f})")
    
    # Изменяем высоту на 20м
    autopilot.set_altitude(20.0)
    
    # Обрабатываем те же изображения при новой высоте
    autopilot.reset_position()
    autopilot.handle(img1)
    autopilot.handle(img2)
    
    state2 = autopilot.get_drone_state()
    print(f"При высоте 20м: ({state2['x']:.2f}, {state2['y']:.2f})")
    
    # Коэффициент должен измениться в 2 раза
    ratio = state2['x'] / state1['x'] if state1['x'] != 0 else 0
    print(f"Коэффициент изменения: {ratio:.2f} (ожидается ~2.0)")

def example_circular_movement():
    """Пример отслеживания кругового движения"""
    print("\n=== Пример кругового движения ===")
    
    autopilot = AutoPilot()
    
    # Создаем базовое изображение
    base_img = create_simple_test_image()
    
    # Параметры кругового движения
    radius = 30
    center_x, center_y = 320, 240
    num_steps = 8
    
    x_history = []
    y_history = []
    angle_history = []
    
    # Симулируем круговое движение
    for i in range(num_steps):
        angle = 2 * np.pi * i / num_steps
        
        # Вычисляем смещение
        dx = radius * np.cos(angle)
        dy = radius * np.sin(angle)
        
        # Создаем смещенное изображение
        current_img = create_simple_test_image(
            offset_x=int(dx), 
            offset_y=int(dy), 
            rotation=angle * 0.1
        )
        
        # Обрабатываем
        autopilot.handle(current_img)
        
        # Сохраняем историю
        state = autopilot.get_drone_state()
        x_history.append(state['x'])
        y_history.append(state['y'])
        angle_history.append(state['angle_degrees'])
        
        print(f"Шаг {i+1}: ({state['x']:.2f}, {state['y']:.2f}), угол: {state['angle_degrees']:.1f}°")
    
    # Визуализируем траекторию
    plot_circular_trajectory(x_history, y_history, angle_history)

def example_position_reset():
    """Пример сброса позиции"""
    print("\n=== Пример сброса позиции ===")
    
    autopilot = AutoPilot()
    
    # Создаем и обрабатываем изображения
    img1 = create_simple_test_image()
    img2 = create_simple_test_image(offset_x=15, offset_y=10)
    
    autopilot.handle(img1)
    autopilot.handle(img2)
    
    state_before = autopilot.get_drone_state()
    print(f"До сброса: ({state_before['x']:.2f}, {state_before['y']:.2f})")
    
    # Сбрасываем позицию
    autopilot.reset_position(x=100.0, y=200.0, angle=np.pi/4)
    
    state_after = autopilot.get_drone_state()
    print(f"После сброса: ({state_after['x']:.2f}, {state_after['y']:.2f}), угол: {state_after['angle_degrees']:.1f}°")

def create_simple_test_image(offset_x=0, offset_y=0, rotation=0):
    """Создает простое тестовое изображение с заданным смещением"""
    width, height = 640, 480
    
    # Создаем базовое изображение с геометрическими фигурами
    img = np.zeros((height, width, 3), dtype=np.uint8)
    
    # Добавляем фигуры
    cv2.rectangle(img, (100, 100), (200, 200), (255, 0, 0), -1)
    cv2.circle(img, (400, 300), 50, (0, 255, 0), -1)
    cv2.line(img, (50, 400), (550, 400), (0, 0, 255), 5)
    cv2.rectangle(img, (300, 150), (350, 200), (255, 255, 0), -1)
    
    # Применяем трансформацию
    if offset_x != 0 or offset_y != 0 or rotation != 0:
        cos_rot = np.cos(rotation)
        sin_rot = np.sin(rotation)
        
        M = np.float32([
            [cos_rot, -sin_rot, offset_x],
            [sin_rot, cos_rot, offset_y]
        ])
        
        img = cv2.warpAffine(img, M, (width, height), 
                            borderMode=cv2.BORDER_REPLICATE)
    
    return Image.fromarray(img)

def plot_circular_trajectory(x_history, y_history, angle_history):
    """Визуализирует круговую траекторию"""
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6))
    
    # График траектории
    ax1.plot(x_history, y_history, 'b-o', linewidth=2, markersize=8)
    ax1.plot(x_history[0], y_history[0], 'go', markersize=12, label='Начало')
    ax1.plot(x_history[-1], y_history[-1], 'ro', markersize=12, label='Конец')
    ax1.set_xlabel('X координата (м)')
    ax1.set_ylabel('Y координата (м)')
    ax1.set_title('Круговая траектория БПЛА')
    ax1.grid(True)
    ax1.legend()
    ax1.axis('equal')
    
    # График угла
    ax2.plot(range(len(angle_history)), angle_history, 'r-o', linewidth=2, markersize=8)
    ax2.set_xlabel('Номер шага')
    ax2.set_ylabel('Угол поворота (градусы)')
    ax2.set_title('Изменение угла поворота')
    ax2.grid(True)
    
    plt.tight_layout()
    plt.show()

def example_error_handling():
    """Пример обработки ошибок и граничных случаев"""
    print("\n=== Пример обработки ошибок ===")
    
    autopilot = AutoPilot()
    
    # Создаем изображение без текстуры (однородное)
    blank_img = Image.fromarray(np.zeros((480, 640, 3), dtype=np.uint8))
    
    # Обрабатываем - должно работать без ошибок
    try:
        autopilot.handle(blank_img)
        autopilot.handle(blank_img)
        print("Обработка однородного изображения прошла успешно")
    except Exception as e:
        print(f"Ошибка при обработке: {e}")
    
    # Проверяем состояние
    state = autopilot.get_drone_state()
    print(f"Состояние после обработки: ({state['x']:.2f}, {state['y']:.2f})")

if __name__ == "__main__":
    print("Примеры использования системы отслеживания БПЛА\n")
    
    # Запускаем все примеры
    example_basic_usage()
    example_altitude_change()
    example_circular_movement()
    example_position_reset()
    example_error_handling()
    
    print("\nВсе примеры выполнены успешно!") 