# pip install opencv-python opencv-contrib-python
# Comando para instalar las librerías necesarias de visión por computadora

import cv2
# Importamos la librería OpenCV, que nos permite trabajar con imágenes y video
import numpy as np
# Importamos la librería NumPy, que nos permite trabajar con arreglos y operaciones matem

# Cambia esta dirección IP por la de tu cámara
cap = cv2.VideoCapture('http://192.168.137.166:8080/video')
# Creamos un objeto que captura video desde una cámara IP
# VideoCapture abre la conexión a la dirección web de la cámara

# Obtenemos el diccionario de marcadores ArUco predefinido (4x4 con 50 códigos)
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
# Establecemos los parámetros para la detección de marcadores ArUco
parameters = cv2.aruco.DetectorParameters()
# Creamos el detector de marcadores ArUco con el diccionario y parámetros
detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)

markersize = 0.0254  # Tamaño del marcador en metros (5 cm)

# Iniciamos un bucle que se ejecuta mientras la cámara esté abierta
while (cap.isOpened()):
    # Leemos un frame (fotograma) de la cámara
    # ret indica si la lectura fue exitosa, frame contiene la imagen
    ret, frame = cap.read()
    # Rotamos el frame 90 grados en sentido horario
    frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
    # Convertimos el frame a escala de grises para mejorar la detección
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # Detectamos los marcadores ArUco en la imagen en escala de grises
    corners, ids, rejected = detector.detectMarkers(gray)

    # Si se detectaron marcadores, dibujamos los marcadores en el frame
    if ids is not None:
        cv2.aruco.drawDetectedMarkers(frame, corners, ids)

        for id in ids:
            if id in [27]:
                index = list(ids).index(id)
                corner = corners[index][0]

                # Calculamos el centro del marcador
                center_x = int((corner[0][0] + corner[2][0]) / 2)
                center_y = int((corner[0][1] + corner[2][1]) / 2)

                # Calculamos la distancia desde el centro del frame
                frame_center_x = frame.shape[1] // 2
                frame_center_y = frame.shape[0] // 2
                distance_x = center_x - frame_center_x
                distance_y = center_y - frame_center_y

                # Calcular la relación de píxeles por metro (asumiendo que el marcador es cuadrado)
                pixel_per_meter = (corner[1][0] - corner[0][0]) / markersize

                # Mostramos la distancia en el frame
                cv2.putText(frame, f"Dist X: {distance_x}px \t {distance_x/pixel_per_meter:.2f}m", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                cv2.putText(frame, f"Dist Y: {distance_y}px \t {distance_y/pixel_per_meter:.2f}m", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

                # Calcular la rotación del marcador
                vector_1 = corner[1] - corner[0]
                angle = cv2.fastAtan2(vector_1[1], vector_1[0])
                cv2.putText(frame, f"Angle: {angle:.2f} deg", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

                # Calcular la distancia euclidiana desde el centro del frame al centro del marcador
                euclidean_distance = np.sqrt(distance_x**2 + distance_y**2)
                cv2.putText(frame, f"Euclidean Dist: {euclidean_distance:.2f}px \t {euclidean_distance/pixel_per_meter:.2f}m", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

                # Dibujamos un círculo en el centro del marcador
                cv2.circle(frame, (center_x, center_y), 5, (255, 0, 0), -1)

                # Dibujamos una línea desde el centro del frame hasta el centro del marcador
                cv2.line(frame, (frame_center_x, frame_center_y), (center_x, center_y), (0, 255, 0), 2)

                # Dibujamos una línea desde el centro del frame mostrando el angulo
                length = 100  # Longitud de la línea
                end_x = int(frame_center_x + length * np.sin(np.deg2rad(angle)))
                end_y = int(frame_center_y - length * np.cos(np.deg2rad(angle)))
                cv2.line(frame, (frame_center_x, frame_center_y), (end_x, end_y), (255, 0, 0), 2)                                                                 

    try:
        # Mostramos el frame en una ventana llamada 'Detected Ids'
        # cv2.resize() redimensiona la imagen a 480x640 píxeles
        cv2.imshow('Detected Ids', cv2.resize(frame, (480, 640)))

        # Capturamos la entrada del teclado (espera 1ms por cada iteración)
        key = cv2.waitKey(1)
        # Si presionamos 'q', salimos del bucle
        if key == ord('q'):
            break
    # Capturamos excepciones de OpenCV por si la transmisión se corta
    except cv2.error:
        print("Stream ended...")
        break

# Liberamos los recursos de la cámara
cap.release()
# Cerramos todas las ventanas creadas por OpenCV
cv2.destroyAllWindows()