from autopilot import AutoPilot, RandomPilot
from simulator import Simulator
from visualization import VisualizationManager
from trajectory_drawer import TrajectoryDrawer
from yandex_map import YandexMap
from time import sleep
from pathlib import Path

import cv2
import matplotlib.pyplot as plt
import numpy as np

import autopilot

def make_global_photo(filename):
    yandexMap = YandexMap()
    yandexMap.save_photo()
    yandexMap.destroy()

def get_trajectory_points(bg_img: str) -> list[(float, float)]:
    trajectoryDrawer = TrajectoryDrawer(bg_img)
    trajectoryDrawer.show()
    trajectoryDrawer.wait()
    points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
    return points

def main():
    # Скриншот местности
    # make_global_photo('map.jpg')

    # Получаем траекторию от пользователя
    # points = get_trajectory_points('map.jpg')
    # print(points)
    points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127), 
np.float64(0.5699957136274587)]

    # Для каждой точки сделаем приближенный снимок
    yandexMap = YandexMap()
    keypoints: list[(any, any)] = []

    plt.ion()
    # for i in range(len(points)):
    #     point = points[i]
    #     yandexMap.scroll(point[0], point[1], 5, True)
    #     sleep(1)
    #     img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
    #     Path('chunks').mkdir(exist_ok=True)
    #     cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
    #     plt.subplot(1, len(points), i+1)
    #     plt.imshow(img)
    #     plt.pause(0.25)
    #     yandexMap.scroll(point[0], point[1], 5, False)

    plt.tight_layout()

    # Выделим на каждой картинке ключевые точки
    for i in range(len(points)):
        orb = cv2.ORB_create(
            nfeatures=1000,
            scaleFactor=1.2,
            nlevels=8,
            edgeThreshold=31,
            firstLevel=0,
            WTA_K=2,
            patchSize=31,
            fastThreshold=20
        )
        img = cv2.imread(Path('chunks') / f'chunk_{i}.png')
        kp, des = orb.detectAndCompute(img, None)
        keypoints.append((kp, des))
        
        plt.subplot(1, len(points), i+1)
        kp_coords = np.array([j.pt for j in kp])
        plt.scatter(kp_coords[:, 0], kp_coords[:, 1], c='red', s=20, alpha=0.7, marker='o')
        plt.pause(0.2)
    plt.ioff()

    plt.show(block=True)

    # Начнём симуляцию полёта с первой точки
    yandexMap.scroll(points[0][0], points[0][1], 5, True)

    vis_manager = VisualizationManager()
    pilot = autopilot.AutoPilot(points, keypoints, vis_manager)
    simulator = Simulator(yandexMap)

    while True:
        photo = simulator.get_photo()
        command = pilot.handle(photo)

        if command.stop:
            break

        simulator.handle(command.dangle)
        vis_manager.update_display()
    
    sleep(30)

if __name__ == "__main__":
    main()