import sys
import socket
import threading
import time
import json

from robot import Robot, ControlledRobot

from mpc_controller import MPCController


class RemoteController:
    def __init__(self):
        self.robots = []
        #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
        #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')]
        #self.robots = [Robot(14, '10.10.11.89')]
        self.robots = [ControlledRobot(13, '192.168.1.13'), ControlledRobot(12, '192.168.1.12')]

        self.robot_ids = {}
        for r in self.robots:
            self.robot_ids[r.id] = r
        self.controlled_robot = self.robots[0].id

        for robot in self.robots:
            robot.connect()

        # thread for handling events received by measurement server (from GUI)
        self.event_thread = threading.Thread(target=self.handle_events)
        self.event_thread.daemon = True

        # connect to socket for events from GUI
        try:
            HOST, PORT = "localhost", 42424
            self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.event_socket.connect((HOST, PORT))
            self.event_socket.sendall("events\n".encode())
            self.event_socket.settimeout(0.1)
            # check if we receive data from the measurement server
            response = self.event_socket.recv(1024)
            if 'error' not in str(response):
                print("... connected! -> start listening for measurements")
                self.event_socket.settimeout(None)
                # if so we start the measurement thread
                self.event_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.")


        #sock.sendall(f"{robot_id}\n".encode())     # request data for robot with given id
        # sock.sendall(f"events\n".encode())        # request events
        # receiving = True
        # while receiving:
        #     received = str(sock.recv(1024), "utf-8")
        #     print("Received: {}".format(received))
        #     receiving = len(received) > 0

        self.t = time.time()

        # start thread for marker position detection
        self.controllers = []
        for r in self.robots:
            c = MPCController()
            self.controllers.append(c)
            r.attach_controller(c)

    def handle_events(self):
        receiving = True
        while receiving:
            received = str(self.event_socket.recv(1024), "utf-8")
            if len(received) > 0:
                last_received = received.split('\n')[-2]
                event_type, payload = json.loads(last_received)
                print(event_type, payload)

                if event_type == 'click':
                    target_pos = (payload['x'], payload['y'], payload['angle'])
                    self.robot_ids[self.controlled_robot].move_to_pos(target_pos=target_pos)
                elif event_type == 'controlled_robot':
                    self.controlled_robot = payload['robot_id']
            else:
                receiving = False
                print(f"measurement server stopped sending event data")

    def run(self):

        #print("waiting until all markers are detected...")
        #while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
        #    pass
        print("starting control")
        #self.controller.interactive_control(robots=self.robots)
        for c in self.controllers:
            c.start()
        pass


def main(args):
    rc = RemoteController()
    rc.run()


if __name__ == '__main__':
    main(sys.argv)

# to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams
# marker positions and GUI events via socket
# next, launch this program (mpc_test.py)
# you can now direct the robots via the GUI (assuming four corner markers for position reference have been detected)
# click any position on the screen and the current robot will drive there
# you can also switch which robot should be controlled in the GUI

# TODO document and improve program structure