import sys
import socket
import threading
import time

from robot import Robot

from mpc_controller import MPCController

from aruco_estimator import ArucoEstimator


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.robot_ids = {}
        for r in self.robots:
            self.robot_ids[r.id] = r

        # socket for movement commands
        self.comm_socket = socket.socket()
        self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

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

        self.comm_socket.bind(('', 1337))
        self.comm_socket.listen(5)

        self.t = time.time()

        # start thread for marker position detection
        self.estimator = ArucoEstimator(self.robot_ids.keys())
        self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
        self.estimator_thread.start()

        self.controller = MPCController(self.estimator)

    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)


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


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