import socket

class Robot:
    def __init__(self, marker_id, ip):
        self.id = marker_id
        self.pos = None
        self.euler = None

        self.ip = ip
        self.socket = socket.socket()

        # currently active control
        self.u1 = 0.0
        self.u2 = 0.0

        self.connected = False

    def connect(self):
        # connect to robot
        try:
            print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
            self.socket.connect((self.ip, 1234))  # connect to robot
            print("connected!")
            self.connected = True
        except socket.error:
            print("could not connect to robot {} with ip {}".format(self.id, self.ip))

    def send_cmd(self, u1=0.0, u2=0.0):
        if self.socket:
            try:
                self.socket.send(f'({u1},{u2})\n'.encode())
            except BrokenPipeError:
                print(f"error: connection to robot {self.id} with ip {self.ip} lost")
                pass
            except ConnectionResetError:
                print(f"error: connection to robot {self.id} with ip {self.ip} lost")
                pass