import numpy as np
from Neko.RealSense import RealSenseController
from Neko.CameraProcessor import CameraProcessor
from Neko.TargetFollower import TargetFollower
from Neko.Visualization3D import Visualization3D


if __name__ == "__main__":
    camera_processor = CameraProcessor()
    detections_info, annotated_image = camera_processor.inline_detection()

    if annotated_image is not None and detections_info:
        image_height, image_width, _ = annotated_image.shape
        camera_position = np.array([image_width / 2, image_height, 0])

        follower = TargetFollower(detections_info, annotated_image, camera_position=camera_position)
        selected_target = follower.select_target('person', nearest=True)  # Задайте имя класса для следования
        follow_vector = follower.calculate_follow_vector()

        visualizer = Visualization3D(camera_position=camera_position)
        visualizer.plot_scene(detections_info, follow_vector=follow_vector)