import numpy as np
import requests
import time
import keyboard
import json

# URL для отправки JSON-команд на ESP32
url = "http://192.168.4.1/js?json="

# Ограничения на движение камеры
PAN_LIMITS = (-60, 60)  # Горизонтальная ось (градусы)
TILT_LIMITS = (-30, 60)  # Вертикальная ось (градусы)

# Границы прямоугольника (будут заполнены при калибровке)
left = None
right = None
top = None
bottom = None

# Задержка ожидания крутилки (для автоматического режима)
t_delay = 1.0

# Текущие абсолютные координаты камеры
current_pan = 0  # Горизонтальное положение (в градусах)
current_tilt = 0  # Вертикальное положение (в градусах)


def set_module_type(module):
    """Установка типа модуля (2 = pan-tilt)"""
    json_data = {"T": 4, "cmd": module}
    try:
        response = requests.get(url + str(json_data).replace("'", '"'))
        print("Установка модуля, ответ ESP32:", response.text)
    except requests.exceptions.RequestException as e:
        print("Ошибка:", e)


def get_current_position():
    """Получение текущих координат pan и tilt"""
    json_data = {"T": 130}
    try:
        response = requests.get(url + str(json_data).replace("'", '"'))
        data = response.json()
        pan = data.get("pan", 0)
        tilt = data.get("tilt", 0)
        print(f"Текущие координаты: pan={pan:.3f}, tilt={tilt:.3f}")
        return pan, tilt
    except requests.exceptions.RequestException as e:
        print("Ошибка при получении координат:", e)
        return None, None
    except json.JSONDecodeError as e:
        print("Ошибка парсинга JSON:", e)
        return None, None


def send_command(pan, tilt):
    """Отправка команды на перемещение камеры"""
    global current_pan, current_tilt
    
    # Ограничение углов в пределах допустимого диапазона
    pan = max(PAN_LIMITS[0], min(PAN_LIMITS[1], pan))
    tilt = max(TILT_LIMITS[0], min(TILT_LIMITS[1], tilt))
    
    # Формирование JSON команды
    json_data = {"T": 133, "X": pan, "Y": tilt, "SPD": 50, "ACC": 0}
    try:
        response = requests.get(url + str(json_data).replace("'", '"'))
        print(f"Перемещение в ({pan:.1f}, {tilt:.1f}), ответ: {response.text}")
        
        # Обновляем текущие значения только после успешной отправки
        current_pan = pan
        current_tilt = tilt
        
        # Небольшая задержка для стабилизации
        time.sleep(0.1)
    except requests.exceptions.RequestException as e:
        print("Ошибка отправки команды:", e)


def calibrate_left():
    """Калибровка левой границы (клавиша A)"""
    global left
    print("\n[A] Калибровка ЛЕВОЙ границы...")
    pan, tilt = get_current_position()
    if pan is not None:
        left = pan
        print(f"✓ Левая граница установлена: {left:.3f}°")


def calibrate_right():
    """Калибровка правой границы (клавиша D)"""
    global right
    print("\n[D] Калибровка ПРАВОЙ границы...")
    pan, tilt = get_current_position()
    if pan is not None:
        right = pan
        print(f"✓ Правая граница установлена: {right:.3f}°")


def calibrate_top():
    """Калибровка верхней границы (клавиша W)"""
    global top
    print("\n[W] Калибровка ВЕРХНЕЙ границы...")
    pan, tilt = get_current_position()
    if tilt is not None:
        top = tilt
        print(f"✓ Верхняя граница установлена: {top:.3f}°")


def calibrate_bottom():
    """Калибровка нижней границы (клавиша S)"""
    global bottom
    print("\n[S] Калибровка НИЖНЕЙ границы...")
    pan, tilt = get_current_position()
    if tilt is not None:
        bottom = tilt
        print(f"✓ Нижняя граница установлена: {bottom:.3f}°")


def generate_figure_eight_points(left, right, top, bottom, num_points=1000):
    """
    Генерирует точки эллиптической восьмёрки в заданном прямоугольнике.
    Возвращает список кортежей (pan, tilt) в градусах.
    """
    # Центр прямоугольника (0, 0)
    center_pan = (left + right) / 2
    center_tilt = (top + bottom) / 2
    
    # Размеры
    width = right - left
    height = top - bottom
    
    # Генерируем параметр t от 0 до 2π
    t = np.linspace(0, 2 * np.pi, num_points)
    
    # Эллиптическая восьмёрка с степенной функцией для сглаживания
    sin_t = np.sin(t)
    sin_2t = np.sin(2 * t)
    
    # Применяем степенную функцию для избежания прямых участков
    pan = center_pan + (width / 2) * np.sign(sin_t) * np.abs(sin_t) ** 1.5
    tilt = center_tilt + (height / 2) * np.sign(sin_2t) * np.abs(sin_2t) ** 1.5
    
    # Округляем до целых градусов (минимальный шаг крутилки)
    pan_rounded = np.round(pan).astype(int)
    tilt_rounded = np.round(tilt).astype(int)
    
    # Формируем список точек
    points = list(zip(pan_rounded, tilt_rounded))
    
    # Убираем последовательные дубликаты
    unique_points = [points[0]]
    for point in points[1:]:
        if point != unique_points[-1]:
            unique_points.append(point)
    
    # Добавляем возврат в начальную точку (0, 0)
    start_point = (int(round(center_pan)), int(round(center_tilt)))
    if unique_points[-1] != start_point:
        unique_points.append(start_point)
    
    return unique_points


def run_manual_cycle():
    """
    Режим R: Проход по восьмёрке с ожиданием Space на каждой точке
    """
    global left, right, top, bottom
    
    if None in [left, right, top, bottom]:
        print("\n✗ ОШИБКА: Сначала откалибруйте все границы (W, A, S, D)!")
        return
    
    print(f"\n{'='*60}")
    print(f"[R] Запуск РУЧНОГО режима (пауза на каждой точке)")
    print(f"Границы: pan=[{left:.1f}°, {right:.1f}°], tilt=[{bottom:.1f}°, {top:.1f}°]")
    print(f"{'='*60}\n")
    
    # Генерируем траекторию
    points = generate_figure_eight_points(left, right, top, bottom)
    print(f"Сгенерировано {len(points)} точек\n")
    
    # Проходим по всем точкам
    for i, (pan, tilt) in enumerate(points, start=1):
        print(f"\n--- Точка {i}/{len(points)}: ({pan}, {tilt}) ---")
        send_command(pan, tilt)
        
        print("Нажмите SPACE для продолжения...")
        keyboard.wait('space')
        print("Продолжаем...")
    
    print(f"\n{'='*60}")
    print("✓ Цикл завершён! Вернулись в начальную точку.")
    print(f"{'='*60}\n")


def run_auto_cycle():
    """
    Режим T: Автоматический проход по восьмёрке с задержкой t_delay
    """
    global left, right, top, bottom, t_delay
    
    if None in [left, right, top, bottom]:
        print("\n✗ ОШИБКА: Сначала откалибруйте все границы (W, A, S, D)!")
        return
    
    print(f"\n{'='*60}")
    print(f"[T] Запуск АВТОМАТИЧЕСКОГО режима (задержка {t_delay}с)")
    print(f"Границы: pan=[{left:.1f}°, {right:.1f}°], tilt=[{bottom:.1f}°, {top:.1f}°]")
    print(f"{'='*60}\n")
    
    # Генерируем траекторию
    points = generate_figure_eight_points(left, right, top, bottom)
    print(f"Сгенерировано {len(points)} точек\n")
    
    # Проходим по всем точкам
    for i, (pan, tilt) in enumerate(points, start=1):
        print(f"Точка {i}/{len(points)}: ({pan}, {tilt})")
        send_command(pan, tilt)
        time.sleep(t_delay)
    
    print(f"\n{'='*60}")
    print("✓ Цикл завершён! Вернулись в начальную точку.")
    print(f"{'='*60}\n")


def return_to_zero():
    """
    Режим Y: Возврат в нулевые координаты
    """
    print("\n[Y] Возврат в нулевую позицию (0, 0)...")
    send_command(0, 0)
    print("✓ Камера в позиции (0, 0)\n")


def print_help():
    """Вывод справки по командам"""
    print(f"\n{'='*60}")
    print("УПРАВЛЕНИЕ PAN-TILT КАМЕРОЙ")
    print(f"{'='*60}")
    print("\nКАЛИБРОВКА ГРАНИЦ:")
    print("  W - Установить ВЕРХНЮЮ границу (top)")
    print("  A - Установить ЛЕВУЮ границу (left)")
    print("  S - Установить НИЖНЮЮ границу (bottom)")
    print("  D - Установить ПРАВУЮ границу (right)")
    print("\nРЕЖИМЫ РАБОТЫ:")
    print("  R - Ручной режим (пауза на каждой точке, Space для продолжения)")
    print("  T - Автоматический режим (задержка между точками)")
    print("  Y - Вернуться в нулевую позицию (0, 0)")
    print("\nДРУГОЕ:")
    print("  H - Показать эту справку")
    print("  Q - Выход из программы")
    print(f"{'='*60}\n")


def main():
    """Главная функция"""
    print("\n" + "="*60)
    print("      PAN-TILT КОНТРОЛЛЕР С ЭЛЛИПТИЧЕСКОЙ ВОСЬМЁРКОЙ")
    print("="*60)
    
    # Инициализация
    print("\nИнициализация...")
    set_module_type(2)  # Устанавливаем модуль pan-tilt
    time.sleep(0.5)
    send_command(0, 0)  # Перемещаем в нулевую позицию
    
    print_help()
    
    # Регистрируем обработчики клавиш
    keyboard.add_hotkey('w', calibrate_top)
    keyboard.add_hotkey('a', calibrate_left)
    keyboard.add_hotkey('s', calibrate_bottom)
    keyboard.add_hotkey('d', calibrate_right)
    keyboard.add_hotkey('r', run_manual_cycle)
    keyboard.add_hotkey('t', run_auto_cycle)
    keyboard.add_hotkey('y', return_to_zero)
    keyboard.add_hotkey('h', print_help)
    
    print("Программа готова к работе! Нажмите H для справки.")
    print("Для выхода нажмите Q\n")
    
    # Ожидаем нажатия Q для выхода
    keyboard.wait('q')
    
    print("\nЗавершение работы...")
    return_to_zero()
    print("Программа завершена.")


if __name__ == "__main__":
    main()