import socket
from robodk import robolink, robomath

ROBOT_IP   = "192.168.0.1"
ROBOT_PORT = 8888
BUFFER_SIZE = 1024

RDK = robolink.Robolink()
ROBOT = RDK.Item('Arctos', robolink.ITEM_TYPE_ROBOT)

socket_robot = None  # Socket global para mantener conexión persistente

# Función auxiliar para recibir respuesta completa
def recv_full_response(sock):
    response = ""
    sock.settimeout(2.0)
    try:
        while True:
            part = sock.recv(BUFFER_SIZE).decode(errors='ignore')
            response += part
            if 'ok' in response.lower() or 'error' in response.lower():
                break
    except socket.timeout:
        pass
    return response.strip()

# Conexión persistente al iniciar RoboDK
def Connect(params=None):
    global socket_robot
    socket_robot = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    socket_robot.settimeout(3.0)
    try:
        socket_robot.connect((ROBOT_IP, ROBOT_PORT))
        socket_robot.sendall(b"?\n")
        resp = recv_full_response(socket_robot)
        if 'ok' in resp.lower():
            print(f"[Arctos_Driver] Conectado exitosamente a {ROBOT_IP}:{ROBOT_PORT}")
            return True
        else:
            print(f"[Arctos_Driver] Fallo en respuesta inicial: {resp}")
            socket_robot.close()
            return False
    except Exception as e:
        print(f"[Arctos_Driver] Error al conectar: {e}")
        socket_robot.close()
        return False

# Cierre de conexión cuando se desconecta
def Disconnect():
    global socket_robot
    if socket_robot:
        socket_robot.close()
        socket_robot = None
        print("[Arctos_Driver] Desconectado correctamente.")

# Envío de comandos manteniendo conexión abierta
def SendRobotCommand(cmd):
    global socket_robot
    if not socket_robot:
        print("[Arctos_Driver] Socket no conectado.")
        return False
    try:
        socket_robot.sendall((cmd.strip() + "\n").encode())
        resp = recv_full_response(socket_robot)
        print(f"[Arctos_Driver] ► Enviado: {cmd} ─ Recibido: {resp}")
        return 'ok' in resp.lower()
    except Exception as e:
        print(f"[Arctos_Driver] Error al enviar comando '{cmd}': {e}")
        return False

# Movimientos tipo Joint (G0 con ejes A-F)
def MoveJ(joints):
    parts = ["G90", "G0"]
    names = ['A', 'B', 'C', 'D', 'E', 'F']
    for i, ang in enumerate(joints[:6]):
        parts.append(f"{names[i]}{ang:.3f}")
    return SendRobotCommand(" ".join(parts))

# Movimientos lineales (X,Y,Z,R,P,W)
def MoveL(pose):
    x, y, z, r, p, w = robomath.pose_2_xyzrpw(pose)
    cmd = f"G90 G1 X{x:.3f} Y{y:.3f} Z{z:.3f} R{r:.3f} P{p:.3f} W{w:.3f}"
    return SendRobotCommand(cmd)

# Prueba rápida standalone (sin cerrar conexión)
if __name__ == "__main__":
    if Connect():
        SendRobotCommand("G91")          # Incremental
        SendRobotCommand("G0 X10 Y0 Z0") # Movimiento simple
        Disconnect()
    else:
        print("No se pudo conectar en standalone.")