import socket
import threading
import json


class Robot:
    def __init__(self, marker_id, ip, measurement_server=('127.0.0.1', 42424)):
        self.id = marker_id

        self.t_last_measurement = None
        self.x = None
        self.y = None
        self.angle = None

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

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

        self.connected = False

        self.measurement_server = measurement_server
        self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # TCP socket
        self.measurement_thread = threading.Thread(target=self.receive_measurements)


    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("error: could not connect to robot {} with ip {}".format(self.id, self.ip))

        # connect to measurement server
        print(f"connecting to measurement server on {self.measurement_server} ...")

        try:
            self.measurement_socket.connect(self.measurement_server)
            self.measurement_socket.sendall(f"{self.id}\n".encode())

            self.measurement_socket.settimeout(0.1)
            # check if we receive data from the measurement server
            response = self.measurement_socket.recv(1024)
            if not 'error' in str(response):
                print("... connected! -> start listening for events")
                self.measurement_socket.settimeout(None)
                # if so we start the measurement thread
                self.measurement_thread.start()
            else:
                print(f"error: cannot communicate with the measurement server.\n       The response was: {response}")
        except socket.timeout:
            print(f"error: the measurement server did not respond with data.")
        except ConnectionRefusedError:
            print(f"error: could not connect to measurement server at {self.measurement_server}.")


    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

    def receive_measurements(self):
        receiving = True
        while receiving:
            received = str(self.measurement_socket.recv(1024), "utf-8")
            if len(received) > 0:
                measurement = json.loads(received)
                self.t_last_measurement = measurement['t']
                self.x = measurement['x']
                self.y = measurement['y']
                self.angle = measurement['angle']
            else:
                receiving = False
                print(f"measurement server stopped sending data for robot {self.id}")

    def get_measurement(self):
        return (self.t_last_measurement, self.x, self.y, self.angle)