import gradio as gr
from lerobot.robots import LeFeetech
import time

# Inicializa conexión con robot Feetech
try:
    robot = LeFeetech(port="/dev/ttyUSB0")  # Cambia este puerto si es necesario
    motors = robot.motors
except Exception as e:
    raise RuntimeError(f"Error al conectar con el robot: {e}")

# Función para mover los motores desde los sliders
def controlar_robot(motor1_pos, motor2_pos, motor3_pos):
    try:
        # Envía posición a cada motor (ajusta según IDs reales)
        motors[0].goto(motor1_pos)
        motors[1].goto(motor2_pos)
        motors[2].goto(motor3_pos)
        
        time.sleep(0.5)  # espera para que el robot se mueva
        # Podrías agregar aquí sensores o cámara para devolver imagen
        return f"Motor1: {motor1_pos}, Motor2: {motor2_pos}, Motor3: {motor3_pos}"
    except Exception as err:
        return f"Error: {err}"

# Interfaz
with gr.Blocks() as demo:
    gr.Markdown("# Control en Tiempo Real del Robot LeRobot con Feetech")
    
    with gr.Row():
        motor1 = gr.Slider(0, 1023, value=512, label="Motor 1")
        motor2 = gr.Slider(0, 1023, value=512, label="Motor 2")
        motor3 = gr.Slider(0, 1023, value=512, label="Motor 3")

    estado = gr.Textbox(label="Estado del Robot")
    
    # Actualiza cada vez que se mueva algún slider
    motor1.change(fn=controlar_robot, inputs=[motor1, motor2, motor3], outputs=estado)
    motor2.change(fn=controlar_robot, inputs=[motor1, motor2, motor3], outputs=estado)
    motor3.change(fn=controlar_robot, inputs=[motor1, motor2, motor3], outputs=estado)

demo.launch()