import cv2
import pyapriltags

at_detector = pyapriltags.Detector(families="tag36h11")


def open_camera():
    # cap = cv2.VideoCapture(config.usb_camera_device)
    cap = cv2.VideoCapture("http://localhost:50085/video")

    if not cap.isOpened():
        print("Cannot open camera")
        exit()

    # cv2.namedWindow("Object and AprilTag Detection", cv2.WINDOW_NORMAL)
    # cv2.resizeWindow("Object and AprilTag Detection", 800, 600)

    while True:
        print("Frame")

        ret, frame = cap.read()

        if not ret:
            break

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        at_results = at_detector.detect(gray)

        for i, at_result in enumerate(at_results):
            # print(
            #    f"Erkannter AprilTag {i + 1}: ID={at_result.tag_id}, Koordinaten={at_result}")

            apriltag_x = 890
            apriltag_y = 500

            print(
                f"ID: {at_result.tag_id} Corners: {at_result.center}")

            corner_x = at_result.center[0]
            corner_y = at_result.center[1]

            offset = 5

            if corner_x > apriltag_x + offset:
                print("move right")

            if corner_x < apriltag_x - offset:
                print("move left")

            if corner_y > apriltag_y + offset:
                print("move back")

            if corner_y < apriltag_y - offset:
                print("move forward")

        # for at_result in at_results:
        #    corners = at_result.corners.astype(int)
        #    cv2.polylines(frame, [corners], isClosed=True,
        #                  color=(0, 255, 255), thickness=2)

        # cv2.imshow("Object and AprilTag Detection", frame)

        # if cv2.waitKey(1) & 0xFF == ord('q'):
        #   break

        # cap.release()
        # cv2.destroyAllWindows()