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

import time
# Importamos la librería time para manejar tiempos de espera
import threading
# Importamos threading para leer la cámara en segundo plano
import cv2
# Importamos la librería OpenCV, que nos permite trabajar con imágenes y video
import matplotlib.pyplot as plt
# Importamos matplotlib para graficar
import numpy as np
# Importamos NumPy para trabajar con arreglos numéricos
from scipy.ndimage import center_of_mass
# Importamos la función center_of_mass para calcular el centro de masa de un arreglo
import ev3_dc as ev3
# Importamos la librería ev3_dc para controlar el robot EV3
from thread_task import Periodic
# Importamos Periodic para crear tareas periódicas
import keyboard
# Importamos keyboard para capturar entradas del teclado

# ===== CONFIGURA AQUÍ TU ROBOT =====
MAC = '00:16:53:50:8A:86'   # dirección MAC de tu ladrillo EV3
RADIUS_WHEEL = 0.0210       # m, radio de la rueda (use el calibrado)
TREAD_DISTANCE = 0.1224     # m, distancia completa entre las ruedas (use la calibrada)
# ====================================

# ===== CONFIGURA AQUÍ TU CÁMARA =====
IP = '192.168.137.49'   # dirección IP que muestra la aplicación del celular
PUERTO = 8080           # 8080 en IP Webcam (Android), 8081 en IP Camera Lite (iPhone/iPad)
IPHONE = False          # True si usas IP Camera Lite (iPhone/iPad), False si usas IP Webcam (Android)
# ====================================

# Cada aplicación usa un formato de dirección distinto. Usamos el video por HTTP
# porque tiene mucho menos retardo que RTSP (medido: ~150 ms contra 0.4–1.2 s).
if IPHONE:
    url = f'http://admin:admin@{IP}:{PUERTO}/'
else:
    url = f'http://{IP}:{PUERTO}/video'


class CamaraUltimoFrame:
    """Lee la cámara en segundo plano y entrega siempre la imagen más reciente.

    Un hilo lee las imágenes apenas llegan y solo guarda la última. Así el
    programa principal nunca espera ni procesa imágenes viejas acumuladas.
    """

    def __init__(self, url):
        self.cap = cv2.VideoCapture(url)
        self.frame = None              # la imagen más reciente
        self.t_ultimo = time.time()    # cuándo llegó la última imagen
        self.activa = self.cap.isOpened()
        self.hilo = threading.Thread(target=self._leer, daemon=True)
        self.hilo.start()

    def _leer(self):
        # Este ciclo corre en paralelo con el programa principal
        while self.activa:
            ret, frame = self.cap.read()
            if ret:
                self.frame = frame
                self.t_ultimo = time.time()

    def leer(self):
        # Devuelve una copia de la última imagen recibida (None si aún no ha llegado ninguna).
        # Es una copia para que dibujar sobre ella no altere la imagen guardada.
        return None if self.frame is None else self.frame.copy()

    def cerrar(self):
        self.activa = False
        self.hilo.join(timeout=2)      # esperamos a que el hilo termine de leer
        self.cap.release()             # antes de soltar la cámara


print("Conectando a", url)
camara = CamaraUltimoFrame(url)
# Creamos la cámara: abre la conexión y empieza a leer en segundo plano
if not camara.activa:
    raise SystemExit("No se pudo abrir la cámara: revise la IP, el puerto y que el servidor de la aplicación esté encendido")

# Configuramos matplotlib para modo interactivo
plt.ion()
# Creamos una figura con dos gráficas: arriba la intensidad, abajo la medida y la referencia en el tiempo
fig, (ax, ax_t) = plt.subplots(2, 1)
# Ajustamos el tamaño de la figura
fig.set_size_inches(6, 8)
ax.set_title('Intensidad de la línea horizontal central')
ax.set_xlim(0, 480)  # Asumiendo un ancho de imagen de 480 píxeles
ax.set_ylim(0, 255)  # Valores de intensidad en escala de grises
intensity_plot = ax.plot([], 'r-')[0]
center_x_plot = ax.plot([], 'ko')[0]

# Gráfica temporal: referencia (centro de la imagen) y medida (centro de la línea)
VENTANA = 20  # segundos que se muestran en la gráfica temporal
ax_t.set_title('Control P: referencia y medida')
ax_t.set_xlabel('tiempo (s)')
ax_t.set_ylabel('posición en la imagen (px)')
ax_t.set_ylim(0, 480)
referencia_plot = ax_t.plot([], [], 'b--', label='referencia (centro de la imagen)')[0]
medida_plot = ax_t.plot([], [], 'r-', label='medida (centro de la línea)')[0]
ax_t.legend(loc='upper right', fontsize=8)
ax_t.grid(True, alpha=0.3)
tiempos, referencias, medidas = [], [], []
t_inicio = time.time()
fig.tight_layout()
fig.canvas.draw()

vt = 0
vr = 0
vd = 0
vi = 0
speed = 0
turn = 0
vt_new = 0.0  # Velocidad translacional constante
# Constante proporcional para el control P
Kp = 0.05


def change_translational_speed(new_vt):
    global vt_new
    vt_new += new_vt
    vt_new = max(min(vt_new, 50), -50)
    print(f"Translational speed set to: {vt_new}")


def change_Kp(new_Kp):
    global Kp
    Kp += new_Kp
    print(f"Kp set to: {Kp:.2f}")


keyboard.add_hotkey('up', lambda: change_translational_speed(5))
keyboard.add_hotkey('down', lambda: change_translational_speed(-5))
keyboard.add_hotkey('left', lambda: change_Kp(0.01))
keyboard.add_hotkey('right', lambda: change_Kp(-0.01))

print("Use arrow keys to adjust translational speed and Kp:")
print("Up/Down: Increase/Decrease translational speed")
print("Left/Right: Increase/Decrease Kp")


def get_turn(vmax, vmin, sign=1):
    if vmax == 0:
        return 0
    ratio = 100 * (1 - vmin / vmax)
    return int(sign * ratio)


def update_speed_turn(vt_new, vr_new):
    global vt, vr, speed, turn, vd, vi
    vt = max(min(int(vt_new), 50), -50)
    vr = max(min(int(vr_new), 50), -50)
    vd = vt + vr
    vi = vt - vr
    if abs(vd) > abs(vi):
        speed = max(min(vd, 100), -100)
        turn = get_turn(vd, vi, 1)
    else:
        speed = max(min(vi, 100), -100)
        turn = get_turn(vi, vd, -1)


with ev3.TwoWheelVehicle(
    RADIUS_WHEEL,  # radius_wheel
    TREAD_DISTANCE,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
    speed=40
) as vehicle:

    def drive():
        global speed, turn
        if speed != 0:
            vehicle.move(speed, turn)
        else:
            vehicle.stop(brake=True)

    drive_task = Periodic(0.1, drive)
    drive_task.start()

    # Iniciamos un bucle que procesa siempre la imagen más reciente
    while True:
        # Si pasan 5 segundos sin recibir imágenes, la transmisión se cortó
        if time.time() - camara.t_ultimo > 5:
            print("Stream ended...")
            break
        # Tomamos la imagen más reciente que recibió el hilo de la cámara
        frame = camara.leer()
        if frame is None:
            # Todavía no ha llegado ninguna imagen: esperamos un momento
            cv2.waitKey(1)
            continue
        # Rotamos el frame 90 grados en sentido horario
        frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
        # Convertimos el frame de BGR (Azul, Verde, Rojo) a escala de grises
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # Aplicamos un filtro gaussiano para reducir el ruido
        # gray = cv2.GaussianBlur(gray, (10, 10), 0)
        # Obtenemos las dimensiones de la imagen en escala de grises
        height, width = gray.shape

        # Estraemos el vector de valores de intensidad de la línea horizontal central
        horizontal_line = gray[height // 2, :]
        horizontal_line_mean = horizontal_line.mean()
        umbral = horizontal_line_mean * 0.7
        pesos = np.maximum(0, umbral - horizontal_line)
        centro_x = center_of_mass(pesos)[0]
        centro_x = centro_x if not np.isnan(centro_x) else width // 2   

        # Calculamos el error como la diferencia entre el centro de la imagen y el centro de masa
        error_x = (width / 2) - centro_x

        # Calculamos la velocidad rotacional basada en el error
        vr_new = Kp * error_x
        update_speed_turn(vt_new, vr_new)

        # Ploteamos la intensidad de la línea horizontal central en la imagen
        intensity_plot.set_xdata(range(len(horizontal_line)))
        intensity_plot.set_ydata(horizontal_line)
        center_x_plot.set_xdata([centro_x])
        center_x_plot.set_ydata([horizontal_line[int(centro_x)]])

        # Agregamos la referencia y la medida de este instante a la gráfica temporal
        t = time.time() - t_inicio
        tiempos.append(t)
        referencias.append(width / 2)
        medidas.append(centro_x)
        # Borramos los datos más viejos que la ventana de tiempo
        while tiempos[0] < t - VENTANA:
            tiempos.pop(0)
            referencias.pop(0)
            medidas.pop(0)
        referencia_plot.set_data(tiempos, referencias)
        medida_plot.set_data(tiempos, medidas)
        ax_t.set_xlim(max(0, t - VENTANA), max(VENTANA, t))
        ax_t.set_ylim(0, width)

        fig.canvas.draw_idle()
        fig.canvas.flush_events()

        # Dibujamos una linea vertical en el centro de la imagen
        cv2.line(frame, (width // 2, 0), (width // 2, height), (0, 0, 200), 2)
        # Dibujamos una línea horizontal en el centro de la imagen
        cv2.line(frame, (0, height // 2), (width, height // 2), (0, 0, 255), 2)
        # Dibujamos un círculo en el centro de masa calculado
        cv2.circle(frame, (int(centro_x), height // 2), 8, (0, 0, 255), -1)
        cv2.circle(frame, (int(centro_x), height // 2), 5, (0, 0, 0), -1)

        # Mostramos el frame en una ventana llamada 'Camara'
        # cv2.resize() redimensiona la imagen a 480x640 píxeles
        cv2.imshow('Camara', 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

    drive_task.stop()
    vehicle.stop(brake=True)
    time.sleep(1)  # Wait for the vehicle to stop
    print("Programa terminado")

# Cerramos la cámara y el hilo que la lee
camara.cerrar()
# Cerramos todas las ventanas creadas por OpenCV
cv2.destroyAllWindows()