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

import itertools
# Importamos itertools para recorrer todas las parejas de marcadores
from pathlib import Path
# Importamos Path para manejar la ruta de la imagen
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áticas

# ===== CONFIGURA AQUÍ =====
IMAGEN = 'foto-cuadrado.jpg'   # ruta de la foto (tomada con el celular, en alta resolución)
MARKER_SIZE = 0.0254           # lado real de los marcadores en metros (2.54 cm = 1 pulgada)
# ==========================

# Leemos la foto desde el disco. Se lee con NumPy y luego se decodifica con OpenCV porque
# cv2.imread no abre en Windows las rutas con tildes o eñes (p. ej. "Robótica").
ruta = Path(IMAGEN)
imagen = cv2.imdecode(np.fromfile(ruta, dtype=np.uint8), cv2.IMREAD_COLOR) if ruta.is_file() else None
if imagen is None:
    raise SystemExit(f"No se pudo leer la imagen '{ruta}': revise la ruta y el nombre del archivo")
print(f"Imagen: {ruta.name}, {imagen.shape[1]} × {imagen.shape[0]} píxeles")

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

# Convertimos la imagen a escala de grises y detectamos los marcadores
gray = cv2.cvtColor(imagen, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = detector.detectMarkers(gray)
if ids is None or len(ids) < 2:
    raise SystemExit("Se necesitan al menos 2 marcadores en la foto para medir distancias")

ids = ids.ravel()
esquinas = {int(i): c[0] for i, c in zip(ids, corners)}   # 4 esquinas (x, y) en píxeles por marcador
print("Marcadores detectados:", sorted(esquinas))

# --- Escala de la foto ---------------------------------------------------------------
# Cada marcador dice cuántos píxeles mide un metro en la foto: lado en píxeles / lado real.
# Si la foto se tomó desde arriba, con el celular paralelo al piso, la escala es la misma en
# toda la foto y basta con promediarla.
centros = {i: c.mean(axis=0) for i, c in esquinas.items()}
lados_px = {i: np.mean([np.linalg.norm(c[k] - c[(k + 1) % 4]) for k in range(4)]) for i, c in esquinas.items()}
escalas = {i: lado / MARKER_SIZE for i, lado in lados_px.items()}   # píxeles por metro
escala = np.mean(list(escalas.values()))
print(f"\nEscala promedio: {escala:.0f} px/m")
for i in sorted(escalas):
    print(f"  marcador {i:2d}: lado = {lados_px[i]:.1f} px -> {escalas[i]:.0f} px/m")

# Si la escala cambia mucho de un marcador a otro, la foto quedó inclinada:
# los marcadores más cercanos al celular se ven más grandes y las distancias salen mal.
diferencia = (max(escalas.values()) - min(escalas.values())) / escala
if diferencia > 0.05:
    print(f"\n¡Atención! La escala cambia un {diferencia:.0%} entre marcadores: la foto está inclinada.")
    print("Tómela de nuevo desde arriba, con el celular paralelo al piso.")

# --- Distancias -------------------------------------------------------------------------
print("\nDistancias entre centros de marcadores")
print("  pareja      píxeles    metros")
for a, b in itertools.combinations(sorted(centros), 2):
    d_px = np.linalg.norm(centros[a] - centros[b])
    d_m = d_px / escala
    print(f"  {a:2d} - {b:2d}   {d_px:9.1f}   {d_m:7.3f}")

    # Dibujamos la línea entre los centros y la distancia en la foto
    pa, pb = tuple(int(v) for v in centros[a]), tuple(int(v) for v in centros[b])
    cv2.line(imagen, pa, pb, (0, 200, 0), 3)
    medio = ((pa[0] + pb[0]) // 2, (pa[1] + pb[1]) // 2)
    cv2.putText(imagen, f"{d_m * 100:.1f} cm", medio, cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 4)

# Dibujamos los marcadores detectados con su ID
cv2.aruco.drawDetectedMarkers(imagen, corners, ids.reshape(-1, 1))

# Guardamos el resultado junto a la foto original
salida = ruta.with_name(ruta.stem + "-distancias.png")
cv2.imencode(".png", imagen)[1].tofile(salida)   # como imread, imwrite no acepta tildes en Windows
print(f"\nResultado guardado en {salida}")

# Mostramos la foto reducida para que quepa en la pantalla; presione cualquier tecla para cerrar
factor = min(1.0, 1000 / max(imagen.shape[:2]))
cv2.imshow('Distancias entre marcadores', cv2.resize(imagen, None, fx=factor, fy=factor))
cv2.waitKey(0)
cv2.destroyAllWindows()